/**
 * --------------------------------------------------------------*/
#include <sics.h>
#include <sicshipadaba.h>
#include <devser.h>
#include <tcl.h>
#include <macro.h>
#include <sicsobj.h>
#include "scriptcontext.h"
/*---------------------------------------------------------------*/
typedef struct {
	pObjectDescriptor pDes;
  pIDrivable pDrivInt; /* Need a drivable interface for "emon list" */
	pEVInterface pEVI;
  EVMode eMode;
  int iPaused;
  char *pName;
	pHdb modeNode, tolNode, errNode;
}SctEmon, *pSctEmon;
/* emon will monitor when mode is "monitor" or "paused" */
/* XXX nopause
static char *modestr[5] = {"idle", "drive", "monitor", "error", "paused"};
static int EVPaused=4;*/
/*---------------------------------------------------------------*/
/**
  @brief a dummy getvalue command to prevent "emon list" from aborting SICS
  */
  static float SCTEmonGetValue(void *pData, SConnection *pCon)
  {
    return -999.666;
  }
  /* XXX nopause
static void SetModeNode(pSctEmon self, EVMode mode)
{
  if (self->modeNode->value.v.text != NULL)
    free(self->modeNode->value.v.text);
  self->modeNode->value.v.text = strdup(modestr[mode]);
}
*/

static void *SCTEmonGetInterface(void *pData, int ID){
    pSctEmon self = NULL; 
 
    self = (pSctEmon)pData;
    assert(self);
    if (self->modeNode == NULL || self->tolNode == NULL || self->errNode == NULL)
      return NULL;

    switch (ID) {
      case DRIVEID:
        return self->pDrivInt;
      case ENVIRINTERFACE:
        return self->pEVI;
      default:
        return NULL;
    }
}
/*----------------------------------------------------------------
Reads node value and returns EVMode
Convert mode string from the evmode textnode value to
        typedef enum { EVIdle, EVDrive, EVMonitor, EVError } EVMode;
"idle", "drive", "monitor", "error"
------------------------------------------------------------------*/
static EVMode SCTEmonGetMode(void *pData)
{
    pSctEmon self = NULL; 
      
      self = (pSctEmon)pData;
      assert(self);
      switch(self->modeNode->value.v.text[0]) {
        case 'I':
        case 'i':
          return EVIdle;
        break;
        case 'D':
        case 'd':
          return EVDrive;
        break;
        case 'M':
        case 'm':
/*XXX nopause        case 'P':
        case 'p':*/
          return EVMonitor;
        break;
        case 'E':
        case 'e':
          return EVError;
        break;
        default:
          return EVIdle;
        //TODO Handle error
      }
}
/*----------------------------------------------------------------
 This routine can return 0 when a limit problem occurred 
 OKOK when the motor was successfully started 
 HWFault when a problem occured starting the device
 Possible errors shall be printed to pCon
 For real motors, this is supposed to try at least three times
 to start the motor in question
 val is the value to drive the motor too
Reads value from the "isintolerance" node (1 or 0)
------------------------------------------------------------------*/
static int SCTEmonIsInTolerance(void *pData)
{
  pSctEmon self = NULL; 
  pExeList   pExe;
  char pBueffel[512];
  char monMode;

  self = (pSctEmon)pData;
  assert(self);

  if(self->tolNode->value.v.intValue == 1) {
    monMode = self->modeNode->value.v.text[0];
    if (self->iPaused) {
      pExe = GetExecutor();
      ContinueExecution(pExe);
      self->iPaused = 0;
      sprintf(pBueffel,"Device %s back in tolerances again", self->pName);
      SCWrite(GetExeOwner(pExe), pBueffel, eWarning);
    }
//XXX nopause  SetModeNode(self, EVMonitor);
    return 1;
  } else {
    return 0;
  }
}
/*----------------------------------------------------------------
 Checks the status of a running motor. Possible return values
   HWBusy The motor is still running
   OKOK or HWIdle when the motor finished driving
   HWFault when a hardware problem ocurred
   HWPosFault when the hardware cannot reach a position
 Errors are duly to be printed to pCon
 For real motors CheckStatus again shall try hard to fix any 
 issues with the motor 

The HandleError node value can be one of Lazy, Pause, Interrupt, Safe
NOTE: emon ignores the returned iStatus value
------------------------------------------------------------------*/
static int SCTEmonHandleError(void *pData)
{
  pSctEmon self = NULL; 
  pExeList   pExe;
  int iRet, iHandler, iStatus;
  char monMode;

  self = (pSctEmon)pData;
  assert(self);

  monMode = self->modeNode->value.v.text[0];
//XXX nopause SetModeNode(self, EVError);
  switch(self->errNode->value.v.text[0]) {
    case 'l':
    case 'L': /* Lazy */
      iStatus = 1;
      break;
    case 'p':
    case 'P':  /* Pause */
      pExe = GetExecutor();
      if(IsCounting(pExe)) {
        if (!self->iPaused) {
          SCWrite(GetExeOwner(pExe),"Pausing till OK",eError);
        }
        PauseExecution(pExe);
        self->iPaused = 1;
//XXX nopause       SetModeNode(self, EVPaused);
        iStatus = 1;           
      }
      break;
    case 'i':
    case 'I': /* Interrupt */
      /* TODO How is this supposed to work?
         SetInterrupt((int)ObVal(self->pParam,INTERRUPT));*/
      iStatus = 1;
      break;
    case 's':
    case 'S': /* Safe, run to a safe place, put him into idle afterwards */
      /*TODO Drive environment to a safevalue */
        break;
    default:
      return 0;       

  }
  /* NOTE: emon ignores the return value */
  return iStatus;
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static pSctEmon SCTEMONMakeObject(){
    pSctEmon self = NULL; 

    self = calloc(sizeof(SctEmon),1);
    if(self == NULL){
    	return NULL;
    }
    self->pDes = CreateDescriptor("SctEmonAdapter");
    if(self->pDes == NULL) {
      free(self);
    	return NULL;
    }
    self->pEVI = CreateEVInterface();
    if(self->pEVI == NULL) {
      DeleteDescriptor(self->pDes);
    	free(self);
    	return NULL;
    }
    self->pDrivInt = CreateDrivableInterface();
    if(self->pDrivInt == NULL) {
      DeleteDescriptor(self->pDes);
      free(self->pEVI);
    	free(self);
    	return NULL;
    }
    
    self->pDes->GetInterface = SCTEmonGetInterface;
    self->pEVI->GetMode = SCTEmonGetMode;
    self->pEVI->IsInTolerance = SCTEmonIsInTolerance;
    self->pEVI->HandleError = SCTEmonHandleError;

    self->pDrivInt->Halt = NULL;
    self->pDrivInt->CheckLimits = NULL;
    self->pDrivInt->SetValue = NULL;
    self->pDrivInt->CheckStatus = NULL;
    self->pDrivInt->GetValue = SCTEmonGetValue;
    return self;
}
/*----------------------------------------------------------------*/
static hdbCallbackReturn SctDummyCallback(Hdb *node, void *userData,
                                          hdbMessage *msg) {
  return hdbContinue;
}
/*----------------------------------------------------------------*/
static void SctEmonDeleteNode(void *pData) {
  pSctEmon self = (pSctEmon)pData;
  self->modeNode = NULL;
  self->tolNode = NULL;
  self->errNode = NULL;
}
/*--------------------------------------------------------------
  args:  name, modepath, tolpath, errpath
*/
int SctMakeEmonAdapter(SConnection *pCon, SicsInterp *pSics, void *object,
               int argc, char *argv[]) {
	pSctEmon pNew = NULL;
  hdbCallback *cb;
	
	if(argc < 5){
		SCWrite(pCon,"ERROR: not enough arguments for SctMakeEmonAdapter", eError);
		return 0;
	}
	
	pNew = SCTEMONMakeObject();
	if(pNew == NULL){
		SCWrite(pCon,"ERROR: out of memory in SctMakeEmonAdapter",
				eError);
		return 0;
	}
	
  pNew->pName = strdup(argv[1]);
	pNew->modeNode = FindHdbNode(NULL,argv[2], pCon);
	pNew->tolNode = FindHdbNode(NULL,argv[3], pCon);
	pNew->errNode = FindHdbNode(NULL,argv[4], pCon);
  pNew->iPaused = 0;
  
/*XXX I'm guessing that SctEmonDeleteNode will be called when the
  script context object is deleted. So the emon functions should check 
  if node == NULL before trying to do something */	
  cb = MakeHipadabaCallback(SctDummyCallback, pNew, SctEmonDeleteNode);
  assert(cb);
  AppendHipadabaCallback(pNew->modeNode, cb);
	
  EVRegisterController(FindEMON(pSics),argv[1],pNew, pCon); 
	return 1;
}
/*---------------------------------------------------------------*/

void SctEmonInit(void) {
  AddCmd("makesctemon", SctMakeEmonAdapter);
}