/*---------------------------------------------------------------------------
 This is a motor driver which is implemented in Tcl. This means
 this code is only a wrapper which calls Tcl functions to do the 
 actual work.

 The Tcl functions to implement the interface are called with the name
 of the motor as first parameter followed by any additional parameters 
 such as the position to run to for run. Functions have to return the proper
 SICS return codes for a motor driver as integer numbers.

 The Tcl function list is initialized from a Tcl-array which holds function
 names for the entries:
   - getpos
   - run
   - status
   - geterror
   - fixit
  This Tcl-array is passed as parameter on creating the motor. In order to
  facilitate error handling, a motor parameter errorcode is available to
  store errors between invocations.

 copyright: see file COPYRIGHT

 Author: Mark Koennecke, December 2005

 ANSTO MODIFICATIONS:
 Jing Chen, Nov 2012:
   Support extra parameters as calls to Tcl code.

 Ferdi Franceschini April 2013:
   Use prefixes imot_, hdb_, and get_ to tell driver how to handle different
   types of functions.  Added debug info support.

 --------------------------------------------------------------------------*/

#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <time.h>
#include "sics.h"
#include "modriv.h"
#include "lld.h"
#include "stringdict.h"
#include "ansto_tclmotdriv.h"

#define FUNCNOTFOUND -11000
#define TCLERROR     -11001
#define NOTCLRESULT  -11002
#define PARANOTFOUND -11003

#define HARDUPPERLIM "hardupperlim"
#define HARDLOWERLIM "hardlowerlim"
#define LONG_NAME    "long_name"
#define ERRORCODE    "errorcode"

void logTclCall(TCLDriv *pDriv, char *fn, char *script) {
  char msg[512];

  snprintf(msg, 511, "tclmot %s.%s -> %s\n", pDriv->motName, fn, script);
  SICSLogWrite(msg, eLog);
}
/*----------------------------------------------------------------------------*/
static int buildStandardCommandPart(TCLDriv * pDriv, char *command,
                                    char *tclCommand, int commandLen)
{
  char tclFunc[132];
  int status;

  status = StringDictGet(pDriv->mappings, command, tclFunc, 131);
  if (status != 1) {
    return 0;
  }
  snprintf(tclCommand,commandLen,"%s %s ", tclFunc, pDriv->motName);
//snprintf(tclCommand, commandLen, "%s", tclFunc);
  return 1;
}


/*----------------------------------------------------------------------------*/
static int GetTclPos(void *self, float *fPos)
{
  TCLDriv *pDriv;
  int status;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  pDriv->errorCode = 0;
  if (!buildStandardCommandPart(pDriv, "getpos", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    free(tclCommand);
    return HWFault;
  }

  if (pDriv->debug)
    logTclCall(pDriv, "getpos", tclCommand);

  status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
  result = Tcl_GetStringResult(pServ->pSics->pTcl);
  free(tclCommand);
  if (result == NULL) {
    pDriv->errorCode = NOTCLRESULT;
    return HWFault;
  }
  if (status != TCL_OK) {
    pDriv->errorCode = TCLERROR;
    strncpy(pDriv->tclError, result, 1023);
    return HWFault;
  }
  sscanf(result, "%f", fPos);
  return OKOK;
}

/*----------------------------------------------------------------------------*/
static int TclRun(void *self, float fVal)
{
  TCLDriv *pDriv;
  char num[80];
  int status;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  pDriv->errorCode = 0;
  if (!buildStandardCommandPart(pDriv, "run", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    free(tclCommand);
    return HWFault;
  }
  snprintf(num, 79, "%f", fVal);
  strncat(tclCommand, num, 1023 - strlen(tclCommand));

  if (pDriv->debug)
    logTclCall(pDriv, "run", tclCommand);

  status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
  result = Tcl_GetStringResult(pServ->pSics->pTcl);
  free(tclCommand);
  if (result == NULL) {
    pDriv->errorCode = NOTCLRESULT;
    return HWFault;
  }
  if (status != TCL_OK) {
    pDriv->errorCode = TCLERROR;
    strncpy(pDriv->tclError, result, 1023);
    return HWFault;
  }
  sscanf(result, "%d", &status);
  return status;
}

/*-------------------------------------------------------------------------*/
static int evaluateInternalErrors(TCLDriv * pDriv, int *iCode,
                                  char *error, int iErrLen)
{
  switch (pDriv->errorCode) {
  case FUNCNOTFOUND:
    strncpy(error, "Config Error: Tcl function for driver not found",
            iErrLen);
    *iCode = pDriv->errorCode;
    return 1;
    break;
  case TCLERROR:
    strncpy(error, pDriv->tclError, iErrLen);
    *iCode = pDriv->errorCode;
    return 1;
    break;
  case NOTCLRESULT:
    strncpy(error, "Tcl function did not return result", iErrLen);
    *iCode = pDriv->errorCode;
    return 1;
    break;
  default:
    return 0;
    break;
  }
  return 0;
}

/*--------------------------------------------------------------------------*/
static void TclGetError(void *self, int *iCode, char *error, int iErrLen)
{
  TCLDriv *pDriv;
  int status = 1;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  if (evaluateInternalErrors(pDriv, iCode, error, iErrLen) == 1) {
    return;
  }

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  if (!buildStandardCommandPart(pDriv, "geterror", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    status = 0;
  }

  if (status != 0) {
  if (pDriv->debug)
    logTclCall(pDriv, "geterror", tclCommand);

    status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
    result = Tcl_GetStringResult(pServ->pSics->pTcl);
    if (result == NULL) {
      pDriv->errorCode = NOTCLRESULT;
    }
    if (status != TCL_OK && result != NULL) {
      pDriv->errorCode = TCLERROR;
      strncpy(pDriv->tclError, result, 1023);
    }
  }

  if (evaluateInternalErrors(pDriv, iCode, error, iErrLen) == 1) {
    free(tclCommand);
    return;
  }
  free(tclCommand);
  strncpy(error, result, iErrLen);
}

/*---------------------------------------------------------------------------*/
static int TclTryAndFixIt(void *self, int iError, float fNew)
{
  TCLDriv *pDriv;
  char num[80];
  int status;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  if (!buildStandardCommandPart(pDriv, "fixit", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    free(tclCommand);
    return HWFault;
  }
  snprintf(num, 79, "%d %f", pDriv->errorCode, fNew);
  strncat(tclCommand, num, 1023 - strlen(tclCommand));

  if (pDriv->debug)
    logTclCall(pDriv, "fixit", tclCommand);

  status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
  result = Tcl_GetStringResult(pServ->pSics->pTcl);
  if (result == NULL) {
    pDriv->errorCode = NOTCLRESULT;
    free(tclCommand);
    return HWFault;
  }
  if (status != TCL_OK) {
    pDriv->errorCode = TCLERROR;
    strncpy(pDriv->tclError, result, 1023);
    free(tclCommand);
    return HWFault;
  }
  sscanf(result, "%d", &status);
  free(tclCommand);
  return status;
}

/*--------------------------------------------------------------------------*/
static int TclHalt(void *self)
{
  TCLDriv *pDriv;
  int status;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  pDriv->errorCode = 0;

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  if (!buildStandardCommandPart(pDriv, "halt", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    free(tclCommand);
    return HWFault;
  }

  if (pDriv->debug)
    logTclCall(pDriv, "halt", tclCommand);

  status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
  result = Tcl_GetStringResult(pServ->pSics->pTcl);
  if (result == NULL) {
    pDriv->errorCode = NOTCLRESULT;
    free(tclCommand);
    return HWFault;
  }
  if (status != TCL_OK) {
    pDriv->errorCode = TCLERROR;
    strncpy(pDriv->tclError, result, 1023);
    free(tclCommand);
    return HWFault;
  }
  free(tclCommand);
  return OKOK;
}

/*--------------------------------------------------------------------------*/
static int TclGetStatus(void *self)
{
  TCLDriv *pDriv;
  int status;
  const char *result = NULL;

  assert(self);
  pDriv = (TCLDriv *) self;

  pDriv->errorCode = 0;

  char *tclCommand = (char *) calloc(1024, sizeof(char));

  if (!buildStandardCommandPart(pDriv, "status", tclCommand, 1023)) {
    pDriv->errorCode = FUNCNOTFOUND;
    free(tclCommand);
    return HWFault;
  }

  if (pDriv->debug)
    logTclCall(pDriv, "status", tclCommand);

  status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
  result = Tcl_GetStringResult(pServ->pSics->pTcl);
  if (result == NULL) {
    pDriv->errorCode = NOTCLRESULT;
    free(tclCommand);
    return HWFault;
  }
  if (status != TCL_OK) {
    pDriv->errorCode = TCLERROR;
    strncpy(pDriv->tclError, result, 1023);
    free(tclCommand);
    return HWFault;
  }
  sscanf(result, "%d", &status);
  free(tclCommand);
  return status;
}

/*-----------------------------------------------------------------------*/
static int TclSetPar(void *self, SConnection * pCon, char *name,
                     float newValue)
{
/*TODO Skip functions in driveable interface ie "run" etc. */
  int status;
  char *result = NULL, pBuffer[512];

  TCLDriv *pDriv = (TCLDriv *) self;

  assert(self);
  assert(pCon);

  if (strcmp(name, HARDUPPERLIM) == 0) {
    pDriv->fUpper = newValue;
    return 1;
  } else if (strcmp(name, HARDLOWERLIM) == 0) {
    pDriv->fLower = newValue;
    return 1;
  } else if (strcmp(name, ERRORCODE) == 0) {
    pDriv->errorCode = (int) newValue;
    return 1;
  } else if (strcmp(name, "debug") == 0) {
    pDriv->debug = (int) newValue;
    return 1;
/*  } else if (strcmp(name, LONG_NAME) == 0) {
    strncpy(pDriv->long_name, &newValue, 255);
    return 1;*/
  } else {
    char *tclCommand = (char *) calloc(1024, sizeof(char));
    if (StringDictExists(pDriv->mappings, name)) {
      if (!buildStandardCommandPart(pDriv, name, tclCommand, 1023)) {
        pDriv->errorCode = FUNCNOTFOUND;
        free(tclCommand);
        return HWFault;
      }
      char *tclCommandPara = (char *) calloc(1024, sizeof(char));
      sprintf(tclCommandPara, "%s %f", tclCommand, newValue);

      if (pDriv->debug)
        logTclCall(pDriv, name, tclCommand);

      status = Tcl_Eval(pServ->pSics->pTcl, tclCommandPara);
      result = Tcl_GetStringResult(pServ->pSics->pTcl);
      if (result == NULL) {
        pDriv->errorCode = NOTCLRESULT;
        free(tclCommand);
        free(tclCommandPara);;
        return HWFault;
      }
      if (status != TCL_OK) {
        pDriv->errorCode = TCLERROR;
        strncpy(pDriv->tclError, result, 1023);
        free(tclCommand);
        free(tclCommandPara);;
        return HWFault;
      } else {
        snprintf(pBuffer, 511, "%s.%s = %s", pDriv->motName, name, result);
        SCWrite(pCon, pBuffer, eValue);
      }
      free(tclCommand);
      free(tclCommandPara);;
      return OKOK;
    } else {
      pDriv->errorCode = PARANOTFOUND;
      free(tclCommand);
      free(tclCommandPara);;
      return 0;
    }

    free(tclCommand);
    return OKOK;
  }
}

/*-----------------------------------------------------------------------*/
int TclGetPar(void *self, char *name, float *value)
{

  int status;
  const char *result = NULL;

  TCLDriv *pDriv = (TCLDriv *) self;

  assert(self);

  if (strcmp(name, ERRORCODE) == 0) {
    *value = (float) pDriv->errorCode;
    return OKOK;
  } else if (strcmp(name, HARDUPPERLIM) == 0) {
    *value = (float) pDriv->fUpper;
    return OKOK;
  } else if (strcmp(name, HARDLOWERLIM) == 0) {
    *value = (float) pDriv->fLower;
    return OKOK;
  } else if (strcmp(name, "debug") == 0) {
    *value = (float) pDriv->debug;
    return OKOK;
  } else {
    char *tclCommand = (char *) calloc(1024, sizeof(char));
    if (StringDictExists(pDriv->mappings, name)) {
      if (!buildStandardCommandPart(pDriv, name, tclCommand, 1023)) {
        pDriv->errorCode = FUNCNOTFOUND;
        free(tclCommand);
        return HWFault;
      }
      if (pDriv->debug)
        logTclCall(pDriv, name, tclCommand);

      status = Tcl_Eval(pServ->pSics->pTcl, tclCommand);
      result = Tcl_GetStringResult(pServ->pSics->pTcl);
      if (result == NULL) {
        pDriv->errorCode = NOTCLRESULT;
        free(tclCommand);
        return HWFault;
      }
      if (status != TCL_OK) {
        pDriv->errorCode = TCLERROR;
        strncpy(pDriv->tclError, result, 1023);
        free(tclCommand);
        return HWFault;
      }
      sscanf(result, "%f", value);
      free(tclCommand);
      return OKOK;
    } else {
      pDriv->errorCode = PARANOTFOUND;
      free(tclCommand);
      return 0;
    }

    return OKOK;
  }
}

/*
 * Functions which include hdb_, imot_, and get_ are treated specially.
 * hdb_, Parameter will be added to hdb tree.
 * imot_, Motor interface function, eg run, status.
 * get_, Tcl code will be evaluated when "mot list" is called.
 * WARNING: Parameters prefixed with hdb_ will be executed when any of the
 *          parameters in the hdb tree are updated, for instance if the
 *          softzero is changed.
 */
void TclListPar(void *self, char *motorname, SConnection * pCon)
{
  char pBueffel[512];
  char motParVal[256];
  float value;
  const char *motPar=NULL;
  TCLDriv *pDriv = (TCLDriv *) self;
  int pos = 0, imot;


  snprintf(pBueffel, 511, "%s.debug = %d", motorname, pDriv->debug);
  SCWrite(pCon, pBueffel, eValue);
  while ( (motPar = StringDictGetNext(pDriv->mappings,motParVal,256)) != NULL ) {
    imot = 0;
    pBueffel[0] = '\0';
    /* sicshipadaba sets motorname="test" when constructing hdb tree */
    if (strcmp(motorname, "test.") == 0 && strstr(motParVal, "hdb_") == NULL)
      continue;

    if (strstr(motParVal, "imot_"))
      imot = 1;
    if (imot && !pDriv->debug)
      continue;

    pos = snprintf(pBueffel, 511, "%s.%s", motorname, motPar);
    if (pDriv->debug)
      pos += snprintf(&pBueffel[pos], 511, "->[%s]", motParVal);

    if (strstr(motParVal, "get_")) {
      TclGetPar(self, (char *)motPar, &value);
      snprintf(&pBueffel[pos], 511, " = %f", value);
    } else if (imot)
      snprintf(&pBueffel[pos], 511, " (imot)");
    else
      snprintf(&pBueffel[pos], 511, " (command)");

    if (pBueffel[0] != '\0')
      SCWrite(pCon, pBueffel, eValue);
  }
}

/*---------------------------------------------------------------------------*/
void KillTCL(void *pData)
{
  TCLDriv *pDriv = (TCLDriv *) pData;

  if (pDriv != NULL) {
    DeleteStringDict(pDriv->mappings);
  }

  return;
}

/*-------------------------------------------------------------------------*/
static int assignMappings(TCLDriv * pDriv, SConnection * pCon,
                          char *arrayName)
{
  const char *funcName = NULL;
  char *funcText[] = { "getpos",
    "run",
    "status",
    "halt",
    "geterror",
    "fixit",
    NULL
  };
  char error[256];
  int count = 0;

  char **argv;
  int i, code, argc;

  code = Tcl_SplitList(pServ->pSics->pTcl, arrayName, &argc, &argv);
  for (i=0; i < argc; i += 2) {
    StringDictAddPair(pDriv->mappings, argv[i], (char *) argv[i+1]);
  }
  Tcl_Free((char *) argv);
  return 1;
}

/*--------------------------------------------------------------------------*/
MotorDriver *CreateTclMotDriv(SConnection * pCon, int argc, char *argv[])
{
  TCLDriv *pDriv = NULL;

  assert(pCon);

  if (argc < 4) {
    SCWrite(pCon, "ERROR: not enough arguments to initilaize Tcl-driver",
            eError);
    return NULL;
  }

  pDriv = (TCLDriv *) malloc(sizeof(TCLDriv));
  if (!pDriv) {
    SCWrite(pCon, "Error allocating memory in TclMotor", eError);
    return NULL;
  }
  memset(pDriv, 0, sizeof(TCLDriv));
  pDriv->debug = 0;
  pDriv->mappings = CreateStringDict();
  if (pDriv->mappings == NULL) {
    SCWrite(pCon, "Error allocating memory in TclMotor", eError);
    free(pDriv);
    return NULL;
  }
  if (assignMappings(pDriv, pCon, argv[3]) != 1) {
    DeleteStringDict(pDriv->mappings);
    free(pDriv);
    return NULL;
  }

  pDriv->name = strdup("Tcl-Driver");
  strncpy(pDriv->motName, argv[1], 131);
  pDriv->GetPosition = GetTclPos;
  pDriv->RunTo = TclRun;
  pDriv->GetStatus = TclGetStatus;
  pDriv->GetError = TclGetError;
  pDriv->TryAndFixIt = TclTryAndFixIt;
  pDriv->SetDriverPar = TclSetPar;
  pDriv->GetDriverPar = TclGetPar;
  pDriv->ListDriverPar = TclListPar;
  pDriv->Halt = TclHalt;
  pDriv->KillPrivate = KillTCL;

  return (MotorDriver *) pDriv;
}