/**
 * This is a special monochromator module for EIGER. EIGER has a ton of 
 * specialities:
 *
 * - d2r and d2l must be moved with a2rot in order to maintain the slit 
 *   width a2w
 * - When interrupting a2, a2 must be stooped and d2l, d2r aligned afterwards.
 * - There is a special drivable a2w which is that slit width.
 * - When driving the monochromator energy, then not only curvatures but also 
 *   a monochromator translation must be driven. This needs extra parameters
 * - There are two additional virtual motors: a2 and a2w. 
 * - There are multiple modes of operation:
 *   * Driving only a2
 *   * Driving only a2w
 *   * Driving the whole monochromator
 *
 * All this shite is addressed in this module. This is already a second version, 
 * the old version contained a virtual motor for a2, a2w only. This fell over because 
 * it did not account for the monochromator translation and a rare bug could not be 
 * fixed in the old module. 
 *
 * Mark Koennecke, February 2013
 *
 * Go to maximum or minimum value for mt when limit problem do not abort
 *
 * Mark Koennecke, April 2013
 */
#include <math.h>
#include <sics.h>
#include <tasub.h>
#include <sicsobj.h>
#include <sicshipadaba.h>

/* motors */
#define A1 0
#define A2ROT 1
#define D2R   2
#define D2L   3
#define MCV   4
#define MCH   5
#define MTX   6
static char *motNames[] = {
	  "a1",
	  "a2rot",
	  "d2r",
          "d2l",
	  "mcv",
	  "mch",
	  "mt"
	};
	


/* states */
#define IDLE      0
#define STARTMONO 1
#define STARTA2   2
#define STARTA2W  3
#define WAITING   4
#define WAITA2    5
#define WAITSLIT  6

/* constants */
#define RIGHTSIZE -3.5
#define LEFTSIZE -101.5

#define ABS(x) (x < 0 ? -(x) : (x))
#define MOTPREC .1
#define NOTSTARTED -100
/*------------------- module private data structure -------------*/
typedef struct {
  pmaCrystal mono;
  ptasUB tasub;
  pIDrivable drivs[7];
  void *motData[7];
  double a2wTarget;
  double a2Target;
  int state;
  long waitID;
}eigerMono, *peigerMono;

/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int EIMOHalt(void *data) {
    pSICSOBJ self = NULL; 
    int i;

    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;

    for(i = 0; i < 7; i++){
      mono->drivs[i]->Halt(mono->motData[i]);
    }
    mono->state = WAITA2;

    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 EIMOCheckLimits(void *data, float val,
                         char *error, int errlen){
    pSICSOBJ self = NULL; 
 
    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;

    /*
      does not make sense 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 long EIMOSetValue(void *data, SConnection *pCon, float val){
    pSICSOBJ self = NULL; 
 
    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;
    
    mono->a2Target = val;
    mono->state = STARTMONO;

    return 1; 
}
/*-----------------------------------------------------------------*/
static long EIMOSetA2Value(void *data, SConnection *pCon, float val){
    pSICSOBJ self = NULL; 
 
    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;
    
    mono->a2Target = val;
    mono->state = STARTA2;

    return 1; 
}
/*----------------------------------------------------------------*/
static long EIMOSetA2WValue(void *data, SConnection *pCon, float val){
    pSICSOBJ self = NULL; 
 
    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;
    
    mono->a2wTarget = val;
    mono->state = STARTA2W;

    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 long startTASMotor(pMotor mot, SConnection * pCon, char *name,
                         double target, int silent, int stopFixed)
{
  float val, fixed, fHard, 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;
    }
    if(MotorCheckBoundary(mot,target,&fHard,buffer,sizeof(buffer)) == 0){
      SCPrintf(pCon,eLog,"ERROR: %s", buffer);
    }
  }
  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); 
    } else {
      /*
	to force updates on targets
      */
      InvokeNewTarget(pServ->pExecutor, name, target);
      writeMotPos(pCon, silent, name, val, target);
    }
    return status;
  }
  return NOTSTARTED;
}
/*---------------------------------------------------------------*/
static void calcSlitTargets(peigerMono self, float a2, float *d2r, float *d2l)
{
	*d2r = RIGHTSIZE - a2 + self->a2wTarget/2;
	*d2l = LEFTSIZE + a2 + self->a2wTarget/2;
}
/*--------------------------------------------------------------*/
static void startMono(pSICSOBJ self, SConnection *pCon)
{
  int stopFixed = 0;
  int silent = 1;
  int status;
  double curve;
  float d2r, d2l, mtx;
  peigerMono mono = (peigerMono)self->pPrivate;
  hdbValue mta, mtb;
  float lim;

  assert(mono != NULL);

  double val = mono->a2Target;

  
  mono->waitID = GetTaskGroupID(pServ->pTasker);
  mono->tasub->monoTaskID = mono->waitID;

  silent = mono->tasub->silent;

  /*
    the meat: A1, A2
  */
  status = startTASMotor((pMotor)mono->motData[A1], pCon, "a1",
                         val / 2., silent, stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    SCWrite(pCon,"ERROR: failed to start required motor A1",eLogError);
    return;
  }
  if(status != NOTSTARTED){
    AddTaskToGroup(pServ->pTasker,status, mono->waitID);
  }

  status = startTASMotor((pMotor)mono->motData[A2ROT], pCon, "a2",
                         val, silent,stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    SCWrite(pCon,"ERROR: failed to start required motor A2",eLogError);
    return ;
  }
  if(status != NOTSTARTED){
    AddTaskToGroup(pServ->pTasker,status, mono->waitID);
  }

  /*
    the slits
  */
  calcSlitTargets(mono,val,&d2r,&d2l);
  status = startTASMotor((pMotor)mono->motData[D2R], pCon, "d2r",
			 d2r, silent,stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    SCWrite(pCon,"WARNING: monochromator d2r failed to start", eLog);
    SCSetInterrupt(pCon,eContinue);
  } else {
    AddTaskToGroup(pServ->pTasker,status, mono->waitID);
  }
  status = startTASMotor((pMotor)mono->motData[D2L], pCon, "d2l",
			 d2l, silent,stopFixed);
  if (status < 0 && status != NOTSTARTED) {
    SCWrite(pCon,"WARNING: monochromator d2l failed to start", eLog);
    SCSetInterrupt(pCon,eContinue);
  } else {
    AddTaskToGroup(pServ->pTasker,status, mono->waitID);
  }


  /*
    curvatures 
  */
  curve = maCalcHorizontalCurvature(*mono->mono,
				    val);
  status = startTASMotor((pMotor)mono->motData[MCH], pCon, "mch",
			 curve, 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,mono->waitID);
  }

  curve = maCalcVerticalCurvature(*mono->mono,
				  val);
  status = startTASMotor((pMotor)mono->motData[MCV], pCon, "mcv",
			 curve, 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, mono->waitID);
  }


  /*
    the translation
  */
  if(status > 0){
    /*
      only run mt when mcv started 
    */
    SICSHdbGetPar(self,pCon,"MTA",&mta);
    SICSHdbGetPar(self,pCon,"MTB",&mtb);
    mtx = mta.v.doubleValue + mtb.v.doubleValue*pow(curve,.75);
    MotorGetPar((pMotor)mono->motData[MTX],"softupperlim",&lim);
    if(mtx > lim){
      SCPrintf(pCon,eLog,"WARNING: correcting mt target %f to %f within upper limit", mtx, lim-.1);
      mtx = lim - .1;
    }
    MotorGetPar((pMotor)mono->motData[MTX],"softlowerlim",&lim);
    if(mtx < lim){
      SCPrintf(pCon,eLog,"WARNING: correcting mt target %f to %f within lower limit", mtx, lim +.1);
      mtx = lim + .1;
    }
    status = startTASMotor((pMotor)mono->motData[MTX], pCon, "mt",
			   mtx, silent,stopFixed);
    if (status < 0 && status != NOTSTARTED) {
      SCWrite(pCon,"WARNING: monochromator translation motor failed to start", eLog);
      SCSetInterrupt(pCon,eContinue);
    } else {
      AddTaskToGroup(pServ->pTasker,status, mono->waitID);
    }
  }

}
/*--------------------------------------------------------------*/
static void startA2(peigerMono mono, SConnection *pCon)
{
  float d2r, d2l;
  int status;

  mono->waitID = GetTaskGroupID(pServ->pTasker);

  status = StartDriveTask(mono->motData[A2ROT],pCon,
			  "a2rot",mono->a2Target);
  if(status > 0) {
    AddTaskToGroup(pServ->pTasker,status,mono->waitID);
  } else {
    return;
  }

  calcSlitTargets(mono,mono->a2Target,&d2r,&d2l);
  status = StartDriveTask(mono->motData[D2R],pCon,
			  "d2r",d2r);
  if(status > 0) {
    AddTaskToGroup(pServ->pTasker,status,mono->waitID);
  } 
  status = StartDriveTask(mono->motData[D2L],pCon,
			  "d2l",d2l);
  if(status > 0) {
    AddTaskToGroup(pServ->pTasker,status,mono->waitID);
  } 
}
/*--------------------------------------------------------------*/
static void startA2W(peigerMono mono, SConnection *pCon)
{
  float d2r, d2l, val;
  int status;

  GetDrivablePosition(mono->motData[A2ROT],pCon,&val);
  if(val < -99999.99) {
    SCWrite(pCon,"ERROR: failed to read a2rot",eError);
    return;
  }
  
  mono->waitID = GetTaskGroupID(pServ->pTasker);
  calcSlitTargets(mono,val,&d2r,&d2l);
  status = StartDriveTask(mono->motData[D2R],pCon,
			  "d2r",d2r);
  if(status > 0) {
    AddTaskToGroup(pServ->pTasker,status,mono->waitID);
  } 
  status = StartDriveTask(mono->motData[D2L],pCon,
			  "d2l",d2l);
  if(status > 0) {
    AddTaskToGroup(pServ->pTasker,status,mono->waitID);
  } 
}
/*----------------------------------------------------------------
 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 EIMOCheckStatus(void *data, SConnection *pCon){
    pSICSOBJ self = NULL; 
    float val;

    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;

    switch(mono->state){
    case IDLE:
      return HWIdle;
      break;
    case STARTMONO:
      startMono(self,pCon);
      mono->state = WAITING;
      break;
    case STARTA2:
      startA2(mono,pCon);
      mono->state = WAITING;
      break;
    case STARTA2W:
      startA2W(mono,pCon);
      mono->state = WAITING;
      break;
    case WAITING:
      if(isTaskGroupRunning(pServ->pTasker, mono->waitID)){
	return HWBusy;
      } else {
	mono->state = IDLE;
	return HWIdle;
      }
      break;
    case WAITA2:
      /*
	after a Halt, wait for A2 to finish, then drive the slits 
	into an open position
      */
      if(isTaskGroupRunning(pServ->pTasker, mono->waitID)){
	return HWBusy;
      } else {
	GetDrivablePosition(mono->motData[A2ROT], pCon, &val);
	if(val > -99999.99){
	  mono->a2Target = val;
	  startA2W(mono,pCon);
	}
	mono->state = WAITING;
	return HWBusy;
      }
      break;
    default:
      assert(1);
      break;
    }
    return HWBusy; 
}
/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float EIMOGetValue(void *data, SConnection *pCon){
    pSICSOBJ self = NULL; 
    float val = -99999999.99;
 
    self = (pSICSOBJ)data;
    peigerMono mono = (peigerMono)self->pPrivate;

    GetDrivablePosition(mono->motData[A2ROT],pCon,&val);

    return val; 
}
/*------------------------------------------------------------------*/
static float EIMOGetA2WValue(void *data, SConnection *pCon){
  pSICSOBJ self = NULL; 
  float vall, valr;
  double d2ro, d2lo, a2w;
 
  self = (pSICSOBJ)data;
  peigerMono mono = (peigerMono)self->pPrivate;

  GetDrivablePosition(mono->motData[D2R],pCon,&valr);
  GetDrivablePosition(mono->motData[D2L],pCon,&vall);

  d2ro = RIGHTSIZE - mono->a2Target;
  d2lo = LEFTSIZE + mono->a2Target;
  a2w =  (d2lo - vall) + (d2ro - valr);
  return ABS(a2w);
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static pIDrivable EIMOMakeDrivable(){
  pIDrivable pDriv;

  pDriv = calloc(1,sizeof(IDrivable));
  if(pDriv == NULL){
    return NULL;
  }
  pDriv->Halt = EIMOHalt;
  pDriv->CheckLimits = EIMOCheckLimits;
  pDriv->SetValue = EIMOSetValue;
  pDriv->CheckStatus = EIMOCheckStatus;
  pDriv->GetValue = EIMOGetValue;

  return pDriv;
}

/*======================================================
 * The A2W Drivable
 ======================================================*/
typedef struct {
	pObjectDescriptor pDes;
	pIDrivable pDriv;
	peigerMono eiger;
}EigerA2W, *eigera2w;

/*------------------------------------------------------*/
static void *eigera2wGetInterface(void *data, int iD){
    eigera2w self = NULL;

    self = (eigera2w)data;
    if(self != NULL && iD == DRIVEID){
         return self->pDriv;
     } else  {
         return NULL;
     }
     return NULL;
}
/*---------------------------------------------------------------*/
static int eigera2wSaveStatus(void *data, char *name, FILE *fd)
{
    eigera2w self = NULL;

    self = (eigera2w)data;
    fprintf(fd,"%s target %f\n", name, self->eiger->a2wTarget);
    return 1;
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static eigera2w eigera2wMakeObject(peigerMono eiger){
    eigera2w self = NULL;

    self = malloc(sizeof(EigerA2W));
    if(self == NULL){
    	return NULL;
    }
    memset(self,0,sizeof(EigerA2W));
    self->pDes = CreateDescriptor("EigerA2W");
    self->pDriv = CreateDrivableInterface();
    if(self->pDes == NULL || self->pDriv == NULL){
    	return NULL;
    }
    self->eiger = eiger;

    self->pDes->GetInterface = eigera2wGetInterface;
    self->pDes->SaveStatus = eigera2wSaveStatus;
    self->pDriv->Halt = EIMOHalt;
    self->pDriv->CheckLimits = EIMOCheckLimits;
    self->pDriv->SetValue = EIMOSetA2WValue;
    self->pDriv->CheckStatus = EIMOCheckStatus;
    self->pDriv->GetValue = EIMOGetA2WValue;

    return self;
}
/*======================================================
 * The A2 Drivable
 ======================================================*/
static void *eigera2GetInterface(void *data, int iD){
    eigera2w self = NULL;

    self = (eigera2w)data;
    if(self != NULL && iD == DRIVEID){
         return self->pDriv;
     } else  {
         return NULL;
     }
     return NULL;
}
/*---------------------------------------------------------------*/
static int eigera2SaveStatus(void *data, char *name, FILE *fd)
{
    eigera2w self = NULL;

    self = (eigera2w)data;
    fprintf(fd,"%s target %f\n", name, self->eiger->a2Target);
    return 1;
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static eigera2w eigera2MakeObject(peigerMono eiger){
    eigera2w self = NULL;

    self = malloc(sizeof(EigerA2W));
    if(self == NULL){
    	return NULL;
    }
    memset(self,0,sizeof(EigerA2W));
    self->pDes = CreateDescriptor("EigerA2");
    self->pDriv = CreateDrivableInterface();
    if(self->pDes == NULL || self->pDriv == NULL){
    	return NULL;
    }
    self->eiger = eiger;

    self->pDes->GetInterface = eigera2GetInterface;
    self->pDes->SaveStatus = eigera2SaveStatus;
    self->pDriv->Halt = EIMOHalt;
    self->pDriv->CheckLimits = EIMOCheckLimits;
    self->pDriv->SetValue = EIMOSetA2Value;
    self->pDriv->CheckStatus = EIMOCheckStatus;
    self->pDriv->GetValue = EIMOGetValue;

    return self;
}
/*---------------------------------------------------------------*/
int EIMODrivableAction(SConnection * pCon, SicsInterp * pSics, void *pData,
                 int argc, char *argv[])
{
  pIDrivable pDriv = NULL;
  pDummy pDum;
  float value;
  char pBuffer[132];
  eigera2w self= NULL;
  eigera2w selfe = NULL;


  assert(pData != NULL);

  if(argc > 1){
	  strtolower(argv[1]);
	  if(strcmp(argv[1],"target") == 0){
		  if(strcmp(argv[0],"a2w") == 0 && argc > 2){
			  self = (eigera2w)pData;
			  self->eiger->a2wTarget = atof(argv[2]);
			  SCSendOK(pCon);
			  return 1;
		  } else if (strcmp(argv[0],"a2") == 0 && argc > 2) {
			  selfe = (eigera2w)pData;
			  selfe->eiger->a2Target = atof(argv[2]);
			  SCSendOK(pCon);
			  return 1;
		  }
	  }
  }
  pDum = (pDummy)pData;
  pDriv = (pIDrivable) pDum->pDescriptor->GetInterface(pDum,DRIVEID);
  value = pDriv->GetValue(pDum, pCon);
  if (value < -9000.) {
    snprintf(pBuffer, 131, "ERROR: failed to read %s", argv[0]);
    SCWrite(pCon, pBuffer, eError);
    return 0;
  }
  snprintf(pBuffer, 131, "%s = %f", argv[0], value);
  SCWrite(pCon, pBuffer, eValue);
  return 1;
}
/*-------------------------------------------------------------------------*/
static int RegisterFunc(pSICSOBJ self, SConnection *pCon, 
			pHdb commandNode, pHdb par[], int nPar)
{
  peigerMono eiger = (peigerMono)self->pPrivate;
  ptasUB tas = NULL;

  /*
    install into TAS
  */
  tas = (ptasUB)FindCommandData(pServ->pSics,"tasub",NULL);
  if(!tas){
    SCWrite(pCon,"ERROR: tasub module not found", eError);
    return 0;
  }
  eiger->mono = &(tas->machine.monochromator);
  eiger->tasub = tas;
  tas->monoData = self;
  tas->mono = EIMOMakeDrivable();
  if(tas->mono == NULL){
    SCWrite(pCon,"ERROR: error initializing EIGER monochromator, tasub now broken",
	    eError);
    return 0;
  }
  return 1;
}
/*-------------------------------------------------------------------------*/
int InitEigerMono(SConnection *pCon, SicsInterp *pSics,
		  void *pData, int argc, char *argv[])
{
  pIDrivable pDriv = NULL;
  eigera2w a2 = NULL;
  eigera2w a2w = NULL;
  pSICSOBJ pNew = NULL;
  peigerMono eiger = NULL;
  int i;
  ptasUB tas = NULL;
  pHdb cmd;

  pNew = MakeSICSOBJ("emo","EigerMonochromator");
  eiger = calloc(1,sizeof(eigerMono));
  if(pNew == NULL || eiger == NULL){
    SCWrite(pCon,"ERROR: out of memory creating Eiger Monochromator",eError);
    return 0;
  }
  pNew->pPrivate = eiger;
  cmd = AddSICSHdbPar(pNew->objectNode, "MTA", usMugger, MakeHdbFloat(.0));
  SetHdbProperty(cmd,"__save","true");
  cmd = AddSICSHdbPar(pNew->objectNode, "MTB", usMugger, MakeHdbFloat(3.7));
  SetHdbProperty(cmd,"__save","true");
  AddSICSHdbPar(pNew->objectNode, "register", usMugger,
                    MakeSICSFunc(RegisterFunc));
	

  /*
    get motors
  */
  for(i = 0; i < 7; i++){
    pDriv = FindDrivable(pSics,motNames[i]);
    if(pDriv == NULL){
      SCPrintf(pCon,eError,"ERROR: %s motor not found", motNames[i]);
      return 0;
    }
    eiger->drivs[i] = pDriv;
    eiger->motData[i] = FindCommandData(pSics,motNames[i],NULL);
  }
	


  /*
    install into interpreter
  */
  a2 = eigera2MakeObject(eiger);
  a2w = eigera2wMakeObject(eiger);
  if(a2 == NULL || a2w == NULL){
    SCWrite(pCon,"ERROR: out of memory creating a2, a2w", eError);
    return 0;
  }
  AddCommand(pSics, "a2", EIMODrivableAction, NULL, a2);
  AddCommand(pSics, "a2w", EIMODrivableAction, NULL, a2w);
  AddCommand(pSics,"emo", InterInvokeSICSOBJ, KillSICSOBJ, pNew);
	
  return 1;
}
