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

                             L I N 2 A N G

  A virtual motor device for driving an angle through a translation table.
  As of now special for TOPSI.

  copyright: see copyright.h

  Mark Koennecke, February 2000

  added zero point handling for the Jochen
  Mark Koennecke, December 2005
  ---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "lin2ang.h"

static const float RD = 57.2957795, pi = 3.1415926;

/* --------- our very own private data structure ------------------------*/
typedef struct __LIN2ANG {
  pObjectDescriptor pDes;
  pIDrivable pDriv;
  pMotor lin;
  float length;
  float zero;
} Lin2Ang, *pLin2Ang;

/*-------------------------- conversion routines -------------------------*/
static float ang2x(pLin2Ang self, float fAngle)
{
  return self->length * tan((fAngle + self->zero) / RD);
}

/*-----------------------------------------------------------------------*/
static float x2ang(pLin2Ang self, float fX)
{
  double dt;

  assert(self->length > 0.);

  dt = fX / self->length;
  return RD * atan(dt) - self->zero;
}

/*============== functions in the interface ============================*/
static void *Lin2AngGetInterface(void *pData, int iID)
{
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);

  if (iID == DRIVEID) {
    return self->pDriv;
  }
  return NULL;
}

/*----------------------------------------------------------------------*/
static int Lin2AngSave(void *pData, char *name, FILE * fd)
{
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  if (!self)
    return 0;

  fprintf(fd, "%s length %f\n", name, self->length);
  fprintf(fd, "%s softzero %f\n", name, self->zero);
  return 1;

}

/*-----------------------------------------------------------------------*/
static int L2AHalt(void *pData)
{
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);

  return self->lin->pDrivInt->Halt(self->lin);
}

/*------------------------------------------------------------------------*/
static int L2ALimits(void *pData, float fVal, char *error, int iErrlen)
{
  float fX;
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);

  fX = ang2x(self, fVal);
  return self->lin->pDrivInt->CheckLimits(self->lin, fX, error, iErrlen);
}

/*-----------------------------------------------------------------------*/
static float L2AGetValue(void *pData, SConnection * pCon)
{
  float fX, zero = 0.;
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);
  MotorGetSoftPosition(self->lin, pCon, &fX);
  return x2ang(self, fX);
}

/*------------------------------------------------------------------------*/
static int L2AStatus(void *pData, SConnection * pCon)
{
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);
  return self->lin->pDrivInt->CheckStatus(self->lin, pCon);
}

/*------------------------------------------------------------------------*/
static long L2ASetValue(void *pData, SConnection * pCon, float fValue)
{
  float fX;
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  assert(self);

  fX = ang2x(self, fValue);
  return self->lin->pDrivInt->SetValue(self->lin, pCon, fX);
}

/*--------------------------------------------------------------------*/
static void KillL2A(void *pData)
{
  pLin2Ang self = NULL;

  self = (pLin2Ang) pData;
  if (!self)
    return;

  if (self->pDes) {
    DeleteDescriptor(self->pDes);
  }
  if (self->pDriv) {
    free(self->pDriv);
  }
  free(self);
}

/*-------------------------------------------------------------------
  Syntax: MakeLin2Ang name motor
*/
int MakeLin2Ang(SConnection * pCon, SicsInterp * pSics, void *pData,
                int argc, char *argv[])
{
  pLin2Ang pNew = NULL;
  char pBueffel[255];
  int iRet;

  /* check number of arguments */
  if (argc < 3) {
    SCWrite(pCon, "ERROR: Insufficient arguments to Lin2Arg", eError);
    return 0;
  }

  /* allocate memory */
  pNew = (pLin2Ang) malloc(sizeof(Lin2Ang));
  if (!pNew) {
    SCWrite(pCon, "ERROR: out of memory in MakeLin2Ang", eError);
    return 0;
  }
  memset(pNew, 0, sizeof(Lin2Ang));
  pNew->pDes = CreateDescriptor("Lin2Ang");
  if (!pNew->pDes) {
    SCWrite(pCon, "ERROR: out of memory in MakeLin2Ang", eError);
    free(pNew);
    return 0;
  }
  pNew->pDriv = CreateDrivableInterface();
  if (!pNew->pDriv) {
    SCWrite(pCon, "ERROR: out of memory in MakeLin2Ang", eError);
    KillL2A(pNew);
    return 0;
  }

  /* check if we got a motor */
  pNew->lin = FindMotor(pSics, argv[2]);
  if (!pNew->lin) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: %s is no motor!", argv[2]);
    SCWrite(pCon, pBueffel, eError);
    KillL2A(pNew);
    return 0;
  }


  /* initialize the data structure */
  pNew->pDes->GetInterface = Lin2AngGetInterface;
  pNew->pDes->SaveStatus = Lin2AngSave;
  pNew->pDriv->Halt = L2AHalt;
  pNew->pDriv->CheckLimits = L2ALimits;
  pNew->pDriv->SetValue = L2ASetValue;
  pNew->pDriv->CheckStatus = L2AStatus;
  pNew->pDriv->GetValue = L2AGetValue;
  pNew->length = 80.;

  /* install command */
  iRet = AddCommand(pSics, argv[1], Lin2AngAction, KillL2A, pNew);
  if (!iRet) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: duplicate Lin2Ang command %s NOT created", argv[1]);
    SCWrite(pCon, pBueffel, eError);
    KillL2A(pNew);
    return 0;
  }
  return 1;
}

/*--------------------------------------------------------------------*/
int Lin2AngAction(SConnection * pCon, SicsInterp * pSics, void *pData,
                  int argc, char *argv[])
{
  pLin2Ang self = NULL;
  char pBueffel[255];
  float fVal, fLow, fHigh;
  double dVal;
  int iRet;

  self = (pLin2Ang) pData;
  assert(self);
  assert(pCon);

  /* without parameter: give value */
  if (argc < 2) {
    fVal = L2AGetValue(self, pCon);
    snprintf(pBueffel,sizeof(pBueffel)-1, "%s = %f", argv[0], fVal);
    SCWrite(pCon, pBueffel, eError);
    return 1;
  }

  /* interpret commands */
  strtolower(argv[1]);
  if (strcmp(argv[1], "length") == 0) {
    if (argc >= 3) {
      iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dVal);
      if (iRet != TCL_OK) {
        SCWrite(pCon, "ERROR: length parameter not recognised as number",
                eError);
        return 0;
      }
      if (!SCMatchRights(pCon, usUser)) {
        SCWrite(pCon, "ERROR: Insufficient privilege to change length",
                eError);
        return 0;
      }
      self->length = dVal;
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%s.length = %f", argv[0], self->length);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  }
/* zero point */
  if (strcmp(argv[1], "softzero") == 0) {
    if (argc >= 3) {
      iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dVal);
      if (iRet != TCL_OK) {
        SCWrite(pCon, "ERROR: softzero parameter not recognised as number",
                eError);
        return 0;
      }
      if (!SCMatchRights(pCon, usUser)) {
        SCWrite(pCon,
                "ERROR: Insufficient privilege to change softzero point",
                eError);
        return 0;
      }
      self->zero = dVal;
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%s.softzero = %f", argv[0], self->zero);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  }
/* limits */
  if (strstr(argv[1], "lim") != NULL) {
    MotorGetPar(self->lin, "softupperlim", &fHigh);
    MotorGetPar(self->lin, "softlowerlim", &fLow);
    fHigh = x2ang(self, fHigh);
    fLow = x2ang(self, fLow);
    snprintf(pBueffel,sizeof(pBueffel)-1, "%s.limits: %f %f\n change through motor limits ",
            argv[0], fLow, fHigh);
    SCWrite(pCon, pBueffel, eValue);
    return 1;
  }
  snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: method %s not found!", argv[1]);
  SCWrite(pCon, pBueffel, eError);
  return 0;
}