#include <sics.h>
#include <asyncprotocol.h>
#include <asyncqueue.h>

#define STX 2
#define ETX 3

static int calc_bcc(const char* text)
{
  int bcc = 0;
  int i = 0;
  int c;
  while (i < 1024) {
    c = text[i] & 0xFF;
    if (c == STX) {
      bcc = 0;
    } else {
      bcc ^= c;
    }
    SICSLogPrintf(eLog, "BCC %02d: char %02X, bcc = %02X", i, c, bcc);
    if (c == ETX)
      break;
    ++text;
  }
  return bcc;
}


/*
 * Protocol transmit function
 * Called by AsyncQueue to transmit a line
 */
static int OMRON_Tx(pAsyncProtocol p, pAsyncTxn myCmd) {
  int iRet = 1;

  if (myCmd) {
    myCmd->txn_status = ATX_ACTIVE;
    iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
    /* TODO handle errors */
    if (iRet < 0) { /* TODO: EOF */
      /*
      iRet = AsyncUnitReconnect(myCmd->unit);
      if (iRet == 0)
      */
        return 0;
    }
  }
  return 1;
}

/*
 * Protocol receive character - characater by character
 */
static int OMRON_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
  int iRet = 1;
  pAsyncTxn myCmd = (pAsyncTxn) ctx;

  switch (myCmd->txn_state) {
    case 0: /* first character */
      if (rxchar != STX) { /* STX */
        /* TODO: error */
        myCmd->txn_state = 99;
        myCmd->txn_status = ATX_COMPLETE;
        break;
      }
      /* normal data */
      myCmd->txn_state = 1;
      /* note fallthrough */
    case 1: /* receiving reply */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == ETX)
        myCmd->txn_state = 2;
      break;
    case 2: /* receiving bcc */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (calc_bcc(myCmd->inp_buf) != rxchar) {
        /* TODO: fail bcc */
        int bcc = calc_bcc(myCmd->inp_buf);
        SICSLogPrintf(eError,
            "Omron BCC mismatch, expected %02X but received %02X on message:",
            bcc, rxchar);
        SICSLogWriteHex(myCmd->inp_buf, myCmd->inp_idx, eError); 
      }
      myCmd->txn_state = 99;
      myCmd->inp_idx -= 3;
      memmove(myCmd->inp_buf, myCmd->inp_buf + 1, myCmd->inp_idx);
      break;
  }
  if (myCmd->txn_state == 99) {
    iRet = 0;
  }
  if (iRet == 0) { /* end of command */
    return AQU_POP_CMD;
  }
  return iRet;
}

/*
 * AsyncProtocol Event callback
 */
static int OMRON_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
  if (event == AQU_TIMEOUT) {
    /* handle command timeout */
    pTxn->txn_status = ATX_TIMEOUT;
    return AQU_POP_CMD;
  }
  return AQU_POP_CMD;
}

static int OMRON_PrepareTxn(pAsyncProtocol p, pAsyncTxn txn, const char* cmd, int cmd_len, int rsp_len) {
  int i, bcc;
  txn->out_buf = (char*) malloc(cmd_len + 3);
  if (txn->out_buf == NULL) {
    SICSLogWrite("ERROR: Out of memory in OMRON_PrepareTxn", eError);
    return 0;
  }
  memcpy(txn->out_buf + 1, cmd, cmd_len);
  txn->out_buf[0] = STX;
  txn->out_buf[cmd_len + 1] = ETX;
  txn->out_buf[cmd_len + 2] = calc_bcc(txn->out_buf);
  txn->out_buf[cmd_len + 3] = 0x0D;
  txn->out_buf[cmd_len + 4] = 0x0A;
  txn->out_len = cmd_len + 5;
  return 1;
}

static pAsyncProtocol OMRON_Protocol = NULL;

/*
 * Protocol Initialisation
 */
void OMRONInitProtocol(SicsInterp *pSics) {
  if (OMRON_Protocol == NULL) {
    OMRON_Protocol = AsyncProtocolCreate(pSics, "OMRON", NULL, NULL);
    OMRON_Protocol->sendCommand = OMRON_Tx;
    OMRON_Protocol->handleInput = OMRON_Rx;
    OMRON_Protocol->handleEvent = OMRON_Ev;
    OMRON_Protocol->prepareTxn = OMRON_PrepareTxn;
    OMRON_Protocol->killPrivate = NULL;
  }
}