/*-------------------------------------------------------------------------
			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, SCGetRunLevel(pCon), fValue);
  /*
     snprintf(pBueffel,sizeof(pBueffel)-1,"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;
  }
}