/*-------------------------------------------------------------------------
			R e g M o t

  This is  a helper module for the Anti Collider. It handles all the
  stuff necessary for dealing with a single motor. For more
  information see the file anticollider.tex.

  copyright: see file copyright
 
  Mark Koennecke, August 2002
-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h> 
#include "fortify.h"
#include "motreg.h"

/*--------------------------------------------------------------------*/
pMotReg RegisterMotor(char *name, SicsInterp *pSics,
		      long (*NewSetValue)(void *pData, SConnection *pCon,
					  float farget),
		      int (*NewCheckStatus)(void *pData,SConnection *pCon) ){
  CommandList *pCom = NULL;
  pIDrivable pDriv = NULL;
  pMotReg pNew = NULL;

  /*
    find motor data structures
  */
  pCom = FindCommand(pSics,name);
  if(pCom == NULL){
    return NULL;
  }
  pDriv = GetDrivableInterface(pCom->pData);
  if(pDriv == NULL){
    return NULL;
  }

  /*
    everything seems OK. Allocate data structure and initialize 
  */
  pNew = (pMotReg)malloc(sizeof(MotReg));
  if(pNew == NULL){
    return NULL;
  }
  memset(pNew,0,sizeof(MotReg));

  pNew->motorData = pCom->pData;
  pNew->motorName = strdup(name);
  pNew->originalSetValue = pDriv->SetValue;
  pNew->originalCheckStatus = pDriv->CheckStatus;
  pNew->iActive = 0;

  pDriv->SetValue = NewSetValue;
  pDriv->CheckStatus = NewCheckStatus;

  return pNew;
}
/*---------------------------------------------------------------------*/
void KillRegMot(void *pData){
  pMotReg self = (pMotReg)pData;

  if(self == NULL){
    return;
  }
  if(self->motorName != NULL){
    free(self->motorName);
  }
  free(self);
}
/*------------------------------------------------------------------------*/
void SetRegMotTarget(pMotReg self, float fValue){
  assert(self);

  self->targetPosition = fValue;
  self->iActive = 1;
}
/*------------------------------------------------------------------------*/
void CreateTargetString(pMotReg self, char pBueffel[80]) {
  assert(self);

  if(strlen(self->motorName) + 20 < 80) {
    sprintf(pBueffel," %s %12.4f ", self->motorName, self->targetPosition);
  }
}
/*-----------------------------------------------------------------------*/
int RegMotMatch(pMotReg self, char *name){
  assert(self);
  if(strcmp(self->motorName, name) == 0) {
    return 1;
  }
  return 0;
}
/*----------------------------------------------------------------------*/
int StartRegMot(pMotReg self, SConnection *pCon, float fValue){
  int ret;
  long (*oldSet)(void *pmotorData, SConnection *pCon, float fValue);
  pIDrivable pDriv = NULL;
  char pBueffel[132];


  assert(self);
  pDriv = GetDrivableInterface(self->motorData);
  assert(pDriv);
  oldSet = pDriv->SetValue;
  pDriv->SetValue = self->originalSetValue;
  ret = StartDevice(pServ->pExecutor, self->motorName,
		    FindDescriptor(self->motorData),
		    self->motorData,
		    pCon,
		    fValue);
  /*
  sprintf(pBueffel,"anticollision started %s to %f",self->motorName,
	  fValue);
  SCWrite(pCon,pBueffel,eValue);
  */

  pDriv->SetValue = oldSet;
  if(ret == 1){
      self->iActive = 1;
  } else {
      snprintf(pBueffel,131,"ERROR: failed to start motor %s",
        self->motorName);
      SCWrite(pCon,pBueffel,eError);
      self->iActive = 0;
  }
  return ret;
}
/*----------------------------------------------------------------------*/
int CheckRegMot(pMotReg self, SConnection *pCon){
  int stat;

  assert(self);
  if(self->iActive){
    stat = self->originalCheckStatus(self->motorData,pCon);
    if(stat != HWBusy){
      self->iActive = 0;
    }
    return stat;
  } else {
    return HWIdle;
  }
}