/*--------------------------------------------------------------------------
  TOPSI switched motors implementation module. Three motors share a common
  EL734 motor. The actual motor is selected through a SPS. More information in
  file switchedmotor.tex or with Jochen Stahn.

  copyright: see file copyright.h

  Mark Koennecke, May 2001
  --------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <stdio.h>
#include <tcl.h>
#include <fortify.h>
#include <sics.h>
#include <SCinter.h>
#include <splitter.h>
#include "hardsup/sinq_prototypes.h"
#include "hardsup/rs232c_def.h"
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.h"
#include <modriv.h>
#include "swmotor.h"
#include "swmotor.i"

  typedef struct __MoDriv {
                           /* 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);
                           
                           
                           /* EL-734 specific fields */
                           int iPort;
                           char *hostname;
                           int iChannel;
                           int iMotor;
                           void *EL734struct;
                           int iMSR;
                          } EL734Driv;
/*========================================================================
 We start of by implementing the interface functions for the various 
 interfaces this module has to implement.
 ==========================================================================*/
static int SWHalt(void *pData)
{
  pSWmot self = (pSWmot)pData;
  assert(self);
  return self->pMaster->pDrivInt->Halt(self->pMaster);
}
/*--------------------------------------------------------------------*/
static int SWCheckLimits(void *pData, float fVal, char *error, int iErrLen)
{
  pSWmot self = (pSWmot)pData;
  assert(self);
  return self->pMaster->pDrivInt->CheckLimits(self->pMaster,
                                              fVal,error,iErrLen);
}
/*--------------------------------------------------------------------*/
static int SWCheckStatus(void *pData, SConnection *pCon)
{
  pSWmot self = (pSWmot)pData;
  assert(self);
  return self->pMaster->pDrivInt->CheckStatus(self->pMaster,
                                              pCon);
}
/*---------------------------------------------------------------------*/
static int SWSaveStatus(void *pData, char *name, FILE *fd)
{
  pSWmot self = (pSWmot)pData;
  assert(self);

  fprintf(fd,"%s savedValue = %f\n", name, 
	  self->positions[self->myNumber]);
  return 1;
}
/*-----------------------------------------------------------------------*/
static void *SWGetInterface(void *pData, int ID)
{
  pSWmot self = (pSWmot)pData;
  assert(self);

  if(ID == DRIVEID)
  {
    return self->pDriv;
  }
  return NULL;
}
/*----------------------------------------------------------------------*/
static float SWGetValue(void *pData, SConnection *pCon)
{
  pSWmot self = (pSWmot)pData;
  float fVal;
  assert(self);

  /*
    we are not selected: return stored data:
  */
  if(self->myNumber != self->selectedMotor[0]) 
  {
    SCWrite(pCon,"WARNING: motor not activem returning stored value",
            eWarning);
    return self->positions[self->myNumber];
  }
  else
  {
    fVal = self->pMaster->pDrivInt->GetValue(self->pMaster, pCon);
    self->positions[self->myNumber] = fVal;
    return fVal;
  }
}
/*-----------------------------------------------------------------------*/
static long SWSetValue(void *pData, SConnection *pCon, float fVal)
{
  pSWmot self = (pSWmot)pData;
  char pCommand[256], pError[132];
  int status;
  EL734Driv *pElli;  
  Tcl_Interp *pTcl;

  assert(self);
  self->errCode = 1;

  /*
   first case: we are already selected
  */
  if(self->myNumber == self->selectedMotor[0])
  {
    return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
  }
  else
  {
    /*
      second case: switch to the requested motor, do a reference run
      and then execute Set command
    */
    sprintf(pCommand,"Switching to motor number %d", self->myNumber);
    SCWrite(pCon,pCommand,eWarning);
    sprintf(pCommand,"%s %d",self->switchFunc, self->myNumber);
    pTcl = (Tcl_Interp *)pServ->pSics->pTcl;
    status  = Tcl_Eval(pTcl,pCommand);
    strncpy(pError,pTcl->result, 131);
    if(status !=  TCL_OK || strstr(pError,"OK") == NULL)
    {
      sprintf(pCommand,"ERROR: %s while switching motor", pError);
      SCWrite(pCon,pCommand, eError);
      self->errCode = -1001;
      return HWFault;
    }
    self->selectedMotor[0] = self->myNumber;

    /*
     switch done! Start a reference run
    */
    SicsWait(10);
    SCWrite(pCon,"Standby, starting reference run... ", eWarning);
    pElli = (EL734Driv *)self->pMaster->pDriver;
    sprintf(pCommand,"R %d\r",pElli->iMotor);
    status = EL734_SendCmnd(&(pElli->EL734struct),
                             pCommand,
                             pError, 131);
    if(status != 1)
    {
      sprintf(pCommand,"ERROR: %s while trying to start reference run",
              pError);
      SCWrite(pCon,pCommand,eError);
      self->errCode = -1002;
      return HWFault;
    }    
    /*
     now loop forever until reference run is done. This is either when the
     motors stops being busy or when  the user interrupts.
    */
    sprintf(pCommand,"SS %d\r",pElli->iMotor);
    for( ; ;)
    {
       status = EL734_SendCmnd(&(pElli->EL734struct),
                               pCommand,
                               pError, 131);
       if(status != 1)
       {
	 sprintf(pCommand,"ERROR: %s during reference run",
		 pError);
	 SCWrite(pCon,pCommand,eError);
	 self->errCode = -1003;
	 return HWFault;
       }
       if(strstr(pError,"?BSY") == NULL)
	 break;
       if(SCGetInterrupt(pCon) != eContinue)
       {
	 self->errCode = -1004;
         SCWrite(pCon,"ERROR: user interrupted reference run",eError);
         return HWFault;
       }
       SicsWait(2);
    }

    /*
     now this is finished. We can really start driving the motor
    */
    SCWrite(pCon,"Reference run completed, starting to drive motor..",
            eWarning);
    return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
  }
}   
/*----------------------------------------------------------------------*/
static void KillSWFull(void *pData)
{
  pSWmot self = (pSWmot)pData;
  if(self == NULL)
    return;
  
  if(self->pDriv)
    free(self->pDriv);
  if(self->pDes)
    DeleteDescriptor(self->pDes);
  if(self->selectedMotor)
    free(self->selectedMotor);
  if(self->switchFunc)
    free(self->switchFunc);
  free(self);
}
/*---------------------------------------------------------------------*/
static void KillSWHalf(void *pData)
{
  pSWmot self = (pSWmot)pData;
  if(self)
    free(self);
}
/*----------------------------------------------------------------------
  Alright, now the interpreter functions follow
  Usage:
         MakeSWMotor master switchFunc slave1 slave2 slave3
  -----------------------------------------------------------------------*/
int MakeSWMotor(SConnection *pCon, SicsInterp *pSics, void *pData,
                   int argc, char *argv[])
{
  pSWmot sw1, sw2, sw3;
  char pBueffel[256];
  int i, status;

  /*
   check that we have enough arguments
  */
  if(argc < 6)
  {
    SCWrite(pCon,"ERROR: insufficient number of arguments to MakeSWMotor",
            eError);
    return 0;
  }
  
  /*
   allocate a new data structure 
  */
  sw1 = (pSWmot)malloc(sizeof(SWmot));
  if(sw1 == NULL)
  {
    SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
    return 0;
  }
  memset(sw1,0,sizeof(SWmot));
  
  /*
    fill it up with stuff
  */
  sw1->pDes = CreateDescriptor("Sparbroetchen");
  sw1->pDriv = CreateDrivableInterface();
  sw1->selectedMotor = (int *)malloc(sizeof(int));
  if(!sw1->pDes || ! sw1->pDriv || !sw1->selectedMotor)
  {
    SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
    return 0;
  }
  sw1->selectedMotor[0] = -1;
  sw1->pMaster = FindMotor(pSics,argv[1]);
  if(!sw1->pMaster)
  {
    sprintf(pBueffel,"ERROR: cannot find master motor %s", argv[1]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }  
  sw1->switchFunc = strdup(argv[2]);
  for(i = 0; i < 3; i++)
  {
    strcpy(sw1->slaves[i],argv[3+i]);
  } 

  /*
    initialize function pointers
  */
  sw1->pDes->SaveStatus = SWSaveStatus;
  sw1->pDes->GetInterface = SWGetInterface;
  sw1->pDriv->GetValue = SWGetValue;
  sw1->pDriv->SetValue = SWSetValue;
  sw1->pDriv->Halt     = SWHalt;
  sw1->pDriv->CheckStatus = SWCheckStatus;
  sw1->pDriv->CheckLimits = SWCheckLimits;

  /*
   create clones of the new data structure ofr the other slaves
  */
  sw2 = (pSWmot)malloc(sizeof(SWmot));
  sw3 = (pSWmot)malloc(sizeof(SWmot));
  if(!sw2 || !sw2) 
  {
    SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
    return 0;
  }
  memcpy(sw2,sw1,sizeof(SWmot));
  sw2->myNumber = 1;
  memcpy(sw3,sw1,sizeof(SWmot));
  sw3->myNumber = 2;

  /*
    install commands
  */
  status = AddCommand(pSics, argv[3], SWMotorAction, KillSWFull, sw1);
  if(!status)
  {
    sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = AddCommand(pSics, argv[4], SWMotorAction, KillSWHalf, sw2);
  if(!status)
  {
    sprintf(pBueffel,"ERROR: command %s already exists!",argv[4]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = AddCommand(pSics, argv[5], SWMotorAction, KillSWHalf, sw3);
  if(!status)
  {
    sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }

  return 1;
}
/*-------------------------------------------------------------------
  some prototypes from functions implemented in motor.c
  --------------------------------------------------------------------*/
void MotorListLL(pMotor self, SConnection *pCon);
void MotorReset(pMotor pM);
/*---------------------------------------------------------------------*/
int SWMotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                   int argc, char *argv[])
{
    char pBueffel[512];
    TokenList *pList = NULL;
    TokenList *pCurrent;
    TokenList *pName; 
    int iRet;
    pSWmot self = NULL;
    float fValue;
    long lID;
    
    assert(pCon);
    assert(pSics);
    assert(pData);
    
    self = (pSWmot)pData;
    
    /* create Tokenlist */
    argtolower(argc,argv);
    pList = SplitArguments(argc,argv);
    if(!pList)
    {
      SCWrite(pCon,"Error parsing argument list in SWMotorAction",eError);
      return 0;
    }
  
     /* first argument can be one of list, reset or parameter name */   
     pCurrent = pList->pNext;
     if(!pCurrent) /* no argument, print value */
     {
        fValue = self->pDriv->GetValue(self,pCon);
        if(fValue < -990.)
        {
              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->pMaster,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->pMaster);
       DeleteTokenList(pList);
       SCSendOK(pCon);
       return 1;
     }
     else if(strcmp(pCurrent->text,"savedValue") == 0)
     {
       pCurrent = pCurrent->pNext;
       if(!pCurrent)
       {
	 /*
          print Value
	 */
         sprintf(pBueffel,"%s.savedValue = %f", argv[0], 
                 self->positions[self->myNumber]);
         SCWrite(pCon,pBueffel,eValue);
	 DeleteTokenList(pList);
         return 1;
       }
       else
       {
	 /*
          set saveValue, must be a numeric parameter
	 */
          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 ",
                    argv[0],"saveValue");
            SCWrite(pCon,pBueffel,eError); 
            DeleteTokenList(pList);
            return 0;
          }
          self->positions[self->myNumber] = fValue;
          SCSendOK(pCon);
          return 1;
       }
     } 
     else if(strcmp(pCurrent->text,"selected") == 0)
     {
       pCurrent = pCurrent->pNext;
       if(!pCurrent)
       {
	 /*
          print Value
	 */
         sprintf(pBueffel,"%s.selected = %d", argv[0], 
                 self->selectedMotor[0]);
         SCWrite(pCon,pBueffel,eValue);
	 DeleteTokenList(pList);
         return 1;
       }
       else
       {
	 /*
          set selected, must be a numeric parameter
	 */
          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 ",
                    argv[0],"selected");
            SCWrite(pCon,pBueffel,eError); 
            DeleteTokenList(pList);
            return 0;
          }
          self->selectedMotor[0] = (int)fValue;
          SCSendOK(pCon);
          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)
          {
            fValue = self->pDriv->GetValue(self,pCon);
            if(fValue < 999.)
            {
              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;
          }
          iRet = MotorGetPar(self->pMaster,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",argv[0],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->pMaster,pCon,pName->text,fValue);
          DeleteTokenList(pList);
          if(iRet)
            SCSendOK(pCon);
          return iRet;
        }
     }
     return 0;
}