#include <ctype.h>
#include <math.h>
#include "ascon.h"
#include "ascon.i"
#include "dynstring.h"
#include "cnvrt.h"

/*
 * this is a (scriptcontext) general binary protocol driver
 *
 * Markus Zolliker Aug 2010
 *
 * conversion to and from binary
 *
 * Syntax:
 * space separated items, the command first, a slash and then the read format
 * 
 * commandItem ... / formatItem ...
 *
 * where a commandItem one of the following
 *
 * a number   converted according to the choosen format, i1 is default
 * int<n>     changing the format to <n> byte integers
 * hex        changing the format to hexadecimal (1 byte at a time)
 * float      changing the format to 4 byte ieee float
 * crc        send crc
 * 
 * and formatItem is one of the follwing
 *
 * skip       skip one byte
 * skip<n>    skip <n> bytes
 * code       returned function code, when bit7 is set, the response is
 *            recogized as an error message and the response is dumped
 *            the result
 * int<n>     convert <n> bytes to integer (most significant byte first)
 *            and append the (decimal coded) number to the result
 *            if <n> is omitted, <n> = 1 is assumed
 * hex        convert 1 byte to hexadecimal coded integer
 *            and append it to the response
 * float      convert 4 bytes from ieee float
 *            and append the number to the response
 * crc        check crc (if wrong, "badCRC" is added to the response)
 *
 * multiple items in the response are space separated
 *
 * Usage example: Modbus read (float)
 * command:  address 250, function 3, start-address 8, size 2, crc
 * response: address, function, byte-count, float, crc
 *
 * sct send 250 3 int2 10 2 crc / skip code skip float crc
 *
 * different crc's might be used (argv[2] opf BinInit):
 * modbus-crc: CRC-16-IBM, order: lo,hi
 * keller-crc: CRC-16-IBM, order: hi,lo
 * sycon-crc: sort of mod 256 checksum, byte stuffing included (no float allowed)
 */

typedef enum {intType, hexType, floatType,
  skipType, codeType, crcType, dumpType, errorType} BinDataType;
// dumpType, errorType must be the last items

typedef enum {modbusCrc, kellerCrc, rsportCrc, syconCrc} CrcAlgorithm;

typedef struct {
  CrcAlgorithm crcAlgorithm;
  pDynString inp;
  char *nextFmt;
  pDynString result;
  int expectedChars;
  BinDataType type;
  long iValue;
  int dumpFrom;
  int syconState;
  unsigned char chksum;
} BinPrivate;

/*----------------------------------------------------------------------------*/
static int calc_crc(char *inp, int inpLen)
{
/** CRC-16-IBM Algorithm
 * crc calculation:
 *   returns the 16bit CRC value of a message
 * crc check:
 *   returns 0 when the crc appended to the message (lo byte first)
 *   is correct
 */

  unsigned int crc = 0xffff;
  unsigned int next;
  int carry;
  int n;

  while (inpLen--) {
    next = *(unsigned char *) inp;
    crc ^= next;
    for (n = 0; n < 8; n++) {
      carry = crc & 1;
      crc >>= 1;
      if (carry) {
        crc ^= 0xA001;
      }
    }
    inp++;
  }
  return crc;
}
/*----------------------------------------------------------------------------*/
static int calc_crc8(char *inp, int inpLen)
{
/** CRC-8 (x8+x5+x4+1) Algorithm
 * crc calculation:
 *   returns the 8bit CRC value of a message
 * crc check:
 *   returns 0 when the crc appended to the message is correct
 */

  unsigned char crc = 0;
  unsigned char data;
  int n;

  while (inpLen--) {
    data = *(unsigned char *) inp;
    for (n = 0; n < 8; n++) {
      if ((crc ^ data) & 1) {
        crc ^= 0x18;
        crc >>= 1;
        crc |= 0x80;
      } else {
        crc >>= 1;
        crc &= 0x7f;
      }
      data >>= 1;
    }
    inp++;
  }
  return crc;
}

/*----------------------------------------------------------------------------*/
int BinReadItem(Ascon *a) {
  BinPrivate *p = a->private;
  char item[32];
  int valen;
  int size;
  
  if (sscanf(p->nextFmt, "%30s%n", item, &valen) <= 0) {
    if (p->type < dumpType) {
      p->dumpFrom = GetDynStringLength(a->rdBuffer);
      p->type= dumpType;
    }
    p->expectedChars = 999;
    return 0;
  }

  if (strcasecmp(item, "crc") == 0) {
    p->type = crcType;
    p->expectedChars = 2;
    p->nextFmt += valen;
    return 1;
  } else if (strncasecmp(item, "int", 3) == 0) {
    size = 1;
    sscanf(item + 3, "%d", &size);
    p->expectedChars = size;
    p->type = intType;
    p->iValue = 0;
  } else if (strcasecmp(item, "float") == 0) {
    p->expectedChars = 4;
    p->type = floatType;
  } else if (strcasecmp(item, "hex") == 0) {
    p->expectedChars = 1;
    p->type = hexType;
  } else if (strcasecmp(item, "code") == 0) {
    p->type = codeType;
  } else if (strncasecmp(item, "skip", 4) == 0) {
    size = 1;
    sscanf(item + 4, "%d", &size);
    p->expectedChars = size;
    p->type = skipType;
   } else {
    return -1;
  }
  p->nextFmt += valen;
  return 1;
}


/*----------------------------------------------------------------------------*/
void BinError(Ascon *a, char *text) {
  BinPrivate *p = a->private;
  
  p->type= errorType;
  p->dumpFrom = 0;
  p->expectedChars = 1;
  DynStringCopy(a->errmsg, "BINERR: ");
  DynStringConcat(a->errmsg, text);
  a->state = AsconFailed;
}

/*----------------------------------------------------------------------------*/
void BinToSycon(pDynString dyn) {
  char *str = strdup(GetCharArray(dyn));
  int l = GetDynStringLength(dyn);
  unsigned char sum, byte;
  int i;
  
  DynStringClear(dyn);
  DynStringConcat(dyn, "\x02\x10\x80"); /* STX ADDR CMD */
  sum = 0x10 + 0x80;
  for (i=0; i<l; i++) {
    byte = str[i];
    sum += byte;
    switch (byte) {
    case 0x02:
      DynStringConcatChar(dyn, 0x07);
      DynStringConcatChar(dyn, '0');
      break;
    case 0x0d:
      DynStringConcatChar(dyn, 0x07);
      DynStringConcatChar(dyn, '1');
      break;
    case 0x07:
      DynStringConcatChar(dyn, 0x07);
      DynStringConcatChar(dyn, '2');
      break;
    default:
      DynStringConcatChar(dyn, byte);
    }
  }
  DynStringConcatChar(dyn, 0x30 + (sum / 16));
  DynStringConcatChar(dyn, 0x30 + (sum % 16));
  DynStringConcatChar(dyn, 0x0d);
  free(str);
}

/*----------------------------------------------------------------------------*/
int BinHandler(Ascon *a) {
  int res;
  char *str;
  int len;
  int i, l, pos;
  unsigned int crc;
  pDynString dyn;
  char item[32];
  char data[8];
  int size;
  int valen;
  BinDataType type;
  long iValue;
  double fValue;
  BinPrivate *p = a->private;

  switch (a->state) {
  case AsconWriteStart:
    /* exchange buffers */
    dyn = p->inp;
    p->inp = a->wrBuffer;
    a->wrBuffer = dyn;
    DynStringClear(dyn);
    str = GetCharArray(p->inp);
    len = GetDynStringLength(p->inp);
    l = 0;
    size = 1;
    type = intType;
    for (pos = 0; pos < len; ) {
      if (sscanf(str + pos, "%30s%n", item, &valen) <= 0) {
        BinError(a, "missing '/'");
        return 1;
      }
      pos += valen;
      if (strcasecmp(item, "/") == 0) {
        break;
      } else if (strcasecmp(item, "crc") == 0) {
        switch (p->crcAlgorithm) {
        case kellerCrc:
          crc = calc_crc(GetCharArray(dyn), l);
          DynStringConcatChar(dyn, crc / 256);
          DynStringConcatChar(dyn, crc % 256);
          break;
        case modbusCrc:
          crc = calc_crc(GetCharArray(dyn), l);
          DynStringConcatChar(dyn, crc % 256);
          DynStringConcatChar(dyn, crc / 256);
          break;
        case rsportCrc:
          crc = calc_crc8(GetCharArray(dyn), l);
          DynStringConcatChar(dyn, crc);
          break;
        }
      } else if (strncasecmp(item, "int", 3) == 0) {
        sscanf(item + 3, "%d", &size);
        type = intType;
      } else if (strcasecmp(item, "hex") == 0) {
        type = hexType;
      } else if (strcasecmp(item, "float") == 0) {
        type = floatType;
      } else {
        switch (type) {
        case intType:
          res = sscanf(item, "%ld", &iValue);
          if (res != 1) {
            BinError(a, "invalid integer");
            return 1;
          }
          for (i = size - 1; i >= 0; i--) {
            if (i < sizeof data) {
              data[i] = iValue % 256;
            }
            iValue /= 256;
          }
          for (i = 0; i < size; i++) {
            if (i >= sizeof data) {
              DynStringConcatChar(dyn, 0);
            } else {
              DynStringConcatChar(dyn, data[i]);
            }
          }
          l += size;
          break;
        case hexType:
          res = sscanf(item, "%lx", &iValue);
          if (res != 1) {
            BinError(a, "invalid hex. integer");
            return 1;
          }
          DynStringConcatChar(dyn, (iValue & 255));
          break;
        case floatType:
          res = sscanf(item, "%lf", &fValue);
          if (res != 1) {
            BinError(a, "invalid float");
            return 1;
          }
          double2ieee(fValue, data);
          DynStringConcatChar(dyn, data[0]);
          DynStringConcatChar(dyn, data[1]);
          DynStringConcatChar(dyn, data[2]);
          DynStringConcatChar(dyn, data[3]);
          l += 4;
          break;
        }
      }
    }
    
    if (p->crcAlgorithm == syconCrc) {
      BinToSycon(dyn);
    }

    p->nextFmt = str + pos;
    p->type = hexType; /* initialize to anything < dumpType */
    do {
      res = BinReadItem(a);
      if (res < 0) {
        BinError(a, "illegal return format: ");
        DynStringConcat(a->errmsg, p->nextFmt);
        return 1;
      }
    } while (res == 1);
    p->nextFmt = str + pos;
    a->wrPos = 0;
    a->state = AsconWriting;
    a->lineCount = 0;
    return 1;
  case AsconReadStart:
    DynStringClear(p->result);
    p->type = hexType; /* initialize to anything < dumpType */
    BinReadItem(a);
    p->syconState = 0;
    break;
  case AsconReading:
    res = AsconBaseHandler(a);
    if (res == 0) {
      if (GetDynStringLength(a->rdBuffer) == 0) {
        /* wait for the first byte - or timeout */
        return res;
      }
      if (p->type >= dumpType) {
        l = GetDynStringLength(a->rdBuffer);
        str = GetCharArray(a->rdBuffer);
        for (i = p->dumpFrom; i < l; i++) {
          snprintf(item, sizeof item, "%2.2x ", (str[i] & 255));
          DynStringConcat(p->result, item);
        }
        if (p->type == errorType) {
          DynStringConcat(a->errmsg, GetCharArray(p->result));
          a->state = AsconFailed;
        } else {
          a->state = AsconReadDone;
        }
        /* exchange buffers */
        dyn = a->rdBuffer;
        a->rdBuffer = p->result;
        p->result = dyn;
        return 1;
      }
      return res;
    }
    if (p->crcAlgorithm == syconCrc) {
      switch (p->syconState) {
      case 0: /* wait for STX */
        if (a->lastChar == 0x02) {
          p->syconState = 1;
          p->chksum = 0;
        }
        return res;
      case 1: /* skip address */
      case 2: /* skip cmd_rsp */
        p->syconState++;
        p->chksum += a->lastChar;
        return res;
      case 3: /* detect stuffed bytes */
        if (a->lastChar == 0x07) {
          p->syconState = 4;
          return res;
        }
        p->chksum += a->lastChar;
        break;
      case 4: /* last byte was 0x07 */
        switch (a->lastChar) {
        case '0':
          a->lastChar = 0x02;
          break;
        case '1':
          a->lastChar = 0x0d;
          break;
        case '2':
          a->lastChar = 0x07;
          break;
        }
        p->chksum += a->lastChar;
        p->syconState = 3;
        break;
      case 5: /* expect 0x0d */
        if (a->lastChar != 0x0d) {
          DynStringConcat(p->result, "noCR ");
        }
        p->syconState = 6;
        p->dumpFrom++; /* skip for dump */
      case 6: /* skip everything else */
        return res;
      }
    }
        
    /* convert according to type */
    switch (p->type) {
    case codeType:
      if ((a->lastChar & 255) < 128) {
        p->expectedChars = 0;
      } else {
        DynStringCopy(a->errmsg, "BINERR: ");
        p->expectedChars = 2;
        p->type = errorType;
        p->dumpFrom = 0;
        /* skip to end */
        p->nextFmt = "";
      }
      break;
    case errorType:
      if (p->expectedChars > 1) {
        p->expectedChars--;
        snprintf(item, sizeof item, "error %d / ", (a->lastChar & 255));
        DynStringCopy(p->result, item);
      }
      break;
    case dumpType:
      break;
    case skipType:
      p->expectedChars--;
      break;
    case intType:
      p->expectedChars--;
      p->iValue = p->iValue * 256 + (a->lastChar & 255);
      if (p->expectedChars <= 0) {  
        snprintf(item, sizeof item, "%ld ", p->iValue);
        DynStringConcat(p->result, item);
      }
      break;
    case hexType:
      p->expectedChars--;
      if (p->expectedChars <= 0) {  
        snprintf(item, sizeof item, "%2.2x ", (a->lastChar & 255));
        DynStringConcat(p->result, item);
      }
      break;
    case floatType: /* does not work with sycon when stuffed */
      p->expectedChars--;
      if (p->expectedChars <= 0) {
        str = GetCharArray(a->rdBuffer) + GetDynStringLength(a->rdBuffer) - 4;
        fValue = ieee2double(str);
        snprintf(item, sizeof item, "%.7g ", fValue);
        DynStringConcat(p->result, item);
      }
      break;
    case crcType:
      p->expectedChars--;
      if (p->expectedChars <= 0) {
        str = GetCharArray(a->rdBuffer);
        l = GetDynStringLength(a->rdBuffer);
        switch (p->crcAlgorithm) {
        case syconCrc: /* ignore crc check */
          /* subtract CRC char (undo the addition) and subtract crc higher four bits */
          p->chksum -= a->lastChar + (a->lastChar & 0x0f); 
          p->syconState = 5;
          if (p->chksum != 0) {
            DynStringConcat(p->result, "badCRC ");
          }
          break;
        case kellerCrc:
          i = str[l-2];
          str[l-2] = str[l-1];
          str[l-1] = i;
          /* fall through */
        case modbusCrc:
        case rsportCrc:
          if (calc_crc(str, l) != 0) {
            DynStringConcat(p->result, "badCRC ");
          }
        }
      } else if (p->crcAlgorithm == syconCrc) {
        /* subtract CRC char (undo the addition) and subtract crc higher four bits */
        p->chksum -= a->lastChar + (a->lastChar & 0x0f) * 16; 
      }
    }
    if (p->expectedChars <= 0) {
      BinReadItem(a);
    }
    return res;
  }
  return AsconBaseHandler(a);
}

static void BinPrivateKill(void *pVoid) {
  BinPrivate *p = pVoid;
  DeleteDynString(p->inp);
  DeleteDynString(p->result);
  free(p);
}

static int BinInit(Ascon * a, SConnection * con, int argc, char *argv[])
{
  BinPrivate *p;

/* argv[2] may be modbus-crc (default), keller-crc, rsport-crc or sycon-crc */
  
  if (argc < 2) {
    return 0;
  }
  p = calloc(sizeof(*p), 1);
  a->private = p;
  a->killPrivate = BinPrivateKill;
  p->crcAlgorithm = modbusCrc;
  if (argc > 2 && strcmp(argv[2], "") != 0) {
    if (strcasecmp(argv[2], "keller-crc") == 0) {
      p->crcAlgorithm = kellerCrc;
    } else if (strcasecmp(argv[2], "sycon-crc") == 0) {
      p->crcAlgorithm = syconCrc;
    } else if (strcasecmp(argv[2], "rsport-crc") == 0) {
      p->crcAlgorithm = rsportCrc;
    } else if (strcasecmp(argv[2], "modbus-crc") != 0) {
      SCPrintf(con, eError, "ERROR: unknown crc-algorithm %s", argv[2]);
      a->private = NULL;
      free(p);
      return 0;
    }
  }
  a->hostport = strdup(argv[1]);
  p->inp = CreateDynString(60,63);
  p->result = CreateDynString(60,63);
  if (argc > 3) {
    a->timeout = atof(argv[3]);
  } else {
    a->timeout = 2.0; /* sec */
  }
  return 1;
}

/*----------------------------------------------------------------------------*/
void AddBinProtocol()
{
  static AsconProtocol binprot;
  binprot.name = "bin";
  binprot.handler = BinHandler;
  binprot.init = BinInit;
  AsconInsertProtocol(&binprot);
}