/*----------------------------------------------------------------------
  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,
		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);
    SCWrite(pCon,Tcl_DStringValue(&command),eError);
    /*
      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 {
	    sprintf(pBueffel,"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);
    sprintf(pBueffel,"%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){
      sprintf(pBueffel,"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]);
    strncpy(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;
}