/*---------------------------------------------------------------------------
                                A M O R 2 T

  A class for controlling the two theta movement of the reflectometer
  AMOR at SINQ. It is not clear if this class may be useful for other
  reflectometers, too. At AMOR the two theta movement of the detector is 
  realized by translating the detector along x and z. Also it can be 
  tilted in omega. Furthermore the height of two diaphragms has to be
  adjusted when moving two theta as well. In polarizing mode the analyzer
  mirror has to be moved as well.

  copyright: see copyright.h

  Mark Koennecke, September 1999
---------------------------------------------------------------------------*/
#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 9
#define MAXPAR 8

#include "amor2t.i"
#include "amor2t.h"

/*
  Defines for accessing various motors and variables. Definition of motor: see
  annotated AMOR drawing. 
*/

/* monochromator omega */
#define MOTMOM 0
/* sample omega */
#define MOTSOM 1
/* detector height movement */
#define MOTCOZ 2
/* detector movement along main axis */
#define MOTCOX 3
/* sample holder height movement */
#define MOTSTZ 4
/* whole sample table height movement */
#define MOTSOZ 5
/* lift for diaphragm 4*/ 
#define MOTD4B 6
/* lift for diaphragm 5 */
#define MOTD5B 7
/* detector omega movement */
#define MOTCOM 8

/* distance detector sample */
#define PARDS  0
/* constant height of sample: height = PARDH + MOTSOZ + MOTSTZ */
#define PARDH  1
/* distance diaphragm 4 - sample */
#define PARDD4 2
/* distance to diaphragm 5 */
#define PARDD5 3
/* interrupt to issue when a motor fails on this */
#define PARINT 4
/* base height of counter station */
#define PARDDH 5
/* height of D4 */
#define PARD4H 6
/* height of D5 */
#define PARD5H 7

/*======================================================================
   The core of it all: The calculation of the settings for the various 
   motors.
========================================================================*/

   static int CalculateAMORE(pAmor2T self, SConnection *pCon, float fNew)
   {
      float fMOM, fSOM, fSTZ, fSOZ;
      double fAngle, fX, fZ, fBase, fPIR;
      float fCOZ, fCOX, fCOM;
      int iRet;
#ifdef DEBUG
      char pBueffel[132];
#endif

      /* get the necessary angles first */
      iRet = MotorGetSoftPosition(self->aEngine[MOTMOM],pCon,&fMOM);
      if(iRet != 1)
      {
        return iRet;
      }
      iRet = MotorGetSoftPosition(self->aEngine[MOTSOM],pCon,&fSOM);
      if(iRet != 1)
      {
        return iRet;
      }
      iRet = MotorGetSoftPosition(self->aEngine[MOTSTZ],pCon,&fSTZ);
      if(iRet != 1)
      {
        return iRet;
      }
      iRet = MotorGetSoftPosition(self->aEngine[MOTSOZ],pCon,&fSOZ);
      if(iRet != 1)
      {
        return iRet;
      }

      /* calculate base height of sample table */
      fBase = fSTZ + fSOZ + ObVal(self->aParameter,PARDH);
      fPIR = 180. / 3.1415926;
      
      /* calculation for detector */
      fAngle = fNew - 2*fMOM;
      if(fAngle < 0)
      {
         fAngle = fAngle + 360.;
      }
      fAngle /= fPIR;  
      fX = ObVal(self->aParameter,PARDS)*cos(fAngle);
      fZ = ObVal(self->aParameter,PARDS)*sin(fAngle);
      self->toStart[0].pMot = self->aEngine[MOTCOX];
      strcpy(self->toStart[0].pName,self->aEngine[MOTCOX]->name);
      self->toStart[0].fTarget = fX - ObVal(self->aParameter,PARDS);
      self->toStart[1].pMot = self->aEngine[MOTCOZ];
      strcpy(self->toStart[1].pName,self->aEngine[MOTCOZ]->name);
      self->toStart[1].fTarget = fZ + fBase  - 
                       ObVal(self->aParameter,PARDDH);
      self->toStart[2].pMot = self->aEngine[MOTCOM];
      strcpy(self->toStart[2].pName,self->aEngine[MOTCOM]->name);
      self->toStart[2].fTarget = fNew - 2*fMOM;
      self->iStart = 3;

      /* calculation for diaphragm 4 */
      fZ = ObVal(self->aParameter,PARDD4) * sin(fAngle);
      self->toStart[3].pMot = self->aEngine[MOTD4B];
      strcpy(self->toStart[3].pName,self->aEngine[MOTD4B]->name);
      self->toStart[3].fTarget = fBase + fZ - 
                                 ObVal(self->aParameter,PARD4H);
      self->iStart = 4;

      /* calculation for diaphragm 5 */
      fZ = ObVal(self->aParameter,PARDD5) * sin(fAngle);
      self->toStart[4].pMot = self->aEngine[MOTD5B];
      strcpy(self->toStart[4].pName,self->aEngine[MOTD5B]->name);
      self->toStart[4].fTarget = fBase + fZ - 
                                 ObVal(self->aParameter,PARD5H);
      self->iStart = 5;
#ifdef DEBUG
      sprintf(pBueffel,"2T    COZ    COX    COM    D4B    D5B    ");
      SCWrite(pCon,pBueffel,eValue);
      sprintf(pBueffel,"%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f",
             fNew, self->toStart[1].fTarget, self->toStart[0].fTarget,
             self->toStart[2].fTarget, self->toStart[3].fTarget,
             self->toStart[4].fTarget);
      SCWrite(pCon,pBueffel,eValue);         
#endif
      
      return 1; 
   }
/*========================================================================
   Definition of interface functions.
=========================================================================*/
   static long A2TSetValue(void *pData, SConnection *pCon, float fNew)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pAmor2T self = (pAmor2T) pData;

      assert(self);

      /* calculation */
      iRet = CalculateAMORE(self,pCon,fNew);
      if(iRet != 1)
      {
        return iRet;
      }
     
      /* start them all */
      for(i = 0; i < self->iStart; i++)
      {
        pDriv = self->toStart[i].pMot->pDescriptor->GetInterface(
                                            self->toStart[i].pMot,DRIVEID);
        if(pDriv != NULL)
	{
           iRet = pDriv->SetValue(self->toStart[i].pMot,pCon,
                                  self->toStart[i].fTarget);
           if(iRet != OKOK)
	   {
              return iRet;
           }        
        }        
      }
      return OKOK;
   } 
/*-------------------------------------------------------------------------*/
   static int A2THalt(void *pData)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pAmor2T self = (pAmor2T) pData;

      assert(self);

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


      assert(self);
      pDumCon = SCCreateDummyConnection(pServ->pSics);
      assert(pDumCon);

      /* calculation */
      iRet = CalculateAMORE(self,pDumCon,fNew);
      SCDeleteConnection(pDumCon);
      if(iRet != 1)
      {
        return iRet;
      }
     
      /* check them all */
      for(i = 0; i < self->iStart; i++)
      {
        pDriv = self->toStart[i].pMot->pDescriptor->GetInterface(
                                            self->toStart[i].pMot,DRIVEID);
        if(pDriv != NULL)
	{
           iRet = pDriv->CheckLimits(self->toStart[i].pMot,
                                     self->toStart[i].fTarget,
                                     error,iErrLen);
           if(iRet != 1)
	   {
              return iRet;
           }        
        }        
      }
      return 1;
   } 
/*------------------------------------------------------------------------*/
   static int A2TStatus(void *pData, SConnection *pCon)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pAmor2T self = (pAmor2T) pData;

      assert(self);

      /* check them all */
      for(i = 0; i < self->iStart; i++)
      {
        pDriv = self->toStart[i].pMot->pDescriptor->GetInterface(
                                            self->toStart[i].pMot,DRIVEID);
        if(pDriv != NULL)
	{
           iRet = pDriv->CheckStatus(self->toStart[i].pMot,pCon);
           if( (iRet != OKOK) && (iRet != HWIdle) )
	   {
              return iRet;
           }        
        }        
      }
      return iRet;
   } 
/*------------------------------------------------------------------------*/
   static float A2TGetValue(void *pData, SConnection *pCon)
   {
     float fVal, fMOM, fResult;
     int iRet;
     pIDrivable pDriv = NULL;
     pAmor2T self = (pAmor2T) pData;
 
     assert(self);

     /* get COM */
     pDriv =  self->aEngine[MOTCOM]->pDescriptor->GetInterface(
                                          self->aEngine[MOTCOM],DRIVEID);
     if(pDriv)
     {
         fVal = pDriv->GetValue(self->aEngine[MOTCOM],pCon);
         if(fVal < -9000)
	 {
           return fVal;
         }
     }
     /* get MOM */
     pDriv =  self->aEngine[MOTCOM]->pDescriptor->GetInterface(
                                          self->aEngine[MOTMOM],DRIVEID);
     if(pDriv)
     {
         fMOM = pDriv->GetValue(self->aEngine[MOTMOM],pCon);
         if(fMOM < -9000)
	 {
           return fMOM;
         }
     }
    
     /* retrocalculate 2 theta */
     fResult =  fVal + 2*fMOM;
     return fResult;
   }
/*-----------------------------------------------------------------------*/
   static void *A2TGetInterface(void *pData, int iID)
   {
     pAmor2T self = (pAmor2T) pData;
 
     assert(self);
     if(iID == DRIVEID)
     {
       return self->pDriv;
     }    
     return NULL;
   }
/*------------------------------------------------------------------------*/
   static int A2TSave(void *pData, char *name, FILE *fd)
   {
     int i;
     pAmor2T self = (pAmor2T) pData;
 
     assert(self);

     fprintf(fd,"%s detectord %f \n", name, ObVal(self->aParameter,PARDS));
     fprintf(fd,"%s sampleh %f \n", name, ObVal(self->aParameter,PARDH));
     fprintf(fd,"%s d4d %f \n", name, ObVal(self->aParameter,PARDD4));
     fprintf(fd,"%s d5d %f \n", name, ObVal(self->aParameter,PARDD5));
     fprintf(fd,"%s interrupt %f \n", name, ObVal(self->aParameter,PARINT));
     fprintf(fd,"%s detectorh %f \n", name, ObVal(self->aParameter,PARDDH));
     fprintf(fd,"%s d4h %f \n", name, ObVal(self->aParameter,PARD4H));
     fprintf(fd,"%s d5h %f \n", name, ObVal(self->aParameter,PARD5H));

     return 1;
   }
/*------------------------------------------------------------------------*/
   static void A2TKill(void *pData)
   {
     pAmor2T self = (pAmor2T) 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 Amor2TFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                     int argc, char *argv[])
   {
       pAmor2T pNew = NULL;
       int i, iRet;
       char pBueffel[512];
       char *pMot = NULL;

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

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

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

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

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

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

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

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

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

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

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

       /* initialize parameters */
       ObParInit(pNew->aParameter,PARDS,"detectord",400.,usMugger);
       ObParInit(pNew->aParameter,PARDH,"sampleh",50.,usMugger);
       ObParInit(pNew->aParameter,PARDD4,"d4d",100.,usMugger);
       ObParInit(pNew->aParameter,PARDD5,"d5d",200.,usMugger);
       ObParInit(pNew->aParameter,PARINT,"interrupt",0.,usMugger);
       ObParInit(pNew->aParameter,PARDDH,"detectorh",40.,usMugger);
       ObParInit(pNew->aParameter,PARD4H,"d4h",40.,usMugger);
       ObParInit(pNew->aParameter,PARD5H,"d5h",400.,usMugger);

       /* initialize interfaces */
       pNew->pDes->GetInterface = A2TGetInterface;
       pNew->pDes->SaveStatus =   A2TSave;
       pNew->pDriv->Halt = A2THalt;
       pNew->pDriv->CheckLimits = A2TCheck;
       pNew->pDriv->SetValue = A2TSetValue;
       pNew->pDriv->CheckStatus = A2TStatus;
       pNew->pDriv->GetValue = A2TGetValue;

       /* install command */
       iRet = AddCommand(pSics,argv[1],
                        Amor2TAction,A2TKill,pNew);
       if(!iRet)
       {
         sprintf(pBueffel,"ERROR: duplicate command %s NOT created",
                 argv[1]);
         SCWrite(pCon,pBueffel,eError);
         A2TKill(pNew);
         return 0;
       }
       return 1;
   }
/*----------------------------------------------------------------------*/
   int Amor2TAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                    int argc, char *argv[])
   {
      pAmor2T self = (pAmor2T)pData;
      char pBueffel[256];
      float fVal;
      double dVal;
      ObPar *pPar = NULL;
      int iRet;

      assert(self);

      if(argc > 1)
      {
         strtolower(argv[1]);
         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
	 {
           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;
      }
   }