/**
* @brief Protocol handler for LG AG1010 Power generator/supplier
*
* The protocol is to handle the Hexidecimal strings sending and receiving from the device,
* the protocol also calculates the CRC for each data sum sent out
*
* Author: Jing Chen, jgn@ansto.gov.au
*
******************************************************************************************/

#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>

static int dbgprintf(char* fmtstr, ...);

typedef unsigned char BYTE;

BYTE updateCRC8(BYTE data, BYTE previousCrc) {
  // Polynomial: x^8 + x^5 + x^4 + 1
  BYTE crc = previousCrc;
  int b;
  for(b = 0; b < 8; b++) {
    if((crc ^ data) & 0x01) {
      crc = crc ^ 0x18;
      crc = crc >> 1;
      crc = crc | 0x80;
    } else {
      crc = crc >> 1;
      crc = crc & 0x7f;
    }
    data = data >> 1;
  }

  return crc;
}

BYTE char_to_int(char c){
  BYTE tmp;
  if ('0' <= c && c <= '9') {
    tmp = c - '0';
  } else if ('a' <= c && c <= 'f') {
    tmp = 10 + c - 'a';
  } else if ('A' <= c && c <= 'F') {
    tmp = 10 + c - 'A';
  } else {
    printf("Error in string.\n");
    return NULL;
  }

  return tmp;
}

BYTE getHexCode(BYTE startPos, char *str){

  BYTE tmp = 0;

  tmp = (0x0F & char_to_int(str[startPos])) << 4;
  tmp = tmp | (0x0F & char_to_int(str[startPos+1]));

  return tmp;
}


/** @brief Send the Hexdecimal string with CRC 
*/
int LFGenWriteStart(Ascon *a) {

  char          *pNewBuffer;
  unsigned short bufSize, bufInd;

  double now;

  // Get real packet information from the buffer
  char *curText = GetCharArray(a->wrBuffer);
  BYTE HEAD, LEN, CTRL, CRC=0;
  HEAD = getHexCode(0, curText);
  LEN  = getHexCode(2, curText);

  // buffer size, 
  pNewBuffer = (char *)malloc((LEN+2)*sizeof(char));
  bufInd=0;
  pNewBuffer[bufInd] = HEAD;
  bufInd++;
  pNewBuffer[bufInd] = LEN;
  bufInd++;
  CRC = updateCRC8(HEAD, CRC);
  CRC = updateCRC8(LEN, CRC);

  if(LEN<2){
    a->state = AsconIdle;
    AsconError(a, "Error Datasum Provided!", 0);
    return 0;
  } else {
    CTRL = getHexCode(4, curText);
    pNewBuffer[bufInd] = CTRL;
    bufInd++;
    CRC = updateCRC8(CTRL, CRC);
    if(LEN>2){
      int i;
      for(i=0; i<LEN-2; i++){
        pNewBuffer[bufInd] = getHexCode(bufInd*2, curText);
        CRC = updateCRC8(pNewBuffer[bufInd], CRC);
        bufInd++;
      }
    }
  }

  pNewBuffer[bufInd] = CRC;
  bufInd++;
  //pNewBuffer[bufSize-1] = '\r';
  DynStringClear(a->wrBuffer);
  DynStringConcatBytes(a->wrBuffer, pNewBuffer, bufInd);
  a->wrPos = 0;
  a->state = AsconWriting;
  /* if there is only the terminator, do not send it */
  if (GetDynStringLength(a->wrBuffer) <= 1) {
    a->state = AsconWriteDone;
  }
  //dbgprintf("HEAD=%02x; LEN=%02x; CTRL=%02x; DATA=%s; CRC=%2x -> %s\n", HEAD, LEN, CTRL, Data, CRC, pNewBuffer);
  free(pNewBuffer);
  return 1;
}


int LFConcatChar(pDynString self, char chr){

  int  ret;
  char *lfChr[2];

  BYTE  lfByte = (BYTE)chr;
  sprintf(lfChr, "%02x", lfByte); 
  DynStringConcat(self, lfChr);
  
  return 1;
}

int LFGenReading(Ascon *a)
{
  int  ret;
  char chr=0;
  BYTE *lfchar, HEAD, LEN;
  //BYTE HEAD, LEN, CTRL, CRC=0, *Data=NULL;

  // Read HEAD info
  usleep(1000*50);
  ret = AsconReadChar(a->fd, &chr);
  a->start = DoubleTime();
  if(ret<0) {
    AsconError(a, "AsconReadChar failed in Head Info reading in LFGenReading:", 0);
    //a->state = AsconReadDone;
    return 0;
  } else {
    while (ret>=0) {
      if(ret>0) {
	HEAD = (BYTE)chr;
	if (HEAD==0x96) {
	  break;
	}
      } 

      if (a->timeout > 0) {
	if (DoubleTime() - a->start > 0.05) {  //a->timeout) {
	  AsconError(a, "read timeout", 0);
	  a->state = AsconTimeout;
	  return 1;
	}      
      }
      ret = AsconReadChar(a->fd, &chr);
    } 
  }

  // read LEN info
  ret = AsconReadChar(a->fd, &chr);
  if(ret<0) {
    AsconError(a, "AsconReadChar failed in LEN reading in LFGenReading:", 0); 
    //a->state = AsconReadDone;
    return 0; 
  }
  LEN = (BYTE)chr;
  if(LEN<2) { 
    AsconError(a, "Reading Error from the LF AG1010 device, wrong LEN info", 0);
    a->state = AsconReadDone;
    return 0;
  }

  lfchar = malloc(sizeof(BYTE)*(LEN+2));
  int ind=0;
  lfchar[ind] = HEAD; ind++;
  lfchar[ind] = LEN; ind++;
 
  // read all other chars
  while(ind<LEN+2) {
    ret = AsconReadChar(a->fd, &chr);
    if (ret < 0) {
      AsconError(a, "AsconReadChar failed in LFGenReading:", 0);
      //a->state = AsconReadDone;
      return 0;
    }
    lfchar[ind] = (BYTE)chr; ind++;
  }

  DynStringClear(a->rdBuffer);
  int i;
  for(i=0; i<ind; i++) {
    LFConcatChar(a->rdBuffer, lfchar[i]);
  }
  a->state = AsconReadDone;

  free(lfchar);
  //DynStringConcatChar(a->rdBuffer, '\0');
  return 1;
}

int LFGenProtHandler(Ascon *a)
{
  int ret;

  switch(a->state){
    case AsconWriteStart:
      ret = LFGenWriteStart(a);
      return ret;
      break;
    case AsconReading:
      return LFGenReading(a);
      break;
    default:
      return AsconStdHandler(a);
  }
}

void AddLFGenProtocol()
{
  AsconProtocol *prot = NULL;

  prot = calloc(sizeof(AsconProtocol), 1);
  prot->name = strdup("lfgen");
  prot->init = AsconStdInit;
  prot->handler = LFGenProtHandler;
  AsconInsertProtocol(prot);
}

#include <stdio.h>
#include <stdarg.h>

static int dbgprintf(char* fmtstr, ...) {
    FILE* fp = NULL;
    int ret = 0;
    fp = fopen("/home/jgn/tmp/lfdbg.txt", "a");

    if (fp != NULL) {
      va_list ap;
      va_start(ap, fmtstr);
      ret = vfprintf(fp, fmtstr, ap);
      va_end(ap);
      fclose(fp);
    }
    return ret;
}