/*--------------------------------------------------------------------------
                               P I M O T O R
                               
   This file contains the implementation of a motor driver for the Physik
   Instrument C-804 DC motor controller. Plus the implementation of an 
   additional wrapper function for handling addtional support commands 
   for this motor controller.
   
   Mark Koenencke, September 1998
----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include "fortify.h"
#include "sics.h"
#include <tcl.h>
#include "hardsup/serialsinq.h"
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.h"
#include "motor.h"
#include "pimotor.h"


/*================== The Driver data structure ============================*/
typedef struct __PIMoDriv {
  /* general motor driver interface 
     fields. REQUIRED!
   */
  float fUpper;                 /* upper limit */
  float fLower;                 /* lower limit */
  char *name;
  int (*GetPosition) (void *self, float *fPos);
  int (*RunTo) (void *self, float fNewVal);
  int (*GetStatus) (void *self);
  void (*GetError) (void *self, int *iCode, char *buffer, int iBufLen);
  int (*TryAndFixIt) (void *self, int iError, float fNew);
  int (*Halt) (void *self);
  int (*GetDriverPar) (void *self, char *name, float *value);
  int (*SetDriverPar) (void *self, SConnection * pCon,
                       char *name, float newValue);
  void (*ListDriverPar) (void *self, char *motorName, SConnection * pCon);
  void (*KillPrivate) (void *self);


  /* C-804 specific fields */
  int iPort;
  char *hostname;
  int iChannel;
  int iMotor;
  void *pSerial;
  int iLastError;
} C804Driv, *pC804Driv;

#define PITMO -845
#define PICOMERR -846
/*--------------------------------------------------------------------------*/
static int PIPosition(void *pData, float *fPos)
{
  pC804Driv self = NULL;
  int iRet;
  char pCommand[20];
  char pReply[80];
  char *pPtr;

  self = (pC804Driv) pData;
  assert(self);

  /* format command */
  sprintf(pCommand, "%1.1dTP\r", self->iMotor);

  /* send command */
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  if (iRet != 1) {
    self->iLastError = iRet;
    *fPos = -99999;
    return HWFault;
  }

  /* catch TMO or bad reply */
  if (strstr(pReply, "TMO") != NULL) {
    self->iLastError = PITMO;
    *fPos = -9999;
    return HWFault;
  }
  if (strstr(pReply, "?") != NULL) {
    self->iLastError = PICOMERR;
    *fPos = -9999;
    return HWFault;
  }


  /* read value */
  pPtr = pReply + 3;
  *fPos = atof(pPtr);
  return OKOK;
}

/*-------------------------------------------------------------------------*/
static int PIRun(void *pData, float fVal)
{
  pC804Driv self = NULL;
  int iRet;
  char pCommand[20];
  char pReply[80];
  int iTmo;

  self = (pC804Driv) pData;
  assert(self);

  /* format drive command */
  sprintf(pCommand, "%1.1dMA%10.10d\r", self->iMotor, (int) fVal);

  iTmo = SerialGetTmo(&self->pSerial);
  SerialConfig(&self->pSerial, 0);

  /* send command */
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  if (iRet != 1) {
    self->iLastError = iRet;
    return HWFault;
  }

  /* reset tmo */
  SerialConfig(&self->pSerial, iTmo);

  /* wait a little to allow to gather some speed before checking on it */
  SicsWait(2);
  return OKOK;
}

/*---------------------------------------------------------------------------*/
static void PIError(void *pData, int *iCode, char *pError, int iErrLen)
{
  pC804Driv self = NULL;

  self = (pC804Driv) pData;
  assert(self);

  *iCode = self->iLastError;

  if (*iCode == PITMO) {
    strlcpy(pError, "Timeout at serial line", iErrLen);
    return;
  }
  if (*iCode == PICOMERR) {
    strlcpy(pError, "C-804 Command Error", iErrLen);
    return;
  }

  SerialError(self->iLastError, pError, iErrLen);
  return;
}

/*--------------------------------------------------------------------------*/
static int PIFixError(void *pData, int iError, float fNew)
{
  pC804Driv self = NULL;
  int iRet;

  self = (pC804Driv) pData;
  assert(self);

  switch (iError) {
    /* network errors */
  case NOCONNECTION:
  case EL734__BAD_FLUSH:
  case EL734__BAD_RECV:
  case EL734__BAD_RECV_NET:
  case EL734__BAD_RECV_UNKN:
  case EL734__BAD_RECVLEN:
  case EL734__BAD_RECV1:
  case EL734__BAD_RECV1_PIPE:
  case EL734__BAD_RNG:
  case EL734__BAD_SEND:
  case EL734__BAD_SEND_PIPE:
  case EL734__BAD_SEND_NET:
  case EL734__BAD_SEND_UNKN:
  case EL734__BAD_SENDLEN:
    SerialClose(&self->pSerial);
    iRet = SerialOpen(&self->pSerial, self->hostname,
                      self->iPort, self->iChannel);
    if (iRet != 1) {
      return MOTREDO;
    } else {
      return MOTFAIL;
    }
    break;
    /* handable protocoll errors */
  case EL734__BAD_TMO:
  case PITMO:
  case TIMEOUT:
    return MOTREDO;
    break;
  case PICOMERR:
    return MOTFAIL;
    break;
  default:
    return MOTFAIL;
    break;
  }
  return MOTFAIL;
}

/*---------------------------------------------------------------------------*/
static int PIHalt(void *pData)
{
  pC804Driv self = NULL;
  char pCommand[20], pReply[80];
  int iTmo;

  self = (pC804Driv) pData;
  assert(self);

  sprintf(pCommand, "%1.1dAB\r", self->iMotor);

  iTmo = SerialGetTmo(&self->pSerial);
  SerialConfig(&self->pSerial, 0);
  SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  SerialConfig(&self->pSerial, iTmo);

  return 1;
}

#define ABS(x) (x < 0 ? -(x) : (x))
/*-------------------------------------------------------------------------*/
static int PIStatus(void *pData)
{
  pC804Driv self = NULL;
  char pCommand[20], pReply[80], *pPtr;
  int iRet;
  float fSpeed;

  self = (pC804Driv) pData;
  assert(self);

  /* read actual velocity: should be 0 when done */
  sprintf(pCommand, "%1.1dTV\r", self->iMotor);
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  if (iRet != 1) {
    self->iLastError = iRet;
    return HWFault;
  }
  /* check reply */
  if (strstr(pReply, "TMO") != NULL) {
    self->iLastError = PITMO;
    return HWFault;
  }
  if (strstr(pReply, "?") != NULL) {
    self->iLastError = PICOMERR;
    return HWFault;
  }

  pPtr = pReply + 3;
  fSpeed = atof(pPtr);
  if (ABS(fSpeed) > 0) {
    return HWBusy;
  }
  return HWIdle;
}

/*------------------------------------------------------------------------*/
static void KillC804(pC804Driv self)
{
  if (!self)
    return;

  if (self->name) {
    free(self->name);
  }
  if (self->hostname) {
    free(self->hostname);
  }
  if (self->pSerial) {
    SerialClose(&self->pSerial);
  }
  free(self);
}

/*--------------------------------------------------------------------------
 * The data necessary for initialising the C804 motor is contained in a
 * Tcl-Array given as pArray parameter. In case of an error the error is
 * returned in the Tcl-interpreter.
 */
static pC804Driv MakeC804Driver(Tcl_Interp * pTcl, char *pArray)
{
  pC804Driv pNew = NULL;
  int iRet, iVal, iTmo;
  double dVal;
  char *pPar = NULL;
  char pCommand[20], pReply[40];

  /* allocate space: the final frontier */
  pNew = (pC804Driv) malloc(sizeof(C804Driv));
  if (!pNew) {
    Tcl_SetResult(pTcl, "Out of memory for C804 Driver", NULL);
    return NULL;
  }
  memset(pNew, 0, sizeof(C804Driv));

  /* connection parameters */
  pPar = (char *) Tcl_GetVar2(pTcl, pArray, "Computer", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to find serial port server host name",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->hostname = strdup(pPar);

  pPar = NULL;
  pPar = (char *) Tcl_GetVar2(pTcl, pArray, "port", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to find serial port server port adress",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  iRet = Tcl_GetInt(pTcl, pPar, &iVal);
  if (iRet != TCL_OK) {
    Tcl_SetResult(pTcl, " Failed to convert port adress to integer", NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->iPort = iVal;

  pPar = NULL;
  pPar = (char *) Tcl_GetVar2(pTcl, pArray, "channel", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to find serial port server channel adress",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  iRet = Tcl_GetInt(pTcl, pPar, &iVal);
  if (iRet != TCL_OK) {
    Tcl_SetResult(pTcl, " Failed to convert channel number to integer",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->iChannel = iVal;
  pPar = NULL;

  pPar = (char *) Tcl_GetVar2(pTcl, pArray, "motor", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to find motor number", NULL);
    KillC804(pNew);
    return NULL;
  }
  iRet = Tcl_GetInt(pTcl, pPar, &iVal);
  if (iRet != TCL_OK) {
    Tcl_SetResult(pTcl, " Failed to convert motor number to integer",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->iMotor = iVal;

  /* limits */
  pPar = NULL;
  pPar =
      (char *) Tcl_GetVar2(pTcl, pArray, "lowerlimit", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to  find lower motor limit", NULL);
    KillC804(pNew);
    return NULL;
  }
  iRet = Tcl_GetDouble(pTcl, pPar, &dVal);
  if (iRet != TCL_OK) {
    Tcl_SetResult(pTcl, " Failed to convert lower limit to double", NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->fLower = dVal;

  pPar = NULL;
  pPar =
      (char *) Tcl_GetVar2(pTcl, pArray, "upperlimit", TCL_LEAVE_ERR_MSG);
  if (!pPar) {
    Tcl_SetResult(pTcl, "Failed to find upper motor limit", NULL);
    KillC804(pNew);
    return NULL;
  }
  iRet = Tcl_GetDouble(pTcl, pPar, &dVal);
  if (iRet != TCL_OK) {
    Tcl_SetResult(pTcl, " Failed to convert motor number to integer",
                  NULL);
    KillC804(pNew);
    return NULL;
  }
  pNew->fUpper = dVal;


  /* open the serialport connection */
  iRet = SerialOpen(&pNew->pSerial, pNew->hostname, pNew->iPort,
                    pNew->iChannel);
  if (iRet != 1) {
    Tcl_SetResult(pTcl,
                  "Failed to open connection to serial port server", NULL);
    KillC804(pNew);
    return NULL;
  }
  SerialConfig(&pNew->pSerial, 50000);

  /* switch on, just to make sure */
  iTmo = SerialGetTmo(&pNew->pSerial);
  SerialConfig(&pNew->pSerial, 0);
  sprintf(pCommand, "%1.1dMN\r", pNew->iMotor);
  SerialWriteRead(&pNew->pSerial, pCommand, pReply, 39);
  SerialConfig(&pNew->pSerial, iTmo + 5000);

  /* configure the connection */
  SerialATerm(&pNew->pSerial, "1\x03");
  SerialSendTerm(&pNew->pSerial, "\r");

  /* configure the function pointers */
  pNew->GetPosition = PIPosition;
  pNew->RunTo = PIRun;
  pNew->GetStatus = PIStatus;
  pNew->GetError = PIError;
  pNew->TryAndFixIt = PIFixError;
  pNew->Halt = PIHalt;

  /* success */
  return pNew;
}

/*--------------------------------------------------------------------------*/
int PISetSpeed(pMotor pMot, SConnection * pCon, float fNew)
{
  int iVal, iRet, iTmo;
  char pCommand[20], pReply[80], pError[132];
  pC804Driv self = NULL;

  assert(pMot);
  self = (pC804Driv) pMot->pDriver;
  assert(self);

  iVal = (int) fNew;
  sprintf(pCommand, "%1.1dSV%7.7d\r", self->iMotor, iVal);
  iTmo = SerialGetTmo(&self->pSerial);
  SerialConfig(&self->pSerial, 0);
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  SerialConfig(&self->pSerial, iTmo);
  if (iRet != 1) {
    self->iLastError = iRet;
    PIError(self, &iRet, pReply, 79);
    sprintf(pError, "ERROR: %s", pReply);
    SCWrite(pCon, pError, eError);
    return 0;
  }
  return 1;
}

/*--------------------------------------------------------------------------*/
int PIHome(pMotor pMot, SConnection * pCon)
{
  int iVal, iRet, iTmo;
  char pCommand[20], pReply[80], pError[132];
  pC804Driv self = NULL;

  assert(pMot);
  self = (pC804Driv) pMot->pDriver;
  assert(self);

  sprintf(pCommand, "%1.1dDH\r", self->iMotor);
  iTmo = SerialGetTmo(&self->pSerial);
  SerialConfig(&self->pSerial, 0);
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  SerialConfig(&self->pSerial, iTmo);
  if (iRet != 1) {
    self->iLastError = iRet;
    PIError(self, &iRet, pReply, 79);
    sprintf(pError, "ERROR: %s", pReply);
    SCWrite(pCon, pError, eError);
    return 0;
  }
  return 1;
}

/*--------------------------------------------------------------------------*/
int PIOn(pMotor pMot, SConnection * pCon)
{
  int iVal, iRet, iTmo;
  char pCommand[20], pReply[80], pError[132];
  pC804Driv self = NULL;

  assert(pMot);
  self = (pC804Driv) pMot->pDriver;
  assert(self);

  sprintf(pCommand, "%1.1dMN\r", self->iMotor);
  iTmo = SerialGetTmo(&self->pSerial);
  SerialConfig(&self->pSerial, 0);
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  SerialConfig(&self->pSerial, iTmo);
  if (iRet != 1) {
    self->iLastError = iRet;
    PIError(self, &iRet, pReply, 79);
    sprintf(pError, "ERROR: %s", pReply);
    SCWrite(pCon, pError, eError);
    return 0;
  }
  return 1;
}

/*-------------------------------------------------------------------------*/
int PIGetSpeed(pMotor pMot, SConnection * pCon, float *fNew)
{
  int iVal, iRet;
  char pCommand[20], pReply[80], pError[132], *pPtr;
  pC804Driv self = NULL;

  assert(pMot);
  self = (pC804Driv) pMot->pDriver;
  assert(self);

  sprintf(pCommand, "%1.1dTY\r", self->iMotor);
  iRet = SerialWriteRead(&self->pSerial, pCommand, pReply, 79);
  if (iRet != 1) {
    self->iLastError = iRet;
    PIError(self, &iRet, pReply, 79);
    sprintf(pError, "ERROR: %s", pReply);
    SCWrite(pCon, pError, eError);
    return 0;
  }
  pPtr = pReply + 3;
  *fNew = atof(pPtr);
  return 1;
}

/*--------------------------------------------------------------------------*/
int PIMotorWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
                   int argc, char *argv[])
{
  pMotor pMot = NULL;
  double dVal;
  float fVal;
  char pBueffel[256];
  int iRet;

  pMot = (pMotor) pData;

  assert(pCon);
  assert(pSics);
  assert(pMot);

  if (argc >= 2) {
    strtolower(argv[1]);
    if (strcmp(argv[1], "speed") == 0) {
      if (argc >= 3) {
        iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dVal);
        if (iRet != TCL_OK) {
          SCWrite(pCon, "ERROR: speed parameter not recognised as number",
                  eError);
          return 0;
        }
        if (!SCMatchRights(pCon, usMugger)) {
          SCWrite(pCon, "ERROR: Insufficient privilege to change speed",
                  eError);
          return 0;
        }
        iRet = PISetSpeed(pMot, pCon, (float) dVal);
        if (!iRet) {
          return 0;
        }
        SCSendOK(pCon);
        return 1;
      } else {
        iRet = PIGetSpeed(pMot, pCon, &fVal);
        if (!iRet) {
          return 0;
        }
        sprintf(pBueffel, "%s.speed = %f", argv[0], fVal);
        SCWrite(pCon, pBueffel, eValue);
        return 1;
      }
    }                           /* end speed */
    if (strcmp(argv[1], "home") == 0) {
      if (!SCMatchRights(pCon, usMugger)) {
        SCWrite(pCon, "ERROR: Insufficient privilege to change hardware 0",
                eError);
        return 0;
      }
      iRet = PIHome(pMot, pCon);
      if (!iRet) {
        return 0;
      }
      SCSendOK(pCon);
      return 1;
    }
    if (strcmp(argv[1], "on") == 0) {
      if (!SCMatchRights(pCon, usUser)) {
        SCWrite(pCon, "ERROR: Insufficient privilege to switch motor on",
                eError);
        return 0;
      }
      iRet = PIOn(pMot, pCon);
      if (!iRet) {
        return 0;
      }
      SCSendOK(pCon);
      return 1;
    }
    if (strcmp(argv[1], "list") == 0) {
      iRet = PIGetSpeed(pMot, pCon, &fVal);
      if (!iRet) {
        return 0;
      }
      sprintf(pBueffel, "%s.speed = %f", argv[0], fVal);
      SCWrite(pCon, pBueffel, eValue);
      return MotorAction(pCon, pSics, pData, argc, argv);
    }

  }

  /* end arguments */
  /* forward everything else to main motor handler */
  return MotorAction(pCon, pSics, pData, argc, argv);
}

/*--------------------------------------------------------------------------*/
static void PIMotorKill(void *self)
{
  pMotor pM;
  assert(self);

  pM = (pMotor) self;

  if (pM->name)
    free(pM->name);

  if (pM->pDrivInt) {
    free(pM->pDrivInt);
  }

  if (pM->pCall) {
    DeleteCallBackInterface(pM->pCall);
  }

  /* kill driver */
  if (pM->drivername) {
    if (strcmp(pM->drivername, "c804") == 0) {
      if (pM->pDriver) {
        KillC804((pC804Driv) pM->pDriver);
      }
    } else if (strcmp(pM->drivername, "SIM") == 0) {
      if (pM->pDriver) {
        KillSIM((void *) pM->pDriver);
      }
    }
  }

  if (pM->drivername) {
    free(pM->drivername);
  }

  /* kill Descriptor */
  DeleteDescriptor(pM->pDescriptor);

  free(pM);
}

/*-------------------------------------------------------------------------*/
int PIMotorFactory(SConnection * pCon, SicsInterp * pSics, void *pData,
                   int argc, char *argv[])
{
  pMotor pNew = NULL;
  MotorDriver *pDriver = NULL;
  char pBueffel[512];
  int iD, iRet;
  Tcl_Interp *ppTcl;

  assert(pCon);
  assert(pSics);

  /* a first check */
  if (argc < 3) {
    SCWrite(pCon, "Insufficient arguments for motor creation", eError);
    return 0;
  }

  /* create the driver */
  strtolower(argv[2]);
  strtolower(argv[1]);
  if (strcmp(argv[2], "c804") == 0) {
    pDriver = (MotorDriver *) MakeC804Driver(pSics->pTcl, argv[3]);
    if (!pDriver) {
      ppTcl = (Tcl_Interp *) pSics->pTcl;
      SCWrite(pCon, ppTcl->result, eError);
      return 0;
    }
    /* create the motor */
    pNew = MotorInit("c804", argv[1], pDriver);
    if (!pNew) {
      sprintf(pBueffel, "Failure to create motor %s", argv[1]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
  } else if (strcmp(argv[2], "sim") == 0) {
    iD = argc - 3;
    pDriver = CreateSIM(pCon, iD, &argv[3]);
    if (!pDriver) {
      return 0;
    }
    /* create the motor */
    pNew = MotorInit("SIM", argv[1], pDriver);
    if (!pNew) {
      sprintf(pBueffel, "Failure to create motor %s", argv[1]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
  } else {
    sprintf(pBueffel, "Motor Type %s cot recognized for motor %s",
            argv[2], argv[1]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* create the interpreter command */
  iRet = AddCommand(pSics, argv[1], PIMotorWrapper, PIMotorKill, pNew);
  if (!iRet) {
    sprintf(pBueffel, "ERROR: duplicate command %s not created", argv[1]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  return 1;
}