/**
 * This module implements a drivable for running the eiger A2 motor.
 * The problem with that one is that the slits have to follow it.
 * In addition the slit width needs to be adjusted too. All this
 * is done in this module.
 *
 * CAVEAT: This module requires for its working the existence
 * of 3 motors in SICS:
 *  a2rot, d2r, d2l
 * As this module is very special to EIGER no special checks have
 * been implemented on this. SICS will assert and fail if this
 * condition is not met.
 *
 * copyright: see file COPYRIGHT
 *
 * Mark Koennecke, July 2011
 */
#include <sics.h>
#include <motorlist.h>
#include <lld.h>
#define ABS(x) (x < 0 ? -(x) : (x))
/*------------------------------ states ------------------------*/
#define IDLE      0
#define RUNNING   1
#define WAITSTOP  2
#define WAITSLIT  3
#define WAITIDLE  4
/* ---------------------------- motor names --------------------*/
#define A2 "a2rot"
#define A2R "d2r"
#define A2L "d2l"
/*----------------------------- data structure -----------------*/
typedef struct {
	pObjectDescriptor pDes;
	pIDrivable pDriv;
	int motorList;
	pIDrivable  listDriv;
	int state;
	float a2Target;
	float a2wTarget;
	float leftSize;
	float rightSize;
}EigerA2, *eigera2;
/*---------------------------------------------------------------*/
static void calcSlitTargets(eigera2 self, float a2, float *d2r, float *d2l)
{
	*d2r = self->rightSize - a2 + self->a2wTarget/2;
	*d2l = self->leftSize + a2 + self->a2wTarget/2;
}
/*---------------------------------------------------------------*/
static float calcA2W(eigera2 self, float d2r, float d2l)
{
	double d2ro, d2lo, a2w;
	d2ro = self->rightSize - self->a2Target;
	d2lo = self->leftSize + self->a2Target;
	a2w =  (d2lo - d2l) + (d2ro - d2r);
	return ABS(a2w);
}
/*---------------------------------------------------------------*/
static void *eigera2GetInterface(void *data, int iD){
    eigera2 self = NULL; 
 
    self = (eigera2)data;
    if(self != NULL && iD == DRIVEID){
         return self->pDriv;
     } else  {
         return NULL;
     }
     return NULL;
}
/*---------------------------------------------------------------*/
static int eigera2SaveStatus(void *data, char *name, FILE *fd)
{
    eigera2 self = NULL;

    self = (eigera2)data;
    fprintf(fd,"%s target %f\n", name, self->a2Target);
    return 1;
}
/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int eigera2Halt(void *data) {
    eigera2 self = NULL; 
 
    self = (eigera2)data;
    if(self->state == RUNNING){
    	self->listDriv->Halt(&self->motorList);
    	self->state = WAITSTOP;
    }
    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 eigera2CheckLimits(void *data, float val,
                         char *error, int errlen){
    eigera2 self = NULL; 
    pIDrivable pDriv = NULL;
    int status;
    float d2r, d2l;

    self = (eigera2)data;

    pDriv = FindDrivable(pServ->pSics,A2);
    assert(pDriv != NULL);
    status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2,NULL), val,
    		error, errlen);
    if(status == 0){
    	return 0;
    }

    calcSlitTargets(self,val,&d2r, &d2l);

    pDriv = FindDrivable(pServ->pSics,A2R);
    assert(pDriv != NULL);
    status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2R,NULL),
    		d2r,
    		error, errlen);
    if(status == 0){
    	return 0;
    }
 
    pDriv = FindDrivable(pServ->pSics,A2L);
    assert(pDriv != NULL);
    status = pDriv->CheckLimits(FindCommandData(pServ->pSics,A2L,NULL),
    		d2l,
    		error, errlen);
    if(status == 0){
    	return 0;
    }


    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 eigera2SetValue(void *data, SConnection *pCon, float val){
    eigera2 self = NULL; 
    float d2r, d2l;
    int status;

    self = (eigera2)data;

    if(self->state != IDLE){
    	SCWrite(pCon,"WARNING: stopping busy A2", eLogError);
	self->a2Target = val;
	eigera2Halt(data);
	self->state = WAITIDLE;
    	return OKOK;
    }
    self->a2Target = val;
    setNewMotorTarget(self->motorList,A2,self->a2Target);
    calcSlitTargets(self,val,&d2r, &d2l);
    setNewMotorTarget(self->motorList,A2R,d2r);
    setNewMotorTarget(self->motorList,A2L,d2l);
    self->state = RUNNING;
    status = self->listDriv->SetValue(&self->motorList, pCon, 27.);
    if(status != OKOK){
    	self->listDriv->Halt(&self->motorList);
    	self->state = WAITSTOP;
    }
    return status;
}
/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float eigera2GetValue(void *data, SConnection *pCon){
    eigera2 self = NULL;
    pIDrivable pDriv = NULL;

    float val = -99999999.99;

    self = (eigera2)data;
    pDriv = FindDrivable(pServ->pSics,A2);
    assert(pDriv != NULL);
    return pDriv->GetValue(FindCommandData(pServ->pSics,A2,NULL),
    		pCon);
}
/*----------------------------------------------------------------
 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 eigera2CheckStatus(void *data, SConnection *pCon){
    eigera2 self = NULL; 
    int status;
    pIDrivable pDriv = NULL;
    float d2r, d2l;

    self = (eigera2)data;

    switch(self->state){
    case WAITIDLE:
    	status = self->listDriv->CheckStatus(&self->motorList,pCon);
    	if(status == HWFault || status == HWIdle){
    		self->state = IDLE;
		eigera2SetValue(data,pCon, self->a2Target);
    	} else {
	  return status;
	}
	break;
    case RUNNING:
    case WAITSLIT:
    	status = self->listDriv->CheckStatus(&self->motorList,pCon);
    	if(status == HWFault || status == HWIdle){
    		self->state = IDLE;
    	}
    	return status;
    case WAITSTOP:
    	/*
    	 * The point of this is to get the slits into the right
    	 * position after a stop.
    	 */
    	status = self->listDriv->CheckStatus(&self->motorList,pCon);
    	if(status == HWIdle){
    		SCWrite(pCon,"WARNING: driving slits into position after stop", eWarning);
    		self->state = WAITSLIT;
    		self->a2Target = eigera2GetValue(data,pCon);
    	    setNewMotorTarget(self->motorList,A2,self->a2Target);
    	    calcSlitTargets(self,self->a2Target,&d2r, &d2l);
    	    setNewMotorTarget(self->motorList,A2R,d2r);
    	    setNewMotorTarget(self->motorList,A2L,d2l);
    	    self->listDriv->SetValue(&self->motorList, pCon, 27.);
    	    return HWBusy;
    	}
    	return HWBusy;
    }
    return 1; 
}
extern int MOLIEigerStatus(void *data, SConnection * pCon); /* motorlist.c */

/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static eigera2 eigera2MakeObject(){
    eigera2 self = NULL; 
    pIDrivable pDriv = NULL;
    float val1, val2;

    self = malloc(sizeof(EigerA2));
    if(self == NULL){
    	return NULL;
    }
    memset(self,0,sizeof(EigerA2));
    self->pDes = CreateDescriptor("EigerA2");
    self->pDriv = CreateDrivableInterface();
    if(self->pDes == NULL || self->pDriv == NULL){
    	return NULL;
    }
    self->motorList = LLDcreate(sizeof(MotControl));
    self->listDriv = makeMotListInterface();
    self->listDriv->CheckStatus = MOLIEigerStatus;

    pDriv = FindDrivable(pServ->pSics,A2);
    assert(pDriv != NULL);
    val1 = pDriv->GetValue(FindCommandData(pServ->pSics,A2,NULL),
    		pServ->dummyCon);
    addMotorToList(self->motorList,A2,val1);
    self->a2Target = val1;

    pDriv = FindDrivable(pServ->pSics,A2R);
    assert(pDriv != NULL);
    val1 = pDriv->GetValue(FindCommandData(pServ->pSics,A2R,NULL),
    		pServ->dummyCon);

    pDriv = FindDrivable(pServ->pSics,A2L);
    assert(pDriv != NULL);
    val2 = pDriv->GetValue(FindCommandData(pServ->pSics,A2L,NULL),
    		pServ->dummyCon);
    addMotorToList(self->motorList,A2R,val1);
    addMotorToList(self->motorList,A2L,val2);
    self->leftSize =  -101.5;
    self->rightSize = -3.5;
    self->a2wTarget = calcA2W(self,val1,val2);

    self->state = IDLE;

    self->pDes->GetInterface = eigera2GetInterface;
    self->pDes->SaveStatus = eigera2SaveStatus;
    self->pDriv->Halt = eigera2Halt;
    self->pDriv->CheckLimits = eigera2CheckLimits;
    self->pDriv->SetValue = eigera2SetValue;
    self->pDriv->CheckStatus = eigera2CheckStatus;
    self->pDriv->GetValue = eigera2GetValue;


    return self;
}
/*======================================================
 * The A2W Drivable
 ======================================================*/
typedef struct {
	pObjectDescriptor pDes;
	pIDrivable pDriv;
	eigera2 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;
}
/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int eigera2wHalt(void *data) {
    eigera2w self = NULL;

    self = (eigera2w)data;
    return self->eiger->pDriv->Halt(self->eiger);
}
/*----------------------------------------------------------------
 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 eigera2wCheckLimits(void *data, float val,
                         char *error, int errlen){
    eigera2w self = NULL;
    float vala2;

    self = (eigera2w)data;
    self->eiger->a2wTarget = val;
    vala2 = eigera2GetValue(self->eiger,pServ->dummyCon);
    return self->eiger->pDriv->CheckLimits(self->eiger,vala2,error,errlen);
}
/*----------------------------------------------------------------
 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 eigera2wSetValue(void *data, SConnection *pCon, float val){
    eigera2w self = NULL;
    float vala2;

    self = (eigera2w)data;
    self->eiger->a2wTarget = val;
    vala2 = eigera2GetValue(self->eiger,pCon);
    return self->eiger->pDriv->SetValue(self->eiger,pCon,vala2);
}
/*----------------------------------------------------------------
 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 eigera2wCheckStatus(void *data, SConnection *pCon){
    eigera2w self = NULL;

    self = (eigera2w)data;
    return self->eiger->pDriv->CheckStatus(self->eiger,pCon);
}
/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float eigera2wGetValue(void *data, SConnection *pCon){
    eigera2w self = NULL;
    float valr, vall;
    pIDrivable pDriv = NULL;

    self = (eigera2w)data;
    pDriv = FindDrivable(pServ->pSics,A2R);
    assert(pDriv != NULL);
    valr = pDriv->GetValue(FindCommandData(pServ->pSics,A2R,NULL),
    		pCon);

    pDriv = FindDrivable(pServ->pSics,A2L);
    assert(pDriv != NULL);
    vall = pDriv->GetValue(FindCommandData(pServ->pSics,A2L,NULL),
    		pCon);

    return calcA2W(self->eiger,valr, vall);
}
/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static eigera2w eigera2wMakeObject(eigera2 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 = eigera2wHalt;
    self->pDriv->CheckLimits = eigera2wCheckLimits;
    self->pDriv->SetValue = eigera2wSetValue;
    self->pDriv->CheckStatus = eigera2wCheckStatus;
    self->pDriv->GetValue = eigera2wGetValue;

    return self;
}
/*---------------------------------------------------------------*/
int DrivableAction(SConnection * pCon, SicsInterp * pSics, void *pData,
                 int argc, char *argv[])
{
  pIDrivable pDriv = NULL;
  pDummy pDum;
  float value;
  char pBuffer[132];
  eigera2w self= NULL;
  eigera2 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 = (eigera2)pData;
			  selfe->a2Target = atof(argv[2]);
			  SCSendOK(pCon);
			  return 1;
		  }
	  }
	  if(strcmp(argv[1],"reset") == 0){
		  selfe = (eigera2)pData;
		  selfe->state = IDLE;
		  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;
}
/*---------------------------------------------------------------*/
int InitEiger(SConnection *pCon, SicsInterp *pSics,
		void *pData, int argc, char *argv[])
{
	pIDrivable pDriv = NULL;
	eigera2 a2 = NULL;
	eigera2w a2w = NULL;

	pDriv = FindDrivable(pSics,A2);
	if(pDriv == NULL){
		SCWrite(pCon,"ERROR: A2 motor not found", eError);
		return 0;
	}
	pDriv = FindDrivable(pSics,A2R);
	if(pDriv == NULL){
		SCWrite(pCon,"ERROR: A2R motor not found", eError);
		return 0;
	}
	pDriv = FindDrivable(pSics,A2L);
	if(pDriv == NULL){
		SCWrite(pCon,"ERROR: A2L motor not found", eError);
		return 0;
	}

	a2 = eigera2MakeObject();
	a2w = eigera2wMakeObject(a2);
	if(a2 == NULL || a2w == NULL){
		SCWrite(pCon,"ERROR: out of memory creating a2, a2w", eError);
		return 0;
	}
	if(argc > 1){
		a2->leftSize = atof(argv[1]);
	}
	if(argc > 2){
		a2->rightSize = atof(argv[2]);
	}

	AddCommand(pSics, "a2", DrivableAction, NULL, a2);
	AddCommand(pSics, "a2w", DrivableAction, NULL, a2w);

	return 1;
  }
