/*---------------------------------------------------------------------------
  REFL2T.c

Paul Hathaway, Nick Hauser, Mark Koennecke, February 2004
---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include "fortify.h"
#include <tcl.h>
#include "sics.h"
#include "motor.h"
#include "obpar.h"

#define DEBUG 1

#define MAXMOT 3
#define MAXPAR 13

#define PI 3.14157

#include "refl2t.h"


/* detector height movement */
#define MOTCOZ 0
/* detector movement along main axis */
#define MOTCOY 1
/* whole sample table height movement */
#define MOTSOZ 2


/*======================================================================
   The core of it all: The calculation of the settings for the various
   motors.
========================================================================*/
   static int CalculateREFL(pRefl2T self, SConnection *pCon, float fNew)
   {
      float fDetPos, HeightOffset, DetLength;
      int iRet;

/* assume: solid sample, horizontal beam, surface at height zero */
/* read vals of motor coy */
      iRet = MotorGetSoftPosition(self->aEngine[MOTCOY],pCon,&fDetPos);
      if(iRet != 1)
      {
        return iRet;
      }

      HeightOffset = ObVal(self->aParameter,PARDHO);
      DetLength = ObVal(self->aParameter,PARDDD) + fDetPos;

/* calculate det height above beam axis */
/* add detector offset height */

      fNew = DetLength * tan((double)(fNew * PI / 180.0)) + HeightOffset ;
      self->fTarget = fNew;
      return 1;

/* Replacement code using MotEntry structure
      self->toStart[MOTCOZ].pMot = self->aEngine[MOTCOZ];
      strcpy(self->toStart[MOTCOZ].pName,self->aEngine[MOTCOZ]->name);
      self->toStart[MOTCOZ].fTarget = DetLength * tan((double)(fNew * PI / 180.0)) + HeightOffset;
 */
   }

/*========================================================================
   Definition of interface functions.
=========================================================================*/
   static long R2TSetValue(void *pData, SConnection *pCon, float fNew)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pRefl2T self = (pRefl2T) pData;

      assert(self);

      /* calculation */
      iRet = CalculateREFL(self,pCon,fNew);
      if(iRet != 1)
      {
        return iRet;
      }

      /* start them all */
      /* for(i = 0; i < self->iStart; i++)
      {*/
        pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
                                            self->aEngine[MOTCOZ].pMot,DRIVEID); /* DRIVEID ??? */
        if(pDriv != NULL)
	{
           iRet = pDriv->SetValue(self->tEngine[MOTCOZ].pMot,pCon,
                                  self->fTarget); /* works for one motor MOTCOZ */
           if(iRet != OKOK)
	   {
              return iRet;
           }
        }
      /* } */
      return OKOK;
   }


/*-------------------------------------------------------------------------*/
   static int R2THalt(void *pData)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pRefl2T self = (pRefl2T) pData;

      assert(self);

      /* stop them all */
      /*for(i = 0; i < self->iStart; i++)
      {*/
        pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
                                            self->aEngine[MOTCOZ].pMot,DRIVEID);
        if(pDriv != NULL)
	{
           iRet = pDriv->Halt(self->aEngine[MOTCOZ].pMot);
        }
      /* } */
      return OKOK;
   }
/*-----------------------------------------------------------------------*/
   static int R2TCheck(void *pData, float fNew, char *error, int iErrLen)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pRefl2T self = (pRefl2T) pData;
      SConnection *pDumCon = NULL;

/* Define code for this function */
      assert(self);
      pDumCon = SCCreateDummyConnection(pServ->pSics);
      assert(pDumCon);

      /* calculation */
      iRet = CalculateREFL(self,pDumCon,fNew);
      SCDeleteConnection(pDumCon);
      if(iRet != 1)
      {
        return iRet;
      }

      /* check them all */
/*      for(i = 0; i < self->iStart; i++)
      {*/
        pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
                                            self->aEngine[MOTCOZ].pMot,DRIVEID);
        if(pDriv != NULL)
	{
           iRet = pDriv->CheckLimits(self->aEngine[MOTCOZ].pMot,
                                     self->aEngine[MOTCOZ].fTarget,
                                     error,iErrLen);
           if(iRet != 1)
	   {
              return iRet;
           }
        }
      /* } */
      return 1;
   }

   static float R2TGetValue(void *pData, SConnection *pCon)
   {

     pRefl2T self = (pRefl2T) pData;

     assert(self);

     return fResult;
   }

   static void *R2TGetInterface(void *pData, int iID)
   {
     pRefl2T self = (pRefl2T) pData;

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


   static void Refl2TKill(void *pData)
   {
     pRefl2T self = (pRefl2T) pData;

     if(self == NULL)
        return;

     if(self->pDes)
        DeleteDescriptor(self->pDes);

     if(self->pDriv)
       free(self->pDriv);

     if(self->aParameter)
        ObParDelete(self->aParameter);

     free(self);
   }
/*--------------------------------------------------------------------------
  Initialization: All is done from the Factory function. This takes an Tcl
  array as parameter which is supposed to hold the names of all motors.
  This must fail if one of the motors cannot be accessed.
--------------------------------------------------------------------------*/
   int Refl2TFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                     int argc, char *argv[])
   {
       pRefl2T pNew;
       int i, iRet;
       char pBueffel[512];
       char *pMot = NULL;

       if(argc < 1)
       {
          SCWrite(pCon,
            "ERROR: Insufficient number of arguments to Refl2tFactory",
             eError);
          return 0;
       }

       /* allocate space ..............*/
       pNew = (pRefl2T)malloc(sizeof(Refl2T));
       if(!pNew)
       {
          SCWrite(pCon,"ERROR: out of memory in Refl2TFactory",eError);
          return 0;
       }
       memset(pNew,0,sizeof(Refl2T));
       pNew->pDes = CreateDescriptor("Refl2T");
       pNew->aParameter = ObParCreate(MAXPAR);
       pNew->pDriv = CreateDrivableInterface();
       if( (!pNew->pDes) || (!pNew->aParameter) || (!pNew->pDriv) )
       {
          SCWrite(pCon,"ERROR: out of memory in Refl2TFactory",eError);
          Refl2TKill(pNew);
          return 0;
       }

       /* find the motors*/
       pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"coz",TCL_GLOBAL_ONLY);
       if(!pMot)
       {
          SCWrite(pCon,"ERROR: no value for coz motr found",eError);
          Refl2TKill(pNew);
          return 0;
       }
       pNew->aMot[MOTCOZ] = FindMotor(pSics,pMot);
       if(!pNew->aEngine[MOTCOZ])
       {
          sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
          SCWrite(pCon,pBueffel,eError);
          Refl2TKill(pNew);
          return 0;
       }

       pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"coy",TCL_GLOBAL_ONLY);
       if(!pMot)
       {
          SCWrite(pCon,"ERROR: no value for coy motor found",eError);
          Refl2TKill(pNew);
          return 0;
       }
       pNew->aEngine[MOTCOY] = FindMotor(pSics,pMot);
       if(!pNew->aEngine[MOTCOY])
       {
          sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
          SCWrite(pCon,pBueffel,eError);
          Refl2TKill(pNew);
          return 0;
       }

       pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"soz",TCL_GLOBAL_ONLY);
       if(!pMot)
       {
          SCWrite(pCon,"ERROR: no value for coz motor found",eError);
          Refl2TKill(pNew);
          return 0;
       }
       pNew->aEngine[MOTSOZ] = FindMotor(pSics,pMot);
       if(!pNew->aEngine[MOTSOZ])
       {
          sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
          SCWrite(pCon,pBueffel,eError);
          Refl2TKill(pNew);
          return 0;
       }

       /* initialize parameters */
       ObParInit(pNew->aParameter,PARDDD,"detectord",1400.,usMugger);
       ObParInit(pNew->aParameter,PARDHO,"heightoffset",50.,usMugger);
       ObParInit(pNew->aParameter,PARDSO,"stageoffset",50.,usMugger);

       /* initialize interfaces */
       pNew->pDes->GetInterface = R2TGetInterface;
       pNew->pDes->SaveStatus =   R2TSave;
       pNew->pDriv->Halt = R2THalt;
       pNew->pDriv->CheckLimits = R2TCheck;
       pNew->pDriv->SetValue = R2TSetValue;
       pNew->pDriv->CheckStatus = R2TStatus;
       pNew->pDriv->GetValue = R2TGetValue;

       /* install commands */
       iRet = AddCommand(pSics,argv[1],
                        Refl2TAction,Refl2TKill,pNew);
       if(!iRet)
       {
         sprintf(pBueffel,"ERROR: duplicate command %s NOT created",
                 argv[1]);
         SCWrite(pCon,pBueffel,eError);
         Refl2TKill(pNew);
         return 0;
       }
       return 1;
   }

/*----------------------------------------------------------------------*/
   int Refl2TAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                    int argc, char *argv[])
   {
      pRefl2T self = (pRefl2T)pData;
      char pBueffel[256];
      float fVal;
      double dVal;
      ObPar *pPar = NULL;
      int iRet;

      assert(self);

      if(argc > 1)
      {
         strtolower(argv[1]);
         /* deal with list */
         if(strcmp(argv[1],"list") == 0)
	 {
            R2TList(self,pCon,argv[0]);
            return 1;
         }
         /* otherwise it should be a parameter command */
         if(argc >= 3)
	 {
           iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
           if(iRet != TCL_OK)
	   {
             sprintf(pBueffel,"ERROR: failed to convert %s to number",
             argv[2]);
             SCWrite(pCon,pBueffel,eError);
             return 0;
           }
           iRet = ObParSet(self->aParameter,argv[0],argv[1],(float)dVal,pCon);
           if(iRet)
	   {
             SCSendOK(pCon);
           }
           return iRet;
         }
         else /* argc = 2 */
	 {
           pPar = ObParFind(self->aParameter,argv[1]);
           if(!pPar)
	   {
             sprintf(pBueffel,"ERROR: parameter %s NOT found",argv[1]);
             SCWrite(pCon,pBueffel,eError);
             return 0;
           }
           sprintf(pBueffel,"%s.%s = %f",argv[0],pPar->name, pPar->fVal);
           SCWrite(pCon,pBueffel,eValue);
           return 1;
         }
      }
      else
      {
        fVal = self->pDriv->GetValue(self,pCon);
        sprintf(pBueffel," %s = %f", argv[0], fVal);
        SCWrite(pCon,pBueffel,eValue);
        return 1;
      }
   }

   static void R2TList(pRefl2T self, SConnection *pCon, char *name)
   {
      char pBueffel[132];
      Tcl_DString tString;

      assert(pCon);
      assert(self);

      Tcl_DStringInit(&tString);
      sprintf(pBueffel,
            "%s.detectord %f \n", name, ObVal(self->aParameter,PARDDD));
      Tcl_DStringInit(&tString);
      sprintf(pBueffel,
            "%s.heightoffset %f \n", name, ObVal(self->aParameter,PARDHO));
      Tcl_DStringInit(&tString);
      sprintf(pBueffel,
            "%s.sampleoffset %f \n", name, ObVal(self->aParameter,PARDSO));
   }