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

  This version has been modified for use with the new high performance
  motor drivers.

  Mark Koennecke, August 2003

  Modified to do all the switching and reference running in Tcl
 
  Mark Koennecke, August 2004
  --------------------------------------------------------------------------*/
#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 <modriv.h>
#include <rs232controller.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 */
  prs232 controller;
  int iMotor;
  float lastValue;
  int errorCode;
  int oredMsr;
} EL734Driv, *pEL734Driv;
/*========================================================================
 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 active, 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;

    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:
         MakeSWHPMotor master switchFunc slave1 slave2 slave3
  -----------------------------------------------------------------------*/
int MakeSWHPMotor(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 of 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;
}
