/*---------------------------------------------------------------------
 SICS interface to the triple axis drivable motors.

 copyright: see file COPYRIGHT

 Mark Koennecke, May 2005


 Modified to rather use drivables then motors

 Mark Koennecke, September 2011

 Modified to use drive tasks

 Mark Koennecke, January 2014
--------------------------------------------------------------------*/
#include <assert.h>
#include "sics.h"
#include "tasdrive.h"

#define ABS(x) (x < 0 ? -(x) : (x))

#define MOTPREC .01
/*------------------- motor indexes in motor data structure ---------
  WARNING: have to match tasub.c !!!!!
--------------------------------------------------------------------*/
#define A1   0
#define A2   1
#define MCV  2
#define MCH  3
#define A3   4
#define A4   5
#define SGU  6
#define SGL  7
#define A5   8
#define A6   9
#define ACV 10
#define ACH 11

/*============== Drivable Interface functions =====================*/
static long TASSetValue(void *pData, SConnection * pCon, float value)
{
  ptasMot self = (ptasMot) pData;
  assert(self);
  int qcodes[] = {3,4,5,8}; /* qh,qk,ql, en */
  int i;

  if (self->code > 5 && self->math->tasMode == ELASTIC) {
    SCWrite(pCon, "ERROR: cannot drive this motor in elastic mode",
            eLogError);
    return HWFault;
  }
  setTasPar(&self->math->target, self->math->tasMode, self->code, value);
  self->math->mustDrive = 1;
  for(i = 0; i < 4; i++){
    if(self->code == qcodes[i]) {
      self->math->mustDriveQ = 1;
    }
  }
  return OKOK;
}

/*----------------------------------------------------------------*/
int readTASMotAngles(ptasUB self, SConnection * pCon,
                            ptasAngles ang)
{
  int status;
  float val, theta;

  /*
     Monochromator
   */
  status = GetDrivablePosition(self->motors[A1], pCon, &val);
  if (status == 0) {
    return status;
  }
  theta = val;/* 	$Id: tasdrive.c,v 1.39 2014/02/18 13:25:32 koennecke Exp $	 */

#ifndef lint
static char vcid[] = "$Id: tasdrive.c,v 1.39 2014/02/18 13:25:32 koennecke Exp $";
#endif /* lint */

  status = GetDrivablePosition(self->motors[A2], pCon, &val);
  if (status == 0) {
    return status;
  }
  ang->monochromator_two_theta = val;
  if (ABS(val / 2. - theta) > .1) {
    if(DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
      SCWrite(pCon, "WARNING: theta monochromator not half of two theta",
	      eWarning);
    }
  }

  /*
     Analyzer
   */
  if (self->tasMode != ELASTIC) {
    status = GetDrivablePosition(self->motors[A5], pCon, &val);
    if (status == 0) {
      return status;
    }
    theta = val;
    status = GetDrivablePosition(self->motors[A6], pCon, &val);
    if (status == 0) {
      return status;
    }
    ang->analyzer_two_theta = val;
    if (ABS(val / 2. - theta) > .1 && DevExecLevelRunning(pServ->pExecutor,RUNRUN) != 1) {
      SCWrite(pCon, "WARNING: theta analyzer not half of two theta",
              eWarning);
    }
  } else {
    ang->analyzer_two_theta = ang->monochromator_two_theta;
  }

  /*
     crystal
   */
  status = GetDrivablePosition(self->motors[A3], pCon, &val);
  if (status == 0) {
    return status;
  }
  ang->a3 = val;
  status = GetDrivablePosition(self->motors[A4], pCon, &val);
  if (status == 0) {
    return status;
  }
  ang->sample_two_theta = val;
  status = GetDrivablePosition(self->motors[SGU], pCon, &val);
  if (status == 0) {
    return status;
  }
  ang->sgu = val;
  status = GetDrivablePosition(self->motors[SGL], pCon, &val);
  if (status == 0) {
    return status;
  }
  ang->sgl = val;
  if(!self->outOfPlaneAllowed){
    ang->sgu = .0;
    ang->sgl = .0;
  }
  return 1;
}

/*-------------------------------------------------------------*/
static float TASGetValue(void *pData, SConnection * pCon)
{
  ptasMot self = (ptasMot) pData;
  int status;
  tasAngles angles;
  double val;

  assert(self);

  if (self->math->mustRecalculate == 1) {
    status = readTASMotAngles(self->math, pCon, &angles);
    if (status != 1) {
      return -999.99;
    }
    status =
        calcTasQEPosition(&self->math->machine, angles,
                          &self->math->current);
    if (status < 0) {
      SCWrite(pCon, "ERROR: out of memory calculating Q-E variables",
              eLogError);
      return -999.99;
    }
    if (self->math->tasMode == ELASTIC) {
      self->math->current.kf = self->math->current.ki;
    }
    self->math->mustRecalculate = 0;
  }
  val = getTasPar(self->math->current, self->code);
  if (self->code > 5 && self->math->tasMode == ELASTIC) {
    SCWrite(pCon,
            "WARNING: value for this motor is meaningless in elastic mode",
            eWarning);
  }
  return (float) val;
}

/*-----------------------------------------------------------------*/
static float TASQMGetValue(void *pData, SConnection * pCon)
{
  ptasMot self = (ptasMot) pData;
  int status;
  tasAngles angles;
  double val;

  assert(self);

  if (self->math->mustRecalculate == 1) {
    status = readTASMotAngles(self->math, pCon, &angles);
    if (status != 1) {
      return -999.99;
    }
    status = calcTasPowderPosition(&self->math->machine,
                                   angles, &self->math->current);
    if (status < 0) {
      SCWrite(pCon, "ERROR: out of memory calculating Q-E variables",
              eLogError);
      return -999.99;
    }
    if (self->math->tasMode == ELASTIC) {
      self->math->current.kf = self->math->current.ki;
    }
    self->math->mustRecalculate = 0;
  }
  val = getTasPar(self->math->current, self->code);
  return (float) val;
}

/*---------------------------------------------------------------------------------*/
static int TASCheckLimits(void *pData, float fVal, char *error,
                          int iErrLen)
{
  /*
     There is no meaningful implementation here. This gets called when starting the motor.
     At that stage not all other values may be known. If the calculation fails, 
     this will die at status check time.
   */
  return 1;
}

/*---------------------------------------------------------------------------------*/
static int TASHalt(void *pData)
{
  ptasMot self = (ptasMot) pData;
  int i, length = 12;
  pIDrivable pDriv = NULL;

  assert(self);
  /**
   * disregard non existing analyzer motors
   */
  if (self->math->tasMode == ELASTIC) {
    length = 8;
  }
  
  /*
    stop monochromator
  */
  self->math->mono->Halt(self->math->monoData);

  /*
    stop rest
  */
  for (i = 4; i < length; i++) {
    if (self->math->motors[i] != NULL) {
      pDriv = GetDrivableInterface(self->math->motors[i]);
      pDriv->Halt(self->math->motors[i]);
    }
  }
  return 1;
}

/*--------------------------------------------------------------------------*/
static void writeMotPos(SConnection * pCon, int silent, char *name,
                        float val, float target)
{
  char pBueffel[132];

  if (silent != 1) {
    snprintf(pBueffel, 131, "Driving %5s from %8.3f to %8.3f",
             name, val, target);
    SCWrite(pCon, pBueffel, eLog);
  }
}

/*--------------------------------------------------------------------------*/
static float getMotorValue(pMotor mot, SConnection * pCon)
{
  float val;

  GetDrivablePosition(mot, pCon, &val);
  return val;
}
/*--------------------- In devexec.c --------------------------------------*/
void InvokeNewTarget(pExeList self, char *name, float target);
/*--------------------------------------------------------------------------*/
static int startTASMotor(pMotor mot, SConnection * pCon, char *name,
                         double target, int silent, int stopFixed, long groupID)
{
  float val, fixed, precision = MOTPREC;
  int status = OKOK;
  char buffer[132];
  pIDrivable pDriv = NULL;
  pDummy dum = NULL;
  pDynString mes = NULL;
  long taskID;

  dum = (pDummy)mot;
  val = getMotorValue(mot, pCon);
  if(strcmp(dum->pDescriptor->name,"Motor") == 0){
    MotorGetPar(mot,"precision",&precision);
    MotorGetPar(mot, "fixed", &fixed);
    if (ABS(fixed - 1.0) < .1) {
      if(stopFixed == 0){
	snprintf(buffer, 131, "WARNING: %s is FIXED", name);
	SCWrite(pCon, buffer, eLog);
      }
      return OKOK;
    }
  }
  mot->stopped = 0;
  if (ABS(val - target) > precision) {
    taskID = StartDriveTask(mot,pCon,name,target);
    if(taskID < 0){
      SCPrintf(pCon,eLog,"ERROR: failed to drive %s to %f", name, target); 
    } else {
      AddTaskToGroup(pServ->pTasker,taskID, groupID);
    }
    /*
      to force updates on targets
    */
    InvokeNewTarget(pServ->pExecutor, name, target);
    writeMotPos(pCon, silent, name, val, target);
  }
  return status;
}
/*-------------------------------------------------------------------------*/
static int StartTASA3(pMotor mot, SConnection *pCon, char *name, 
		      double target, int silent, int stopFixed, 
		      long groupID)
{
  float val;
  int status;
  char buffer[80];

  val = getMotorValue(mot,pCon);
  if(ABS(target-val) > 190) {
    SCPrintf(pCon,eLog,"WARNING: driving a3 more then 190 degree: %f, please confirm with y", 
	     ABS(target-val));
    status = SCPromptTMO(pCon,NULL,buffer,sizeof(buffer),120);
    if(status == 1){
      if(buffer[0] == 'y'){
	return startTASMotor(mot,pCon,name,target,silent,stopFixed,groupID);
      }
    }
    SCWrite(pCon,"ERROR: no or wrong reply, aborting scan",eLogError);
    SCSetInterrupt(pCon,eAbortScan);
    return HWFault;
  } else {
    return startTASMotor(mot,pCon,name,target,silent,stopFixed,groupID);
  }
}
/*---------------------------------------------------------------------------*/
static int startMotors(ptasMot self, tasAngles angles,
                       SConnection * pCon, int driveQ, int driveTilt)
{
  double curve;
  int status, silent, stopFixed;
  long monoID;

  silent = self->math->silent;
  stopFixed = self->math->stopFixed;
  self->math->mustRecalculate = 1;

  self->math->groupID = GetTaskGroupID(pServ->pTasker);

  /*
     monochromator
   */
  status = self->math->mono->SetValue(self->math->monoData,
  				      pCon,angles.monochromator_two_theta);
  /*
    The call to CheckStatus is necessary because the eiger monochromator may not
    start until then. Deferred until all parameters are known.
   */
  self->math->mono->CheckStatus(self->math->monoData,pCon);
  if(status != OKOK){
    return status;
  } else {
    AddTaskToGroup(pServ->pTasker, self->math->monoTaskID, self->math->groupID);
  }
  /* monoID = StartDriveTask(self->math->monoData, pCon,"mono",  */
  /* 			  angles.monochromator_two_theta); */
  /* self->math->mono->CheckStatus(self->math->monoData,pCon);  */
  /* if(monoID < 0){ */
  /*   SCWrite(pCon,"ERROR: failed to start monochromator",eLogError); */
  /* } else { */
  /*     AddTaskToGroup(pServ->pTasker,monoID, self->math->groupID); */
  /* } */

  /*
     analyzer
   */
  if (self->math->tasMode != ELASTIC) {
    status = startTASMotor(self->math->motors[A5], pCon, "a5",
                           angles.analyzer_two_theta / 2.0, silent,stopFixed,self->math->groupID);
    if (status != OKOK) {
      return status;
    }
    status = startTASMotor(self->math->motors[A6], pCon, "a6",
                           angles.analyzer_two_theta, silent,stopFixed,self->math->groupID);
    if (status != OKOK) {
      return status;
    }

    if (self->math->motors[ACV] != NULL) {
      curve = maCalcVerticalCurvature(self->math->machine.analyzer,
                                      angles.analyzer_two_theta);
      status = startTASMotor(self->math->motors[ACV], pCon, "acv",
                             curve, silent,stopFixed,self->math->groupID);
      if (status != OKOK) {
          SCWrite(pCon,"WARNING: analyzer vertical curvature motor failed to start", eLog);
	  SCSetInterrupt(pCon,eContinue);
      }
    }
    if (self->math->motors[ACH] != NULL) {
      curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
                                        angles.analyzer_two_theta);
      status = startTASMotor(self->math->motors[ACH], pCon, "ach",
                             curve, silent,stopFixed,self->math->groupID);
      if (status != OKOK) {
          SCWrite(pCon,"WARNING: analyzer horizontal curvature motor failed to start", eLog);
	  SCSetInterrupt(pCon,eContinue);
      }
    }
  }

  if (driveQ == 0) {
    return OKOK;
  }

  /*
     crystal
   */
  status = StartTASA3(self->math->motors[A3], pCon, "a3",
		      angles.a3, silent,stopFixed,self->math->groupID);
  if (status != OKOK) {
    return status;
  }
  status = startTASMotor(self->math->motors[A4], pCon, "a4",
                         angles.sample_two_theta, silent,stopFixed,self->math->groupID);
  if (status != OKOK) {
    return status;
  }

  if (driveTilt == 1) {
    status = startTASMotor(self->math->motors[SGL], pCon, "sgl",
                           angles.sgl, silent,stopFixed, self->math->groupID);
    if (status != OKOK) {
      return status;
    }
    status = startTASMotor(self->math->motors[SGU], pCon, "sgu",
                           angles.sgu, silent,stopFixed,self->math->groupID);
    if (status != OKOK) {
      return status;
    }
  }
  self->math->mustDrive = 0;
  return OKOK;
}

/*---------------------------------------------------------------------------*/
static int checkQMotorLimits(ptasMot self, SConnection * pCon,
                             tasAngles angles, int driveTilt)
{
  int status, retVal = 1;
  char error[131];
  char pBueffel[256];
  float val;
  pIDrivable pDrivInt = NULL;

  MotorGetPar(self->math->motors[A3], "fixed", &val);
  if ((int) val != 1) {
	pDrivInt = GetDrivableInterface(self->math->motors[A3]);
    status = pDrivInt->CheckLimits(self->math->motors[A3],
                                angles.a3, error,131);
    if (status != 1) {
      retVal = 0;
      snprintf(pBueffel, 256, "ERROR: limit violation an a3: %s", error);
      SCWrite(pCon, pBueffel, eLogError);
    }
  }

  pDrivInt = GetDrivableInterface(self->math->motors[A4]);
  status = pDrivInt->CheckLimits(self->math->motors[A4],
                                                    angles.
                                                    sample_two_theta,
                                                    error, 131);
  if (status != 1) {
    retVal = 0;
    snprintf(pBueffel, 256, "ERROR: limit violation an a4: %s", error);
    SCWrite(pCon, pBueffel, eLogError);
  }

  if (driveTilt == 1) {
	pDrivInt = GetDrivableInterface(self->math->motors[SGU]);
    status = pDrivInt->CheckLimits(self->math->motors[SGU],
                                                      angles.sgu, error,
                                                      131);
    if (status != 1) {
      retVal = 0;
      snprintf(pBueffel, 256, "ERROR: limit violation an SGU: %s", error);
      SCWrite(pCon, pBueffel, eLogError);
    }

    pDrivInt = GetDrivableInterface(self->math->motors[SGL]);
    status = pDrivInt->CheckLimits(self->math->motors[SGL],
                                                       angles.sgl, error,
                                                       131);
    if (status != 1) {
      retVal = 0;
      snprintf(pBueffel, 256, "ERROR: limit violation an SGL: %s", error);
      SCWrite(pCon, pBueffel, eLogError);
    }
  }
  return retVal;
}
/*-----------------------------------------------------------------------------*/
static int calculateAndDrive(ptasMot self, SConnection * pCon)
{
  tasAngles angles;
  int status, driveQ = 1, driveTilt = 1;
  MATRIX scatteringPlaneNormal = NULL;

  if (self->math->ubValid == 0) {
    SCWrite(pCon, "WARNING: UB matrix invalid", eWarning);
  }

  status = calcAllTasAngles(&self->math->machine, self->math->target,
                            &angles);
  self->math->mustDrive = 0;

  switch (status) {
  case ENERGYTOBIG:
	SCWrite(pCon, "ERROR: desired energy to big", eLogError);
	return HWFault;
	break;
  case UBNOMEMORY:
	SCWrite(pCon, "ERROR: out of memory calculating angles", eLogError);
	driveQ = 0;
	break;
  case BADRMATRIX:
	SCWrite(pCon, "ERROR: bad crystallographic parameters or bad UB",
			eLogError);
	driveQ = 0;
	break;
  case BADUBORQ:
	SCWrite(pCon, "ERROR: bad UB matrix or bad Q-vector", eLogError);
	driveQ = 0;
	break;
  case TRIANGLENOTCLOSED:
	SCWrite(pCon, "ERROR: cannot close scattering triangle", eLogError);
	driveQ = 0;
	break;
  }
  /**
   * check out of plane condition and permission
   */
  if (self->math->outOfPlaneAllowed != 1) {
    scatteringPlaneNormal = calcScatteringPlaneNormal(self->math->r1.qe,
                                                      self->math->r2.qe);
    if (scatteringPlaneNormal == NULL) {
      SCWrite(pCon, "ERROR: unable to calculate scattering plane normal",
              eError);
      driveQ = 0;
    } else {
      if (!isInPlane(scatteringPlaneNormal, self->math->target)) {
        SCWrite(pCon,
                "WARNING: scattering vector is out of plane and you disallowed me to try to drive there",
                eWarning);
      }
      mat_free(scatteringPlaneNormal);
    }
    driveTilt = 0;
  }

  if(self->math->mustDriveQ == 0){
    driveQ = 0;
  } 


  if (driveQ != 0) {
    if (!checkQMotorLimits(self, pCon, angles, driveTilt)) {
      driveQ = 0;
    }
  }

  if (driveQ == 0 && self->math->mustDriveQ == 1) {
    SCWrite(pCon, "WARNING: NOT driving Q-vector because of errors",
            eLogError);
  }
  /* 
   * reset the Q flag
   */
  self->math->mustDriveQ = 0;
  return startMotors(self, angles, pCon, driveQ, driveTilt);
}

/*-----------------------------------------------------------------------------*/
static int checkMotors(ptasMot self, SConnection * pCon)
{
  self->math->mustRecalculate = 1;

  if(isTaskGroupRunning(pServ->pTasker,self->math->groupID) ||
     isTaskGroupRunning(pServ->pTasker,self->math->monoTaskID)){
    return HWBusy;
  } else {
    return HWIdle;
  }
}

/*------------------------------------------------------------------------------*/
static int TASGetStatus(void *pData, SConnection * pCon)
{
  ptasMot self = (ptasMot) pData;
  int status;
  assert(self);

  if (self->math->mustDrive == 1) {
    status = calculateAndDrive(self, pCon);
    if (status != OKOK) {
      TASHalt(pData);
      return HWBusy;
    } else {
      return HWBusy;
    }
  } else {
    return checkMotors(self, pCon);
  }
}

/*---------------------------------------------------------------------------*/
static int startQMMotors(ptasMot self, tasAngles angles,
                         SConnection * pCon)
{
  float val;
  double curve;
  int status, silent, stopFixed;

  silent = self->math->silent;
  stopFixed = self->math->stopFixed;

  self->math->groupID = GetTaskGroupID(pServ->pTasker);

  /*
    monochromator
  */
  status = self->math->mono->SetValue(self->math->monoData,
				      pCon,angles.monochromator_two_theta);
  if(status != OKOK){
    return status;
  } else {
    AddTaskToGroup(pServ->pTasker, self->math->monoTaskID, self->math->groupID);
  }


  /*
     analyzer
   */
  status = startTASMotor(self->math->motors[A5], pCon, "a5",
                         angles.analyzer_two_theta / 2.0, silent,stopFixed,self->math->groupID);
  if (status != OKOK) {
    return status;
  }
  status = startTASMotor(self->math->motors[A6], pCon, "a6",
                         angles.analyzer_two_theta, silent,stopFixed,self->math->groupID);
  if (status != OKOK) {
    return status;
  }

  if (self->math->motors[ACV] != NULL) {
    curve = maCalcVerticalCurvature(self->math->machine.analyzer,
                                    angles.analyzer_two_theta);
    status = startTASMotor(self->math->motors[ACV], pCon, "acv",
                           curve, silent,stopFixed,self->math->groupID);
    if (status != OKOK) {
      return status;
    }
  }
  if (self->math->motors[ACH] != NULL) {
    curve = maCalcHorizontalCurvature(self->math->machine.analyzer,
                                      angles.analyzer_two_theta);
    status = startTASMotor(self->math->motors[ACH], pCon, "ach",
                           curve, silent,stopFixed,self->math->groupID);
    if (status != OKOK) {
      return status;
    }
  }

  /*
     crystal
   */
  status = startTASMotor(self->math->motors[A4], pCon, "a4",
                         angles.sample_two_theta, silent,stopFixed,self->math->groupID);
  if (status != OKOK) {
    return status;
  }

  self->math->mustDrive = 0;
  return OKOK;
}

/*-----------------------------------------------------------------------------*/
static int calculateQMAndDrive(ptasMot self, SConnection * pCon)
{
  tasAngles angles;
  int status;

  status = calcTasPowderAngles(&self->math->machine, self->math->target,
                               &angles);
  switch (status) {
  case ENERGYTOBIG:
    SCWrite(pCon, "ERROR: desired energy to big", eLogError);
    return HWFault;
    break;
  case TRIANGLENOTCLOSED:
    SCWrite(pCon, "ERROR: cannot close scattering triangle", eLogError);
    return HWFault;
    break;
  default:
    return startQMMotors(self, angles, pCon);
  }
  return HWFault;
}

/*------------------------------------------------------------------------------*/
static int TASQMGetStatus(void *pData, SConnection * pCon)
{
  ptasMot self = (ptasMot) pData;
  int status;
  assert(self);

  if (self->math->mustDrive == 1) {
    status = calculateQMAndDrive(self, pCon);
    if (status != OKOK) {
      return HWFault;
    } else {
      return HWBusy;
    }
  } else {
    return checkMotors(self, pCon);
  }
}

/*================== Life and Death =========================================*/
static void KillTasMot(void *pData)
{
  ptasMot self = (ptasMot) pData;
  if (self == NULL) {
    return;
  }

  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
    self->pDes = NULL;
  }
  if (self->pDriv != NULL) {
    free(self->pDriv);
    self->pDriv = NULL;
  }
  free(self);
}

/*---------------------------------------------------------------------------*/
static void *GetTasInterface(void *pData, int interfaceID)
{
  ptasMot self = (ptasMot) pData;

  if (self != NULL && interfaceID == DRIVEID) {
    return self->pDriv;
  } else {
    return NULL;
  }
}

/*-------------------------------------------------------------------------*/
int InstallTasMotor(SicsInterp * pSics, ptasUB self, int tasVar,
                    char *name)
{
  ptasMot pNew = NULL;

  pNew = (ptasMot) malloc(sizeof(tasMot));
  if (!pNew) {
    return 0;
  }
  memset(pNew, 0, sizeof(tasMot));
  pNew->pDes = CreateDescriptor("TasMot");
  pNew->pDriv = CreateDrivableInterface();
  if (!pNew->pDes || !pNew->pDriv) {
    free(pNew);
    return 0;
  }
  pNew->pDes->GetInterface = GetTasInterface;
  pNew->pDriv->Halt = TASHalt;
  pNew->pDriv->CheckLimits = TASCheckLimits;
  pNew->pDriv->SetValue = TASSetValue;
  pNew->pDriv->CheckStatus = TASGetStatus;
  pNew->pDriv->GetValue = TASGetValue;
  pNew->math = self;
  pNew->code = tasVar;

  return AddCommand(pSics, name, TasMot, KillTasMot, pNew);

}

/*-------------------------------------------------------------------------*/
int InstallTasQMMotor(SicsInterp * pSics, ptasUB self)
{
  ptasMot pNew = NULL;

  pNew = (ptasMot) malloc(sizeof(tasMot));
  if (!pNew) {
    return 0;
  }
  memset(pNew, 0, sizeof(tasMot));
  pNew->pDes = CreateDescriptor("TasMot");
  pNew->pDriv = CreateDrivableInterface();
  if (!pNew->pDes || !pNew->pDriv) {
    free(pNew);
    return 0;
  }
  pNew->pDes->GetInterface = GetTasInterface;
  pNew->pDriv->Halt = TASHalt;
  pNew->pDriv->CheckLimits = TASCheckLimits;
  pNew->pDriv->SetValue = TASSetValue;
  pNew->pDriv->CheckStatus = TASQMGetStatus;
  pNew->pDriv->GetValue = TASQMGetValue;
  pNew->math = self;
  pNew->code = QM;

  return AddCommand(pSics, "qm", TasMot, KillTasMot, pNew);
}

/*------------------------------------------------------------------------*/
int TasMot(SConnection * pCon, SicsInterp * pSics, void *pData,
           int argc, char *argv[])
{
  ptasMot self = (ptasMot) pData;
  float value;
  char pBueffel[132];

  if (argc > 1) {
    strtolower(argv[1]);
    if (strcmp(argv[1], "target") == 0) {
      snprintf(pBueffel, 131, "%s.target = %f",
               argv[0], getTasPar(self->math->target, self->code));
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    } else {
      snprintf(pBueffel, 131, "ERROR: tasmot does not understand %s",
               argv[1]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
  }
  value = self->pDriv->GetValue(self, pCon);
  snprintf(pBueffel, 131, "%s = %f", argv[0], value);
  SCWrite(pCon, pBueffel, eValue);
  return 1;
}