/*--------------------------------------------------------------------------
                               P I M O T O R
                               
   This file contains the implementation of a motor driver for the Physik
   Instrumente Piezo Controller E-255. It is handled as a motor though it
   is a Piezo device. This gets a preset voltage in millivolts and that
   determines it's elongation and that is it. 

   Mark Koennecke, February 1999

----------------------------------------------------------------------------*/
#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"


/*================== The Driver data structure ============================*/
typedef struct __PIPiezo {
  /* 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);


  /* PiPiezo specific fields */
  int iPort;
  char *hostname;
  int iChannel;
  int iMotor;
  void *pSerial;
  int iLastError;
  int iLastVolt;
} PiPiezo, *pPiPiezo;

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

  self = (pPiPiezo) pData;
  assert(self);

  /* format command */
  sprintf(pCommand, "%1.1dTO\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)
{
  pPiPiezo self = NULL;
  int iRet;
  char pCommand[20];
  char pReply[80];
  int iTmo;

  self = (pPiPiezo) pData;
  assert(self);

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

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

  return OKOK;
}

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

  self = (pPiPiezo) pData;
  assert(self);

  *iCode = self->iLastError;

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

  /* wait a little to give this thing a reaction time, otherwise
     trouble reading responses
   */
  SicsWait(2);
  SerialError(self->iLastError, pError, iErrLen);
  return;
}

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

  self = (pPiPiezo) 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)
{

  /* just drive to 0 volt */
  PIRun(pData, 0.0);

  return 1;
}

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

  self = (pPiPiezo) pData;
  assert(self);

  /* read actual velocity: should be 0 when done */
  sprintf(pCommand, "%1.1dTO", 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;
  iSpeed = atoi(pPtr);
  iDiff = iSpeed - self->iLastVolt;
  if (ABS(iDiff) > 5) {
    return HWBusy;
  }
  return HWIdle;
}

/*------------------------------------------------------------------------*/
void KillPiPiezo(void *pData)
{
  pPiPiezo self = (pPiPiezo) pData;

  if (!self)
    return;

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

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

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

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

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

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

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

  pNew->fUpper = 12000.;
  pNew->fLower = -2000.;

  /* 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);
    KillPiPiezo(pNew);
    return NULL;
  }

  /* 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;
  pNew->KillPrivate = KillPiPiezo;

  /* success */
  return (MotorDriver *) pNew;
}