/***************************************************************************
**
**  This file is part of WaranCore.
**
**  WaranCore is free software: you can redistribute it and/or modify
**  it under the terms of the GNU General Public License as published by
**  the Free Software Foundation, either version 3 of the License, or
**  (at your option) any later version.
**
**  WaranCore is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.
**
**  You should have received a copy of the GNU General Public License
**  along with Foobar.  If not, see <http://www.gnu.org/licenses/>
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2009-07-01
**  Copyright: 2009-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <stdlib.h>
#include <stdio.h>

#include "GpsBlock.h"
#include "GpsFix.h"

namespace WaranCore {

/*!
  \class GpsBlock GpsBlock.h
  \brief Brief description of class still missing

  Full description of class still missing
*/

inline unsigned char GpsBlock::size(unsigned int flags, unsigned char flagOffset)
{
  switch((flags >> flagOffset) & 0x00000003) {
  case 0:
    break;
  case 1:
    return 1;
  case 2:
    return 2;
  case 3:
    return 4;
  }
  return 0;
}

/*!
  Parse buffer \a bytes. \a bytes must be at least 4-byte long.
*/
GpsBlock::GpsBlock(const char * bytes)
{
  _bytes=bytes;
  unsigned int flags=*reinterpret_cast<const unsigned int *>(_bytes);
  _sizes[0]=size(flags, Longitude);
  _sizes[1]=size(flags, Latitude);
  _sizes[2]=size(flags, Altitude);
  _sizes[3]=size(flags, HorizontalAccuracy);
  _sizes[4]=size(flags, HorizontalDop);
  _sizes[5]=size(flags, NorthingDop);
  _sizes[6]=size(flags, EastingDop);
  _sizes[7]=size(flags, SateliteCount);
}

unsigned char GpsBlock::index() const
{
  return *reinterpret_cast<const unsigned int *>(_bytes) & 0x000000FF;
}

unsigned char GpsBlock::state() const
{
  return (*reinterpret_cast<const unsigned int *>(_bytes) & 0x00000300) >> 8;
}

/*!
  Returns the total size of the payload (including state, index and flags). Initialize the internal individual sizes.
*/
unsigned char GpsBlock::totalSize() const
{
  unsigned char s=0;
  for(int i=0;i<8;i++) {
    s+=_sizes[i];
    //printf("size[%i]=%i\n", i, _sizes[i]);
  }
  //printf("flags %08X\n",flags);
  return s+4;
}

inline void GpsBlock::decode(const char *& ptr, char size, int& value) const
{
  switch(size) {
  case 4:
    //printf("Diff=%i\n", *reinterpret_cast<const int*>(ptr));
    value+=*reinterpret_cast<const int*>(ptr);
    ptr+=4;
    break;
  case 2:
    //printf("Diff=%i\n", *reinterpret_cast<const short*>(ptr));
    value+=*reinterpret_cast<const short*>(ptr);
    ptr+=2;
    break;
  case 1:
    //printf("Diff=%i\n", *reinterpret_cast<const char*>(ptr));
    value+=*reinterpret_cast<const char*>(ptr);
    ptr++;
    break;
  default:
    break;
  }
}

inline void GpsBlock::decode(const char *& ptr, char size, unsigned short& value) const
{
  switch(size) {
  case 2:
    //printf("Diff=%hi\n", *reinterpret_cast<const short*>(ptr));
    value+=*reinterpret_cast<const short*>(ptr);
    ptr+=2;
    break;
  case 1:
    //printf("Diff=%hhi\n", *reinterpret_cast<const char*>(ptr));
    value+=*reinterpret_cast<const char*>(ptr);
    ptr++;
    break;
  default:
    break;
  }
}

inline void GpsBlock::decode(const char *& ptr, char size, unsigned char& value) const
{
  if(size==1) {
    //printf("Diff=%hhi\n", *reinterpret_cast<const char*>(ptr));
    value+=*reinterpret_cast<const char*>(ptr);
    ptr++;
  }
}

/*!

*/
void GpsBlock::decode(GpsFix& fix) const
{
  const char * ptr=_bytes+4;
  decode(ptr, _sizes[0], fix.longitude());
  decode(ptr, _sizes[1], fix.latitude());
  decode(ptr, _sizes[2], fix.altitude());
  decode(ptr, _sizes[3], fix.horizontalAccuracy());
  decode(ptr, _sizes[4], fix.horizontalDop());
  decode(ptr, _sizes[5], fix.northingDop());
  decode(ptr, _sizes[6], fix.eastingDop());
  decode(ptr, _sizes[7], fix.sateliteCount());
  //printf("Longitude %hhi %i\n",_sizes[0], fix.longitude());
  //printf("Latitude %hhi %i\n",_sizes[1], fix.latitude());
}

inline unsigned int GpsBlock::encode(char *& ptr, unsigned char flagOffset, int oldValue, int newValue)
{
  int diff=newValue-oldValue;
  //printf("Diff[%hhi]=%i\n", flagOffset, diff);
  int posDiff=abs(diff);
  if(posDiff>0x00007FFF) {
    //printf("size[%hhi]=4\n", flagOffset);
    *reinterpret_cast<int*>(ptr)=diff;
    ptr+=4;
    return 3 << flagOffset;
  } else if(posDiff>0x0000007F) {
    //printf("size[%hhi]=2\n", flagOffset);
    *reinterpret_cast<short*>(ptr)=diff;
    ptr+=2;
    return 2 << flagOffset;
  } else if(posDiff>0x00000000) {
    //printf("size[%hhi]=1\n", flagOffset);
    *reinterpret_cast<char*>(ptr)=diff;
    ptr++;
    return 1 << flagOffset;
  } else {
    //printf("size[%hhi]=0\n", flagOffset);
    return 0;
  }
}

inline unsigned int GpsBlock::encode(char *& ptr, unsigned char flagOffset, short oldValue, short newValue)
{
  short diff=newValue-oldValue;
  //printf("Diff[%hhi]=%hi\n", flagOffset, diff);
  short posDiff=abs(diff);
  if(posDiff>0x007F) {
    //printf("size[%hhi]=2\n", flagOffset);
    *reinterpret_cast<short*>(ptr)=diff;
    ptr+=2;
    return 2 << flagOffset;
  } else if(posDiff>0x0000) {
    //printf("size[%hhi]=1\n", flagOffset);
    *reinterpret_cast<char*>(ptr)=diff;
    ptr++;
    return 1 << flagOffset;
  } else {
    //printf("size[%hhi]=0\n", flagOffset);
    return 0;
  }
}

inline unsigned int GpsBlock::encode(char *& ptr, unsigned char flagOffset, char oldValue, char newValue)
{
  char diff=newValue-oldValue;
  //printf("Diff[%hhi]=%hhi\n", flagOffset, diff);
  if(diff!=0x0000) {
    //printf("size[%hhi]=1\n", flagOffset);
    *reinterpret_cast<char*>(ptr)=diff;
    ptr++;
    return 1 << flagOffset;
  } else {
    //printf("size[%hhi]=0\n", flagOffset);
    return 0;
  }
}

/*!
    24 bits    flags for size (number of bytes) of each value
                 0 to 3 (2 bits) for int values
                 0 to 2 (2 bits) for short values
                 0 to 1 (2 bits) for char values
     2 bits    fix state
     8 bits    index of block
               values

  Values are stored in order(maximum length):
    longitude(int)
    latitude(int)
    altitude(int)
    horizontal accuracy(int)
    horizontal dop(short)
    northing dop(short)
    easting dop(short)
    satellite count(char)

  Returned pointer must be deleted manually.
*/
char * GpsBlock::encode(unsigned char index, const GpsFix& oldFix,
                        const GpsFix& newFix, unsigned int& byteCount)
{
  char * bytes=new char[32];
  unsigned int& flags=*reinterpret_cast<unsigned int *>(bytes);
  flags=index;
  flags|=(newFix.state() & 0x03) << 8;
  char * ptr=bytes+4;
  flags|=encode(ptr, Longitude, oldFix.longitude(), newFix.longitude());
  flags|=encode(ptr, Latitude, oldFix.latitude(), newFix.latitude());
  flags|=encode(ptr, Altitude, oldFix.altitude(), newFix.altitude());
  flags|=encode(ptr, HorizontalAccuracy, oldFix.horizontalAccuracy(), newFix.horizontalAccuracy());
  flags|=encode(ptr, HorizontalDop, oldFix.horizontalDop(), newFix.horizontalDop());
  flags|=encode(ptr, NorthingDop, oldFix.northingDop(), newFix.northingDop());
  flags|=encode(ptr, EastingDop, oldFix.eastingDop(), newFix.eastingDop());
  flags|=encode(ptr, SateliteCount, oldFix.sateliteCount(), newFix.sateliteCount());
  byteCount=ptr-bytes;
  //printf("Longitude %i, %i\n", (flags >> Longitude) & 0x00000003, newFix.longitude());
  //printf("Latitude %i, %i\n", (flags >> Latitude) & 0x00000003,newFix.latitude());
  return bytes;
}

} // namespace WaranCore
