/*-----------------------------------------------------------------------
  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){
      strncpy(error,self->scriptError,errLen);
      return 0;
    }

    status = parseCommandList(self,commandList);
    if(status != 1){
      strncpy(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);
    }
    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,
			      commandContext cc)
  {
     pRegisteredInfo pInfo = NULL;
     char pBueffel[80];
     EventInfo *pEventData;
     
     assert(pEvent);
     assert(pUser);
  
     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);
       SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
     }
     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 = pCon;
        value = ConfGetValue(self,pCon);
        if(!iRet)
        {
              sprintf(pBueffel,"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, SCGetContext(pCon),MOTDRIVE, InterestCallback, pRegInfo, KillInfo);
        SCRegister(pCon,pSics, self->pCall,lID);
        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;
  }
}