/*-------------------------------------------------------------------------
  LS340 - LAKESHORE340
  Support for the Lakeshore 340 Temperature Controller

  The meaning and working of the functions defined is as desribed for a
  general environment controller.

  Adapted by Rodney Davies from orhvps code written by:
  Douglas Clowes, December 2007

  Copyright:

  Labor fuer Neutronenstreuung
  Paul Scherrer Institut
  CH-5423 Villigen-PSI


  The authors hereby grant permission to use, copy, modify, distribute,
  and license this software and its documentation for any purpose, provided
  that existing copyright notices are retained in all copies and that this
  notice is included verbatim in any distributions. No written agreement,
  license, or royalty fee is required for any of the authorized uses.
  Modifications to this software may be copyrighted by their authors
  and need not follow the licensing terms described here, provided that
  the new terms are clearly indicated on the first page of each file where
  they apply.

  IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
  FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
  ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
  DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.

  THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE
  IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
  NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
  MODIFICATIONS.
  ----------------------------------------------------------------------------
 */

#include "ls340.h"
#include "sics.h"
#include "asyncqueue.h"
#include "nwatch.h"
#include "fsm.h"
#include "anstoutil.h"
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>

#include <string.h>
#include <ctype.h>
#include <math.h>


#define CMDLEN  64
#define LS340_ERR_NONE (0)
#define LS340_ERR_LOCKED (-1)
#define LS340_ERR_RANGE (-2)

#define NUM_INPUT_SENSORS 10  /* Maximum Number of Input Sensors */

char *strcasestr(const char *hay, const char *needle);

/* Note: data structure supports Lakeshore 340 with 10101010101010101010 sensor inputs and 2 PID Loops */
typedef struct ls340_s {
  pEVControl controller;
  int iPidLoop;	/* PID Loop number 1..2 */
  int iCtrlSens; /* control sensor (this is the index in the list below, not the Lakeshore sensor number) 0..NUM_INPUT_SENSORS-1 */
  char cCtrlSensName[3]; /* control sensor name eg: A1, B, C, etc */
  char cValidSensors[NUM_INPUT_SENSORS][3]; /* list of valid sensors for this pid loop eg: A, A1, B4, C, D etc*/
  float fSensorValues[NUM_INPUT_SENSORS];  /* each valid sensor value */
  int iNumSensors; /* number of input sensors */
  float fSetPoint;   /* setpoint temperature of control loop */
  int iError;   /* error code */
  float fSetPointLimit; /* Setpoint temperature limit */
  float fPosSlope;      /* Positive Slope value */
  float fNegSlope;      /* Negative Slope value */
  int iMaxCurrent;	/* Maximum Current on output */
  int   iMaxRange;      /* Heater Range 0..5 */
  int iRange;	/* Heater Range 0..5 (0 being off) */
  int iHeaterStatus; /* heater status - 1 - on, 0 - off */
  float fValue; /* current temperature of control sensor in degrees K */
  float fTarget; /* requested target temperature */
  int iSettleTime; /* settling time (sec) */
  float fTolerance; /* settline tolerance level 0..100 */
  bool isLocked; /* changes no longer permitted */
  unsigned long ulIdleDelay; /* idle timer timeout duration in milliseconds */
  unsigned long ulIdlePollRate; /* idle timer timeout duration in milliseconds */
  unsigned long ulMonitorPollRate; /* Monitoring sensor polling cycle duration in milliseconds */
  unsigned long ulMonitorDelay; /* Monitoring timer timeout duration in milliseconds */
  char* name;
  pAsyncUnit asyncUnit;
  StateMachine fsm;
  pNWTimer state_timer; /**< state timer */
} LS340Driv, *pLS340Driv;

static int LS340GetValue( pEVDriver self, float* fPos);
static int LS340SetValue( pEVDriver self, float fPos);
static int LS340Send(pEVDriver self, char *pCommand, char *pReply, int iLen);
static int LS340Error(pEVDriver self, int *iCode, char *error, int iErrLen);
static int LS340Fix(pEVDriver self, int iError);
static int LS340Init(pEVDriver self);
static int LS340Close(pEVDriver self);
static void LS340KillPrivate(void *pData);
static void LS340Notify(void* context, int event);

/* get the index in the valid sensors list of a given sensor name */
static int getSensorIndex(pLS340Driv priv, char sname[]) {

   int i;

   for (i = 0; i < priv->iNumSensors; i++) {
     if (strcmp(priv->cValidSensors[i], sname) == 0)
         return i;
   }

   return -1;
}

static int LS340_SendCmd(pLS340Driv self,
    char* command,
    int cmd_len,
    AsyncTxnHandler callback)
{
  pStateMachine sm = &self->fsm;
  return AsyncUnitSendTxn(self->asyncUnit,
      command, cmd_len,
      callback, sm, CMDLEN);
}

/**
 * \brief Sends a command and waits for a response
 *
 * \param self motor data
 * \param cmd command to send
 * \param reply space to return response
 * \return
 */
static int LS340_SendReceive(pLS340Driv self,
    char *cmd,
    int cmd_len,
    char* reply,
    int *rep_len) {
  int status;

  status = AsyncUnitTransact(self->asyncUnit, cmd, cmd_len, reply, rep_len);

  if (status != 1) {
    return FAILURE;
  }

  return OKOK;
}

static void LS340State_Unknown(pStateMachine sm, pEvtEvent event);
static void LS340State_Idle(pStateMachine sm, pEvtEvent event);
static void LS340State_Raising(pStateMachine sm, pEvtEvent event);
static void LS340State_Lowering(pStateMachine sm, pEvtEvent event);

static void str_n_cat(char* s1, int len, const char* s2) {
  int i = strlen(s1);
  const char* p = s2;
  while (i < len - 3 && *p) {
    if (*p == '\r') {
      s1[i++] = '\\';
      s1[i++] = 'r';
      ++p;
    }
    else if (*p == '\n') {
      s1[i++] = '\\';
      s1[i++] = 'n';
      ++p;
    }
    else
      s1[i++] = *p++;
  }
  s1[i] = '\0';
}

static const char* state_name(StateFunc func)
{
  if (func == NULL) return "<null_state>";
  if (func == LS340State_Unknown) return "LS340State_Unknown";
  if (func == LS340State_Idle) return "LS340State_Idle";
  if (func == LS340State_Raising) return "LS340State_Raising";
  if (func == LS340State_Lowering) return "LS340State_Lowering";
  return "<unknown_state>";
}

static const char* event_name(pEvtEvent event, char* text, int length)
{
  char line[1024];
  if (event == NULL)
    return "<null_event>";
  switch (event->event_type) {
    case eStateEvent:
      snprintf(text, length, "eStateEvent");
      return text;
    case eTimerEvent:
      snprintf(text, length, "eTimerEvent");
      return text;
    case eMessageEvent:
      snprintf(text, length, "eMessageEvent:");
      fsm_textify(event->event.msg.cmd->out_buf,
          event->event.msg.cmd->out_len,
            line, sizeof(line));
      str_n_cat(text, length, line);
      str_n_cat(text, length, "|");
      fsm_textify(event->event.msg.cmd->inp_buf,
          event->event.msg.cmd->inp_idx,
            line, sizeof(line));
      str_n_cat(text, length, line);
      return text;
    case eCommandEvent:
      /* TODO Command Events */
      snprintf(text, length, "eCommandEvent:unknown");
      return text;
    case eTimeoutEvent:
      snprintf(text, length, "eTimeoutEvent");
      return text;
    default:
      snprintf(text, length, "<unknown_event>");
      return text;
  }
}

static void LS340State_Unknown(pStateMachine sm, pEvtEvent event) {
  char cmd[CMDLEN];

  pEVDriver driv = (pEVDriver) sm->context;
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;
  switch (event->event_type) {
    case eStateEvent:
      if (priv->state_timer)
        NetWatchRemoveTimer(priv->state_timer);
      priv->state_timer = NULL;
      sprintf(cmd, "*IDN?");
      LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      sm->mySubState = 1;
      return;
    case eTimerEvent:
      priv->state_timer = NULL;
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        pCmd->inp_buf[pCmd->inp_idx] = '\0';

        if (sm->mySubState == 1) {
          /* IDN Request */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';
            sprintf(line, "IDN: %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);
          }
#if 0
          sprintf(cmd, "RANGE 0"); /* ensure heater is off */
          LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
#endif
          sprintf(cmd, "CSET %d, %s, 1, %d, 1", priv->iPidLoop, priv->cValidSensors[priv->iCtrlSens], priv->iHeaterStatus);
          LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

          sprintf(cmd, "CLIMIT? %d", priv->iPidLoop);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);

          sm->mySubState = 2;

          return;
        }

        if (sm->mySubState == 2) {
	  /* Handle CLIMIT data */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';

            sprintf(line, "CLIMIT (Unknown): %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);

            sscanf(pCmd->inp_buf, "%f,%f,%f,%d,%d", &priv->fSetPointLimit, &priv->fPosSlope, &priv->fNegSlope, &priv->iMaxCurrent, &priv->iMaxRange);
         }

	  sprintf(cmd, "SETP? %d", priv->iPidLoop);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
          sm->mySubState = 3;

          return;

        }
        if (sm->mySubState == 3) {

	  /* Handle SETP? data */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';

            sprintf(line, "SETP: %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);
            sscanf(pCmd->inp_buf, "%f", &priv->fSetPoint);
	    priv->fTarget = priv->fSetPoint; /* Set target and setpoints to current values from controller */
          }

	  sprintf(cmd, "KRDG? %s", priv->cValidSensors[0]);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
          sm->mySubState = 4;

 	  return;
        }

        if (sm->mySubState >= 4 && sm->mySubState <= priv->iNumSensors+3) {
      	  /* KRDG? Requests */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';
            priv->fSensorValues[sm->mySubState-4] = atof(pCmd->inp_buf);

            if (sm->mySubState-4 == priv->iCtrlSens) {
		priv->fValue = priv->fSensorValues[sm->mySubState-4];
            }
            sprintf(line, "KRDG? (Unknown) sensor %s = %8.2f", priv->cValidSensors[sm->mySubState-4], priv->fSensorValues[sm->mySubState-4]);
            SICSLogWrite(line, eLog);
       	  }

          if (sm->mySubState >= priv->iNumSensors+3) {
                fsm_change_state(sm, LS340State_Idle);
 		return;
	  }
          sm->mySubState++;
          sprintf(cmd, "KRDG? %s", priv->cValidSensors[sm->mySubState-4]);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);

          return;
        }
      } while (0);
      return;
    case eCommandEvent:
      return;
    case eTimeoutEvent:
      sprintf(cmd, "*IDN?");
      LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      sm->mySubState = 1;
      return;
  }
  return;
}

static void LS340State_Idle(pStateMachine sm, pEvtEvent event){

  char cmd[CMDLEN];

  pEVDriver driv = (pEVDriver) sm->context;
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;
  switch (event->event_type) {
    case eStateEvent:
      if (priv->state_timer)
        NetWatchRemoveTimer(priv->state_timer);

      NetWatchRegisterTimer(&priv->state_timer,
          priv->ulIdleDelay,
          fsm_tmr_callback, sm);

      sprintf(cmd, "CLIMIT? %d", priv->iPidLoop);
      LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      sm->mySubState = 1;

      return;

    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        pCmd->inp_buf[pCmd->inp_idx] = '\0';

        if (sm->mySubState == 1) { /* CLIMIT message */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';

            sprintf(line, "CLIMIT (Idle): %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);

            sscanf(pCmd->inp_buf, "%f,%f,%f,%d,%d", &priv->fSetPointLimit, &priv->fPosSlope, &priv->fNegSlope, &priv->iMaxCurrent, &priv->iMaxRange);
          }

          sm->mySubState++;

        } else if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {
      	  /* KRDG? Requests */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';
            priv->fSensorValues[sm->mySubState-2] = atof(pCmd->inp_buf);
            if (sm->mySubState-2 == priv->iCtrlSens) priv->fValue = priv->fSensorValues[sm->mySubState-2];
            sprintf(line, "KRDG? (Idle) sensor %s = %8.2f", priv->cValidSensors[sm->mySubState-2], priv->fSensorValues[sm->mySubState-2]);
            SICSLogWrite(line, eLog);
       	  }

          if (sm->mySubState >= priv->iNumSensors+1) {
		fsm_change_state(sm, LS340State_Idle);
		return;
	  }

          sm->mySubState++;

          return;
        }
      } while (0);
      return;
    case eTimerEvent:
      priv->state_timer = NULL;

     if (priv->controller) {
	   char line[132];
            sprintf(line, "LS340 eMode: = %d", priv->controller->eMode);
            SICSLogWrite(line, eLog);

     }


      if (priv->fTarget > priv->fSetPoint) {
        priv->fSetPoint = priv->fTarget; /* set the setpoint to be the target */
        priv->iHeaterStatus = 1;
        sprintf(cmd, "SETP %d, %8.2f", priv->iPidLoop, priv->fSetPoint);  /* tell the Lakeshore to change the current setpoint to target */
        LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

        fsm_change_state(sm, LS340State_Raising);
        return;
      }
      if (priv->fTarget < priv->fSetPoint) {
        priv->fSetPoint = priv->fTarget; /* set the setpoint to be the target */

        priv->iHeaterStatus = 1;
        sprintf(cmd, "SETP %d, %8.2f", priv->iPidLoop, priv->fSetPoint);  /* tell the Lakeshore to change the current setpoint to target */
        LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
        fsm_change_state(sm, LS340State_Lowering);
        return;
      }
      if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {
	   char line[132];
          sprintf(cmd, "KRDG? %s", priv->cValidSensors[sm->mySubState-2]);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
          sprintf(line, "eTimerEvent (Idle): Sent: %s", cmd);
          SICSLogWrite(line, eLog);
      }

      /* restart timer */
      NetWatchRegisterTimer(&priv->state_timer,
          priv->ulIdleDelay,
          fsm_tmr_callback, sm);
      return;
    case eCommandEvent:
      return;
    case eTimeoutEvent:
      return;
  }
  return;
}

static void LS340State_Raising(pStateMachine sm, pEvtEvent event){
  pEVDriver driv = (pEVDriver) sm->context;
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;
  char cmd[CMDLEN];

  switch (event->event_type) {

    case eStateEvent:
      if (priv->state_timer)
        NetWatchRemoveTimer(priv->state_timer);

      NetWatchRegisterTimer(&priv->state_timer,
          priv->ulMonitorDelay,
          fsm_tmr_callback, sm);


      sprintf(cmd, "RANGE %d", priv->iRange); /* set the Range value > 0 turns heater on */
      LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

      sprintf(cmd, "CLIMIT? %d", priv->iPidLoop);
      LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      sm->mySubState = 1;

      return;

    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        pCmd->inp_buf[pCmd->inp_idx] = '\0';

        if (sm->mySubState == 1) { /* CLIMIT message */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';

            sprintf(line, "CLIMIT (Raising): %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);

            sscanf(pCmd->inp_buf, "%f,%f,%f,%d,%d", &priv->fSetPointLimit, &priv->fPosSlope, &priv->fNegSlope, &priv->iMaxCurrent, &priv->iMaxRange);
          }

          sm->mySubState++;

        } else if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {

     	  /* KRDG? Requests */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';
            priv->fSensorValues[sm->mySubState-2] = atof(pCmd->inp_buf);
            if (sm->mySubState-2 == priv->iCtrlSens) priv->fValue = priv->fSensorValues[sm->mySubState-2];
            sprintf(line, "KRDG? (Raising) sensor %s = %8.2f", priv->cValidSensors[sm->mySubState-2], priv->fSensorValues[sm->mySubState-2]);
            SICSLogWrite(line, eLog);

       	  }

          sm->mySubState++;

        }

        /* restart timer */
        NetWatchRegisterTimer(&priv->state_timer,
              priv->ulMonitorDelay,
              fsm_tmr_callback, sm);

      } while (0);
      return;

    case eTimerEvent:
      priv->state_timer = NULL;

      if (priv->controller->eMode != EVDrive) {
        fsm_change_state(sm, LS340State_Idle);
        return;
      }
      if (priv->fTarget < priv->fSetPoint) {
        priv->fSetPoint = priv->fTarget; /* set the setpoint to be the target */

        sprintf(cmd, "SETP %d, %8.2f", priv->iPidLoop, priv->fSetPoint);  /* tell the Lakeshore to change the current setpoint to target */
        LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
        fsm_change_state(sm, LS340State_Lowering);
        return;
      }

      if (sm->mySubState > priv->iNumSensors+1) sm->mySubState = 2;

      if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {
          sprintf(cmd, "KRDG? %s", priv->cValidSensors[sm->mySubState-2]);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      }
      return;
    case eCommandEvent:
      return;
    case eTimeoutEvent:
      return;
  }
  return;
}

static void LS340State_Lowering(pStateMachine sm, pEvtEvent event){
  pEVDriver driv = (pEVDriver) sm->context;
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;

  char cmd[CMDLEN];


  switch (event->event_type) {

    case eStateEvent:
      if (priv->state_timer)
        NetWatchRemoveTimer(priv->state_timer);

      sprintf(cmd, "RANGE %d", priv->iRange); /* set the Range value > 0 turns heater on */
      LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

      sprintf(cmd, "CLIMIT? %d", priv->iPidLoop);
      LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      sm->mySubState = 1;

      return;

    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        pCmd->inp_buf[pCmd->inp_idx] = '\0';

        if (sm->mySubState == 1) { /* CLIMIT message */
          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';

            sprintf(line, "CLIMIT (lowering): %s", pCmd->inp_buf);
            SICSLogWrite(line, eLog);

            sscanf(pCmd->inp_buf, "%f,%f,%f,%d,%d", &priv->fSetPointLimit, &priv->fPosSlope, &priv->fNegSlope, &priv->iMaxCurrent, &priv->iMaxRange);
          }

          sm->mySubState++;

        } else if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {

     	  /* KRDG? Requests */

          char* p = strchr(pCmd->inp_buf, '\r');
          if (p) {
            char line[132];
            *p = '\0';
            priv->fSensorValues[sm->mySubState-2] = atof(pCmd->inp_buf);
            if (sm->mySubState-2 == priv->iCtrlSens) priv->fValue = priv->fSensorValues[sm->mySubState-2];
            sprintf(line, "KRDG? (Lowering) sensor %s = %8.2f", priv->cValidSensors[sm->mySubState-2], priv->fSensorValues[sm->mySubState-2]);
            SICSLogWrite(line, eLog);

       	  }

          sm->mySubState++;

        }

        /* restart timer */
        NetWatchRegisterTimer(&priv->state_timer,
              priv->ulMonitorDelay,
              fsm_tmr_callback, sm);

      } while (0);
      return;
    case eTimerEvent:
      priv->state_timer = NULL;

      if (priv->controller->eMode != EVDrive) { /* return to IDLE state when not driving */
        fsm_change_state(sm, LS340State_Idle);
        return;
      }
      if (priv->fTarget > priv->fSetPoint) {

        priv->fSetPoint = priv->fTarget; /* set the setpoint to be the target */

        sprintf(cmd, "SETP %d, %8.2f", priv->iPidLoop, priv->fSetPoint);  /* tell the Lakeshore to change the current setpoint to target */
        LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

        fsm_change_state(sm, LS340State_Raising);
        return;
      }

      if (sm->mySubState > priv->iNumSensors+1) sm->mySubState = 2;

      if (sm->mySubState >= 2 && sm->mySubState <= priv->iNumSensors+1) {
          sprintf(cmd, "KRDG? %s", priv->cValidSensors[sm->mySubState-2]);
          LS340_SendCmd(priv, cmd, strlen(cmd), fsm_msg_callback);
      }

      return;
    case eCommandEvent:
      return;
    case eTimeoutEvent:
      return;
  }
  return;
}


static int LS340GetValue( pEVDriver self, float* fPos) {
  pLS340Driv me = NULL;
  assert(self);
  assert(self->pPrivate);
  me = (pLS340Driv) self->pPrivate;
  *fPos = me->fValue;
  return 1;
}

static int LS340SetValue( pEVDriver self, float fPos) {
  pLS340Driv me = NULL;
  assert(self);
  assert(self->pPrivate);
  me = (pLS340Driv) self->pPrivate;
  if (me->isLocked) {
    me->iError = LS340_ERR_LOCKED;
    return 0;
  }
  if (fPos < 0.0 || fPos > me->fSetPointLimit) {
    me->iError = LS340_ERR_RANGE;
    return 0;
  }
  me->fTarget = fPos;
  return 1;
}

static int LS340Send(pEVDriver self, char *pCommand, char *pReply, int iLen) {

    int rsp_len;
    rsp_len = iLen;

    LS340_SendReceive(self->pPrivate, pCommand, strlen(pCommand), pReply, &rsp_len);

    return 1;
}

static int LS340Error(pEVDriver self, int *iCode, char *error, int iErrLen) {
  pLS340Driv priv = (pLS340Driv) self->pPrivate;
  *iCode = priv->iError;
  switch (priv->iError) {
    case LS340_ERR_RANGE:
      strncpy(error,"Value out of range",iErrLen);
      break;
    case LS340_ERR_LOCKED:
      strncpy(error,"Object is locked",iErrLen);
      break;
    default:
      strncpy(error,"TODO Error Messages",iErrLen);
      break;
  }
  return 1;
}
static int LS340Fix(pEVDriver self, int iError) {
  /* TODO */
  return DEVFAULT;
}
static int LS340Init(pEVDriver self) {
  /* TODO */
  return 1;
}
static int LS340Close(pEVDriver self) {
  /* TODO */
  return -1;
}
static void LS340KillPrivate(void *pData) {
  pLS340Driv pMe = (pLS340Driv) pData;
  if (pMe) {
    if (pMe->asyncUnit) {
      AsyncUnitDestroy(pMe->asyncUnit);
      pMe->asyncUnit = NULL;
    }
    if (pMe ->name) {
      free(pMe  ->name);
      pMe ->name = NULL;
    }
    /* Not required as performed in caller
     * free(pMe);
     */
    return;
  }
}

static void LS340Notify(void* context, int event)
{
  /* TODO */
#if 0
  pEVDriver self = (pEVDriver) context;
  char line[132];

  switch (event) {
    case AQU_DISCONNECT:
      snprintf(line, 132, "Disconnect on Device '%s'", self->name);
      SICSLogWrite(line, eLog);
      /* TODO: disconnect */
      break;
    case AQU_RECONNECT:
      snprintf(line, 132, "Reconnect on Device '%s'", self->name);
      SICSLogWrite(line, eLog);
      /* TODO: reconnect */
      if (self->has_fsm) {
        /* Reset the state machine */
        if (self->state_timer)
          NetWatchRemoveTimer(self->state_timer);
        self->state_timer = 0;
        change_state(self, DMCState_Unknown);
        /* Schedule a timer event as soon as possible */
        NetWatchRegisterTimer(&self->state_timer,
            0,
            state_tmr_callback, self);
      }
      break;
  }
#endif
  return;
}

static pAsyncProtocol LS340_Protocol = NULL;

static int LS340_Tx(pAsyncProtocol p, pAsyncTxn ctx)
{
  int iRet = 1;
  pAsyncTxn myCmd = (pAsyncTxn) ctx;

  if (myCmd) {

    if (strchr(myCmd->out_buf, '?')) {
      myCmd->txn_status = ATX_ACTIVE;
    } else {
      myCmd->txn_status = ATX_COMPLETE;
    }

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

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

  switch (myCmd->txn_state) {
    case 0: /* first character */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == '\r' || rxchar == '\n')
        myCmd->txn_state = 99;
      break;
  }
  if (myCmd->txn_state == 99) {
    iRet = 0;
  }
  if (iRet == 0) { /* end of command */
    myCmd->txn_status = ATX_COMPLETE;
    return AQU_POP_CMD;
  }
  return iRet;
}

static int LS340_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 LS340_PrepareTxn(pAsyncProtocol p, pAsyncTxn txn, const char* cmd, int cmd_len, int rsp_len) {

  txn->out_buf = (char*) malloc(cmd_len+2);
  if (txn->out_buf == NULL) {
    SICSLogWrite("ERROR: Out of memory in LS340_PrepareTxn", eError);
    return 0;
  }
  memcpy(txn->out_buf, cmd, cmd_len);
  txn->out_len = cmd_len;

  if (txn->out_buf[txn->out_len-1] != '\r') {
    txn->out_buf[txn->out_len++] = '\r';
//    txn->out_buf[txn->out_len++] = '\r';
  }

//  txn->out_buf[txn->out_len++] = '\0';
  return 1;
}

void LS340InitProtocol(SicsInterp *pSics) {
  if (LS340_Protocol == NULL) {
    LS340_Protocol = AsyncProtocolCreate(pSics, "ls340", NULL, NULL);
    LS340_Protocol->sendCommand = LS340_Tx;
    LS340_Protocol->handleInput = LS340_Rx;
    LS340_Protocol->handleEvent = LS340_Ev;
    LS340_Protocol->prepareTxn = LS340_PrepareTxn;
    LS340_Protocol->killPrivate = NULL;
  }
}


pEVDriver CreateLS340Driver(int argc, char *argv[])
{
  int i, d = 0;
  int k, v;
  char cname[3];

  pEVDriver self = NULL;
  pLS340Driv priv = NULL;


  /* tcl script eg: EvFactory new tc1 ls340 sertemp1 1 B
   * Argv[] entering CreateLS340Driver -
   * 0 -
   */

  if (argc < 1)
    return NULL;
  self = CreateEVDriver(argc, argv);
  if (!self)
    return NULL;

  priv = (pLS340Driv)malloc(sizeof(LS340Driv));
  if(!priv) {
    DeleteEVDriver(self);
    return NULL;
  }
  memset(priv,0,sizeof(LS340Driv));
  priv->fValue = 0.0;

  if (!AsyncUnitCreate(argv[4], &priv->asyncUnit)) {
    char line[132];
    snprintf(line, 132, "Error: did not find AsyncQueue %s for Device %s", argv[3], argv[4]);
    DeleteEVDriver(self);
    free(priv);
    return NULL;
  }
  AsyncUnitSetNotify(priv->asyncUnit, self, LS340Notify);
  AsyncUnitSetDelay(priv->asyncUnit, 50);

  /* initialise function pointers */
  self->SetValue = LS340SetValue;
  self->GetValue = LS340GetValue;
  self->Send     = LS340Send;
  self->GetError = LS340Error;
  self->TryFixIt = LS340Fix;
  self->Init     = LS340Init;
  self->Close    = LS340Close;

  self->pPrivate = priv;
  self->KillPrivate = LS340KillPrivate;

  priv->iHeaterStatus = 1; /* heater on by default */

  priv->fsm.context = self;
  priv->fsm.state_name = state_name;
  priv->fsm.event_name = event_name;
  priv->name = strdup(argv[3]);
  priv->iPidLoop = atoi(argv[5]); /* PID Loop 1..2 */


  if (argc < 7) return NULL;

  strncpy(priv->cCtrlSensName, argv[6], strlen(argv[6])); /* control sensor name  */
  if (strlen(priv->cCtrlSensName) > 0) {
    strncpy(priv->cValidSensors[0], priv->cCtrlSensName, strlen(priv->cCtrlSensName));
    priv->iCtrlSens = 0;
    d++;
  }

  /* parse out valid sensor names from argv[7] */
  for (i = d; i < NUM_INPUT_SENSORS; i++) {
    priv->cValidSensors[i][0] = 'X'; /* clear the list, first */
    priv->cValidSensors[i][1] = '\0'; /* clear the list, first */
    priv->cValidSensors[i][2] = '\0'; /* clear the list, first */
  }

  i = 0;
  v = 0;
  while (i < strlen(argv[7])) {
    if (isalpha(argv[7][i]) > 0) {
      cname[0] = argv[7][i];
      cname[1] = '\0';
      cname[2] = '\0';

      if (isdigit(argv[7][i+1]) > 0) {
        cname[1] = argv[7][i+1];
        i++;
      }

      v = 0;
      /* check to see if sensor is already in list */
      for (k = 0; k < NUM_INPUT_SENSORS; k++) {
         if (priv->cValidSensors[k][0] == cname[0]) {
           if (strcmp(priv->cValidSensors[k], cname) == 0) {
              v = 1;
              break; /* already in list, skip */
           }
           /* C vs C2 */
           if (priv->cValidSensors[k][1] == '\0' && isdigit(cname[1]) > 0) {
              v = 1;
              break;
           }

           /* C2 vs C */
           if (isdigit(priv->cValidSensors[k][1]) > 0 && cname[1] == '\0') {
              v = 1;
              break;
           }

         }
      }
      if (v == 0) { /* not in list, so add it in! */
         strncpy(priv->cValidSensors[d], cname, strlen(cname));
         d++;
      }
    }
    i++;
  }
  priv->iNumSensors = d; /* assign number of sensors found */


  priv->ulIdlePollRate = 5000;
  priv->ulIdleDelay = priv->ulIdlePollRate / priv->iNumSensors+1; /* +1 to include CLIMIT query */

  priv->ulMonitorPollRate = 200;
  priv->ulMonitorDelay = priv->ulMonitorPollRate / priv->iNumSensors+1; /* +1 to include CLIMIT query */
  fsm_change_state(&priv->fsm, LS340State_Unknown);

  return self;
}

void LS340Register(pEVControl self, pEVDriver driv)
{
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;
  priv->controller = self;
  if (self->pName) {
    if (priv->name)
      free(priv->name);
    priv->name = strdup(self->pName);
  }

}

int LS340Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
    int argc, char *argv[])
{
  pEVControl self = (pEVControl)pData;
  pEVDriver driv = self->pDriv;
  pLS340Driv priv = (pLS340Driv) driv->pPrivate;
  assert(self);
  assert(pCon);
  assert(pSics);

  if(argc < 2)
  {
    return EVControlWrapper(pCon,pSics,pData,argc,argv);
  }
  if (strcasecmp("send", argv[1]) == 0) {
    char cmd[CMDLEN];
    int cmd_len;
    char rsp[CMDLEN];
    int rsp_len;

    /* Managers only */
    if (!SCMatchRights(pCon, usMugger)) {
      return 0;
    }

    if (argc < 2) return 0;

    sprintf(cmd, "%s", argv[2]);
    cmd_len = strlen(cmd);

    rsp_len = CMDLEN;
    LS340_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len);
    SCWrite(pCon, rsp, eValue);

    return 1;
  }

  /* assign control sensor index */
  if (strcasecmp("controlsensor", argv[1]) == 0) {
    char cmd[64];
    char cname[3];

    char rsp[CMDLEN];
    char line[132];
    int i, v;
    char *p = NULL;

    v = -1;
    if (argc > 2) {

      p = (char *)strcasestr(argv[2], "sensor");
      if (p) {
        if (strlen(p) > 6) {
          p+=6; /* point to next char after the word sensor */
        }
      } else {
	snprintf(line, sizeof(line),  "control sensor %s specified not in list", argv[2]);
        SCWrite(pCon, line, eError);
        return 0;
      }

      /* swap control sensor locations from [0] */
      strncpy(cname, p, sizeof(cname));

      /* algorithm:
       * 1. check to see if new sensor given is in list
       *    IF YES, swap [0] with location found
       */
       for (i = 0; i < priv->iNumSensors; i++) {
         if (strcmp(priv->cValidSensors[i], cname) == 0) { /* Yes, given sensor is in list! */
            v = i;
            break;
          }
       }


      if (v > -1) { /* in list, swap values */
       float temp;

       strcpy(priv->cValidSensors[0], priv->cValidSensors[v]);
       temp = priv->fSensorValues[0];
       priv->fSensorValues[0] = priv->fSensorValues[v];
       priv->fSensorValues[v] = temp;
       strcpy(priv->cValidSensors[v], priv->cCtrlSensName);

       strcpy(priv->cCtrlSensName, cname);

       sprintf(cmd, "CSET %d, %s, 1, %d, 1", priv->iPidLoop, priv->cValidSensors[priv->iCtrlSens], priv->iHeaterStatus);

       LS340_SendCmd(priv, cmd, strlen(cmd), NULL);

      } else {
	snprintf(line, sizeof(line),  "control sensor %s specified not in list", cname);
        SCWrite(pCon, line, eError);
        return 0;
      }
    }

    snprintf(rsp, CMDLEN, "%s.controlsensor = sensor%s", priv->name, priv->cValidSensors[priv->iCtrlSens]);
    SCWrite(pCon, rsp, eValue);
    return 1;
  }

  if (strcasecmp("debug", argv[1]) == 0) {
    char rsp[CMDLEN];
    if (argc > 2) {
      int debug = atoi(argv[2]);
      if (debug != 0)
        priv->fsm.debug = true;
      else
        priv->fsm.debug = false;
    }
    snprintf(rsp, CMDLEN, "%s.debug = %d", priv->name, priv->fsm.debug ? 1 : 0);
    SCWrite(pCon, rsp, eValue);
    return 1;
  }
  if (strcasecmp("setpoint", argv[1]) == 0) {
    char rsp[CMDLEN];
    snprintf(rsp, CMDLEN, "%s.setpoint = %8.2f", priv->name, priv->fSetPoint);
    SCWrite(pCon, rsp, eValue);

    return 1;
  }
  if (strcasecmp("numsensors", argv[1]) == 0) {
    char rsp[CMDLEN];
    snprintf(rsp, CMDLEN, "%s.numsensors = %d", priv->name, priv->iNumSensors);
    SCWrite(pCon, rsp, eValue);

    return 1;
  }
  if (strcasecmp("pollingrate", argv[1]) == 0) {
    char rsp[CMDLEN];
    if (argc > 2) {
       priv->ulIdlePollRate = (unsigned)atol(argv[2]);
       priv->ulIdleDelay = priv->ulIdlePollRate / priv->iNumSensors+1; /* +1 to include CLIMIT query */

       fsm_change_state(&priv->fsm, LS340State_Unknown);
    }
    snprintf(rsp, CMDLEN, "%s.pollingrate = %lu", priv->name, priv->ulIdlePollRate);
    SCWrite(pCon, rsp, eValue);

    return 1;
  }

  if (strcasecmp("heateron", argv[1]) == 0) {
    char rsp[CMDLEN];
    char cmd[CMDLEN];
    if (argc > 2) {
       priv->iHeaterStatus = atoi(argv[2]);
       if (priv->iHeaterStatus == 1) { /* turn heater on */
       	/* sprintf(cmd, "RANGE %d", priv->iRange);
         LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
        */
       } else {
       	 sprintf(cmd, "RANGE 0"); /* turn heater off otherwise */
         LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
       }
    }
    snprintf(rsp, CMDLEN, "%s.heateron = %d", priv->name, priv->iHeaterStatus);
    SCWrite(pCon, rsp, eValue);

    if (argc > 2) fsm_change_state(&priv->fsm, LS340State_Idle); /* move back to Idle state? */
    return 1;
  }
  if (strcasecmp("range", argv[1]) == 0) {
    char rsp[CMDLEN];
    char cmd[CMDLEN];
    if (argc > 2) {
       priv->iRange = atoi(argv[2]);

       if (priv->iRange > 0) priv->iHeaterStatus = 1;

       sprintf(cmd, "RANGE %d", priv->iRange);
       LS340_SendCmd(priv, cmd, strlen(cmd), NULL);
    }
    snprintf(rsp, CMDLEN, "%s.range = %d", priv->name, priv->iRange);
    SCWrite(pCon, rsp, eValue);

    return 1;
  }


  if (strcasecmp("sensorlist", argv[1]) == 0) {

     int i;
     char rsp[CMDLEN];
     char senslist[132];
     char temp[132];

      strcpy(senslist, ""); /* prepare string */

      for (i = 0; i < priv->iNumSensors; i++) {
       sprintf(temp, "sensor%s", priv->cValidSensors[i]);
       strcat(senslist, temp);
       if (i < priv->iNumSensors-1) strcat(senslist, ",");
      }

      snprintf(rsp, CMDLEN, "%s.sensorlist = %s", priv->name, senslist);

      SCWrite(pCon, rsp, eValue);

      return 1;
  }

  if (strncasecmp("sensor", argv[1], 6) == 0) {
    char rsp[CMDLEN];
    int i;
    char name[3];

    name[0] = '\0';
    name[1] = '\0';
    name[2] = '\0';

    if (strlen(argv[1]) == 7) { /* sensorA */
       name[0] = argv[1][6];
    }

    if (strlen(argv[1]) == 8) { /* sensorC1 */
       name[0] = argv[1][6];
       name[1] = argv[1][7];
    }

    i = getSensorIndex(priv, name);

    if (i >= 0) {
        snprintf(rsp, CMDLEN, "%s.sensor%s = %8.2f", priv->name, priv->cValidSensors[i], priv->fSensorValues[i]);
    } else {
        snprintf(rsp, CMDLEN, "%s.sensor%s is not in list", priv->name, name);
    }


    SCWrite(pCon, rsp, eValue);
    return 1;

  }


  if (strcasecmp("list", argv[1]) == 0) {
     int i;
     int iRet;
     char rsp[CMDLEN];
     char senslist[132];
     char temp[132];

     iRet = EVControlWrapper(pCon,pSics,pData,argc,argv);
     if (iRet) {
      snprintf(rsp, CMDLEN, "%s.pidloop = %d", priv->name, priv->iPidLoop);
      SCWrite(pCon, rsp, eValue);

      snprintf(rsp, CMDLEN, "%s.controlsensor = sensor%s", priv->name, priv->cValidSensors[priv->iCtrlSens]);
      SCWrite(pCon, rsp, eValue);

      snprintf(rsp, CMDLEN, "%s.numsensors = %d", priv->name, priv->iNumSensors);
      SCWrite(pCon, rsp, eValue);

      strcpy(senslist, ""); /* prepare string */

      for (i = 0; i < priv->iNumSensors; i++) {
       sprintf(temp, "sensor%s", priv->cValidSensors[i]);
       strcat(senslist, temp);
       if (i < priv->iNumSensors-1) strcat(senslist, ",");
      }

      snprintf(rsp, CMDLEN, "%s.sensorlist = %s", priv->name, senslist);

      SCWrite(pCon, rsp, eValue);

      for (i = 0; i < priv->iNumSensors; i++) {
        snprintf(rsp, CMDLEN, "%s.sensor%s = %8.2f", priv->name, priv->cValidSensors[i], priv->fSensorValues[i]);
        SCWrite(pCon, rsp, eValue);
      }

      snprintf(rsp, CMDLEN, "%s.pollingrate = %lu", priv->name, priv->ulIdlePollRate);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.setpoint = %8.2f", priv->name, priv->fSetPoint);
      SCWrite(pCon, rsp, eValue);

      snprintf(rsp, CMDLEN, "%s.target = %8.2f", priv->name, priv->fTarget);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.setpointlimit = %8.2f", priv->name, priv->fSetPointLimit);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.positiveslope = %8.2f", priv->name, priv->fPosSlope);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.negativeslope = %8.2f", priv->name, priv->fNegSlope);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.maxcurrent = %d", priv->name, priv->iMaxCurrent);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.maxrange = %d", priv->name, priv->iMaxRange);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.range = %d", priv->name, priv->iRange);
      SCWrite(pCon, rsp, eValue);
      snprintf(rsp, CMDLEN, "%s.heateron = %d", priv->name, priv->iHeaterStatus);
      SCWrite(pCon, rsp, eValue);
     }
     return iRet;
  }
  return EVControlWrapper(pCon,pSics,pData,argc,argv);
}