/**
 * This is a generalized SICS get/put routine. It tries to hard to match a name 
 * with the data. 
 * 
 * The other difference is that this uses the message pipe system for implementation. 
 * See messsagepipe.c for a pointer to documentation.  
 *
 * copyright: see file COPYRIGHT
 *
 */
#include <ctype.h>
#include <sics.h>
#include <sicsget.h>
#include <messagepipe.h>
#include <sicshipadaba.h>
#include <splitter.h>
#include <stptok.h>
#include <sicsdata.h>

static pMP getPipe, setPipe, sicsPipe, tclPipe;


typedef struct {
  int success;
  char *name;
  hdbValue *v;
} SSGMessage, *pSSGMessage;
/*---------------------------------------------------------------------------*/
typedef struct {
  int success;
  char *command;
  char *response;
  hdbValue *v;
} ParseMessage, *pParseMessage;

extern char *trim(char *txt);
/*---------------------------------------------------------------------------*/
int sget(char *name, hdbValue *v)
{
  SSGMessage ssg;
  int status;
  
  ssg.success = 0;
  ssg.name = strdup(name);
  ssg.v = v;

  MPprocess(getPipe,&ssg);
  free(ssg.name);
  
  return ssg.success;
}
/*---------------------------------------------------------------------------*/
int sput(char *name, hdbValue v)
{
  SSGMessage ssg;
  int status;
  
  ssg.success = 0;
  ssg.name = strdup(name);
  ssg.v = &v;

  MPprocess(setPipe,&ssg);
  free(ssg.name);
  
  return ssg.success;
}
/*---------------------------------------------------------------------------*/
static int SICSGetCommand(SConnection * pCon, SicsInterp * pSics, void *pData,
                      int argc, char *argv[])
{
  int status;
  hdbValue v;
  pDynString data;

  if(argc < 2) {
    SCWrite(pCon,"ERROR: need at least address for sicsget",eError);
    return 0;
  }

  status = sget(argv[1], &v);
  if(status){
    data = formatValue(v,NULL);
    if(data != NULL){
      SCPrintf(pCon,eValue,"%s",GetCharArray(data));
      DeleteDynString(data);
    } else {
      SCPrintf(pCon,eError,"ERROR: formatting value for %s failed", argv[1]);
      ReleaseHdbValue(&v);
      return 0;
    }
  } else {
    SCPrintf(pCon,eError,"ERROR: value for %s not found", argv[1]);
    return 0;
  }
  ReleaseHdbValue(&v);
  return 1;
}
/*---------------------------------------------------------------------------*/
static int SICSPutCommand(SConnection * pCon, SicsInterp * pSics, void *pData,
                      int argc, char *argv[])
{
  int status;
  hdbValue v;
  char buffer[2048];
  char error[132];

  if(argc < 2) {
    SCWrite(pCon,"ERROR: need at least address for sicsget",eError);
    return 0;
  }

  status = sget(argv[1],&v);
  if(!status){
    SCPrintf(pCon,eError,"ERROR: %s not found",argv[1]);
    return 0;
  }

  Arg2Text(argc-2, &argv[2], buffer,sizeof(buffer));
  status = readHdbValue(&v,buffer,error,sizeof(error));
  if(status == 0){
    SCPrintf(pCon,eError,"ERROR: failed to parse data for %s, error %s", 
	     argv[1], error);
    return 0;
  }
  
  status = sput(argv[1],v);
  if(status == 0){
    SCPrintf(pCon,eError,"ERROR: error setting %s, value invalid?", 
	     argv[1]);
  } else {
    SCSendOK(pCon);
  }

  return status;
}
/*---------------------------------------------------------------------------*/
static int InvokeSICSFunc(void *ms, void *userData)
{
  pParseMessage self = (pParseMessage)ms;
  int status;

  SCsetMacro(pServ->dummyCon,1);
  status = InterpExecute(pServ->pSics, pServ->dummyCon, self->command);
  SCsetMacro(pServ->dummyCon,0);
  if(!status){
    self->success = 0;
    return MPSTOP;
  }
  self->response = strdup(Tcl_GetStringResult(InterpGetTcl(pServ->pSics)));
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int CheckSICSError(void *ms, void *userData)
{
  pParseMessage self = (pParseMessage)ms;

  if(strstr(self->response,"ERROR") != NULL){
    self->success = 0;
    return MPSTOP;
  }
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int SplitOffEqual(void *ms, void *userData)
{
  pParseMessage self = (pParseMessage)ms;
  char *pPtr, *pEQ;

  pPtr = self->response;
  if((pEQ = strstr(pPtr, "=")) != NULL){
    self->response = strdup(trim(pEQ+1));
    free(pPtr);
  }
  return MPCONTINUE;
}
/*---------------------------------------------------------------------------*/
static int isNumber(char *txt)
{
  if(*txt == '\0'){
    return 1;
  }
  if(isalpha(*txt)){
    return 0;
  } else {
    return isNumber(txt+1);
  }
}
/*---------------------------------------------------------------------------*/
static int countWords(char *txt)
{
  char number[80];

  if(txt == NULL){
    return 0;
  } else {
    return 1 + countWords(stptok(txt,number,sizeof(number)," "));
  }
}
/*---------------------------------------------------------------------------*/
static hdbValue makeArray(char *txt, int type, int count)
{
  hdbValue ar;
  char *pPtr;
  char number[80];
  int i = 0;

  if(type == HIPINT){
    ar = makeHdbValue(HIPINTAR,count);
  } else {
    ar = makeHdbValue(HIPFLOATAR,count);
  }
  pPtr = txt;
  while((pPtr = stptok(pPtr,number,sizeof(number)," ")) != NULL){
    if(type == HIPINT){
      ar.v.intArray[i] = atoi(number); 
    } else {
      ar.v.floatArray[i] = atof(number); 
    }
  }
  return ar;
}
/*---------------------------------------------------------------------------*/
static int ReturnAsNumber(void *ms, void *userData)
{
  pParseMessage self = (pParseMessage)ms;
  int type, count;
  hdbValue v;

  if(isNumber(self->response)){
    if(strstr(self->response,".") != NULL){
      type = HIPFLOAT;
    } else {
      type = HIPINT;
    }
    count = countWords(self->response);
    if(count > 1) {
      v = makeArray(self->response, type, count);
    } else {
      if(type == HIPINT){
	v = MakeHdbInt(atoi(self->response));
      } else {
	v = MakeHdbFloat(atof(self->response));
      }
    }
    cloneHdbValue(&v,self->v);
    ReleaseHdbValue(&v);
    self->success = 1;
    return MPSTOP;
  } else {
    return MPCONTINUE;
  }
}
/*---------------------------------------------------------------------------*/
static int ReturnAsText(void *ms, void *userData)
{
  hdbValue v;

  pParseMessage self = (pParseMessage)ms;
  self->success = 1;
  v = MakeHdbText(strdup(self->response));
  cloneHdbValue(&v,self->v);
  ReleaseHdbValue(&v);
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static void configureSICSPipe()
{
  sicsPipe = MakeMP();
  AppendMPFilter(sicsPipe, InvokeSICSFunc, NULL, NULL); 
  AppendMPFilter(sicsPipe, CheckSICSError, NULL, NULL); 
  AppendMPFilter(sicsPipe, SplitOffEqual, NULL, NULL); 
  AppendMPFilter(sicsPipe, ReturnAsNumber, NULL, NULL); 
  AppendMPFilter(sicsPipe, ReturnAsText, NULL, NULL); 
  
}
/*----------------------------------------------------------------------------*/
static int FindTclVar(void *ms, void *userData)
{
  pParseMessage self = (pParseMessage)ms;
  
  self->response = Tcl_GetVar(InterpGetTcl(pServ->pSics),self->command, TCL_GLOBAL_ONLY);
  if(self->response == NULL){
    return MPSTOP;
  }
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static void configureTclPipe()
{
  tclPipe = MakeMP();
  AppendMPFilter(tclPipe, FindTclVar, NULL, NULL); 
  AppendMPFilter(tclPipe, SplitOffEqual, NULL, NULL); 
  AppendMPFilter(tclPipe, ReturnAsNumber, NULL, NULL); 
  AppendMPFilter(tclPipe, ReturnAsText, NULL, NULL); 
  
}
/*----------------------------------------------------------------------------*/
static int GetHdbFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pHdb node = NULL;

  node = FindHdbNode(NULL,self->name,NULL);
  if(node != NULL){
    cloneHdbValue(&node->value, self->v);
    self->success = 1;
    return MPSTOP;
  } else {
    return MPCONTINUE;
  }
}
/*---------------------------------------------------------------------------*/
static int GetSICSFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  ParseMessage ps;

  memset(&ps,0,sizeof(ParseMessage));
  ps.command = strdup(self->name);
  ps.v = self->v;
  MPprocess(sicsPipe,&ps);
  self->success = ps.success;
  free(ps.command);
  if(ps.response != NULL){
    free(ps.response);
  }
  if(self->success){
    return MPSTOP;
  }
  return MPCONTINUE;
}
/*---------------------------------------------------------------------------*/
static int GetTclFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  ParseMessage ps;

  memset(&ps,0,sizeof(ParseMessage));
  ps.command = strdup(self->name);
  ps.v = self->v;
  MPprocess(tclPipe,&ps);
  self->success = ps.success;
  free(ps.command);
  if(self->success){
    return MPSTOP;
  }
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int GetDrivableFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  void *data = NULL;
  pIDrivable pDriv = NULL;
  float fVal;
  hdbValue v;

  data = FindCommandData(pServ->pSics, self->name,NULL);
  if(data != NULL){
    pDriv = GetDrivableInterface(data);
    if(pDriv != NULL){
      fVal = pDriv->GetValue(data,pServ->dummyCon);
      v = MakeHdbFloat(fVal);
      self->success = 1;
      cloneHdbValue(&v,self->v);
      return MPSTOP;
    }
  }
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int GetSicsdataFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pSICSData data = NULL;
  hdbValue v;
  int i;

  data = FindCommandData(pServ->pSics,self->name, "SICSData");
  if(data != NULL){
    if(data->dataType[0] == INTTYPE){
      v = makeHdbValue(HIPINTVARAR, data->dataUsed);
      for(i = 0; i < data->dataUsed; i++){
	v.v.intArray[i] = data->data[i];
      }
    } else {
      v = makeHdbValue(HIPFLOATVARAR, data->dataUsed);
      for(i = 0; i < data->dataUsed; i++){
	v.v.floatArray[i] = (double)data->data[i];
      }
    }
    cloneHdbValue(&v, self->v);
    ReleaseHdbValue(&v);
    self->success = 1;
    return MPSTOP;
  }

  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static void configureGetPipe()
{
  getPipe = MakeMP();
  AppendMPFilter(getPipe, GetHdbFunc, NULL, NULL);
  AppendMPFilter(getPipe, GetDrivableFunc, NULL, NULL);
  AppendMPFilter(getPipe, GetSicsdataFunc, NULL, NULL);
  AppendMPFilter(getPipe, GetSICSFunc, NULL, NULL);
  AppendMPFilter(getPipe, GetTclFunc, NULL, NULL);
}
/*----------------------------------------------------------------------------*/
static int PutHdbFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pHdb node = NULL;
  int status;

  node = FindHdbNode(NULL,self->name,NULL);
  if(node != NULL){
    status = UpdateHipadabaPar(node,*(self->v),NULL);  
    self->success = status;
    if(status == 1){
      return MPSTOP;
    }
  } 
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int PutSICSFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pDynString data = NULL;
  int status;
  char sicsobj[80];
  void *cdata;


  data = formatValue(*(self->v),NULL);
  if(data != NULL){
    DynStringInsert(data," ", 0);
    DynStringInsert(data,self->name, 0);
    status = InterpExecute(pServ->pSics, pServ->dummyCon, GetCharArray(data));
    DeleteDynString(data);
    if(status == 1) {
      self->success = 1;
      return MPSTOP;
    } else {
      /*
	Without this test bad sets on SICS commands may fall 
        through to Tcl where they succedd
      */
      stptok(self->name,sicsobj,sizeof(sicsobj)," ");
      cdata = FindCommandData(pServ->pSics,sicsobj,NULL);
      if(cdata != NULL){
	self->success = 0;
	return MPSTOP;
      }
    }
  } 
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int PutTclFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pDynString data = NULL;
  int status;
  char *pPtr = NULL;

  data = formatValue(*(self->v),NULL);
  if(data != NULL){
    pPtr = Tcl_SetVar(InterpGetTcl(pServ->pSics),self->name,GetCharArray(data),TCL_GLOBAL_ONLY);
    DeleteDynString(data);
    if(pPtr != NULL) {
      self->success = 1;
      return MPSTOP;
    }
  } 
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int PutDrivableFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  void *data = NULL;
  pIDrivable pDriv = NULL;
  float fVal;
  hdbValue v;

  data = FindCommandData(pServ->pSics, self->name,NULL);
  if(data != NULL){
    pDriv = GetDrivableInterface(data);
    if(pDriv != NULL){
      StartMotor(pServ->pExecutor,pServ->pSics, pServ->dummyCon, 
		 self->name, RUNRUN, (float)self->v->v.doubleValue);
      self->success = 1;
      return MPSTOP;
    }
  }
  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static int PutSicsdataFunc(void *ms, void *userData)
{
  pSSGMessage self = (pSSGMessage)ms;
  pSICSData data = NULL;
  int i;

  data = FindCommandData(pServ->pSics,self->name, "SICSData");
  if(data != NULL){
    if(self->v->dataType == HIPINTAR || self->v->dataType == HIPINTVARAR){
      for(i = 0; i < self->v->arrayLength; i++){
	setSICSDataInt(data,i,self->v->v.intArray[i]);
      }
    } else if(self->v->dataType == HIPFLOATAR || self->v->dataType == HIPFLOATVARAR){
      for(i = 0; i < self->v->arrayLength; i++){
	setSICSDataFloat(data,i,(float)self->v->v.floatArray[i]);
      }
    } else {
      return MPSTOP;
    }
    self->success = 1;
    return MPSTOP;
  }

  return MPCONTINUE;
}
/*----------------------------------------------------------------------------*/
static void configureSetPipe()
{
  setPipe = MakeMP();
  AppendMPFilter(setPipe, PutHdbFunc, NULL, NULL);
  AppendMPFilter(setPipe, PutDrivableFunc, NULL, NULL);
  AppendMPFilter(setPipe, PutSicsdataFunc, NULL, NULL);
  AppendMPFilter(setPipe, PutSICSFunc, NULL, NULL);
  AppendMPFilter(setPipe, PutTclFunc, NULL, NULL);
}
/*----------------------------------------------------------------------------*/
void SICSGetInit(void)
{
  if (getPipe == NULL) {
    configureGetPipe();
    configureSetPipe();
    configureSICSPipe();
    configureTclPipe();
    AddCmd("sget", SICSGetCommand);
    AddCmd("sput", SICSPutCommand);
  }
}