/*
 * 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 "asyncqueue.h"
#include "nwatch.h"
#include "safetyplc.h"
#include "sicsvar.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)

static pAsyncProtocol PLC_Protocol = NULL;

int PLC_UserPriv = 0; /* Internal */
typedef enum {
Unknown_low, Invalid_high, Enabled, Disabled,
Opened, Closed, Locked, Unlocked, True, False,}PLC_STATUS;

char *plc_states[] = {
"UNKNOWN_LOW", "INVALID_HIGH", "ENABLED",
"DISABLED", "OPEN", "CLOSED",
"LOCKED", "UNLOCKED", "TRUE", "FALSE"};

typedef enum {
Key,Secondary,Tertiary,MotionControl,Access,
DC,Exit,Trip,Fault,Operate,Relay,Ready,}PLC_PARAM;

char *plc_parname[] = {
"plc_key","plc_secondary","plc_tertiary","plc_motioncontrol",
"plc_access","plc_dc","plc_exit","plc_trip",
"plc_fault","plc_operate","plc_relay","plc_ready"};

typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;

struct __SafetyPLCController {
  pObjectDescriptor pDes;
  pAsyncUnit unit;         /* associated AsyncUnit object */
  int       iGetOut;
  int       iValue;
  int       oldValue;
  pNWTimer  nw_tmr;       /* periodic timer handle */
  pNWTimer  oneshot;      /* oneshot timer handle */
  int       timeout;
};

static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState);

static int PLC_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar)
{
  int iRet = 1;

  switch (myCmd->txn_state) {
    case 0: /* first character */
      /* 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 == 0x0D)
        myCmd->txn_state = 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->txn_state = 99;
        /* end of line */
      }
      else
        myCmd->txn_state = 1;
      break;
  }
  if (myCmd->txn_state == 99) {
    myCmd->inp_buf[myCmd->inp_idx] = '\0';
    iRet = 0;
    myCmd->txn_state = 0;
    myCmd->txn_status = ATX_COMPLETE;
  }
  if (iRet == 0) { /* end of command */
    return AQU_POP_CMD;
  }
  return iRet;
}

static int PLC_Ev(pAsyncProtocol p, pAsyncTxn myCmd, int event)
{
  if (event == AQU_TIMEOUT) {
    /* TODO: handle command timeout */
    myCmd->txn_status = ATX_TIMEOUT;
    return AQU_POP_CMD;
  }
  return AQU_POP_CMD;
}

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

  switch (event) {
    case AQU_RECONNECT:
      do {
        mkChannel* sock = AsyncUnitGetSocket(self->unit);
        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(pAsyncTxn txn)
{
  int iRet;
  unsigned int iRead;
  char* resp = txn->inp_buf;
  int resp_len = txn->inp_idx;
  PLC_STATUS plcState;
  pSicsVariable plcVar=NULL;

  pSafetyPLCController self = (pSafetyPLCController) txn->cntx;
  if (resp_len < 0) {
    DMC2280MotionControl = -1;
  }
  else {
    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;
  }
  if (self->oldValue != self->iValue) {
    unsigned int i;
    for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
      plcVar = (pSicsVariable)FindCommandData(pServ->pSics,plc_parname[i],"SicsVariable");
      PLC_GetState(self,plc_parname[i],&plcState);
      VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
    }
  }

  self->oldValue = self->iValue;
  self->iGetOut = 0;
  return 0;
}

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

static int MyOneShotCallback(void* context, int mode)
{
  pSafetyPLCController self = (pSafetyPLCController) context;
  self->oneshot = 0;
  AsyncUnitSendTxn(self->unit, "WRITE 0",  7, NULL, NULL, 132);
  return 0;
}

/*
 * \brief PutCallback is the callback for the write command.
 */
static int PutCallback(pAsyncTxn txn)
{
  pSafetyPLCController self = (pSafetyPLCController) txn->cntx;
  if (self->oneshot)
    NetWatchRemoveTimer(self->oneshot);
  NetWatchRegisterTimer(&self->oneshot, 500, MyOneShotCallback, self);
  return 0;
}

static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState)
{
  pSafetyPLCController self = (pSafetyPLCController) pData;
  if (strcasecmp(param, plc_parname[Key]) == 0) {
    *retState = Unknown_low;
    if ((self->iValue & KEY_BOTH_BITS) == KEY_BOTH_BITS)
      *retState = Invalid_high;
    else if (self->iValue & KEY_ENABLED_BIT)
      *retState = Enabled;
    else if (self->iValue & KEY_DISABLED_BIT)
      *retState = Disabled;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Secondary]) == 0) {
    *retState = Unknown_low;
    if ((self->iValue & SEC_BOTH_BITS) == SEC_BOTH_BITS)
      *retState = Invalid_high;
    if (self->iValue & SEC_OPENED_BIT)
      *retState = Opened;
    else if (self->iValue & SEC_CLOSED_BIT)
      *retState = Closed;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Tertiary]) == 0) {
    *retState = Unknown_low;
    if ((self->iValue & TER_BOTH_BITS) == TER_BOTH_BITS)
      *retState = Invalid_high;
    if (self->iValue & TER_OPENED_BIT)
      *retState = Opened;
    else if (self->iValue & TER_CLOSED_BIT)
      *retState = Closed;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[MotionControl]) == 0) {
    *retState = Unknown_low;
    if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS)
      *retState = Invalid_high;
    else if (self->iValue & MOTOR_ENABLED_BIT)
      *retState = Enabled;
    else if (self->iValue & MOTOR_DISABLED_BIT)
      *retState = Disabled;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Access]) == 0) {
    *retState = Unknown_low;
    if ((self->iValue & ACCESS_BOTH_BITS) == ACCESS_BOTH_BITS)
      *retState = Invalid_high;
    else if (self->iValue & ACCESS_LOCKED_BIT)
      *retState = Locked;
    else if (self->iValue & ACCESS_UNLOCKED_BIT)
      *retState = Unlocked;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[DC]) == 0) {
    *retState = False;
    if (self->iValue & DC_POWEROK_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Exit]) == 0) {
    *retState = False;
    if (self->iValue & EXIT_INPROGRESS_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Trip]) == 0) {
    *retState = False;
    if (self->iValue & SAFETY_TRIPPED_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Fault]) == 0) {
    *retState = False;
    if (self->iValue & SAFETY_MALFUNCTION_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Operate]) == 0) {
    *retState = False;
    if (self->iValue & TER_OPERATE_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Relay]) == 0) {
    *retState = False;
    if (self->iValue & RELAY_ENABLED_BIT)
      *retState = True;
    return OKOK;
  }
  if (strcasecmp(param, plc_parname[Ready]) == 0) {
    *retState = False;
    if (self->iValue & INST_READY_BIT)
      *retState = True;
    return OKOK;
  }
  return 0;
}

static int PLC_Print(SConnection *pCon, SicsInterp *pSics,
                        void *pData, char *name, char *param)
{
  char line[132];
  PLC_STATUS state;

  if (PLC_GetState(pData, param, &state) != OKOK) {
    return 0;
  } else {
    snprintf(line, 132, "%s.%s = %s", name, param, plc_states[state]);
    SCWrite(pCon, line, eValue);
    return OKOK;
  }
}

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, eValue);
    return OKOK;
  }
  else if (argc == 2) {
    if (strcasecmp(argv[1], "list") == 0) {
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Key]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Secondary]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Tertiary]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[MotionControl]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Access]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[DC]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Exit]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Trip]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Fault]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Operate]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Relay]);
      PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Ready]);
      return OKOK;
    }
    if (PLC_Print(pCon, pSics, pData, argv[0], argv[1]))
      return OKOK;
  } else if (argc == 3) {
    if (strcasecmp(argv[1], "hattach") == 0) {
    }
    else if (strcasecmp(argv[1], "shutter") == 0) {
      //if (strcasecmp(argv[2], "open") == 0) {
      if (strcasecmp(argv[2], "on") == 0) {
        /* open shutter */
        //AsyncUnitSendTxn(self->unit, "set shutter=on", 14, PutCallback, self, 132);
        AsyncUnitSendTxn(self->unit, "READ", 4, PutCallback, self, 132);
        return OKOK;
      }
      //else if (strcasecmp(argv[2], "close") == 0 ||
      else if (strcasecmp(argv[2], "off") == 0) {
        //  strcasecmp(argv[2], "shut") == 0) {
        /* close shutter */
        AsyncUnitSendTxn(self->unit, "set shutter=off", 15, PutCallback, self, 132);
        return OKOK;
      }
      else if (strcasecmp(argv[2], "auto") == 0) {
        AsyncUnitSendTxn(self->unit, "set shutter=auto", 16, PutCallback, self, 132);
        return OKOK;
      }
      else {
        snprintf(line, 132, "%s %s does not understand %s", argv[0], argv[1], argv[2]);
        SCWrite(pCon, line, eError);
        return 0;
      }
    }
    else if (strcasecmp(argv[1], "focuslight") == 0) {
      if (strcasecmp(argv[2], "on") == 0) {
        AsyncUnitSendTxn(self->unit, "set focuslight=on", 17, PutCallback, self, 132);
        return OKOK;
      } else if (strcasecmp(argv[2], "off") == 0) {
        AsyncUnitSendTxn(self->unit, "set focuslight=off", 18, PutCallback, self, 132);
        return OKOK;
      } else {
        snprintf(line, 132, "%s %s does not understand %s", argv[0], argv[1], argv[2]);
        SCWrite(pCon, line, eError);
        return 0;
      }
    }
  } else {
    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)
{
  pSafetyPLCController self = NULL;

  self = (pSafetyPLCController) malloc(sizeof(SafetyPLCController));
  if (self == NULL)
    return NULL;
  memset(self, 0, sizeof(SafetyPLCController));
  if (AsyncUnitCreate(pName, &self->unit) == 0) {
    free(self);
    return NULL;
  }
  AsyncUnitSetNotify(self->unit, 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, 1000,
      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);
  if (self->pDes) {
    DeleteDescriptor(self->pDes);
    self->pDes = NULL;
  }
  free(self);
  return;
}

void SafetyPLCInitProtocol(SicsInterp *pSics) {
  if (PLC_Protocol == NULL) {
    PLC_Protocol = AsyncProtocolCreate(pSics, "SafetyPLC", NULL, NULL);
    PLC_Protocol->sendCommand = NULL;
    PLC_Protocol->handleInput = PLC_Rx;
    PLC_Protocol->handleEvent = PLC_Ev;
    PLC_Protocol->prepareTxn = NULL;
    PLC_Protocol->killPrivate = NULL;
  }
}

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

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

  /*
   create data structure and open port
  */
  pNew = PLC_Create(argv[2]);

  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);
  }

  for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
    plcVar = VarCreate(PLC_UserPriv,veText,plc_parname[i]);
    PLC_GetState(pNew,plc_parname[i],&plcState);
    VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
    AddCommand(pSics,plc_parname[i],VarWrapper,(KillFunc)VarKill,plcVar);
  }
  /*
    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;
 }
  SCSendOK(pCon);
  return 1;
}