/*--------------------------------------------------------------------------
                               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)
      {
        strncpy(pError,"Timeout at serial line", iErrLen);
        return;
      }  
      if(*iCode == PICOMERR)
      {
        strncpy(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;
  }