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

                             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*sin((fAngle+self->zero)/RD);
  } 
/*-----------------------------------------------------------------------*/
  static float x2ang(pLin2Ang self, float fX)
  {
    double dt;

    assert(self->length > 0.);

    dt = fX/self->length;
    return RD*asin(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)
     {
       sprintf(pBueffel,"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)
     {
       sprintf(pBueffel,
          "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);
           sprintf(pBueffel,"%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
           {
              sprintf(pBueffel,"%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
           {
              sprintf(pBueffel,"%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);
          sprintf(pBueffel,"%s.limits: %f %f\n change through motor limits ",
                  argv[0],fLow,fHigh);
          SCWrite(pCon,pBueffel,eValue);
          return 1;
        }       
        sprintf(pBueffel,"ERROR: method %s not found!",argv[1]);
        SCWrite(pCon, pBueffel,eError);
        return 0;
  }