/*-----------------------------------------------------------------------
  Oscillator runs a motor back and forth between its software limits.
  
  copyright: see file COPYRIGHT

  Mark Koennecke, November 2004
------------------------------------------------------------------------*/
#include <stdio.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "task.h"
#include "commandlog.h"
#include "oscillate.h"

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

/*================== real work =========================================*/
static void StopOscillation(pOscillator self){
  assert(self != NULL);
  if(self->taskID > 0){
    self->pMot->pDriver->Halt(self->pMot->pDriver);
    self->stopFlag = 1;
    self->taskID = -1;
  }
  MotorSetPar(self->pMot,self->pCon,"accesscode",usUser);
  if(self->debug > 0){
    WriteToCommandLog("oscillator>> ","Stopping");
  }
}
/*-------------------------------------------------------------------*/
static float getNextPos(pOscillator self){
  float pos;
  if(self->nextTargetFlag == 1){
    pos = self->upperLimit;
    self->nextTargetFlag = 0;
  } else {
    pos = self->lowerLimit;
    self->nextTargetFlag = 1;
  }
  return pos;
}
/*-------------------------------------------------------------------*/
static float getCurrentTarget(pOscillator self){
  float pos;
  if(self->nextTargetFlag == 1){
    pos = self->lowerLimit;
  } else {
    pos = self->upperLimit;
  }
  return pos;
}
/*---------------------------------------------------------------------*/
static int OscillationTask(void *data){
  pOscillator self = (pOscillator)data;
  int status, code, errStatus;
  char error[256], message[132];
  float pos, curPos;


  assert(self);
  if(self->stopFlag == 1){
    return 0;
  }

  status = self->pMot->pDriver->GetStatus(self->pMot->pDriver);
  switch(status){
  case HWFault:
  case HWPosFault:
    self->pMot->pDriver->GetError(self->pMot->pDriver,&code,error,255);
    WriteToCommandLog("oscillator>> ",error);
    pos = getCurrentTarget(self);
    errStatus = self->pMot->pDriver->TryAndFixIt(self->pMot->pDriver,code,pos);
    self->errorCount++;
    if(errStatus == MOTFAIL){
      /*
	try driving the other way on a serious error
      */
      pos = getNextPos(self);
      status = MotorRun(self->pMot,self->pCon,pos);
      if(self->debug > 0){
	snprintf(message,131,"Started oscillation to %f, ret code = %d",
		 pos,status);
	WriteToCommandLog("oscillator>>",message);
      }
    }
    break;
  case HWWarn:
    MotorGetSoftPosition(self->pMot,self->pCon,&curPos);
    pos = getCurrentTarget(self);
    if(ABS(curPos - pos) < .5){
      status = MotorRun(self->pMot,self->pCon,getNextPos(self));
    } 
    break;
  case HWBusy:
    break;
  case HWIdle:
    pos = getNextPos(self);
    status = MotorRun(self->pMot,self->pCon,pos);
    if(status == OKOK){
      self->pMot->pDrivInt->iErrorCount = 0;
    }
    if(self->debug > 0){
      snprintf(message,131,"Started oscillation to %f, ret code = %d",
	       pos,status);
      WriteToCommandLog("oscillator>>",message);
    }
  }
  return 1;
}
/*--------------------------------------------------------------------*/
static int StartOscillation(pOscillator self, SConnection *pCon){
  float fval;
  int status;
  char error[80], pBueffel[255];  

  assert(self);

  if(self->taskID > 0){
    SCWrite(pCon,"WARNING: oscillation already running",eWarning);
    SCWrite(pCon,"WARNING: restarting .. ",eWarning);
    StopOscillation(self);
    SicsWait(2);
  } 

  MotorGetPar(self->pMot,"softlowerlim",&self->lowerLimit);
  self->lowerLimit += .5;
  MotorGetPar(self->pMot,"softupperlim",&self->upperLimit);
  self->upperLimit -= .5;
  MotorSetPar(self->pMot,self->pCon,"accesscode",(float)usInternal);
  self->nextTargetFlag = 0;
  self->errorCount = 0;
  self->stopFlag = 0;

  /*
    check reachability of limits
  */
  status = MotorCheckBoundary(self->pMot,self->lowerLimit,&fval,error,79);
  if(!status){
    snprintf(pBueffel,255,"ERROR: cannot reach %f: %s reported",
	     self->lowerLimit,error);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = MotorCheckBoundary(self->pMot,self->upperLimit,&fval,error,79);
  if(!status){
    snprintf(pBueffel,255,"ERROR: cannot reach %f: %s reported",
	     self->upperLimit,error);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }

  /*
    start task
  */
  self->taskID = TaskRegister(pServ->pTasker,
			      OscillationTask,
			      NULL,
			      NULL,
			      self,
			      10);
  if(self->taskID < 0){
    SCWrite(pCon,"ERROR: failed to start oscillation task",eError);
    return 0;
  }
  return 1;
}
/*===================== life and death =================================*/
static void KillOscillator(void *data){
  pOscillator self = (pOscillator)data; 
  if(self != NULL){
    if(self->pDes != NULL){
      DeleteDescriptor(self->pDes);
    }
    if(self->pCon != NULL){
      SCDeleteConnection(self->pCon);
    }
    free(self);
  }
}
/*========================================================================*/
int MakeOscillator(SConnection *pCon, SicsInterp *pSics, void *pData,
		   int argc, char *argv[]){
  pOscillator pNew = NULL;
  pMotor pMot = NULL;
  char pBueffel[132];
  int status;

  if(argc < 3){
    SCWrite(pCon,"ERROR: insufficient number of arguments to MakeOscilator",
	    eError);
    return 0;
  }

  pMot = FindMotor(pSics,argv[2]);
  if(pMot == NULL){
    snprintf(pBueffel,131,"ERROR: %s is no motor",argv[2]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }

  pNew = (pOscillator)malloc(sizeof(Oscillator));
  if(pNew == NULL){
    SCWrite(pCon,"ERROR: out of memory creating oscillator",eError);
    return 0;
  }
  memset(pNew,0,sizeof(Oscillator));
  pNew->pDes = CreateDescriptor("Oscillator");
  pNew->pMot = pMot;
  pNew->pCon = SCCreateDummyConnection(pSics);
  if(!pNew->pDes || !pNew->pCon){
    SCWrite(pCon,"ERROR: out of memory creating oscillator",eError);
    return 0;
  }
  SCSetWriteFunc(pNew->pCon,SCFileWrite);
  SCSetRights(pNew->pCon,usInternal);
  
  status = AddCommand(pSics,argv[1],
		      OscillatorWrapper,
		      KillOscillator,
		      pNew);
  if(!status){
    snprintf(pBueffel,131,"ERROR: duplicate command %s not created",argv[1]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  return 1;
}
/*========================================================================*/
int OscillatorWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
		      int argc, char *argv[]){
  pOscillator self = (pOscillator)pData;
  char pBueffel[256];

  assert(self);
  if(argc < 2){
    SCWrite(pCon,"ERROR: need start/stop argument for oscillator",eError);
    return 0;
  }

  if(!SCMatchRights(pCon,usUser)){
    return 0;
  }
  
  strtolower(argv[1]);
  if(strcmp(argv[1],"start") == 0){
    return StartOscillation(self,pCon);
  } else if(strcmp(argv[1],"stop") == 0) {
    StopOscillation(self);
    snprintf(pBueffel,255,"Oscillation stopped with %d errors, %s",
	     self->errorCount,
	     "see commandlog for details");
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  } else if(strcmp(argv[1],"debug") == 0) {
    if(argc >= 3){
      self->debug = atoi(argv[2]);
      SCSendOK(pCon);
      return 1;
    }
    snprintf(pBueffel,255,"%s.debug = %d", argv[0],self->debug);
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  } else if(strcmp(argv[1],"status") == 0) {
    if(self->taskID > 0){
      snprintf(pBueffel,255,"Oscillation running, %d errors so far, %s",
	       self->errorCount,
	       " error details in commandlog");
    } else {
      snprintf(pBueffel,255,"Oscillation stopped");
    }
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  } else {
    SCWrite(pCon,"ERROR: invalid sub command for oscillator requested",
	    eError);
    return 0;
  }
  return 1;
}