/*------------------------------------------------------------------------
                          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.
     
     This is a totally revamped version which uses the Hipadaba for 
     data storage.
     
     copyright: see file COPYRIGHT
     
     Mark Koennecke, July 2006 
-----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <tcl.h>
#include "sics.h"
#include "devexec.h"
#include "motor.h"
#include "splitter.h"
#include "status.h"
#include "servlog.h"
#include "tclmotdriv.h"
#include "site.h"
#include "sicshipadaba.h"
#include "stptok.h"
/*-------------------------------------------------------------------------
   some lokal defines
*/
#define ZEROINACTIVE 0
#define INTCONT 0.
#define INTSCAN 1.
#define INTBATCH 2.
#define INTHALT  3.
#define ABS(x) (x < 0 ? -(x) : (x)) 
/*------------------------------------------------------------------------
   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;

    self->stopped = 1;
    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;
        }
        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);
     strncpy(pBueffel,name,511);
     strncat(pBueffel," ",511);
     SaveSICSHipadaba(fd,self->pDescriptor->parNode,pBueffel);
     return 1;
}
/*-------------------------------------------------------------------------*/
static void MotorInterrupt(SConnection *pCon, int iVal){
      if(SCGetInterrupt(pCon) < iVal){
        SCSetInterrupt(pCon,iVal);
      }
}
/*---------------------------------------------------------------------*/
static int statusRunTo(pMotor self, SConnection *pCon)
{
  char pBueffel[256];
  float posCount;
  
  assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,"maxretry",
    &posCount) == 1);

  if(self->retryCount >= (int)posCount)
  {
    snprintf(pBueffel,255,"ERROR: aborting motor %s after %d retries",
	     self->name, self->retryCount);
    SCWrite(pCon,pBueffel,eError);
    return HWFault;
  }
  if(SCGetInterrupt(pCon) != eContinue){
  	return HWFault;
  }
  self->retryCount++;
  snprintf(pBueffel,255,"WARNING: restarting %s, %d time",
	   self->name,self->retryCount);
  SCWrite(pCon,pBueffel,eWarning);
  self->pDriver->RunTo(self->pDriver,self->fTarget);
  return HWBusy;
}
/*--------------------------------------------------------------------*/
static int checkPosition(pMotor self, SConnection *pCon)
{
  float fHard, precision;
  char pBueffel[132];
  int status;

  assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
    "precision",&precision) == 1);

  MotorGetHardPosition(self,pCon,&fHard);
  self->fPosition = fHard;
  if(ABS(fHard - self->fTarget) > precision){
     if(SCGetInterrupt(pCon) != eContinue){
       return HWFault;
     }
     if(self->stopped){
       snprintf(pBueffel,131,"WARNING: %s stopped", self->name);
       SCWrite(pCon,pBueffel, eWarning);                    
       return HWFault;
     }
     snprintf(pBueffel,131,"WARNING: %s off position by %f",
                 self->name, ABS(fHard - self->fTarget));
     SCWrite(pCon,pBueffel, eWarning);                    
     status = statusRunTo(self,pCon);
     return status;
  }
  return HWIdle; 
}
/*--------------------------------------------------------------------*/
static void finishDriving(pMotor self, SConnection *pCon){
  MotCallback sCall;
  MotorGetSoftPosition(self,pCon,&sCall.fVal);
  sCall.pName = self->name;
  InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
  InvokeCallBack(self->pCall, MOTEND, &sCall);
  UpdateHipadabaPar(self->pDescriptor->parNode, 
    MakeHdbFloat((double)sCall.fVal),pCon);
}
/*--------------------------------------------------------------------*/
static int reportAndFixError(pMotor self, SConnection *pCon){
  char pBueffel[256], pError[131];
  int iCode, iRet, newStatus;

  self->pDriver->GetError(self->pDriver,&iCode, pError,131);
  iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
  switch(iRet){
  case MOTFAIL:
    snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
    SCWrite(pCon,pBueffel,eError);
    newStatus = HWFault;
    break;
  case MOTREDO:
    snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
    SCWrite(pCon,pBueffel,eWarning);
    newStatus = statusRunTo(self,pCon);
    break;
  case MOTOK:
    snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
    SCWrite(pCon,pBueffel,eWarning);
    newStatus = HWIdle;
    break;
  default:
    SCWrite(pCon,"WARNING: bad status code in motor.c:reportAndFixError",
	    eWarning);
    SCWrite(pCon,"You may continue, but show this to a SICS programmer",
	    eWarning);
    newStatus = HWIdle;
    break;
  }
  return newStatus;
}
/*---------------------------------------------------------------------*/
static int evaluateStatus(pMotor self, SConnection *pCon){
  int iRet, iCode, newStatus;
  MotCallback sCall;
  char pBueffel[256], pError[132];
  float fHard, ignorefault, interrupt;

  assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
    "ignorefault",&ignorefault) == 1);
  assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
    "interruptmode",&interrupt) == 1);

  iRet = self->pDriver->GetStatus(self->pDriver);
  newStatus = iRet;
  switch(iRet){
  case OKOK:
  case HWIdle:
    newStatus = checkPosition(self,pCon);
    if(newStatus != HWBusy){
      finishDriving(self,pCon);
    }
    break;
  case HWFault:
    newStatus = reportAndFixError(self,pCon);
    break;
  case HWPosFault:
    newStatus = reportAndFixError(self,pCon);
    if(newStatus == HWFault && ignorefault < 1){
      newStatus = HWPosFault;
    } 
    if(newStatus == HWIdle || newStatus == OKOK){
      newStatus = checkPosition(self,pCon);
      if(newStatus != HWBusy){
    	finishDriving(self,pCon);
      }
    }
    break;
  case HWBusy:
    newStatus = HWBusy;
    break;
  case HWWarn:
    self->pDriver->GetError(self->pDriver,&iCode, pError,131);
    snprintf(pBueffel,255,"WARNING: motor reported: %s", pError);
    SCWrite(pCon,pBueffel,eWarning);
    newStatus = HWIdle;
    break;
  default:
    SCWrite(pCon,"WARNING: Bad status in motor.c:evaluatStatus",eWarning);
    SCWrite(pCon,"You may continue, but show this to a SICS programmer",
	    eWarning);
    break;
  }
  if(newStatus == HWFault){
    MotorInterrupt(pCon,(int)interrupt);
    self->retryCount = 0;
  }
  return newStatus;
}
/*---------------------------------------------------------------------*/
static void handleMoveCallback(pMotor self, SConnection *pCon){
    MotCallback sCall;
    float movecount;

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
     "movecount",&movecount) == 1);
  
    self->posCount++;
    if(self->posCount >= (int)movecount){
      MotorGetSoftPosition(self,pCon,&sCall.fVal);
      sCall.pName = self->name;
      InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
      self->posCount = 0;
      UpdateHipadabaPar(self->pDescriptor->parNode, 
        MakeHdbFloat((double)sCall.fVal),pCon);
    }
}
/*-----------------------------------------------------------------------*/
static int MotorStatus(void *sulf, SConnection *pCon){
     pMotor self = NULL;
     int status;

     assert(sulf);
     self = (pMotor)sulf;

     status = evaluateStatus(self,pCon);
     if (self->pDrivInt->drivableStatus!=status) {
       ((SConnection *)pCon)->conEventType=STATUS;
       ((SConnection *)pCon)->conStatus=status;
       SCWrite(pCon, "", eEvent);
       self->pDrivInt->drivableStatus=status;
     }
     if(status == HWBusy){
       handleMoveCallback(self,pCon);
     }
     return status;
}
/*---------------------------------------------------------------------------*/
static int MotorZeroCallback(void *userData, void *callData, pHdb node,
    hdbValue v){
    pMotor self = NULL;
    float limit, oldZero, diff;
    SConnection *pCon = NULL;
    
    self = (pMotor)userData;
    assert(self != NULL);
    pCon = (SConnection *)callData;
    
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
     "softzero",&oldZero) == 1);
    
    diff  = v.v.doubleValue - oldZero;
    
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
     "softupperlim",&limit) == 1);
    limit -= diff;
    SICSHdbSetFloat(self->pDescriptor->parNode,(SConnection *)callData,
        "softupperlim",limit);

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
     "softlowerlim",&limit) == 1);
    limit -= diff;
    SICSHdbSetFloat(self->pDescriptor->parNode,(SConnection *)callData,
        "softlowerlim",limit);

    UpdateHipadabaPar(node,v,callData);
    return 1;
}
/*--------------------------------------------------------------------------*/
static int MotorSignCallback(void *userData, void *callData, pHdb node,
    hdbValue v){
    pMotor self = NULL;
    SConnection *pCon = NULL;
    pHdb zero = NULL;
    
    self = (pMotor)userData;
    pCon = (SConnection *)callData;
    
    if(ABS(v.v.doubleValue - 1.) > .01){
        if(pCon!= NULL){
            SCWrite(pCon,"ERROR: invalid sign value",eError);
            return 0;
        }
    }
    SICSHdbSetFloat(self->pDescriptor->parNode,pCon,
        "softlowerlim",self->pDriver->fLower*v.v.doubleValue);
    SICSHdbSetFloat(self->pDescriptor->parNode,pCon,
        "softupperlim",self->pDriver->fUpper*v.v.doubleValue);
    zero = GetHipadabaNode(self->pDescriptor->parNode,"softzero");
    if(zero != NULL){
        UpdateHipadabaPar(zero,MakeHdbFloat(.0),pCon);
    }
    UpdateHipadabaPar(node,v,callData);
    return 1;
}
/*--------------------------------------------------------------------------*/
static int MotorDriverReadCallback(void *userData, void *callData, pHdb node,
    hdbValue v){
    pMotor self = NULL;
    SConnection *pCon = NULL;
    int status;
    float value;
    
    self = (pMotor)userData;
    assert(self);
    pCon = (SConnection *)callData;
    
    if(self->pDriver->GetDriverPar != NULL){
        status = self->pDriver->GetDriverPar(self->pDriver,node->name,
          &value);
        v.dataType = HIPFLOAT;
        v.v.doubleValue = (double)value;
        node->value.v.doubleValue = (double)value;
        return status;
    }  
    return 1;
}
/*--------------------------------------------------------------------------*/
static int MotorDriverSetCallback(void *userData, void *callData, pHdb node,
    hdbValue v){
    pMotor self = NULL;
    SConnection *pCon = NULL;
    int status;
    float value;
    
    self = (pMotor)userData;
    assert(self);
    pCon = (SConnection *)callData;
    
    if(pCon != NULL && self->pDriver->SetDriverPar != NULL){
        status = self->pDriver->SetDriverPar(self->pDriver,pCon, node->name,
          (float)v.v.doubleValue);
        if(status == 1){
            UpdateHipadabaPar(node,v,callData);
        }
        return status;
    }  
    return 1;
}
/*--------------------------------------------------------------------------*/
static pHdb MakeMotorDriverNode(pMotor pM, char *name){
    pHdb node = NULL;
    
    node = MakeHipadabaNode(name,HIPFLOAT,1);
    if(node == NULL){
        return NULL;
    }
    AppendHipadabaCallback(node,HCBSET, 
        MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
    AppendHipadabaCallback(node,HCBREAD, 
        MakeHipadabaCallback(MotorDriverReadCallback,pM,NULL,-1,-1));
    return node;
}
/*--------------------------------------------------------------------------*/
static int MotorHardCallback(void *userData, void *callData, pHdb node,
    hdbValue v){
    int status;
    pMotor self = NULL;
    float value;
    SConnection *pCon = NULL;
    
    self = (pMotor)userData;
    pCon = (SConnection *)callData;
    assert(self != NULL);
    assert(pCon != NULL);
    
    status = MotorGetHardPosition(self,pCon,&value);
    node->value.v.doubleValue = (double)value;
    v.v.doubleValue = (double)value;
    return status;
}
/*--------------------------------------------------------------------------*/
static char *getDriverParList(MotorDriver *pDriv){
    SConnection *pCon = NULL;
    pDynString list = NULL;
    char *listData = NULL;
    
    if(pDriv->ListDriverPar != NULL){
        pCon = SCCreateDummyConnection(pServ->pSics);
        if(pCon == NULL){
            return NULL;
        }
        SCStartBuffering(pCon);
        pDriv->ListDriverPar(pDriv,"test.", pCon);
        list = SCEndBuffering(pCon);
        if(list != NULL){
            listData = strdup(GetCharArray(list));
            SCDeleteConnection(pCon);
        } else {
            listData = NULL;
        }
        return listData;
    }
    return NULL;
}
/*--------------------------------------------------------------------------*/
extern char *trim(char *str);
/*--------------------------------------------------------------------------*/
static char *extractName(char *line){
    char *name = NULL, *pEnd = NULL;
    
    name = strchr(line,'.');
    assert(name != NULL);
    while(*name == '.'){
        name++;
    }
    pEnd = strchr(name,'=');
    assert(pEnd != NULL);
    *pEnd = '\0';
    return trim(name); 
}
/*---------------------------------------------------------------------------
 * This currently uses the ListDriverPar function of the driver. In 
 * a later stage this should be modified to test for and use a new 
 * driver function, CreateDriverNodes. But this scheme allows to use
 * existing drivers without a change, at the expense of some complicated
 * code.
 * --------------------------------------------------------------------------*/ 
static int CreateDriverParameters(pMotor pM, pHdb parent){
    char *listPtr = NULL, line[80], *pPtr, *name;
    pHdb node = NULL;
    
    listPtr = getDriverParList(pM->pDriver);
    if(listPtr == NULL){
        /*
         * no driver parameters
         */
        return 1;
    }
    pPtr = listPtr;
    while((pPtr = stptok(pPtr,line,79,"\n")) != NULL){
        name = extractName(line);
        node = MakeMotorDriverNode(pM, name);
        if(node != NULL){
            AddHipadabaChild(parent,node);
        }
    }
    free(listPtr);
    return 1;
}
/*---------------------------------------------------------------------------*/
pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv){
    pMotor pM =  NULL;
    pHdb node = NULL, child = NULL;
    
    assert(drivername);
    assert(pDriv);
    assert(name);
    
    /* get memory */
    pM = (pMotor)malloc(sizeof(Motor));
    if(!pM){
      return NULL;
    }

    /* initialise object descriptor */    
    pM->pDescriptor = CreateDescriptor("Motor");
    if(!pM->pDescriptor){
      free(pM);
      return NULL;
    }
    pM->pDescriptor->GetInterface = MotorGetInterface;
    pM->pDescriptor->SaveStatus = MotorSaveStatus;
    
    /* copy arguments */
    pM->pDriver =    pDriv;
    pM->drivername = strdup(drivername);
    pM->name =       strdup(name);
    
    
    /* create and initialize parameters */
    node = MakeSICSHdbDriv(name,usUser,pM,HIPFLOAT);
    if(node == NULL){
        free(pM);
        return NULL;
    }    
    pM->pDescriptor->parNode = node;

    child = MakeSICSROPar("targetposition",MakeHdbFloat(pM->fPosition));
    AppendHipadabaCallback(child,HCBREAD, 
        MakeMemReadCallback(&pM->fTarget));
    AddHipadabaChild(node,child);
    child = MakeSICSROPar("hardposition",MakeHdbFloat(pM->fPosition));
    AppendHipadabaCallback(child,HCBREAD, 
        MakeHipadabaCallback(MotorHardCallback,pM,NULL,-1,-1));
    AddHipadabaChild(node,child);
    child = MakeHipadabaNode("sign",HIPFLOAT, 1);
    AppendHipadabaCallback(child,HCBSET, MakeCheckPermissionCallback(usUser));
    AppendHipadabaCallback(child,HCBSET, 
        MakeHipadabaCallback(MotorSignCallback,pM,NULL,-1,-1));
    SetHipadabaPar(child,MakeHdbFloat(1.),NULL);
    AddHipadabaChild(node,child);
    child = MakeHipadabaNode("hardlowerlim",HIPFLOAT, 1);
    AppendHipadabaCallback(child,HCBREAD, 
        MakeMemReadCallback(&pM->pDriver->fLower));
    AppendHipadabaCallback(child,HCBSET, 
        MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
    AddHipadabaChild(node,child);
    child = MakeHipadabaNode("hardupperlim",HIPFLOAT, 1);
    AppendHipadabaCallback(child,HCBREAD, 
        MakeMemReadCallback(&pM->pDriver->fUpper));
    AppendHipadabaCallback(child,HCBSET, 
        MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
    AddHipadabaChild(node,child);
    AddHipadabaChild(node,MakeSICSHdbPar("softlowerlim",usUser,
        MakeHdbFloat(pDriv->fLower)));
    AddHipadabaChild(node,MakeSICSHdbPar("softupperlim",usUser,
        MakeHdbFloat(pDriv->fUpper)));
    child = MakeHipadabaNode("softzero",HIPFLOAT, 1);
    AppendHipadabaCallback(child,HCBSET, MakeCheckPermissionCallback(usUser));
    AppendHipadabaCallback(child,HCBSET, 
        MakeHipadabaCallback(MotorZeroCallback,pM,NULL,-1,-1));
    AddHipadabaChild(node,child);
    AddHipadabaChild(node,MakeSICSHdbPar("fixed",usUser,
        MakeHdbFloat(-1.)));
    AddHipadabaChild(node,MakeSICSHdbPar("interruptmode",usMugger,
        MakeHdbFloat(.0)));
    AddHipadabaChild(node,MakeSICSHdbPar("precision",usMugger,
        MakeHdbFloat(.1)));
    AddHipadabaChild(node,MakeSICSHdbPar("accesscode",usMugger,
        MakeHdbFloat((double)usUser)));
    AddHipadabaChild(node,MakeSICSHdbPar("failafter",usMugger,
        MakeHdbFloat((double)3.0)));
    AddHipadabaChild(node,MakeSICSHdbPar("maxretry",usMugger,
        MakeHdbFloat((double)3.0)));
    AddHipadabaChild(node,MakeSICSHdbPar("ignorefault",usMugger,
        MakeHdbFloat((double).0)));
    AddHipadabaChild(node,MakeSICSHdbPar("movecount",usMugger,
        MakeHdbFloat((double)10.0)));
    pDriv->GetPosition(pDriv,&(pM->fPosition));
    child = MakeSICSROPar("position",MakeHdbFloat(pM->fPosition));
    AppendHipadabaCallback(child,HCBREAD, 
        MakeMemReadCallback(&pM->fPosition));
    AddHipadabaChild(node,child);
    

    pM->fTarget = pM->fPosition;
    pM->endScriptID = 0;

    CreateDriverParameters(pM,pM->pDescriptor->parNode);
    
    /* initialise Drivable interface */
    pM->pDrivInt = CreateDrivableInterface();
    if(!pM->pDrivInt){
      DeleteDescriptor(pM->pDescriptor);
      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;
     
    /* MotorHalt(pM); */
     
     if(pM->name)
       free(pM->name);
     
     if(pM->pDrivInt){
       free(pM->pDrivInt);
     }
     
     if(pM->pCall){
       DeleteCallBackInterface(pM->pCall);
     }
     
     /* kill driver */
     if(pM->drivername){  
       if(pM->pDriver->KillPrivate != NULL){
    	 pM->pDriver->KillPrivate(pM->pDriver);
       }
	   if(pM->pDriver->name != NULL){
	       free(pM->pDriver->name);
       }
	   free(pM->pDriver);
       free(pM->drivername);
     }
     
     /* kill Descriptor */
     DeleteDescriptor(pM->pDescriptor);
     
     free(pM);
} 
/*--------------------------------------------------------------------------*/
int MotorGetPar(pMotor self, char *name, float *fVal){
    char pBueffel[512];
    snprintf(pBueffel,511,"%s ", name);
    return SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
        pBueffel, fVal);
}
/*---------------------------------------------------------------------------*/
int MotorSetPar(pMotor self, SConnection *pCon, char *name, float fVal){
    int status;
    
    status = SICSHdbSetFloat(self->pDescriptor->parNode,
        pCon,name,fVal);
    if(status == 1){
        SCparChange(pCon);
        return 1;
    } else {
        return 0;
    }
} 
/*---------------------------------------------------------------------------
  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, fixed, lowerlim, upperlim, sign;
    char pBueffel[512];

    assert(self);

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "fixed",&fixed) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "softzero",&fZero) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "softlowerlim",&lowerlim) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "softupperlim",&upperlim) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "sign",&sign) == 1);
    
    
    /* check for fixed */
    if(fixed >= 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 > upperlim){
      sprintf(pBueffel,"%f violates upper software limit %f on %s",
              fVal, upperlim,self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    if(fVal < lowerlim){
      sprintf(pBueffel,"%f violates lower software limit %f  on %s",
              fVal,lowerlim,self->name );
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    
    /* correct for zero point */
    fZero = -fZero;
    fHard = fVal - fZero;

    /* apply sign */
    fHard = fHard*sign; 
    
    /* check for hardware limits */
    if(fHard > self->pDriver->fUpper){
      sprintf(pBueffel,"%f violates upper hardware  limit %f on %s",
              fVal,self->pDriver->fUpper,self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    if(fHard < self->pDriver->fLower){
      sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
              fVal,self->pDriver->fLower,self->name);
      strncpy(pError,pBueffel,iErrLen);
      return 0;  
    }
    
    *fNew = fHard;
    return 1;
}
/*---------------------------------------------------------------------------*/
long MotorRun(void *sulf, SConnection *pCon, float fNew){
    float fHard, rights,  ignore, maxerr, interrupt;
    int i, iRet, iCode;
    char pBueffel[512];
    char pError[132];
    pMotor self;
    long lTime;
    float fDelta;
    
    self = (pMotor)(sulf);
    assert(self);
    assert(pCon);
    
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "accesscode",&rights) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "ignorefault",&ignore) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "failafter",&maxerr) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "interruptmode",&interrupt) == 1);
    
    
    /* check if I'am allowed to move this motor */
    if(!SCMatchRights(pCon,(int)rights)){
       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 if the bad motor flag is set */
    if((int)ignore > 0){
      snprintf(pBueffel,511,"WARNING: motor %s is unreliable",
	       self->name);
      SCWrite(pCon,pBueffel,eWarning);
      self->pDrivInt->iErrorCount = 0;
    }

    /* check our error count and interrupt if to much */
    if(self->pDrivInt->iErrorCount > (int)maxerr){
      /* 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->posFaultCount = 0;
    self->retryCount = 0;
    self->stopped = 0;
    self->fTarget = fHard;
    self->posCount = 0;
    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)interrupt);
                       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)interrupt);
       return HWFault;
    }
    return OKOK;
}
/*------------------------------------------------------------------------*/
int MotorGetHardPosition(pMotor self,SConnection *pCon, float *fHard){
     int iRet, iCode;
     float fVal, interrupt, sign;
     char pBueffel[512],pError[256];
     
     assert(self);
     assert(pCon);

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "interruptmode",&interrupt) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "sign",&sign) == 1);

    iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
    if(iRet == OKOK) {
     *fHard = fVal;
      return 1;
    } else { 
         /* 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)interrupt);
                       *fHard = fVal;
                       return 0;
           case MOTOK:
           case MOTREDO:
                       iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
                       if(iRet){
                          *fHard = fVal*sign;
                          return 1;
                       }   else{
                         sprintf(pBueffel,"ERROR: cannot fix motor %s",
                                 self->name);
                         SCSetInterrupt(pCon,(int)interrupt);
                         SCWrite(pCon,pBueffel,eError);   
                         *fHard = fVal;
                         return 0;
                       }
        }  
    }
    *fHard = fVal*sign;
    return 0;
  }  
/*------------------------------------------------------------------------*/
float MotorHardToSoftPosition(pMotor self, float fValue){
    float sign, zero;

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "sign",&sign) == 1);
    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "softzero",&zero) == 1);
    
    /* apply zeropoint */
    if(sign < 0.){
       fValue += zero;
    }else {
       fValue -= zero;
    }
    /* apply sign */
    return fValue*sign;
  }
/* ------------------------------------------------------------------------*/
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;
    }
    *fVal = MotorHardToSoftPosition(self,fValue);
    return 1;
  }  
/*---------------------------------------------------------------------------*/
int MotorCheckPosition(void *sulf, SConnection *pCon){
    float fHard, precision;
    int i, iRet, iCode;
    char pBueffel[512];
    pMotor self;
    
    self = (pMotor)sulf;
    assert(self);
    assert(pCon);

    assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
     "precision",&precision) == 1);
 
    /* try to find current position */
    iRet = MotorGetHardPosition(self,pCon,&fHard);
    if(iRet) {
      self->fPosition = fHard;
      if(ABS(fHard - self->fTarget) < precision){
         return 1;
      } else {
        /* Oooopppsss error */
           return 0;
      }
    } else {
            /* error getting hold of position, MotorGetHard already tried to
            solve the problem and FAILED, client already knows...*/
       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;
      pSite site = NULL;

      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],"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],"tclmot") == 0)
      {
         pDriver = CreateTclMotDriv(pCon,argc,argv);
         if(!pDriver)
         {
          return 0;
         }
         /* create the motor */
         pNew  = MotorInit("TCLMOT",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],"regress") == 0)
      {
         pDriver = RGMakeMotorDriver();
         if(!pDriver)
         {
          return 0;
         }
         /* create the motor */
         pNew  = MotorInit("regress",argv[1],pDriver);
         if(!pNew)
         {
           sprintf(pBueffel,"Failure to create motor %s",argv[1]);
           SCWrite(pCon,pBueffel,eError);
           return 0; 
         }
      }  
      else 
      {
	 site = getSite();
	 if(site != NULL)
	 {
	    pNew = site->CreateMotor(pCon,argc-1,&argv[1]);
	 }
         if(pNew == NULL)
	 {
	   sprintf(pBueffel,"Motor Type %s not 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 -------------*/
  void MotorListLL(pMotor self, SConnection *pCon)
  {
    char pBueffel[256];
    
    snprintf(pBueffel,255,"Parameter Listing for motor %s",self->name);
    SCWrite(pCon,pBueffel,eValue);
    snprintf(pBueffel,255,"%s ",self->name);
    PrintSICSParList(self->pDescriptor->parNode,pCon,pBueffel);
  }
/*--------------------------------------------------------------------------*/
  void MotorReset(pMotor pM)
  {
    SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softlowerlim",
        pM->pDriver->fLower);
    SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softupperlim",
        pM->pDriver->fUpper);
    SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softzero",
        .0);
    SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"fixed",
        -1.);
  }  
/*------------------------------------------------------------------------*/
  typedef struct {
                  char *pName;
                  SConnection *pCon;
                  float lastValue; 
                 } 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,
			      commandContext cc)
  {
     pMotInfo pInfo = NULL;
     char pBueffel[80];
     MotCallback *psCall;
     
     assert(pEvent);
     assert(pUser);
  
     psCall = (MotCallback *)pEvent;
     pInfo = (MotInfo *)pUser;

     if (pInfo->lastValue != psCall->fVal) {
       pInfo->lastValue = psCall->fVal;    
       (pInfo->pCon)->conEventType=POSITION;
       sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
       SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
     }
     return 1;
  }  
/*------------------------------------------------------------------------*/ 
  static void KillScript(void *pData)
  {
    if(pData != NULL)
    {
      free(pData);
    }
  }
/*------------------------ The endscript callback function ----------------*/
  static int EndScriptCallback(int iEvent, void *pEvent, void *pUser,
			       commandContext cc)
  {
     char *pScript = NULL;
     MotCallback *psCall;
     char pCommand[1024];
     int iRet;

     assert(pEvent);
     assert(pUser);
  
     psCall = (MotCallback *)pEvent;
     pScript = (char *)pUser;

     sprintf(pCommand,"%s %f",pScript,psCall->fVal);
     iRet = Tcl_Eval(pServ->pSics->pTcl,pCommand);
     return iRet;
  }      
/*----------------------------------------------------------------------------
 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  
    motorname endscript                    : script to call when motor
                                             finishes 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,eError);
              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;
        iRet = MotorGetSoftPosition(self,pCon,&fValue);
        if(!iRet)
        {
              sprintf(pBueffel,"Failed to register interest, Reason:Error obtaining current position for %s",argv[0]);
              SCWrite(pCon,pBueffel,eError);
              DeleteTokenList(pList);
              return 0;
         }
        pMoti->lastValue = fValue;

        lID = RegisterCallback(self->pCall, SCGetContext(pCon),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 if(strcmp(pCurrent->text,"endscript") == 0) /* endscript */
     {
        if(!SCMatchRights(pCon,usMugger))
	{
	  return 0;
        }
        if(self->endScriptID != 0)
	{
	  RemoveCallback(self->pCall, self->endScriptID);
	  self->endScriptID = 0; 
	}
	pCurrent = pCurrent->pNext;
	if(!pCurrent)
	{
	  SCWrite(pCon,"ERROR: scriptname argument missing",eError);
	  return 0;
        }
        self->endScriptID = 
	  RegisterCallback(self->pCall, SCGetContext(pCon),MOTEND, EndScriptCallback,
                               strdup(pCurrent->text), KillScript);
        SCRegister(pCon,pSics, self->pCall,self->endScriptID);
        DeleteTokenList(pList);
        SCSendOK(pCon);
        return 1;                       
     }
     else /* one of the parameter commands or error left now */
     {
        snprintf(pBueffel,511,"%s ", argv[0]);
        iRet = ProcessSICSHdbPar(self->pDescriptor->parNode,
            pCon, pBueffel, argc-1, &argv[1]);
        if(iRet == -1) {
            SCWrite(pCon,"ERROR: subcommand to motor not understood",
                eError);
            return 0;
        }  else if(iRet < -1) {
            return 0;
        }
        return iRet;
     }
     return 0;
  }