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

     /* 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);
     }
     /* get rid of parameter space */
     if(pM->ParArray)
     {
       ObParDelete(pM->ParArray);
     }
     
     /* 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;
   }