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

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

extern int DMC2280MotionControl;

#define KEY_ENABLED_BIT         (1 << 0)
#define KEY_DISABLED_BIT        (1 << 1)
#define SEC_OPENED_BIT          (1 << 2)
#define SEC_CLOSED_BIT          (1 << 3)
#define TER_OPENED_BIT          (1 << 4)
#define TER_CLOSED_BIT          (1 << 5)
#define MOTOR_ENABLED_BIT       (1 << 6)
#define MOTOR_DISABLED_BIT      (1 << 7)
#define ACCESS_LOCKED_BIT       (1 << 8)
#define ACCESS_UNLOCKED_BIT     (1 << 9)
#define DC_POWEROK_BIT          (1 << 10)
#define EXIT_INPROGRESS_BIT     (1 << 11)
#define SAFETY_TRIPPED_BIT      (1 << 12)
#define SAFETY_MALFUNCTION_BIT  (1 << 13)
#define TER_OPERATE_BIT         (1 << 14)
#define RELAY_ENABLED_BIT       (1 << 15)
#define INST_READY_BIT          (1 << 16)
#define LAMP_TEST_BIT           (1 << 17)

#define KEY_BOTH_BITS     (KEY_ENABLED_BIT | KEY_DISABLED_BIT) 
#define SEC_BOTH_BITS     (SEC_OPENED_BIT | SEC_CLOSED_BIT) 
#define TER_BOTH_BITS     (TER_OPENED_BIT | TER_CLOSED_BIT) 
#define MOTOR_BOTH_BITS   (MOTOR_ENABLED_BIT | MOTOR_DISABLED_BIT) 
#define ACCESS_BOTH_BITS  (ACCESS_LOCKED_BIT | ACCESS_UNLOCKED_BIT) 

typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;

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

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

struct __command {
  pSafetyPLCController plc;
  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) {
    gettimeofday(&myCmd->plc->tvSend, NULL); /* refresh */
    iRet = MultiChanWrite(myCmd->plc->mcc, myCmd->out_buf, myCmd->out_len);
    /* TODO handle errors */
    if (iRet < 0) { /* TODO: EOF */
      iRet = MultiChanReconnect(myCmd->plc->mcc);
      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;
}

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

  assert(self);
  assert(self->mcc);
  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->plc = self;
  gettimeofday(&self->tvSend, NULL); /* refresh */
  return MultiChanEnque(self->mcc, 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;
  unsigned int iRead;
  pSafetyPLCController self = (pSafetyPLCController) ctx;
  iRet = sscanf(resp,"READ %x", &iRead);
  if(iRet != 1) { // Not a number, probably an error response
    self->iValue = 0;
  }
  else {
    if ((iRead & LAMP_TEST_BIT) == 0)
      self->iValue = iRead;
  }
  if ((self->iValue & MOTOR_BOTH_BITS) == 0) /* neither */
    DMC2280MotionControl = -1;
  else if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS) /* both */
    DMC2280MotionControl = -1;
  else if ((self->iValue & MOTOR_ENABLED_BIT))  /* enabled */
    DMC2280MotionControl = 1;
  else /* disabled */
    DMC2280MotionControl = 0;

  self->iGetOut = 0;
  return 0;
}

static int MyTimerCallback(void* context, int mode)
{
  pSafetyPLCController self = (pSafetyPLCController) context;
  if (self->iGetOut) {
    struct timeval now;
    gettimeofday(&now, NULL);
    /* TODO error handling */
    if (((now.tv_sec - self->tvSend.tv_sec) * 1000
          + (now.tv_usec / 1000)
          - (self->tvSend.tv_usec / 1000)) < self->timeout)
      return 1;
    DMC2280MotionControl = -1;
  }
  self->iGetOut = 1;
  PLC_SendCmd(self, "READ", 4, GetCallback, self, 132);
  return 1;
}

static int PLC_Print(SConnection *pCon, SicsInterp *pSics,
                        void *pData, char *name, char *param)
{
  char line[132];
  pSafetyPLCController self = (pSafetyPLCController) pData;
  if (strcasecmp(param, "key") == 0) {
    char* state = "unknown(low)";
    if ((self->iValue & KEY_BOTH_BITS) == KEY_BOTH_BITS)
      state = "invalid(high)";
    else if (self->iValue & KEY_ENABLED_BIT)
      state = "enabled";
    else if (self->iValue & KEY_DISABLED_BIT)
      state = "disabled";
    snprintf(line, 132, "%s.Key = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "secondary") == 0) {
    char* state = "unknown(low)";
    if ((self->iValue & SEC_BOTH_BITS) == SEC_BOTH_BITS)
      state = "invalid(high)";
    if (self->iValue & SEC_OPENED_BIT)
      state = "opened";
    else if (self->iValue & SEC_CLOSED_BIT)
      state = "closed";
    snprintf(line, 132, "%s.Secondary = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "tertiary") == 0) {
    char* state = "unknown(low)";
    if ((self->iValue & TER_BOTH_BITS) == TER_BOTH_BITS)
      state = "invalid(high)";
    if (self->iValue & TER_OPENED_BIT)
      state = "opened";
    else if (self->iValue & TER_CLOSED_BIT)
      state = "closed";
    snprintf(line, 132, "%s.Tertiary = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "motioncontrol") == 0) {
    char* state = "unknown(low)";
    if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS)
      state = "invalid(high)";
    else if (self->iValue & MOTOR_ENABLED_BIT)
      state = "enabled";
    else if (self->iValue & MOTOR_DISABLED_BIT)
      state = "disabled";
    snprintf(line, 132, "%s.MotionControl = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "access") == 0) {
    char* state = "unknown(low)";
    if ((self->iValue & ACCESS_BOTH_BITS) == ACCESS_BOTH_BITS)
      state = "invalid(high)";
    else if (self->iValue & ACCESS_LOCKED_BIT)
      state = "locked";
    else if (self->iValue & ACCESS_UNLOCKED_BIT)
      state = "unlocked";
    snprintf(line, 132, "%s.Access = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "dc") == 0) {
    char* state = "false";
    if (self->iValue & DC_POWEROK_BIT)
      state = "true";
    snprintf(line, 132, "%s.DC = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "exit") == 0) {
    char* state = "false";
    if (self->iValue & EXIT_INPROGRESS_BIT)
      state = "true";
    snprintf(line, 132, "%s.Exit = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "trip") == 0) {
    char* state = "false";
    if (self->iValue & SAFETY_TRIPPED_BIT)
      state = "true";
    snprintf(line, 132, "%s.Trip = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "fault") == 0) {
    char* state = "false";
    if (self->iValue & SAFETY_MALFUNCTION_BIT)
      state = "true";
    snprintf(line, 132, "%s.Fault = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "operate") == 0) {
    char* state = "false";
    if (self->iValue & TER_OPERATE_BIT)
      state = "true";
    snprintf(line, 132, "%s.Operate = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "relay") == 0) {
    char* state = "false";
    if (self->iValue & RELAY_ENABLED_BIT)
      state = "true";
    snprintf(line, 132, "%s.Relay = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  if (strcasecmp(param, "ready") == 0) {
    char* state = "false";
    if (self->iValue & INST_READY_BIT)
      state = "true";
    snprintf(line, 132, "%s.Ready = %s", name, state);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  return 0;
}

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 = %06X", argv[0], self->iValue & 0xffffff);
    SCWrite(pCon, line, eStatus);
    return OKOK;
  }
  else if (argc == 2) {
    if (strcasecmp(argv[1], "list") == 0) {
      PLC_Print(pCon, pSics, pData, argv[0], "key");
      PLC_Print(pCon, pSics, pData, argv[0], "secondary");
      PLC_Print(pCon, pSics, pData, argv[0], "tertiary");
      PLC_Print(pCon, pSics, pData, argv[0], "motioncontrol");
      PLC_Print(pCon, pSics, pData, argv[0], "access");
      PLC_Print(pCon, pSics, pData, argv[0], "dc");
      PLC_Print(pCon, pSics, pData, argv[0], "exit");
      PLC_Print(pCon, pSics, pData, argv[0], "trip");
      PLC_Print(pCon, pSics, pData, argv[0], "fault");
      PLC_Print(pCon, pSics, pData, argv[0], "operate");
      PLC_Print(pCon, pSics, pData, argv[0], "relay");
      PLC_Print(pCon, pSics, pData, argv[0], "ready");
      return OKOK;
    }
    if (PLC_Print(pCon, pSics, pData, argv[0], argv[1]))
      return OKOK;
  }
  snprintf(line, 132, "%s does not understand %s", argv[0], argv[1]);
  SCWrite(pCon, line, eError);
  return 0;
}

static pSafetyPLCController PLC_Create(const char* pName, int port)
{
  pSafetyPLCController self = 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)
    NetWatchRemoveTimer(self->nw_tmr);
  NetWatchRegisterTimerPeriodic(&self->nw_tmr,
      1000, 100,
      MyTimerCallback,
      self);
  self->timeout=120000; /* huge */
  return 1;
}

static void PLC_Kill(void* pData)
{
  pSafetyPLCController self = (pSafetyPLCController) pData;
  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;
}