/*-----------------------------------------------------------------------------

  Based on SINQ code 1996-2003 by Mark Koennecke
  
  Modifications: Paul Hathaway April 2004
    SimRun failure rate independent of polling rate
    Fault condition determined at initial drive command and reported
    at appropriate time:
        ASIM_NO_FAULT - run to destination, report no error (OKOK,HWIdle)
        ASIM_HW_FAULT - no movement, report HWFault
        ASIM_POS_FAULT - run to error position, report no error (OKOK,HWIdle)
        ASIM_STALL_FAULT - run to stall position, report HWFault

  Copyright: See COPYRIGHT.txt

  Instrument definition file usage:
  Motor motname ASIM lowerlimit upperlimit failpercent [speed]
  
----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <time.h>
#include <math.h>

/* SICS kernel declarations */
#include "sics.h"
#include "modriv.h" /* includes abstract motor defn */
#include "fortify.h"
#include "conman.h"
#include "servlog.h"
#include "splitter.h"
#include "SCinter.h"

/* #include "motor_driver.h" */
/* Ansto-specific simulation driver */
/* #include "sim_motor.h" */
#include "motor_asim.h"

/* ----------------------- Simulation -----------------------------------*/
/* #ifndef SICS_ANSTO_MOTOR_ASIM */
#define SICS_ANSTO_MOTOR_ASIM 
#define MAX_LONG 2147483647

/* device internal status and error codes */
/*      BUSY - motor in motion toward target 
 *      IDLE - movement complete, normal condition
 *      FAULT - no motion, motor awaiting reset
 *      CRASH - no motion, unable to recover
 */
#define ASIM_BUSY 1
#define ASIM_IDLE 2
#define ASIM_FAULT 3
#define ASIM_CRASH 4

 /* the first fields of ASIMDriv structure HAVE to be IDENTICAL to the 
  * abstract motor driver structure in modriv.h (motor_driver.h)
  */                                

  typedef struct ___MoDriv {
    /* general motor driver interface fields. REQUIRED! */
        float fUpper; /* upper limit */
        float fLower; /* lower limit */
        char *name;
        int   (*GetPosition)(void *self,float *fPos);
        int   (*RunTo)(void *self, float fNewVal);
        int   (*GetStatus)(void *self);
        void  (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
        int   (*TryAndFixIt)(void *self,int iError, float fNew);
        int   (*Halt)(void *self);
        int   (*GetDriverPar)(void *self, char *name, float *value);
        int   (*SetDriverPar)(void *self,SConnection *pCon, char *name, 
                float newValue);
        void  (*ListDriverPar)(void *self, char *motorName, SConnection *pCon);
        void  (*KillPrivate)(void *self);
                           
        /* ASIM-specific fields */
        float fFailure;   /* percent random failures */
        int   iFaultType; /* used internally and for debugging */
        float fSpeed;
        long  iDuration;  /* expected duration of move (secs) */
        long  iTime;      /* expected completion time (secs since 1969) */
        long  iRunningTime; /* time (sec) running current move */
        long  iLastPollTime;
        float fPos;       /* simulated current position */
        float fTarget;    /* simulation target position including fault */
        float fDestination; /* requested final position */
        int   iStatus;
  } ASIMDriv;

/* fault types */
#define ASIM_NO_FAULT 0
#define ASIM_HW_FAULT 1
#define ASIM_POS_FAULT 2
#define ASIM_STALL_FAULT 3

/* thresholds for determining relative proportion of fault types   */
/*static const float TH_HW_FAULT = 20.0;    /* ie (20-0) % of faults */
/*static const float TH_STALL_FAULT = 40.0; /* ie (40-20)% of faults */
/*static const float TH_POS_FAULT = 100.0;  /* ie remaining faults   */

/*static const float MIN_SPEED = 0.0001; */

#define TH_HW_FAULT 20.0
#define TH_STALL_FAULT 40.0
#define TH_POS_FAULT 100.0
#define MIN_SPEED 0.0001

/* void KillASIM(void *pData); */
static void UpdateRunningTime(ASIMDriv *);
static void UpdatePosition(ASIMDriv *);
static float SimRandom(void);
static int SimHalt(void *);
static int RunComplete(ASIMDriv *);
static int SimGetPos(void *, float *);
static int SimRun(void *, float);
static void SimError(void *self, int *iCode, char *error, int iErrLen);
static int SimFix(void *self, int iError, float fNew);
static int SimHalt(void *self);
static int SimStat(void *self);

/*-------------------------------------------------------------------------*/
  static float SimRandom(void)
  {
    float fVal;
    fVal = ( (float) rand() / (float)RAND_MAX ) * 100.0;
    return fVal;
  }
/*---------------------------------------------------------------------------*/
  static void UpdateRunningTime(ASIMDriv *pDriv)
  {
    time_t tD;

    time(&tD); /* This fn valid between Jan 1970 and Jan 2038 */
    if (ASIM_BUSY == pDriv->iStatus)
    {
        pDriv->iRunningTime += ((long)tD - pDriv->iLastPollTime);
    }
    pDriv->iLastPollTime = (long)tD;
  }   
/*----------------------------------------------------------------------------*/
  static void UpdatePosition(ASIMDriv *pDriv)
  {
    float fDirn = 1.0;
    UpdateRunningTime(pDriv);
    switch (pDriv->iStatus)
    {
        case ASIM_BUSY:
	        if (pDriv->iRunningTime >= pDriv->iDuration)
		    {
	            switch (pDriv->iFaultType)
	            {
	                case ASIM_POS_FAULT: /* no break */
	                case ASIM_NO_FAULT: 
	                    pDriv->fPos = pDriv->fTarget;
	                    pDriv->iStatus = ASIM_IDLE;
	                    break;
                    default : 
                        pDriv->iFaultType = ASIM_HW_FAULT; /* no break */
	                case ASIM_HW_FAULT:
	                    pDriv->iStatus = ASIM_FAULT;
	                    break;
	                case ASIM_STALL_FAULT:
	                    pDriv->fPos = pDriv->fTarget;
	                    pDriv->iStatus = ASIM_FAULT;                    
	                    break;
	            }
		    }
	        else /* Running. Calculate intermediate position */
	        {
	            switch (pDriv->iFaultType)
	            {
	                case ASIM_POS_FAULT:
	                case ASIM_NO_FAULT: 
	                case ASIM_STALL_FAULT:
                        if (pDriv->fTarget < pDriv->fPos) {
							fDirn = -1.0;
						} else fDirn = 1.0;
	                    pDriv->fPos = pDriv->fTarget - fDirn * pDriv->fSpeed * (pDriv->iDuration - pDriv->iRunningTime);
	                    pDriv->iStatus = ASIM_BUSY;
	                    break;
	                case ASIM_HW_FAULT:
	                    pDriv->iStatus = ASIM_FAULT;
	                    break;
	                default: break; /* error: unknown fault type */
	            }
	        }
            break;
        case ASIM_FAULT:
        case ASIM_IDLE:
        case ASIM_CRASH:
        default: break;
    }    
  }
/*---------------------------------------------------------------------------*/
  static int RunComplete(ASIMDriv *pDriv)
  {
    UpdatePosition(pDriv);
    if(ASIM_BUSY!=pDriv->iStatus) return 1; else return 0;
  }
/*----------------------------------------------------------------------------*/
  static int SimGetPos(void *self, float *fPos)
  {
    int motCode = OKOK;
    ASIMDriv *pDriv;
    assert(self);
    pDriv = (ASIMDriv *)self;
    UpdatePosition(pDriv);
    *fPos = pDriv->fPos;
    switch (pDriv->iStatus)
    {
        case ASIM_IDLE: /* no break */
        case ASIM_BUSY:
                        motCode = OKOK;
                        break;
        default:
        case ASIM_CRASH:
        case ASIM_FAULT:
                        motCode = HWFault;
                        break;
    }    
    return motCode;
  } 
/*----------------------------------------------------------------------------*/
  static int SimRun(void *self, float fVal)
  {
    ASIMDriv *pDriv;
    float fDiff;
    float fFault;
    int   iSucceed;
    float fDirection;
    float fDelta;
    float fDuration;
    time_t tD;
    
    assert(self);
    pDriv = (ASIMDriv *)self;

    switch (pDriv->iStatus)
    {
        case ASIM_IDLE:     pDriv->iStatus = ASIM_BUSY;
        case ASIM_BUSY:   break; 
        default:                      pDriv->iStatus = ASIM_CRASH; 
        case ASIM_CRASH:
        case ASIM_FAULT:  return HWFault;
                                           break;
    }
    
    /* New run command */
    /* Determine desired distance and direction (fDiff >= 0.0)*/
    pDriv->fDestination = fVal;
    fDiff = fVal - pDriv->fPos;
    if(0.0 > fDiff) 
    {
        fDiff = -fDiff;
        fDirection = -1.0;
    }
    else
        fDirection = 1.0;

    /* Determine whether fault will occur */
    if ((0.0 >= pDriv->fFailure)||(SimRandom() > pDriv->fFailure))
        iSucceed = 1;
    else
        iSucceed = 0;
        
    /* Determine delta from actual target by fault mode */
    if (0 == iSucceed)
    {
        fFault = SimRandom();
        if ((TH_HW_FAULT > fFault)||(HWFault==pDriv->iStatus))
        {   /* fail as soon as possible */
            pDriv->iStatus = ASIM_FAULT;
            pDriv->iFaultType = ASIM_HW_FAULT;
            fDelta = -fDiff;
        }
        else 
        {
            if (TH_STALL_FAULT > fFault) 
	        {   /* stall before 80% complete */
                pDriv->iFaultType = ASIM_STALL_FAULT;
	            fDelta = (-1.0) * fDiff * (0.2 + SimRandom()/125.0);
	        }
    	    else 
            {   /* position fault to +/- 10% */
                pDriv->iFaultType = ASIM_POS_FAULT;
                fDelta = fDiff * SimRandom()/1000.0;
                if (50.0 > SimRandom()) fDelta = -fDelta;
            }
        }
    }
    else /* no failure */
    {
        pDriv->iFaultType = ASIM_NO_FAULT;
        fDelta = 0.0;
    }

    /* Calculate target and time using speed */
    if (pDriv->fSpeed >= MIN_SPEED)
    {
        fDiff = fDiff + fDelta;           
        fDuration = fDiff/(pDriv->fSpeed);
        if (fDuration > MAX_LONG) 
            pDriv->iDuration = 2000000000;
        else
            pDriv->iDuration = (long)fDuration;
    }
    else
    {
        fDiff = 0.0;
        pDriv->iDuration = 0;
    }
    pDriv->fTarget = pDriv->fPos + fDirection * fDiff;
    pDriv->iRunningTime = 0;
    pDriv->iLastPollTime = time(&tD);
    pDriv->iTime = tD + pDriv->iDuration;
    
    if(ASIM_BUSY==pDriv->iStatus)
            return OKOK;
    else
            return HWFault;

  } /* SimRun */
  
/*--------------------------------------------------------------------------*/
/* iErrLen fixed to 131 in fn [reportAndFixError] of <motor.c>
 * iCode reported to SimFix via [reportAndFixError]
 */
  static void SimError(void *self, int *iCode, char *error, int iErrLen)
  {
    int motCode = HWFault;
    ASIMDriv *pDriv;
    
    assert(self);
    pDriv = (ASIMDriv *)self;

    switch (pDriv->iStatus)
    {
        case ASIM_IDLE:   motCode = HWIdle; break;
        case ASIM_BUSY:   motCode = HWBusy; break; 
        case ASIM_CRASH:
        case ASIM_FAULT:  
        default:          motCode = HWFault; break;
    }
    
    *iCode = pDriv->iFaultType;
    
    switch (pDriv->iFaultType)
    {
        case ASIM_POS_FAULT:
            strncpy(error,"ERROR: ASIM Position Fault",iErrLen);
            break;
        case ASIM_NO_FAULT: 
            strncpy(error,"STATUS: ASIM No Fault",iErrLen);
            break;
        case ASIM_HW_FAULT:
            strncpy(error,"ERROR: ASIM Motor Hardware Fault",iErrLen);
            break;
        case ASIM_STALL_FAULT:
            strncpy(error,"ERROR: ASIM Motor Stall",iErrLen);
            break;
        default: /* error: unknown fault type */
            strncpy(error,"ERROR: Unknown fault type",iErrLen);
            break;
    }
    assert(strlen(error)<131);
  } /* SimError */
  
/*---------------------------------------------------------------------------*/
  static int SimFix(void *self, int iError, float fNew)
  {
    ASIMDriv *pDriv;
    int iRet;
    float fRand;

    assert(self);
    pDriv = (ASIMDriv *)self;    
 
    switch (pDriv->iStatus)
    {
        case ASIM_IDLE: 
                switch (pDriv->iFaultType)
                {
                        case ASIM_POS_FAULT:   return MOTOK;
                        case ASIM_NO_FAULT:    return MOTOK;
                        case ASIM_HW_FAULT:    return MOTFAIL;
                        case ASIM_STALL_FAULT: return MOTREDO;
                        default: return MOTFAIL; /* error: unknown fault type */
                }
                break;
        case ASIM_BUSY:  return MOTOK;   break; 
        case ASIM_CRASH: return MOTFAIL; break;
        case ASIM_FAULT:  
                SimHalt(self);
                if(TH_HW_FAULT >= SimRandom()) 
                {
                    pDriv->iStatus = ASIM_CRASH;
                    return MOTFAIL;
                }
                pDriv->iStatus = ASIM_IDLE;
                pDriv->iFaultType = ASIM_NO_FAULT;
                return MOTOK;
                break;
        default:      
                pDriv->iStatus = ASIM_CRASH; 
                pDriv->iFaultType = ASIM_HW_FAULT;
                return MOTFAIL;
    } 
/*    SICSLogWrite("Attempt fix for simulated motor",eHWError); */
  } /* SimFix */

/*--------------------------------------------------------------------------*/
  static int SimHalt(void *self)
  {
    int motCode = OKOK;
    ASIMDriv *pDriv;
    assert(self);
    
    pDriv = (ASIMDriv *)self;
    UpdatePosition(pDriv);
    pDriv->iTime = 0;
    pDriv->iDuration = 0;
    pDriv->fTarget = pDriv->fPos;
    pDriv->fDestination = pDriv->fPos;
    switch (pDriv->iStatus)
    {
        case ASIM_BUSY: pDriv->iStatus = ASIM_IDLE;
        case ASIM_IDLE: motCode = OKOK;
                        break;
        default:        motCode = HWFault;
                        break;
    }
    return motCode;
  }
/*--------------------------------------------------------------------------*/
  static int SimStat(void *self)
  {
    int motCode = HWIdle;
    ASIMDriv *pDriv;
    assert(self);
    pDriv = (ASIMDriv *)self;

    UpdatePosition(pDriv);    
    switch (pDriv->iStatus)
    {
        case ASIM_BUSY: motCode = HWBusy;  break;
        case ASIM_IDLE: motCode = HWIdle;  break;
        default:        motCode = HWFault; break;
    }
    return motCode;
  }
/*-----------------------------------------------------------------------*/
static int SimSetPar(void *self, SConnection *pCon, char *name, float newValue)
{
  int iSuccess = 0;
  ASIMDriv *pDriv = (ASIMDriv *) self;

  assert(self);
  assert(pCon);

  if(strcmp(name,"hardupperlim") == 0)
  {
    pDriv->fUpper = newValue;
    iSuccess = 1;
  }
  if(strcmp(name,"hardlowerlim") == 0)
  {
    pDriv->fLower = newValue;
    iSuccess = 1;
  }
  if(strcmp(name,"speed") == 0)
  {
    pDriv->fSpeed = newValue;
    iSuccess = 1;
  } 
  return iSuccess;
}

/*-----------------------------------------------------------------------*/
static int SimGetPar(void *self, char *name, float *value)
{
    int iSuccess = 0;
    ASIMDriv *pDriv = (ASIMDriv *) self;
    assert(self);

    if (0==strcmp("hardupperlim",name))
    {
        *value = pDriv->fUpper;
        iSuccess = 1;
    }
    if (0==strcmp("hardlowerlim",name))
    {
        *value = pDriv->fLower;
        iSuccess=1;
    }
    return iSuccess;
}

/*-----------------------------------------------------------------------*/
static void SimListPar(void *self, char *motorName, SConnection *pCon)
{
    ASIMDriv *pDriv = (ASIMDriv *) self;
    char pBuffer[256];
    
    assert(self);
    assert(pCon);

    sprintf(pBuffer,"%s.%s = %f",motorName,"hardupperlim",pDriv->fUpper);
    SCWrite(pCon,pBuffer,eValue);
    sprintf(pBuffer,"%s.%s = %f",motorName,"hardlowerlim",pDriv->fLower);
    SCWrite(pCon,pBuffer,eValue);
}

/*---------------------------------------------------------------------------*/
 static void SimKill(void *pData)
{
  /*
    the controller is owned by the controller object and will be
    deleted when that object is removed
  */
    return;
}
 
/*--------------------------------------------------------------------------*/
MotorDriver *CreateASIM(SConnection *pCon, int argc, char *argv[])
{
    TokenList *pList = NULL;
    TokenList *pCurrent;
    ASIMDriv *pDriv = NULL;
    time_t tD;
     
    assert(pCon);
     
    /* check number of arguments */
    if(argc < 3)
    {
        SCWrite(pCon,"Insufficient numbers of arguments for ASIM Motor",eError);
        return NULL;
    }
     
     /* split arguments */
     pList = SplitArguments(argc, argv);
     if(!pList)
     {
       SCWrite(pCon,"Error parsing arguments in ASIM Motor",eError);
       return NULL;
     }
     
     /* allocate memory */
     pDriv = (ASIMDriv *)malloc(sizeof(ASIMDriv));
     if(!pDriv)
     {
       SCWrite(pCon,"Error allocating memory in ASIM Motor",eError);
       DeleteTokenList(pList);
       return NULL;
     }
     memset(pDriv,0,sizeof(ASIMDriv));
     
     /* check and enter args, first lowerLimit */
     pCurrent = pList;
     if(pCurrent->Type == eInt)
     {
       pDriv->fLower = (float)pCurrent->iVal;
     }
     else if(pCurrent->Type == eFloat)
     {
       pDriv->fLower = pCurrent->fVal;
     }
     else
     {
       SCWrite(pCon,"ERROR: Expect numerical 'lower limit' at argument 1 after Motor ASIM",eError);
       free(pDriv);
       DeleteTokenList(pList);
       return NULL;
     }

     /* then, upper limit */     
     pCurrent = pCurrent->pNext;
     if(pCurrent->Type == eInt)
     {
       pDriv->fUpper = (float)pCurrent->iVal;
     }
     else if(pCurrent->Type == eFloat)
     {
       pDriv->fUpper = pCurrent->fVal;
     }
     else
     {
       SCWrite(pCon,"ERROR: Expect numerical 'upper limit' at argument 2 after Motor ASIM",eError);
       free(pDriv);
       DeleteTokenList(pList);
       return NULL;
     }
     
     /* thirdly, failure rate */
     pCurrent = pCurrent->pNext;
     if(pCurrent->Type == eInt)
     {
       pDriv->fFailure = (float)pCurrent->iVal;
     }
     else if(pCurrent->Type == eFloat)
     {
       pDriv->fFailure = pCurrent->fVal;
     }
     else
     {
       SCWrite(pCon,"ERROR: Expect numerical 'failure rate' at argument 3 after Motor ASIM",eError);
       free(pDriv);
       DeleteTokenList(pList);
       return NULL;
     }
    
    /* calculate current position, initialise func pters */
    pDriv->fPos = (pDriv->fUpper + pDriv->fLower)/2.0;
    /*     pDriv->name = strdup("SICS_ANSTO_MOTOR_ASIM"); */    
    pDriv->name = malloc((1+strlen("SICS_ANSTO_MOTOR_ASIM"))*sizeof(char));
    strcpy(pDriv->name,"SICS_ANSTO_MOTOR_ASIM");
    
    pDriv->GetPosition = SimGetPos;
    pDriv->RunTo = SimRun;
    pDriv->GetStatus = SimStat;
    pDriv->GetError = SimError;
    pDriv->TryAndFixIt = SimFix;
    pDriv->Halt = SimHalt;
    pDriv->SetDriverPar = SimSetPar;
    pDriv->GetDriverPar = SimGetPar;
    pDriv->ListDriverPar = SimListPar;
    pDriv->KillPrivate = SimKill;
     
    /* set default parameters */
    pDriv->fSpeed = 0.01;
    pDriv->iTime = time(&tD);
    pDriv->iFaultType = ASIM_NO_FAULT;
    pDriv->iDuration = 0;
    pDriv->iRunningTime = 0;
    pDriv->iLastPollTime = pDriv->iTime;
    pDriv->fTarget = pDriv->fPos;
    pDriv->fDestination = pDriv->fPos;
    pDriv->iStatus = OKOK;
    
    srand( (unsigned)time( NULL ) );

    /* check for optional speed paramter */
    pCurrent = pCurrent->pNext;
    if(pCurrent)
    {
        if(pCurrent->Type == eFloat)
        {
            pDriv->fSpeed = pCurrent->fVal;
            if(pDriv->fSpeed < 0.0) pDriv->fSpeed = 0.0 - pDriv->fSpeed;
        }
    }
    DeleteTokenList(pList);
    return (MotorDriver *)pDriv;
}