/*------------------------------------------------------------------------
                          M O T O R S
                          
     This file implements the SICS motor handling. This is the logical
     level, the nitty gritty hardware interface is in the driver.
     


     Mark Koennecke,    November 1996
     revised: Mark Koennecke, June 1997
     callback added: Mark Koennecke, August 1997

     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.


     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 <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <tcl.h>
#include "fortify.h"
#include "conman.h"
#include "SCinter.h"
#include "devexec.h"
#include "motor.h"
#include "splitter.h"
#include "Scommon.h"
#include "status.h"
#include "servlog.h"
#include "event.h"

/*-------------------------------------------------------------------------
   some lokal defines
*/
#define ZEROINACTIVE 0
#define INTCONT 0.
#define INTSCAN 1.
#define INTBATCH 2.
#define INTHALT  3.

#define HLOW 0
#define HUPP 1
#define SLOW 2
#define SUPP 3
#define SZERO 4
#define FIX   5
#define INT   6
#define PREC  7
#define USRIGHTS 8
#define SPEED   9
#define SIGN    10
#define ECOUNT  11
/*------------------------------------------------------------------------
   a tiny structure used in CallBack work
*/
  typedef struct {
                 float fVal;
                 char *pName;
                } MotCallback; 

/*-------------------------------------------------------------------------*/
  static void *MotorGetInterface(void *pData, int iID)
  {
    pMotor self = NULL;
    
    self = (pMotor)pData;
    assert(self);
    if(iID == DRIVEID)
    {
      return self->pDrivInt;
    } 
    else if(iID == CALLBACKINTERFACE)
    {
      return self->pCall;
    }
    return NULL;
  }
/*------------------------------------------------------------------------*/
  static int MotorHalt(void *sulf)
  {
     pMotor self;
     
    assert(sulf);
    self = (pMotor)sulf;
   
    /* reduce the error count by 1. This is because the driver is
       expected to return an error when the motor had been stopped.
       However, a stop is usually a consequence of a user intervention
       or program logic. This prevents to a false motor alarm when the
       motor was repeatedly stopped for other reasons.
    */
    self->pDrivInt->iErrorCount--;
    if(self->pDrivInt->iErrorCount < 0)
      self->pDrivInt->iErrorCount = 0;

    return self->pDriver->Halt((void *)self->pDriver);
  }
  
/*--------------------------------------------------------------------------*/
  static int MotorLimits(void *sulf, float fVal, char *error, int iErrLen)
  {
     float fHard;
     pMotor self;
     
     assert(sulf);
     
     self = (pMotor)sulf;
     
     return MotorCheckBoundary(self,fVal,&fHard,error,iErrLen);
  }
/*---------------------------------------------------------------------------*/
   static float MotorGetValue(void *pData, SConnection *pCon)
   {
        int iRet;
        float fVal = 0.;
        
        assert(pData);
        iRet = MotorGetHardPosition((pMotor)pData,pCon,&fVal);
        if(iRet != OKOK)
        {
           fVal = -9999999.99;
           SCSetError(pCon,iRet);
        }
        return fVal;
   }  
/*------------------------------------------------------------------------*/
  static int MotorSaveStatus(void *pData, char *name, FILE *fd)
  {
     pMotor self = NULL;
     char pBueffel[512];
    
     assert(pData);
     assert(fd);
     
     self= (pMotor)pData;
     fprintf(fd,"# Motor %s\n",name);
     sprintf(pBueffel,"%s SoftZero %f\n",name,ObVal(self->ParArray,SZERO));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s SoftLowerLim %f\n",name,ObVal(self->ParArray,SLOW));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s SoftUpperLim %f\n",name,ObVal(self->ParArray,SUPP));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s Fixed %f\n",name,ObVal(self->ParArray,FIX));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s sign %f\n",name,ObVal(self->ParArray,SIGN));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s InterruptMode %f\n",name,ObVal(self->ParArray,INT));
     fputs(pBueffel,fd);
     sprintf(pBueffel,"%s AccessCode %f\n",name,ObVal(self->ParArray,USRIGHTS));
     fputs(pBueffel,fd);
     return 1;
  }
/*-------------------------------------------------------------------------*/
   static float absf(float f)
   {
     if(f < 0.)
     {
       return -f;
     }
     else
     {
       return f;
     }
   }  
/*-------------------------------------------------------------------------*/
  static void MotorInterrupt(SConnection *pCon, int iVal)
  {
      if(SCGetInterrupt(pCon) < iVal)
      {
        SCSetInterrupt(pCon,iVal);
      }
  }
/*--------------------------------------------------------------------------*/
  static int MotorStatus(void *sulf, SConnection *pCon)
  {
     float fHard;
     pMotor self;
     int iRet,i, iCode;
     static int iRetry = 0;
     char pError[132];
     char pBueffel[256];
     static int iPosFault = 0;
     MotCallback sCall;
     
     
     assert(sulf);
     
     self = (pMotor)sulf;

     iRet = self->pDriver->GetStatus(self->pDriver);
     fHard = MotorGetValue((void *)self,pCon);
     if(fHard < -9999990)
     {
        self->pDriver->GetError(self->pDriver,&iCode, pError,131);
        iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
        if(iRet == MOTFAIL)
        {
               sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
               SCWrite(pCon,pBueffel,eError);
               MotorInterrupt(pCon,ObVal(self->ParArray,INT));
               iRetry = 0;
               return HWFault;
        }
        else if(iRet == MOTREDO)
        {
           iRetry++;
           if(iRetry >= 3)
           {
              iRetry = 0;
              return HWFault;
           }
        }
        else
        {
             iRetry = 0;
             return HWBusy;
        }  
     }      

     /* invoke callback  */
     sCall.fVal = fHard;
     sCall.pName = self->name;
     InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
   
  /* motor thinks he is done */  
     if( (iRet == OKOK) || (iRet == HWIdle)) 
     {
          self->fPosition = fHard;
          if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
          {
            sprintf(pBueffel,"WARNING: %s off position by %f",
                    self->name, absf(fHard - self->fTarget));
            SCWrite(pCon,pBueffel, eWarning);                    
            MotorInterrupt(pCon,ObVal(self->ParArray,INT));
            iRetry = 0;
            return HWPosFault;
          }
          iRetry = 0;
          return HWIdle;  
     }
   /* motor suggests a fault */  
     else if(iRet == HWFault)
     {
       self->pDriver->GetError(self->pDriver,&iCode, pError,131);
       iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
       if(iRet == MOTFAIL)
       {
         sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
         SCWrite(pCon,pBueffel,eError);
         MotorInterrupt(pCon,ObVal(self->ParArray,INT));
         iRetry = 0;
         return HWFault;
       }
       else if(iRet == MOTREDO)
       {
         self->pDriver->RunTo(self->pDriver,self->fTarget);
         iRetry++;
         if(iRetry >= 3)
         {
            iRetry = 0;
            return HWFault;
         }
         return HWBusy;
       }
       else
       {
         iRetry = 0;
         return HWBusy;
       }  
     } 
   /* a positioning fault */  
     else if(iRet == HWPosFault)
     {
       self->pDriver->GetError(self->pDriver,&iCode, pError,131);
       iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
       if(iRet == MOTFAIL)
       {
         sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
         SCWrite(pCon,pBueffel,eError);
         MotorInterrupt(pCon,ObVal(self->ParArray,INT));
         iRetry = 0;
         return HWFault;
       }
       else if(iRet == MOTREDO)
       {
         iPosFault++;
         if(iPosFault == 2)
         {
           iPosFault = 0;
           iRetry = 0;
           return HWPosFault;
         }
         self->pDriver->RunTo(self->pDriver,self->fTarget);
         return HWBusy;
       }
       else
       {
         return HWBusy;
       }  
     }
     else if(iRet == HWWarn)
     {
        self->pDriver->GetError(self->pDriver,&iCode,pError,131);
        sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
        SCWrite(pCon,pBueffel,eStatus);
        if(self->pDriver->ContinueAfterWarn(self->pDriver))
        {
           iRetry = 0;
           return HWBusy;
        }
        else
        {
          iRetry = 0;
          return HWIdle;
        }   
     }
     iRetry = 0;
     return iRet;
  }
/*---------------------------------------------------------------------------*/  
  pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
  {
    pMotor pM =  NULL;
    
    assert(drivername);
    assert(pDriv);
    assert(name);
    
    /* get memory */
    pM = (pMotor)malloc(sizeof(Motor));
    if(!pM)
    {
      return NULL;
    }
    
    
    /* create and initialize parameters */
    pM->ParArray = ObParCreate(12);
    if(!pM->ParArray)
    {
       free(pM);
       return NULL;
    }
    ObParInit(pM->ParArray,HLOW,"hardlowerlim",pDriv->fLower,usInternal);
    ObParInit(pM->ParArray,HUPP,"hardupperlim",pDriv->fUpper,usInternal);
    ObParInit(pM->ParArray,SLOW,"softlowerlim",pDriv->fLower,usUser);
    ObParInit(pM->ParArray,SUPP,"softupperlim",pDriv->fUpper,usUser);
    ObParInit(pM->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
    ObParInit(pM->ParArray,FIX,"fixed",-1,usUser);
    ObParInit(pM->ParArray,INT,"interruptmode",INTCONT,usMugger);
    ObParInit(pM->ParArray,PREC,"precision",0.01,usMugger);
    ObParInit(pM->ParArray,USRIGHTS,"accesscode",(float)usUser,usMugger);
    ObParInit(pM->ParArray,SPEED,"speed",0.02,usInternal);
    ObParInit(pM->ParArray,SIGN,"sign",1.0,usMugger);
    ObParInit(pM->ParArray,ECOUNT,"failafter",3.0,usMugger);
    pDriv->GetPosition(pDriv,&(pM->fPosition));
    pM->fTarget = pM->fPosition;

    /* copy arguments */
    pM->pDriver =    pDriv;
    pM->drivername = strdup(drivername);
    pM->name =       strdup(name);
 
    
    /* initialise object descriptor */    
    pM->pDescriptor = CreateDescriptor("Motor");
    if(!pM->pDescriptor)
    {
      ObParDelete(pM->ParArray);
      free(pM);
      return NULL;
    }
    pM->pDescriptor->GetInterface = MotorGetInterface;
    pM->pDescriptor->SaveStatus = MotorSaveStatus;
    
    /* initialise Drivable interface */
    pM->pDrivInt = CreateDrivableInterface();
    if(!pM->pDrivInt)
    {
      DeleteDescriptor(pM->pDescriptor);
      ObParDelete(pM->ParArray);
      free(pM);
      return NULL;
    }
    pM->pDrivInt->SetValue  = MotorRun;
    pM->pDrivInt->CheckLimits = MotorLimits;
    pM->pDrivInt->CheckStatus =  MotorStatus;
    pM->pDrivInt->GetValue   =  MotorGetValue;
    pM->pDrivInt->Halt = MotorHalt;

   /* initialise callback interface */
   pM->pCall = CreateCallBackInterface();
   if(!pM->pCall)
   {
     MotorKill(pM);
     return NULL;
   }
         
    /* done */
    return pM;
  }
/*--------------------------------------------------------------------------*/

extern void KillPiPiezo(void *pData);
	
  void MotorKill(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)
     {   /* edit here to include more drivers */ 
       if(strcmp(pM->drivername,"EL734") == 0)
       {   
           KillEL734((void *)pM->pDriver);
       }
       else if(strcmp(pM->drivername,"EL734DC") == 0)
       {   
           KillEL734((void *)pM->pDriver);
       }
       else if(strcmp(pM->drivername,"SIM") == 0)
       {
         KillSIM((void *)pM->pDriver);
       }
       else if(strcmp(pM->drivername,"PIPIEZO") == 0)
       {
         KillPiPiezo((void *)pM->pDriver);
       }
       free(pM->drivername);
     }
     
     /* get rid of parameter space */
     if(pM->ParArray)
     {
       ObParDelete(pM->ParArray);
     }
     
     /* kill Descriptor */
     DeleteDescriptor(pM->pDescriptor);
     
     free(pM);
     
  } 
/*--------------------------------------------------------------------------*/
  int MotorGetPar(pMotor self, char *name, float *fVal)
  {
    ObPar *pPar = NULL;
    assert(self);
    
    pPar = ObParFind(self->ParArray,name);
    if(pPar)
    {
      *fVal = pPar->fVal;
      return 1;
    }
    else
    {
       /* can still be position */
       if(strcmp(name,"position") == 0)
       {
         *fVal = self->fPosition;
         return 1;
       }
       else
       {
         return 0;
       }
    } 
  }
/*---------------------------------------------------------------------------*/
  int MotorSetPar(pMotor self, SConnection *pCon, char *name, float fVal)
  {
     ObPar *pPar = NULL;
     char pBueffel[512];
     int iRet;
     float fLimit, fOld, fChange;
     
     assert(self);
     assert(pCon);

     if(strcmp(name,"softzero") == 0)
     {
       /* set it first, this also tests the necessary privileges */
        fOld = ObVal(self->ParArray,SZERO); 
        iRet = ObParSet(self->ParArray,self->name,name,fVal,pCon);
        if(!iRet)
        {
          return iRet;
        }
        /* shift the limits by the difference between old and new */
        fChange = fVal - fOld;
        /* upper limit */
        fLimit = ObVal(self->ParArray,SUPP);
        fLimit -= fChange;
        ObParSet(self->ParArray,self->name,"softupperlim",fLimit,pCon);
        /* lower limit */
        fLimit = ObVal(self->ParArray,SLOW);
        fLimit -= fChange;
        ObParSet(self->ParArray,self->name,"softlowerlim",fLimit,pCon);

        return 1;
     }
     
     iRet = ObParSet(self->ParArray,self->name,name,fVal,pCon);
     if(!iRet)
     {
       return iRet;
     }
     if(strcmp(name,"sign") == 0)
     {
        if((absf(fVal)-1.0) > 0.01)
        {
          SCWrite(pCon,"ERROR: Invalid Sign Value",eError);
          return 0;
        }        
        if(fVal < 0.0)
	{
          ObParInit(self->ParArray,SLOW,"softlowerlim",
                    self->pDriver->fUpper*fVal,usUser);
          ObParInit(self->ParArray,SUPP,"softupperlim",
                    self->pDriver->fLower*fVal,usUser);
          ObParInit(self->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
        }
     }
     return iRet;
  } 
/*---------------------------------------------------------------------------
  MotorCheckBoundary checks for violation of boundary conditions and 
  transforms from SoftCoordinates to hard coordinates.
 */
   
  int MotorCheckBoundary(pMotor self, float fVal, float *fNew,
                         char *pError, int iErrLen)
  {
    float fHard;
    float fZero;
    char pBueffel[512];

    assert(self);
    
    /* check for fixed */
    if(ObVal(self->ParArray,FIX) >= 0)
    {
      sprintf(pBueffel,"Motor %s is Fixed",self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  /* is this an error? */ 
    }
    
    /* check against software boundaries */
    if(fVal > ObVal(self->ParArray,SUPP))
    {
      sprintf(pBueffel,"%f violates upper software limit %f on %s",
              fVal, ObVal(self->ParArray,SUPP),self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    if(fVal < ObVal(self->ParArray,SLOW))
    {
      sprintf(pBueffel,"%f violates lower software limit %f  on %s",
              fVal,ObVal(self->ParArray,SLOW),self->name );
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    
    /* correct for zero point */
    fZero = ObVal(self->ParArray,SZERO);
    fZero = -fZero;
    fHard = fVal - fZero;

    /* apply sign */
    fHard = fHard*ObVal(self->ParArray,SIGN); 
    
    /* check for hardware limits */
    if(fHard > ObVal(self->ParArray,HUPP))
    {
      sprintf(pBueffel,"%f violates upper hardware  limit %f on %s",
              fVal,ObVal(self->ParArray,HUPP),self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    if(fHard < ObVal(self->ParArray,HLOW))
    {
      sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
              fVal,ObVal(self->ParArray,HLOW),self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    
    *fNew = fHard;
    return 1;
  }
/*---------------------------------------------------------------------------*/
  long MotorRun(void *sulf, SConnection *pCon, float fNew)
  {
    float fHard;
    int i, iRet, iCode;
    char pBueffel[512];
    char pError[132];
    pMotor self;
    long lTime;
    float fDelta;
    
    self = (pMotor)(sulf);
    assert(self);
    assert(pCon);
    
    /* check if I'am allowed to move this motor */
    if(!SCMatchRights(pCon,(int)ObVal(self->ParArray,USRIGHTS)))
    {
       sprintf(pBueffel,"ERROR: You are not authorised to move motor %s",
               self->name);
       SCWrite(pCon,pBueffel,eError);
       SCSetInterrupt(pCon,eAbortBatch);
       return 0;
    }
    
    /* check boundaries first */
    iRet = MotorCheckBoundary(self,fNew,&fHard,pBueffel,511);
    if(!iRet)
    {
      SCWrite(pCon,pBueffel,eStatus);
      SCSetInterrupt(pCon,eAbortOperation);
      return 0;  
    }
    
    /* check our error count and interrupt if to much */
    iCode = (int)ObVal(self->ParArray,ECOUNT);
    if(self->pDrivInt->iErrorCount > iCode)
    {
      /* big alarm */
      ServerWriteGlobal("ERROR: !!! MOTOR ALARM !!! MOTOR ALARM !!!",eError);
      sprintf(pBueffel,
        "ERROR: too many position errors counted at motor %s",
        self->name);
      ServerWriteGlobal(pBueffel,eError);
      SCSetInterrupt(pCon,eAbortBatch);
      self->pDrivInt->iErrorCount = 0;
      return 0;
    }

    /* Boundaries OK, send command */
    self->fTarget = fHard;
    iRet = self->pDriver->RunTo(self->pDriver,fHard);
    if(iRet != OKOK)
    {  /* try three times to fix it */
       for(i = 0; (i < 3) && (iRet != OKOK); i++)
       {
         self->pDriver->GetError(self->pDriver,&iCode, pError,131);
         sprintf(pBueffel,"WARNING: Trying to fix: %s",pError);
         SCWrite(pCon,pBueffel,eWarning);
         iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fHard);
         switch(iRet)
         {
           case MOTFAIL:
                       SCWrite(pCon,pError,eError);
                       SCWrite(pCon,"\n",eError);
                       SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
                       return HWFault;
           case MOTREDO:
                       iRet = self->pDriver->RunTo(self->pDriver,fHard);
                       if(iRet == OKOK)
                       {
                         return OKOK;
                       }
                       break;
           case MOTOK:
                      return OKOK;
                      break;         
         }  
       }
       /* tried three times, refuses to work */
       SCWrite(pCon,pError,eError);
       SCWrite(pCon,"\n",eError);
       SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
       return HWFault;
    }
    return OKOK;
  }
/*------------------------------------------------------------------------*/
  int MotorGetHardPosition(pMotor self,SConnection *pCon, float *fHard)
  {
     int iRet, iCode;
     float fVal;
     char pBueffel[512],pError[256];
     
     assert(self);
     assert(pCon);

    iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
    if(iRet == OKOK) /* all went well, the exception */
    {
      *fHard = fVal;
      return 1;
    }
    else /* a problem, the usual case: try fix the problem */
    {  /* no point in trying this three times */
       self->pDriver->GetError(self->pDriver,&iCode, pError,255);
       iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fVal);
       sprintf(pBueffel,"WARNING: Trying to fix %s",pError);
       SCWrite(pCon,pBueffel,eWarning);
       switch(iRet)
       {
           case MOTFAIL:
                       sprintf(pBueffel,"ERROR: cannot fix motor %s",
                          self->name);
                       SCWrite(pCon,pBueffel,eError);   
                       SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
                       *fHard = fVal;
                       return 0;
           case MOTOK:
           case MOTREDO:
                       iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
                       if(iRet)
                       {
                          *fHard = fVal*ObVal(self->ParArray,SIGN);
                          return 1;
                       }   
                       else
                       {
                         sprintf(pBueffel,"ERROR: cannot fix motor %s",
                                 self->name);
                         SCSetInterrupt(pCon,(int)ObVal(self->ParArray,INT));
                         SCWrite(pCon,pBueffel,eError);   
                         *fHard = fVal;
                         return 0;
                       }
        }  
    }
    *fHard = fVal*ObVal(self->ParArray,SIGN);
    return 0;
  }  
/* ------------------------------------------------------------------------*/
  int MotorGetSoftPosition(pMotor self, SConnection *pCon, float *fVal)
  {
    int iRet;
    float fValue;
    
    assert(self);
    assert(pCon);
    
    /* get the hard position */
    iRet = MotorGetHardPosition(self,pCon,&fValue);
    if(!iRet)
    {
      *fVal = fValue;
      return 0;
    }
    
    /* apply zeropoint */
    fValue -= ObVal(self->ParArray,SZERO);
    *fVal = fValue;

    /* apply sign */
    *fVal = fValue*ObVal(self->ParArray,SIGN);
    return 1;
  }  
/*---------------------------------------------------------------------------*/
  int MotorCheckPosition(void *sulf, SConnection *pCon)
  {
    float fHard;
    int i, iRet, iCode;
    char pBueffel[512];
    pMotor self;
    
    self = (pMotor)sulf;
    assert(self);
    assert(pCon);
 
    /* try to find current position */
    iRet = MotorGetHardPosition(self,pCon,&fHard);
    if(iRet)
    {
      if(absf(fHard - self->fTarget) < ObVal(self->ParArray,PREC))
      {
         self->fPosition = fHard;
         return 1;
      }
      else 
      {
        /* Oooopppsss error */
           return 0;
      }
    }
    else /* error getting hold of position, MotorGetHard already tried to
            solve the problem and FAILED, the fucking bastard 
            even messaged the connection accordingly */
    {
       return -1;
    }
  }
/* --------------------------------------------------------------------------
   The Factory function for creating a motor. Usage:
      MotorCreate name DriverName.
      
      Drivernames currently understood are:
               EL734   SINQ EL734 stepper motor
               EL734DC SINQ EL734 DC motor
               PiPiezo Physik-Instrumente E-255 Piezo Controller
               SIM     simulated motor
*/
   
extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);

   int MotorCreate(SConnection *pCon, SicsInterp *pSics, void *pData,
                        int argc, char *argv[])
   {
      pMotor pNew = NULL;
      MotorDriver *pDriver = NULL;
      char pBueffel[512];
      int iD, iRet;
      Tcl_Interp *pTcl = (Tcl_Interp *)pSics->pTcl;
      
      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],"el734") == 0)
      {
         iD = argc - 3;
         pDriver = CreateEL734(pCon,iD,&argv[3]);
         if(!pDriver)
         {
          return 0;
         }
         /* create the motor */
         pNew  = MotorInit("EL734",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],"el734dc") == 0)
      {
         iD = argc - 3;
         pDriver = CreateEL734DC(pCon,iD,&argv[3]);
         if(!pDriver)
         {
          return 0;
         }
         /* create the motor */
         pNew  = MotorInit("EL734DC",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 if (strcmp(argv[2],"pipiezo") == 0)
      {
         pDriver = MakePiPiezo(pSics->pTcl,argv[3]);
         if(!pDriver)
         {
             
              SCWrite(pCon,pTcl->result,eError);
              return 0;
         }
         /* create the motor */
         pNew  = MotorInit("PIPIEZO",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],MotorAction,MotorKill,pNew);
      if(!iRet)
      {
        sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[1]);
        SCWrite(pCon,pBueffel,eError);
        return 0;
      }
      return 1;
   }
/* ----------------- some private functions used in MotorAction -------------*/
  static void MotorListLL(pMotor self, SConnection *pCon)
  {
    char pBueffel[512];
    int i, iLen;
    
    iLen = ObParLength(self->ParArray);
    sprintf(pBueffel,"Parameter Listing for motor %s\n",self->name);
    SCWrite(pCon,pBueffel,eStatus);
    for(i = 0; i < iLen; i++)
    {
      sprintf(pBueffel,"%s.%s = %f\n",self->name,
              self->ParArray[i].name,self->ParArray[i].fVal);
      SCWrite(pCon,pBueffel,eStatus);
    }
    sprintf(pBueffel,"%s.Position = %f\n", self->name,self->fPosition);
    SCWrite(pCon,pBueffel,eStatus);
    sprintf(pBueffel,"%s.TargetPosition = %f\n", self->name,self->fTarget);
    SCWrite(pCon,pBueffel,eStatus);
    
  }
/*--------------------------------------------------------------------------*/
  static void MotorReset(pMotor pM)
  {
    ObParInit(pM->ParArray,SLOW,"softlowerlim",pM->pDriver->fLower,usUser);
    ObParInit(pM->ParArray,SUPP,"softupperlim",pM->pDriver->fUpper,usUser);
    ObParInit(pM->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
    ObParInit(pM->ParArray,FIX,"fixed",-1,usUser);
  }  
/*------------------------------------------------------------------------*/
  typedef struct {
                  char *pName;
                  SConnection *pCon;
                 } MotInfo, *pMotInfo;   
/*-----------------------------------------------------------------------*/
  static void KillInfo(void *pData)
  {
    pMotInfo self = NULL;
    
    assert(pData);
    self = (pMotInfo)pData;
    if(self->pName)
    {
      free(self->pName);
    }
    free(self);
  }                 
/*------------------- The CallBack function for interest ------------------*/
  static int InterestCallback(int iEvent, void *pEvent, void *pUser)
  {
     pMotInfo pInfo = NULL;
     char pBueffel[80];
     MotCallback *psCall;
     
     assert(pEvent);
     assert(pUser);
  
     psCall = (MotCallback *)pEvent;
     pInfo = (MotInfo *)pUser;
          
     sprintf(pBueffel,"%s.position = %f ", pInfo->pName, psCall->fVal);
     SCWrite(pInfo->pCon,pBueffel,eValue);
     return 1;
  }  
     
/*----------------------------------------------------------------------------
 The wrapper function for a motor. Commands currently supported are:
 
    motorname parametername newval         : sets a parameter
    motorname parametername                : gets the value for a par
    motorname list                         : lists all parameters
    motorname reset                        : puts softlimits to default
    motorname interest                     : starts sending automatic 
                                             notifications when driving  
-----------------------------------------------------------------------------*/
  int MotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                  int argc, char *argv[])
  {
    char pBueffel[512];
    TokenList *pList = NULL;
    TokenList *pCurrent;
    TokenList *pName; 
    int iRet;
    pMotor self;
    float fValue;
    long lID;
    pMotInfo pMoti = NULL;
    
    assert(pCon);
    assert(pSics);
    assert(pData);
    
    self = (pMotor)pData;
    
    /* create Tokenlist */
    argtolower(argc,argv);
    pList = SplitArguments(argc,argv);
    if(!pList)
    {
      SCWrite(pCon,"Error parsing argument list in MotorAction",eError);
      return 0;
    }
  
     /* first argument can be one of list, reset or parameter name */   
     pCurrent = pList->pNext;
     if(!pCurrent) /* no argument, print value */
     {
        iRet = MotorGetSoftPosition(self,pCon,&fValue);
        if(!iRet)
        {
              sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
              SCWrite(pCon,pBueffel,eValue);
              DeleteTokenList(pList);
              return 0;
         }
         sprintf(pBueffel,"%s = %f",argv[0],fValue);
         SCWrite(pCon,pBueffel,eValue);
         DeleteTokenList(pList);
         return 1; 
     }
     
     /* check for list */  
     if(strcmp(pCurrent->text,"list") == 0)
     {
        MotorListLL(self,pCon);
        DeleteTokenList(pList);
        return 1;
     } /* check for reset */
     else if(strcmp(pCurrent->text,"reset") == 0)
     {
       if(!SCMatchRights(pCon,usUser))
       {
         sprintf(pBueffel,"Insufficient privilege to reset %s",
                 argv[0]);
         SCWrite(pCon,pBueffel,eError);
         DeleteTokenList(pList);
         return 0;         
       }
       MotorReset(self);
       DeleteTokenList(pList);
       SCSendOK(pCon);
       return 1;
     }
     else if(strcmp(pCurrent->text,"interest") == 0) /* interest */
     {
        pMoti = (pMotInfo)malloc(sizeof(MotInfo));
        if(!pMoti)
        {
          SCWrite(pCon,"ERROR: out of memory in motor.c",eError);
          return 0;
        }
        pMoti->pName = strdup(argv[0]);
        pMoti->pCon = pCon;
        lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
                               pMoti, KillInfo);
        SCRegister(pCon,pSics, self->pCall,lID);
        DeleteTokenList(pList);
        SCSendOK(pCon);
        return 1;                       
     
     }
     else if(strcmp(pCurrent->text,"uninterest") == 0)
     {
       RemoveCallback2(self->pCall,pCon);
       SCSendOK(pCon);
       DeleteTokenList(pList);
       return 1;
     }
     else /* one of the parameter commands or error left now */
     {
        pName = pCurrent;
        pCurrent = pCurrent->pNext;
        if(!pCurrent) /* no third par: print val */
        {
          /* deal with position first */
          if(strcmp(pName->text,"position") == 0)
          {
            iRet = MotorGetSoftPosition(self,pCon,&fValue);
            if(!iRet)
            {
              sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
              SCWrite(pCon,pBueffel,eValue);
              DeleteTokenList(pList);
              return 0;
            }
            sprintf(pBueffel,"%s.SoftPosition = %f",argv[0],fValue);
            SCWrite(pCon,pBueffel,eValue);
            DeleteTokenList(pList);
            return 1;
          }
          else if(strcmp(pName->text,"hardposition") == 0)
          {
            iRet = MotorGetHardPosition(self,pCon,&fValue);
            if(!iRet)            {
              sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
              SCWrite(pCon,pBueffel,eValue);
              DeleteTokenList(pList);
              return 0;
            }
            sprintf(pBueffel,"%s.HardPosition = %f",argv[0],fValue);
            SCWrite(pCon,pBueffel,eValue);
            DeleteTokenList(pList);
            return 1;
          }
          iRet = MotorGetPar(self,pName->text,&fValue);
          if(!iRet)
          { 
            sprintf(pBueffel,"Parameter %s not found ",pName->text);
            SCWrite(pCon,pBueffel,eValue);
            DeleteTokenList(pList);
            return 0;
          }
          else
          {
            sprintf(pBueffel, " %s.%s = %f",self->name,pName->text,fValue);
            SCWrite(pCon,pBueffel,eValue);
            DeleteTokenList(pList);
            return 1;
          }
        }
        else /* set */
        { /* set command */
          /* parameter must be numerical value */
          if(pCurrent->Type == eInt)
          {
            fValue = (float)pCurrent->iVal;
          } 
          else if(pCurrent->Type == eFloat)
          {
            fValue = pCurrent->fVal;
          }
          else
          {
            sprintf(pBueffel,"Wrong argument type for %s %s set",
                    argv[0],pName->text);
            SCWrite(pCon,pBueffel,eError); 
            DeleteTokenList(pList);
            return 0;
          }
          iRet = MotorSetPar(self,pCon,pName->text,fValue);
          DeleteTokenList(pList);
          if(iRet)
            SCSendOK(pCon);
          return iRet;
        }
     }
     return 0;
  }

