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

  Mark Koennecke, November 2004

  Fixed to work with second generation motors too

  Mark Koennecke, September 2013
------------------------------------------------------------------------*/
#include <stdio.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "task.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->pDrivInt->Halt(self->pMot);
    self->stopFlag = 1;
    self->taskID = -1;
  }
  MotorSetPar(self->pMot, self->pCon, "accesscode", usUser);
  if (self->debug > 0) {
    Log(INFO,"com","%s","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->pDrivInt->CheckStatus(self->pMot,pServ->dummyCon);
  switch (status) {
  case HWFault:
  case HWPosFault:
    Log(ERROR,"com","%s:%s","oscillator:", 
		      "ERROR occurred in oscillation, try running motor manually to find out more");
    Log(INFO,"com","%s:%s","oscillator", 
		      "Trying to run other direction");
    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);
      Log(INFO,"com","%s:%s","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);
      Log(INFO,"com","%s:%s","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
   */
  snprintf(pBueffel,sizeof(pBueffel),"Oscillate-%s", self->pMot->name);
  self->taskID = TaskRegisterN(pServ->pTasker,pBueffel,
                              OscillationTask, NULL, NULL, self, TASK_PRIO_HIGH);
  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;
}