/*
 * S A F E T Y P L C
 *
 * Douglas Clowes, February 2007
 *
 */

#include <netinet/tcp.h>
#include <sics.h>
#include "network.h"
#include "multichan.h"
#include "nwatch.h"
#include "safetyplc.h"

extern int DMC2280MotionControl;

typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;

struct __SafetyPLCController {
  pObjectDescriptor pDes;
  pMultiChan mcc;         /* associated MultiChan object */
  int       iGetOut;
  int       iValue;
  pNWTimer  nw_tmr;       /* NetWait timer handle */
};

typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len);

struct __command {
  pMultiChan unit;
  int cstate;
  int lstate;
  char* out_buf;
  int out_len;
  int out_idx;
  char* inp_buf;
  int inp_len;
  int inp_idx;
  CommandCallback func;
  void* cntx;
};

static int PLC_Tx(void* ctx)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

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

static int PLC_Rx(void* ctx, int rxchar)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

  switch (myCmd->cstate) {
    case 0: /* first character */
      /* normal data */
      myCmd->cstate = 1;
      /* note fallthrough */
    case 1: /* receiving reply */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0D)
        myCmd->cstate = 2;
      break;
    case 2: /* received CR and looking for LF */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0A) {
        myCmd->cstate = 99;
        /* end of line */
      }
      else
        myCmd->cstate = 1;
      break;
  }
  if (myCmd->cstate == 99) {
    myCmd->inp_buf[myCmd->inp_idx] = '\0';
    if (myCmd->func)
      iRet = myCmd->func(myCmd->cntx, myCmd->inp_buf, myCmd->inp_idx);
    else
      iRet = 0;
    myCmd->cstate = 0;
    myCmd->inp_idx = 0;
  }
  if (iRet == 0) { /* end of command */
    free(myCmd->out_buf);
    free(myCmd->inp_buf);
    free(myCmd);
  }
  return iRet;
}

int PLC_SendCmd(pMultiChan unit,
    char* command, int cmd_len,
    CommandCallback callback, void* context, int rsp_len)
{
  pCommand myCmd = NULL;

  assert(unit);
  myCmd = (pCommand) malloc(sizeof(Command));
  assert(myCmd);
  memset(myCmd, 0, sizeof(Command));
  myCmd->out_buf = (char*) malloc(cmd_len + 5);
  memcpy(myCmd->out_buf, command, cmd_len);
  myCmd->out_len = cmd_len;
  if (myCmd->out_len < 2 ||
      myCmd->out_buf[myCmd->out_len - 1] != 0x0A ||
      myCmd->out_buf[myCmd->out_len - 2] != 0x0D) {
    myCmd->out_buf[myCmd->out_len++] = 0x0D;
    myCmd->out_buf[myCmd->out_len++] = 0x0A;
  }
  myCmd->out_buf[myCmd->out_len] = '\0';
  myCmd->func = callback;
  myCmd->cntx = context;
  if (rsp_len == 0)
    myCmd->inp_buf = NULL;
  else {
    myCmd->inp_buf = malloc(rsp_len + 1);
    memset(myCmd->inp_buf, 0, rsp_len + 1);
  }
  myCmd->inp_len = rsp_len;
  myCmd->unit = unit;
  return MultiChanEnque(unit, myCmd, PLC_Tx, PLC_Rx);
}

static void PLC_Notify(void* context, int event)
{
  pSafetyPLCController self = (pSafetyPLCController) context;

  switch (event) {
    case MCC_RECONNECT:
      do {
        mkChannel* sock = MultiChanGetSocket(self->mcc);
        int flag = 1;
        setsockopt(sock->sockid, /* socket affected */
            IPPROTO_TCP,     /* set option at TCP level */
            TCP_NODELAY,     /* name of option */
            (char *) &flag,  /* the cast is historical cruft */
            sizeof(int));    /* length of option value */
        return;
      } while (0);
  }
  return;
}

/*
 * \brief GetCallback is the callback for the read command.
 */
static int GetCallback(void* ctx, const char* resp, int resp_len)
{
  int iRet;
  int iRead;
  pSafetyPLCController self = (pSafetyPLCController) ctx;
  iRet = sscanf(resp,"READ %0xd",&iRead);
  if(iRet != 1) { // Not a number, probably an error response
    self->iValue = 0;
  }
  else {
    if (iRead < 0)
      iRead = -iRead;
    self->iValue = iRead;
  }
  if ((self->iValue & (1 << 6))) 
    DMC2280MotionControl = 1;
  else
    DMC2280MotionControl = 0;

  self->iGetOut = 0;
  return 0;
}

static int MyTimerCallback(void* context, int mode)
{
  pSafetyPLCController self = (pSafetyPLCController) context;
  if (self->iGetOut) {
    /* TODO error handling */
    DMC2280MotionControl = 0;
  }
  PLC_SendCmd(self->mcc, "READ", 4,
      GetCallback, self, 132);
  return 1;
}

static int PLC_Action(SConnection *pCon, SicsInterp *pSics,
                        void *pData, int argc, char *argv[])
{
  char line[132];
  pSafetyPLCController self = (pSafetyPLCController) pData;
  if (argc == 1) {
    snprintf(line, 132, "%s.iValue = %006x", argv[0], self->iValue & 0xffffff);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  } else if (argc == 2) {
    if (strcmp(argv[1], "motioncontrol") == 0) {
        snprintf(line, 132, "%s.MotionControl = %d", argv[0], DMC2280MotionControl);
        SCWrite(pCon, line, eStatus);
        return OKOK;
        }
  }
  /* TODO: handle private stuff */
  return MultiChanAction(pCon, pSics, self->mcc, argc, argv);
}

static pSafetyPLCController PLC_Create(const char* pName, int port)
{
  int i;
  pSafetyPLCController self = NULL;
  pMultiChan ctlr = NULL;

  self = (pSafetyPLCController) malloc(sizeof(SafetyPLCController));
  if (self == NULL)
    return NULL;
  memset(self, 0, sizeof(SafetyPLCController));
  if (MultiChanCreate(pName, &self->mcc) == 0) {
    free(self);
    return NULL;
  }
  MultiChanSetNotify(self->mcc, self, PLC_Notify);

  self->pDes = CreateDescriptor("SafetyPLC");
  return self;
}

static int PLC_Init(pSafetyPLCController self)
{
  /* TODO: Init the controller */
  if (self->nw_tmr == NULL)
    NetWatchRegisterTimerPeriodic(&self->nw_tmr,
        1000, 1000,
        MyTimerCallback,
        self);
  return 1;
}

static void PLC_Kill(pSafetyPLCController self)
{
  if (self->nw_tmr)
    NetWatchRemoveTimer(self->nw_tmr);
  free(self);
  return;
}

int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
    void *pData, int argc, char *argv[])
{
  pSafetyPLCController pNew = NULL;
  int iRet, status;
  char pError[256];

  if(argc < 4)
  {
    SCWrite(pCon,"ERROR: insufficient no of arguments to SafetyPLCFactory",
            eError);
    return 0;
  }

  /*
   create data structure and open port 
  */
  pNew = PLC_Create(argv[2], atoi(argv[3]));
  
  if(!pNew)
  {
    SCWrite(pCon,"ERROR: failed to create SafetyPLC in SafetyPLCFactory",eError);
    return 0;
  }

  status = PLC_Init(pNew);
  if(status != 1)
  {
     sprintf(pError,"ERROR: failed to connect to %s",argv[2]);
     SCWrite(pCon,pError,eError);
  }
  
  /*
    create the command
  */
  iRet = AddCommand(pSics, argv[1], PLC_Action, PLC_Kill, pNew);
  if(!iRet)
  {
        sprintf(pError,"ERROR: duplicate command %s not created", argv[1]);
        SCWrite(pCon,pError,eError);
        PLC_Kill(pNew);
        return 0;
 }
 return 1;
}