/**
 * Triple axis monochromator module
 * EIGER made it necessary to abstract out the monochromator. This is 
 * the default implementation for a sane, normal TAS. This implements a 
 * drivable interface. It gets an A2 value from the tasub module and is 
 * supposed to drive and monitor all the dependent monochromator motors.
 *
 * copyright: see file COPYRIGHT
 *
 * Mark Koennecke, February 2013
 *
 * TODO: this may need a refactoring towards a monochromator object with its 
 * own parameters and such. Eiger would have benefitted from this. This here 
 * just implements a drivable interface on top of the  tasub parameters. As EIGER
 * is now working; this has gone low priority.
 *
 * Mark Koennecke, September 2014
 */
#include <sics.h>
#include "tasmono.h"
#include "tasub.h"

#define MOTPREC .01
#define A1   0
#define A2   1
#define MCV  2
#define MCH  3
#define NOTSTARTED -100


#define ABS(x) (x < 0 ? -(x) : (x))
 
/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int TAMOHalt(void *data) {
    ptasUB self = NULL; 
    pIDrivable pDriv; 
    int i;
 
    self = (ptasUB)data;
    for(i = 0; i < 4; i++){
      if (self->motors[i] != NULL) {
	pDriv = GetDrivableInterface(self->motors[i]);
	pDriv->Halt(self->motors[i]);
      }
    }
    return OKOK; 
}
/*----------------------------------------------------------------
 This routine can return either 1 or 0. 1 means the position can 
 be reached, 0 NOT
 If 0, error shall contain up to errlen characters of information
 about which limit was violated
------------------------------------------------------------------*/
static int TAMOCheckLimits(void *data, float val,
                         char *error, int errlen){
    ptasUB self = NULL; 
 
    self = (ptasUB)data;

    /*
      Not meaningful in this context
    */
    return 1; 
}
/*----------------------------------------------------------------
 This routine can return 0 when a limit problem occurred 
 OKOK when the motor was successfully started 
 HWFault when a problem occured starting the device
 Possible errors shall be printed to pCon
 For real motors, this is supposed to try at least three times
 to start the motor in question
 val is the value to drive the motor too
------------------------------------------------------------------*/
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 long startTASMotor(pMotor mot, SConnection * pCon, char *name,
                         double target, int silent, int stopFixed)
{
  float val, fixed, precision = MOTPREC;
  long status = NOTSTARTED;
  char buffer[132];
  pIDrivable pDriv = NULL;
  pDummy dum = NULL;
  pDynString mes = NULL;
  

  dum = (pDummy)mot;
  GetDrivablePosition(mot, pCon,&val);
  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 NOTSTARTED;
    }
  }
  mot->stopped = 0;
  if (ABS(val - target) > precision) {
    status = StartDriveTask(mot, pCon, name, (float)target); 
    if(status < 0){
      SCPrintf(pCon,eLog,"ERROR: failed to drive %s to %f", name, target); 
    }
    /*
      to force updates on targets
    */
    InvokeNewTarget(pServ->pExecutor, name, target);
    writeMotPos(pCon, silent, name, val, target);
    return status;
  } else {
    return NOTSTARTED;
  }
}
/*-------------------------------------------------------------------------*/
static long TAMOSetValue(void *data, SConnection *pCon, float val){
  ptasUB self = NULL;  
  self = (ptasUB)data;
  int stopFixed = 0;
  double curve;
  int status;

  self->monoTaskID = GetTaskGroupID(pServ->pTasker);

  status = startTASMotor(self->motors[A1], pCon, "a1",
                         val / 2., self->silent, stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    return HWFault;
  }
  if(status != NOTSTARTED){
    AddTaskToGroup(pServ->pTasker,status, self->monoTaskID);
  }

  status = startTASMotor(self->motors[A2], pCon, "a2",
                         val, self->silent,stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    return HWFault;
  }
  if(status != NOTSTARTED){
    AddTaskToGroup(pServ->pTasker,status, self->monoTaskID);
  }


  if (self->motors[MCV] != NULL) {
    curve = maCalcVerticalCurvature(self->machine.monochromator,
                                    val);
    status = startTASMotor(self->motors[MCV], pCon, "mcv",
                           curve, self->silent,stopFixed);
    if (status < 0 && status != NOTSTARTED) {
      SCWrite(pCon,"WARNING: monochromator vertical curvature motor failed to start", eLog);
      SCSetInterrupt(pCon,eContinue);
    } else {
      AddTaskToGroup(pServ->pTasker,status, self->monoTaskID);
    }
  }

  if (self->motors[MCH] != NULL) {
    curve = maCalcHorizontalCurvature(self->machine.monochromator,
                                      val);
    status = startTASMotor(self->motors[MCH], pCon, "mch",
                           curve, self->silent,stopFixed);
    if (status < 0 && status != NOTSTARTED) {
      SCWrite(pCon,"WARNING: monochromator horizontal curvature motor failed to start", eLog);
      SCSetInterrupt(pCon,eContinue);
    }else {
      AddTaskToGroup(pServ->pTasker,status,self->monoTaskID);
    }
  }
  return OKOK; 
}
/*----------------------------------------------------------------
 Checks the status of a running motor. Possible return values
   HWBusy The motor is still running
   OKOK or HWIdle when the motor finished driving
   HWFault when a hardware problem ocurred
   HWPosFault when the hardware cannot reach a position
 Errors are duly to be printed to pCon
 For real motors CheckStatus again shall try hard to fix any 
 issues with the motor 
------------------------------------------------------------------*/
static int TAMOCheckStatus(void *data, SConnection *pCon){
    ptasUB self = NULL; 
 
    self = (ptasUB)data;
    if(isTaskGroupRunning(pServ->pTasker,self->monoTaskID)) {
      return HWBusy;
    } else {
      return HWIdle;
    }
    return 1; 
}
/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float TAMOGetValue(void *data, SConnection *pCon){
    ptasUB self = NULL; 
    float val = -99999999.99;

    int status;

    self = (ptasUB)data;
    status = GetDrivablePosition(self->motors[A2], pCon, &val);
    if (status == 0) {
      return -99999999.99;
    } 
    return val; 
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
pIDrivable MakeTasMono(){
  pIDrivable pDriv;

  pDriv = calloc(1,sizeof(IDrivable));
  if(pDriv == NULL){
    return NULL;
  }
  pDriv->Halt = TAMOHalt;
  pDriv->CheckLimits = TAMOCheckLimits;
  pDriv->SetValue = TAMOSetValue;
  pDriv->CheckStatus = TAMOCheckStatus;
  pDriv->GetValue = TAMOGetValue;

  return pDriv;
}