/*
   Copyright (C) 2004 T. Scott Dattalo

This file is part of gpsim.

gpsim 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 2, or (at your option)
any later version.

gpsim 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 gpsim; see the file COPYING.  If not, write to
the Free Software Foundation, 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.  */


#include "protocol.h"

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

#ifdef putc
#undef putc
#endif

unsigned int a2i(char b)
{

  if( b>='0'  && b<='9')
    return b-'0';

  if( b>='A' && b<='F')
    return b-'A'+10;

  if( b>='a' && b<='f')
    return b-'a'+10;

  return 0;

}
char i2a(unsigned int i)
{
  i &= 0x0f;
  if(i < 10)
    return '0' + i;

  return 'A'+ i- 10;
}

unsigned int ascii2uint(char **buffer, int digits)
{

  unsigned int ret = 0;
  char *b = *buffer;

  for(int i=0; i<digits; i++)
    ret = (ret << 4) + a2i(*b++);

  *buffer = b;

  return ret;
}


static unsigned int ascii2uint64(char *buffer, int digits)
{

  unsigned int ret = 0;
  char *b = buffer;

  for(int i=0; i<digits; i++)
    ret = (ret << 4) + a2i(*b++);

  return ret;
}

static unsigned int ascii2uint(char *buffer, int digits)
{

  unsigned long long i = ascii2uint64(buffer,digits);

  return (unsigned int) i;
}

//========================================================================

PacketBuffer::PacketBuffer(unsigned int _size)
  : size(_size)
{
  buffer = new char[_size];
  index = 0;
}

PacketBuffer::~PacketBuffer()
{
  delete [] buffer;
}


void PacketBuffer::puts(const char *s, int len)
{
  if(!s || len <= 0)
    return;

  unsigned int ulen = len;
  if(ulen > (size - index)) {
    ulen = size-index;
  }

  if(ulen) {
    memcpy(&buffer[index],s, ulen);
    index += ulen;
  }
}

void PacketBuffer::advanceIndex(unsigned int amount)
{
  if(index + amount <  size)
    index += amount;
  else
    index = size-1;
}

void PacketBuffer::terminate()
{
  if(index < size)
    buffer[index]=0;
}
//========================================================================
Packet::Packet(unsigned int rxsize, unsigned int txsize)
{
  rxBuffer = new PacketBuffer(rxsize);
  txBuffer = new PacketBuffer(txsize);
}

bool Packet::DecodeHeader()
{
  
  if(*rxBuffer->buffer == '$') {
    rxBuffer->index = 1;
    return true;
  }

  rxBuffer->index = 0;

  return false;
}

bool Packet::DecodeChar(char c)
{
  if(*rxBuffer->getBuffer() == c) {
    rxBuffer->index++;
    return true;
  }

  return false;
}

bool Packet::DecodeUInt32(unsigned int &i)
{
  char *b = rxBuffer->getBuffer();

  if(ascii2uint(&b,2) == eGPSIM_TYPE_UINT32) {

    i = ascii2uint(b,8);
    rxBuffer->index += 2+8;

    return true;
  }
  return false;
}

bool Packet::DecodeUInt64(guint64  &i)
{
  char *b = rxBuffer->getBuffer();

  if(ascii2uint(&b,2) == eGPSIM_TYPE_UINT64) {

    i = ascii2uint64(b,16);
    rxBuffer->index += 2+16;

    return true;
  }
  return false;
}

bool Packet::DecodeBool(bool  &b)
{
  char *buff = rxBuffer->getBuffer();

  if(ascii2uint(&buff,2) == eGPSIM_TYPE_BOOLEAN) {

    if(*buff == '0') 
      b = false;
    else if(*buff == '1')
      b = true;
    else
      return false;

    rxBuffer->index += 2+1;

    return true;
  }
  return false;
}

bool Packet::DecodeFloat(double  &d)
{
  char *b = rxBuffer->getBuffer();

  if(ascii2uint(&b,2) == eGPSIM_TYPE_FLOAT) {

    double dtry = strtod(b, &b);
    unsigned int len = b - rxBuffer->buffer; 
    if( len < rxBuffer->size - rxBuffer->index) {

      rxBuffer->index += len;
      d = dtry;
      return true;
    }
  }
  return false;
}

bool Packet::DecodeObjectType(unsigned int &i)
{
  i = ascii2uint(rxBuffer->getBuffer(),2);
  rxBuffer->index += 2;

  return true;
}

bool Packet::DecodeString(char *retStr, int maxLen)
{

  char *b = rxBuffer->getBuffer();

  if(ascii2uint(&b,2) == eGPSIM_TYPE_STRING) {
    int length = ascii2uint(&b,2);

    maxLen--;   // reserve space for a terminating 0.

    length = (maxLen < length) ? maxLen  : length;

    strncpy(retStr, b, length);
    retStr[length] = 0;

    //*buffer = b + length;

    rxBuffer->index += 2+2+length;
    return true;
  }

  return false;
}

bool Packet::EncodeHeader()
{

  txBuffer->putc('$');
  txBuffer->terminate();

  return true;
}

bool Packet::EncodeUInt32(unsigned int i)
{

  txBuffer->putc(i2a(eGPSIM_TYPE_UINT32 /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_UINT32 ));

  for(int j=7; j>=0; j--)
    txBuffer->putc ( i2a ( i>> (4*j)));

  return true;
}

bool Packet::EncodeUInt64(guint64  i)
{

  txBuffer->putc(i2a(eGPSIM_TYPE_UINT64 /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_UINT64 ));

  for(int j=15; j>=0; j--)
    txBuffer->putc ( i2a ( i>> (4*j)));

  return true;
}


bool Packet::EncodeObjectType(unsigned int i)
{
  EncodeHeader();

  //txBuffer->putc(i2a(eGPSIM_TYPE_OBJECT /16));
  //txBuffer->putc(i2a(eGPSIM_TYPE_OBJECT ));

  txBuffer->putc ( i2a ( i>> (4*1)));
  txBuffer->putc ( i2a ( i>> (4*0)));

  return true;

}

bool Packet::EncodeBool(bool b)
{

  txBuffer->putc(i2a(eGPSIM_TYPE_BOOLEAN /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_BOOLEAN ));

  if(b)
    txBuffer->putc('1');
  else
    txBuffer->putc('0');


  return true;
}

bool Packet::EncodeFloat(double  d)
{
  txBuffer->putc(i2a(eGPSIM_TYPE_FLOAT /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_FLOAT ));

  char buff[256];

  snprintf(buff,sizeof(buff),"%8E~",d);

  txBuffer->puts(buff,strlen(buff));

  return true;
}


bool Packet::EncodeString(const char *str, int len)
{
  if(!str)
    return false;

  txBuffer->putc(i2a(eGPSIM_TYPE_STRING /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_STRING ));

  if(len < 0)
    len = strlen(str);

  txBuffer->putc(i2a(len>>4));
  txBuffer->putc(i2a(len));

  txBuffer->puts(str, len);

  return true;
}

bool Packet::EncodeCustom(const char *str, int len)
{
  if(!str)
    return false;

  txBuffer->putc(i2a(eGPSIM_TYPE_CUSTOM /16));
  txBuffer->putc(i2a(eGPSIM_TYPE_CUSTOM ));

  txBuffer->putc(i2a(len>>4));
  txBuffer->putc(i2a(len));

  txBuffer->puts(str, len);

  return true;
}