/*-----------------------------------------------------------------------
  A configurable virtual motor object. At motor start a script is called
  which returns the motors and positions to drive as a comma separated 
  list holding entries of the form mot=value. 

  This is the implementation file.

  COPYRIGHT: see file COPYRIGHT

  Mark Koennecke, August 2004
  interest added: Ferdi Franceschini, April 2006
-------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include "fortify.h"
#include <tcl.h>
#include "lld.h"
#include "splitter.h"
#include "confvirtmot.h"
#include "confvirtmot.i"

extern char *stptok(char *s, char *t, int len, char *brk);

/*---------------------------------------------------------------------------
 An internal data structure which holds information required to control
 the real motors
----------------------------------------------------------------------------*/
typedef struct {
  char name[132];
  void *data;
  pIDrivable pDriv;
  float value;
  int running;
} RealMotor, *pRealMotor;

/* Data passed by event generating object */
typedef struct {
  float fVal;
  char *pName;
} EventInfo;

/* Data available when registering interest */
typedef struct {
  char *pName;
  SConnection *pCon;
  float lastValue;
} RegisteredInfo, *pRegisteredInfo;

/*---------------------------------------------------------------------*/
static int HaltMotors(void *pData)
{
  pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
  RealMotor tuktuk;
  int iRet;

  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &tuktuk);
    tuktuk.pDriv->Halt(tuktuk.data);
    iRet = LLDnodePtr2Next(self->motorList);
  }
  return 1;
}

/*----------------------------------------------------------------------*/
static char *invokeMotorScript(pConfigurableVirtualMotor self,
                               float newValue)
{
  char pBueffel[512];
  Tcl_Interp *pTcl;
  int status;

  self->parseOK = 1;
  if (self->scriptName == NULL) {
    snprintf(self->scriptError, 511,
             "ERROR: no script configured for configurable virtual motor");
    self->parseOK = 0;
    return NULL;
  }

  snprintf(pBueffel, 510, "%s  %f", self->scriptName, newValue);
  pTcl = InterpGetTcl(pServ->pSics);

  status = Tcl_Eval(pTcl, pBueffel);
  if (status != TCL_OK) {
    snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
             Tcl_GetStringResult(pTcl));
    self->parseOK = 0;
    return NULL;
  }
  return (char *) Tcl_GetStringResult(pTcl);
}


/*--------------------------------------------------------------------*/
static int parseEntry(pConfigurableVirtualMotor self, char *entry)
{
  RealMotor tuktuk;
  char *pPtr = NULL;
  char number[80], pError[256];
  CommandList *pCom = NULL;
  int status;

  pPtr = entry;
  pPtr = stptok(pPtr, tuktuk.name, 131, "=");
  if (pPtr == NULL) {
    snprintf(self->scriptError, 510, "ERROR: cannot interpret %s, %s",
             entry, "require format: motor=value");
    self->parseOK = 0;
    return 0;
  }
  tuktuk.pDriv = (pIDrivable) FindDrivable(pServ->pSics, tuktuk.name);
  pCom = FindCommand(pServ->pSics, tuktuk.name);
  if (!pCom) {
    snprintf(self->scriptError, 510, "ERROR: %s not found", tuktuk.name);
    self->parseOK = 0;
    return 0;
  }
  tuktuk.data = pCom->pData;

  if (tuktuk.pDriv == NULL) {
    snprintf(self->scriptError, 510, "ERROR: %s is not drivable",
             tuktuk.name);
    self->parseOK = 0;
    return 0;
  }

  pPtr = stptok(pPtr, number, 79, "=");
  tuktuk.value = atof(number);

  LLDnodeAppendFrom(self->motorList, &tuktuk);

  return 1;
}

/*----------------------------------------------------------------------*/
static int parseCommandList(pConfigurableVirtualMotor self,
                            char *commandList)
{
  char pEntry[256], *pPtr;
  int status;

  LLDdelete(self->motorList);
  self->motorList = LLDcreate(sizeof(RealMotor));

  pPtr = commandList;
  while ((pPtr = stptok(pPtr, pEntry, 255, ",")) != NULL) {
    status = parseEntry(self, pEntry);
    if (status != 1) {
      return 0;
    }
  }
  return 1;
}

/*-----------------------------------------------------------------------*/
static int startMotorList(pConfigurableVirtualMotor self,
                          SConnection * pCon)
{
  RealMotor tuktuk;
  int status, iRet;

  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &tuktuk);
    status = tuktuk.pDriv->SetValue(tuktuk.data, pCon, tuktuk.value);
    if (status != 1) {
      return status;
    }
    tuktuk.running = 1;
    LLDnodeDataFrom(self->motorList, &tuktuk);
    iRet = LLDnodePtr2Next(self->motorList);
  }
  return OKOK;
}

/*------------------------------------------------------------------------*/
static long ConfSetValue(void *pData, SConnection * pCon, float newValue)
{
  pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
  char *commandList = NULL;
  int status;

  assert(self != NULL);

  commandList = invokeMotorScript(self, newValue);
  if (commandList == NULL) {
    SCWrite(pCon, self->scriptError, eError);
    return 0;
  }

  status = parseCommandList(self, commandList);
  if (status != 1) {
    SCWrite(pCon, self->scriptError, eError);
    return 0;
  }
  self->targetValue = newValue;
  self->targetReached = 0;
  self->posCount = 0;

  status = startMotorList(self, pCon);
  if (status != OKOK) {
    HaltMotors(self);
  }
  return OKOK;
}

/*-----------------------------------------------------------------------*/
static int ConfCheckLimits(void *pData, float fVal, char *error,
                           int errLen)
{
  pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
  char *commandList = NULL;
  int status, iRet;
  RealMotor tuktuk;

  assert(self != NULL);

  if (self->targetValue != fVal) {
    commandList = invokeMotorScript(self, fVal);
    if (commandList == NULL) {
      strlcpy(error, self->scriptError, errLen);
      return 0;
    }

    status = parseCommandList(self, commandList);
    if (status != 1) {
      strlcpy(error, self->scriptError, errLen);
      return 0;
    }
  }

  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &tuktuk);
    status =
        tuktuk.pDriv->CheckLimits(tuktuk.data, tuktuk.value, error,
                                  errLen);
    if (status != 1) {
      return status;
    }
    iRet = LLDnodePtr2Next(self->motorList);
  }
  return 1;
}

static void KillInfo(void *pData)
{
  pRegisteredInfo self = NULL;

  assert(pData);
  self = (pRegisteredInfo) pData;
  if (self->pName) {
    free(self->pName);
  }
  if (self->pCon) {
    SCDeleteConnection(self->pCon);
  }
  free(self);
}

/*------------------- The CallBack function for interest ------------------
 * iEvent: Event ID, see event.h for SICS events
 * pEvent: May contain data from event generating object
 * pUser: Data available when registering interest, see RegisteredInfo struct
 *        defined above for available info 
--------------------------------------------------------------------------*/
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
{
  pRegisteredInfo pInfo = NULL;
  char pBueffel[80];
  EventInfo *pEventData;

  assert(pEvent);
  assert(pUser);

  if (!SCisConnected(pInfo->pCon)) {
    return -1;
  }

  pEventData = (EventInfo *) pEvent;
  pInfo = (RegisteredInfo *) pUser;

  if (pInfo->lastValue != pEventData->fVal) {
    pInfo->lastValue = pEventData->fVal;
    (pInfo->pCon)->conEventType = POSITION;
    sprintf(pBueffel, "%s.position = %f ", pInfo->pName, pInfo->lastValue);
    SCWrite(pInfo->pCon, pBueffel, eEvent);
  }
  return 1;
}

/*------------------------------------------------------------------------*/
static int ConfCheckStatus(void *pData, SConnection * pCon)
{
  pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
  RealMotor tuktuk;
  int iRet, status, result;
  EventInfo event;

  result = HWIdle;
  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &tuktuk);
    if (tuktuk.running == 1) {
      status = tuktuk.pDriv->CheckStatus(tuktuk.data, pCon);
      switch (status) {
      case HWIdle:
        tuktuk.running = 0;
        LLDnodeDataFrom(self->motorList, &tuktuk);
        break;
      case HWBusy:
        result = HWBusy;
        break;
      case HWFault:
      case HWPosFault:
        return status;
        break;
      default:
        /*
           this is a programming error and has to be fixed
         */
        assert(0);
      }
    }
    iRet = LLDnodePtr2Next(self->motorList);
  }

  if (result == HWIdle) {
    event.pName = self->name;
    event.fVal = self->pDriv->GetValue(self, pCon);
    InvokeCallBack(self->pCall, MOTDRIVE, &event);
  } else if (result == HWBusy) {
    self->posCount++;
    if (self->posCount >= 10 /*ObVal(self->ParArray,MOVECOUNT) */ ) {
      event.pName = self->name;
      event.fVal = self->pDriv->GetValue(self, pCon);
      InvokeCallBack(self->pCall, MOTDRIVE, &event);
      self->posCount = 0;
    }
  }
  return result;
}

/*---------------------------------------------------------------------*/
static float invokeReadScript(pConfigurableVirtualMotor self,
                              SConnection * pCon)
{
  Tcl_Interp *pTcl;
  int status;

  pTcl = InterpGetTcl(pServ->pSics);

  status = Tcl_Eval(pTcl, self->readScript);
  if (status != TCL_OK) {
    snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
             Tcl_GetStringResult(pTcl));
    SCWrite(pCon, self->scriptError, eError);
  }
  return atof(Tcl_GetStringResult(pTcl));
}

/*---------------------------------------------------------------------*/
static void checkMotorValues(pConfigurableVirtualMotor self,
                             SConnection * pCon)
{
  int iRet;
  float value;
  RealMotor tuktuk;
  char pBueffel[512];

  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &tuktuk);
    value = tuktuk.pDriv->GetValue(tuktuk.data, pCon);
    value -= tuktuk.value;
    if (value < .0) {
      value *= -1;
    }
    if (value > .1) {
      snprintf(pBueffel, 510, "WARNING: motor %s of position by %f",
               tuktuk.name, value);
      SCWrite(pCon, pBueffel, eWarning);
      return;
    }
    iRet = LLDnodePtr2Next(self->motorList);
  }
  self->targetReached = 1;
}

/*-----------------------------------------------------------------------*/
static float ConfGetValue(void *pData, SConnection * pCon)
{
  pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
  float currentValue = -9999.99;

  assert(self != NULL);

  if (self->readScript != NULL) {
    currentValue = invokeReadScript(self, pCon);
  } else {
    if (self->targetReached == 1) {
      return self->targetValue;
    } else {
      checkMotorValues(self, pCon);
      if (self->targetReached) {
        currentValue = self->targetValue;
      }
    }
  }
  return currentValue;
}

/*-----------------------------------------------------------------------*/
static void *GetConfigurableVirtualMotorInterface(void *pData, int iID)
{
  pConfigurableVirtualMotor self = NULL;

  self = (pConfigurableVirtualMotor) pData;
  assert(self);
  if (iID == DRIVEID) {
    return self->pDriv;
  } else if (iID == CALLBACKINTERFACE) {
    return self->pCall;
  }
  return NULL;
}

/*-----------------------------------------------------------------------*/
static void KillConfigurableVirtualMotor(void *data)
{
  pConfigurableVirtualMotor self = NULL;

  self = (pConfigurableVirtualMotor) data;
  if (self != NULL) {
    if (self->pDes != NULL) {
      DeleteDescriptor(self->pDes);
      self->pDes = NULL;
    }
    LLDdelete(self->motorList);
    if (self->name != NULL) {
      free(self->name);
      self->name = NULL;
    }
    if (self->scriptName != NULL) {
      free(self->scriptName);
      self->scriptName = NULL;
    }
    if (self->readScript != NULL) {
      free(self->readScript);
      self->readScript = NULL;
    }
    free(self);
    self = NULL;
  }
}

/*------------------------------------------------------------------------*/
int MakeConfigurableVirtualMotor(SConnection * pCon, SicsInterp * pSics,
                                 void *data, int argc, char *argv[])
{
  pConfigurableVirtualMotor pNew = NULL;
  char pBueffel[512];

  if (argc < 2) {
    SCWrite(pCon, "ERROR: need name to create configurable motor", eError);
    return 0;
  }

  pNew =
      (pConfigurableVirtualMotor) malloc(sizeof(ConfigurableVirtualMotor));
  if (pNew == NULL) {
    SCWrite(pCon, "ERROR: no memory to create configurable virtual motor",
            eError);
    return 0;
  }
  memset(pNew, 0, sizeof(ConfigurableVirtualMotor));

  pNew->name = strdup(argv[1]);
  pNew->posCount = 0;
  pNew->pDes = CreateDescriptor("ConfigurableVirtualMotor");
  pNew->pDriv = CreateDrivableInterface();
  pNew->motorList = LLDcreate(sizeof(RealMotor));
  if (pNew->pDes == NULL || pNew->pDriv == NULL || pNew->motorList < 0) {
    SCWrite(pCon, "ERROR: no memory to create configurable virtual motor",
            eError);
    return 0;
  }

  /*
     assign functions
   */
  pNew->pDes->GetInterface = GetConfigurableVirtualMotorInterface;
  pNew->pDriv->Halt = HaltMotors;
  pNew->pDriv->CheckLimits = ConfCheckLimits;
  pNew->pDriv->SetValue = ConfSetValue;
  pNew->pDriv->CheckStatus = ConfCheckStatus;
  pNew->pDriv->GetValue = ConfGetValue;

  /* initialise callback interface */
  pNew->pCall = CreateCallBackInterface();
  if (!pNew->pCall) {
    KillConfigurableVirtualMotor(pNew);
    return 0;
  }
  /*
     install command
   */
  if (AddCommand(pSics, argv[1], ConfigurableVirtualMotorAction,
                 KillConfigurableVirtualMotor, pNew) != 1) {
    snprintf(pBueffel, 510, "ERROR: duplicate command %s not created",
             argv[1]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  } else {
    SCSendOK(pCon);
  }

  return 1;
}

/*-----------------------------------------------------------------------*/
int ConfigurableVirtualMotorAction(SConnection * pCon, SicsInterp * pSics,
                                   void *data, int argc, char *argv[])
{
  pConfigurableVirtualMotor self = NULL;
  char pBueffel[512];
  float value;
  int iRet;
  long lID;
  pRegisteredInfo pRegInfo = NULL;

  self = (pConfigurableVirtualMotor) data;
  assert(self != NULL);

  if (argc > 1) {
    strtolower(argv[1]);
    if (strcmp(argv[1], "drivescript") == 0) {
      if (argc > 2) {
        /* set case */
        Arg2Text(argc - 2, &argv[2], pBueffel, 510);
        self->scriptName = strdup(pBueffel);
        SCSendOK(pCon);
        return 1;
      } else {
        /* inquiry */
        if (self->scriptName == NULL) {
          snprintf(pBueffel, 510, "%s.drivescript = UNDEFINED", argv[0]);
          SCWrite(pCon, pBueffel, eValue);
          return 1;
        } else {
          snprintf(pBueffel, 510, "%s.drivescript = %s", argv[0],
                   self->scriptName);
          SCWrite(pCon, pBueffel, eValue);
          return 1;
        }
      }
    } else if (strcmp(argv[1], "readscript") == 0) {
      if (argc > 2) {
        /* set case */
        Arg2Text(argc - 2, &argv[2], pBueffel, 510);
        self->readScript = strdup(pBueffel);
        SCSendOK(pCon);
        return 1;
      } else {
        /* inquiry */
        if (self->readScript == NULL) {
          snprintf(pBueffel, 510, "%s.readscript = UNDEFINED", argv[0]);
          SCWrite(pCon, pBueffel, eValue);
          return 1;
        } else {
          snprintf(pBueffel, 510, "%s.readscript = %s", argv[0],
                   self->readScript);
          SCWrite(pCon, pBueffel, eValue);
          return 1;
        }
      }
    } else if (strcmp(argv[1], "interest") == 0) {
      pRegInfo = (pRegisteredInfo) malloc(sizeof(RegisteredInfo));
      if (!pRegInfo) {
        SCWrite(pCon, "ERROR: out of memory in confvirtualmot.c", eError);
        return 0;
      }
      pRegInfo->pName = strdup(argv[0]);
      pRegInfo->pCon = SCCopyConnection(pCon);
      value = ConfGetValue(self, pCon);
      if (!iRet) {
        snprintf(pBueffel,511,
                "Failed to register interest, Reason:Error obtaining current position for %s",
                argv[0]);
        SCWrite(pCon, pBueffel, eError);
        return 0;
      }
      pRegInfo->lastValue = value;

      lID = RegisterCallback(self->pCall, MOTDRIVE,
                             InterestCallback, pRegInfo, KillInfo);
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel, 5120, "ERROR: subcommand %s to %s unknown",
               argv[1], argv[0]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
  } else {
    value = self->pDriv->GetValue(self, pCon);
    snprintf(pBueffel, 510, "%s = %f", argv[0], value);
    SCWrite(pCon, pBueffel, eValue);
    return 1;
  }
}