/*----------------------------------------------------------------------
  This is the implementation file for the AntiCollider, a complex movements
  control module for SICS. See anticollider.tex for more information.
  
  copyright: see file copyright

  Mark Koennecke, August 2002
------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "lld.h"
#include "motreglist.h"
#include "anticollider.i"
#include "anticollider.h"

/*---------------------------------------------------------------------
  As there should be only one AntiCollider in a system, I use a static
  pointer to the AntiCollider here in order to facilitate access.
  Otherwise more complex mechanisms must be devised in order to pass this
  pointer into ColliderSetValue and ColliderCheckStatus
  ----------------------------------------------------------------------*/
static pAntiCollider myCollider = NULL;

/*--------------------------------------------------------------------
  the replacement function for the motor's drivable interface SetValue
  function. It enters the new target into the motor list.
  ---------------------------------------------------------------------*/
static long ReplacementSetValue(void *pData, SConnection * pCon,
                                float fTarget)
{
  pMotReg pMot = NULL;

  assert(myCollider != NULL);

  pMot = FindMotFromDataStructure(myCollider->motorList, pData);
  if (pMot != NULL) {
    SetRegMotTarget(pMot, fTarget);
    myCollider->isDirty = 1;
  } else {
    return 0;
  }

  return 1;
}

/*---------------------------------------------------------------------
  The replacement CheckStatus function for controlled motors.
  Start AntiCollider if not running and finish. Rest of work done by
  AntiCollider.
  -----------------------------------------------------------------------*/
static int ReplacementCheckStatus(void *pData, SConnection * pCon)
{
  pMotReg pMot = NULL;

  assert(myCollider != NULL);

  if (myCollider->isDirty == 1) {
    myCollider->isDirty = 0;
    StartDevice(pServ->pExecutor,
                "anticollider", myCollider->pDes, myCollider, 
                pCon,pCon->runLevel,  77.77);
    return HWIdle;
  } else {
    return HWIdle;
  }
}

/*------------------------------------------------------------------------
  The collider SetValue function
  -------------------------------------------------------------------------*/
static long ColliderSetValue(void *pData, SConnection * pCon,
                             float fTarget)
{
  pAntiCollider self = (pAntiCollider) pData;
  int iRet;
  pMotReg pMot = NULL;
  char pBueffel[80];
  char *ruenBuffer = NULL;

  Tcl_DString command;

  /*
     build command list
   */
  if (self->colliderScript == NULL) {
    SCWrite(pCon, "ERROR: no collider script defined", eError);
    return 0;
  }

  Tcl_DStringInit(&command);
  Tcl_DStringAppend(&command, self->colliderScript, -1);
  iRet = LLDnodePtr2First(self->motorList);
  while (iRet != 0) {
    LLDnodeDataTo(self->motorList, &pMot);
    if (pMot != NULL) {
      if (pMot->iActive) {
        CreateTargetString(pMot, pBueffel);
        Tcl_DStringAppend(&command, pBueffel, -1);
        pMot->iActive = 0;
      }
    }
    iRet = LLDnodePtr2Next(self->motorList);
  }

  /*
     kill old collider sequence
   */
  LLDdelete(self->sequenceList);
  self->sequenceList = LLDcreate(sizeof(Sequence));
  self->level = -1;             /* otherwise level 0 will not be started */

  /*
     evaluate colliderScript
   */
  iRet = Tcl_Eval(pServ->pSics->pTcl, Tcl_DStringValue(&command));
  if (iRet != TCL_OK) {
    SCWrite(pCon, "ERROR: Movement not possible or bad collider script",
            eError);
    SCPrintf(pCon,eError, "%s returned %s", Tcl_DStringValue(&command), Tcl_GetStringResult(pServ->pSics->pTcl));
    /*
       SCWrite(pCon,pServ->pSics->pTcl->result,eError);
     */
    SCSetInterrupt(pCon, eAbortOperation);
    return 0;
  }

  /*
     we are set
   */
  Tcl_DStringFree(&command);
  return 1;
}

/*----------------------------------------------------------------------
  The Collider CheckStatus function
  -----------------------------------------------------------------------*/
static int ColliderCheckStatus(void *pData, SConnection * pCon)
{
  int count = 0;
  pAntiCollider self = (pAntiCollider) pData;

  assert(self);

  if (SCGetInterrupt(pCon) != eContinue) {
    return HWIdle;
  }

  count = CheckAllMotors(self->motorList, pCon);
  if (count == 0) {
    self->level++;
    count = StartLevel(self->level,
                       self->sequenceList, self->motorList, pCon);
    if (count == 0) {
      /*
         no more levels. All done
       */
      return HWIdle;
    } else {
      return HWBusy;
    }
  } else {
    return HWBusy;
  }
}

/*----------------------------------------------------------------------
  Most of these are dummies........
  -----------------------------------------------------------------------*/
static int ColliderHalt(void *pData)
{
  pAntiCollider self = (pAntiCollider) pData;

  StopAllMotors(self->motorList);
  self->level = 999999999;
  return 1;
}

/*---------------------------------------------------------------------*/
static int ColliderLimits(void *self, float fVal, char *error, int iErren)
{
  return 1;
}

/*--------------------------------------------------------------------*/
static float ColliderGetValue(void *self, SConnection * pCon)
{
  return 77.77;
}

/*--------------------------------------------------------------------*/
int StartLevel(int level, int sequenceList, int motorList,
               SConnection * pCon)
{
  Sequence seq;
  pMotReg pMot = NULL;
  int iRet, status;
  int count = 0;
  char pBueffel[132];

  iRet = LLDnodePtr2First(sequenceList);
  while (iRet != 0) {
    LLDnodeDataTo(sequenceList, &seq);
    if (seq.level == level) {
      pMot = FindMotEntry(motorList, seq.pMotor);
      if (pMot) {
        status = StartRegMot(pMot, pCon, seq.target);
        /*
         * I have to ignore the problem here: if I do not increment the count
         * all the other levels will not be drive and the anticollider
         * gets into a mess
         */
        count++;
      } else {
        snprintf(pBueffel,131,
                "ERROR: motor %s, requested from anticollider script",
                seq.pMotor);
        SCWrite(pCon, pBueffel, eError);
        SCWrite(pCon, "ERROR: motor NOT found, fix script!", eError);
      }
    }
    iRet = LLDnodePtr2Next(sequenceList);
  }
  return count;
}

/*--------------------------------------------------------------------*/
static void ListSequence(int sequenceList, SConnection * pCon)
{
  Sequence seq;
  int iRet;
  char pBueffel[132];

  SCWrite(pCon, "level     motor target", eValue);
  iRet = LLDnodePtr2First(sequenceList);
  while (iRet != 0) {
    LLDnodeDataTo(sequenceList, &seq);
    snprintf(pBueffel,131, "%d %s %f", seq.level, seq.pMotor, seq.target);
    SCWrite(pCon, pBueffel, eValue);
    iRet = LLDnodePtr2Next(sequenceList);
  }
}

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

  self = (pAntiCollider) pData;
  assert(self);

  if (iID == DRIVEID) {
    return self->pDriv;
  }
  return NULL;
}

/*----------------------------------------------------------------------*/
void KillCollider(void *pData)
{
  pAntiCollider self = (pAntiCollider) pData;

  if (self == NULL) {
    return;
  }

  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
  }
  if (self->pDriv != NULL) {
    free(self->pDriv);
  }
  if (self->colliderScript != NULL) {
    free(self->colliderScript);
  }
  if (self->motorList > 0) {
    KillMotList(self->motorList);
  }
  if (self->sequenceList > 0) {
    LLDdelete(self->sequenceList);
  }
  free(self);
  myCollider = NULL;
}

/*-----------------------------------------------------------------------*/
int AntiColliderFactory(SConnection * pCon, SicsInterp * pSics,
                        void *pData, int argc, char *argv[])
{

  myCollider = (pAntiCollider) malloc(sizeof(AntiCollider));
  if (myCollider == NULL) {
    SCWrite(pCon, "ERROR: out of memory when generating AntiCollider",
            eError);
    return 0;
  }
  memset(myCollider, 0, sizeof(AntiCollider));

  myCollider->pDes = CreateDescriptor("AntiCollider");
  myCollider->pDriv = CreateDrivableInterface();
  if (!myCollider->pDes || !myCollider->pDriv) {
    KillCollider(myCollider);
    SCWrite(pCon, "ERROR: out of memory when generating AntiCollider",
            eError);
    return 0;
  }

  myCollider->pDes->GetInterface = ColliderGetInterface;
  myCollider->pDriv->Halt = ColliderHalt;
  myCollider->pDriv->CheckLimits = ColliderLimits;
  myCollider->pDriv->SetValue = ColliderSetValue;
  myCollider->pDriv->CheckStatus = ColliderCheckStatus;
  myCollider->pDriv->GetValue = ColliderGetValue;

  myCollider->motorList = LLDcreate(sizeof(void *));
  myCollider->sequenceList = LLDcreate(sizeof(Sequence));

  AddCommand(pSics, "anticollision", AntiColliderAction,
             KillCollider, myCollider);

  return 1;
}

/*------------------------------------------------------------------------*/
int AntiColliderAction(SConnection * pCon, SicsInterp * pSics,
                       void *pData, int argc, char *argv[])
{
  pAntiCollider self = (pAntiCollider) pData;
  Sequence seq;
  char pBueffel[256];
  pMotReg pMot = NULL;

  assert(self != NULL);

  if (argc > 1) {
    if (strcmp(argv[1], "clear") == 0) {
      if (!SCMatchRights(pCon, usUser)) {
        return 0;
      }
      LLDdelete(self->sequenceList);
      self->sequenceList = LLDcreate(sizeof(Sequence));
      SCSendOK(pCon);
      return 1;
    } else if (strcmp(argv[1], "list") == 0) {
      ListSequence(self->sequenceList, pCon);
      return 1;
    }
  }

  if (argc < 3) {
    SCWrite(pCon,
            "ERROR : insufficient number of arguments to anticollision",
            eError);
    return 0;
  }

  strtolower(argv[1]);
  if (strcmp(argv[1], "script") == 0) {
    if (!SCMatchRights(pCon, usMugger)) {
      return 0;
    }
    if (self->colliderScript != NULL) {
      free(self->colliderScript);
    }
    self->colliderScript = strdup(argv[2]);
    SCSendOK(pCon);
    return 1;
  } else if (strcmp(argv[1], "register") == 0) {
    if (!SCMatchRights(pCon, usMugger)) {
      return 0;
    }
    if (FindDrivable(pSics, argv[2]) == NULL) {
      snprintf(pBueffel,255, "ERROR: %s is NOT drivable, cannot register",
              argv[2]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    pMot = RegisterMotor(argv[2], pSics,
                         ReplacementSetValue, ReplacementCheckStatus);
    if (pMot) {
      LLDnodeAppendFrom(self->motorList, &pMot);
      SCSendOK(pCon);
      return 1;
    } else {
      SCWrite(pCon, "ERROR: out of memory registering motor", eError);
      return 0;
    }
  } else if (strcmp(argv[1], "add") == 0) {
    if (argc < 5) {
      SCWrite(pCon,
              "ERROR: Insufficient number of arguments to anticollicion add",
              eError);
      return 0;
    }
    seq.level = atoi(argv[2]);
    strlcpy(seq.pMotor, argv[3], 79);
    seq.target = atof(argv[4]);
    LLDnodeAppendFrom(self->sequenceList, &seq);
    SCSendOK(pCon);
    return 1;
  }
  SCWrite(pCon, "ERROR: anticollider command not understood", eError);
  return 0;
}