/*--------------------------------------------------------------------------
  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

  Reworked a bit to work with second generation motors
  Mark Koennecke, February 2011
  --------------------------------------------------------------------------*/
#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 "macro.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);
    strlcpy(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) {
	snprintf(pBueffel,sizeof(pBueffel),"%s list", self->pMaster->name);
	MacroPush(pCon);
	iRet = Tcl_Eval(InterpGetTcl(pSics),pBueffel);
	MacroPop();
	SCWrite(pCon,(char *)Tcl_GetStringResult(InterpGetTcl(pSics)), eValue);
    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;
    }
	snprintf(pBueffel,sizeof(pBueffel),"%s reset", self->pMaster->name);
	MacroPush(pCon);
	iRet = Tcl_Eval(InterpGetTcl(pSics),pBueffel);
	MacroPop();
	SCWrite(pCon,(char *)Tcl_GetStringResult(InterpGetTcl(pSics)), eValue);
    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;
}