/*---------------------------------------------------------------------------
                                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

  Bugs fixed, analyzer included for A2T. Then there is a second thing:
  aoz2t which allows to scan the analyzer in two-theta during alignment
  of the instrument. As all the parameters are already held in the a2t 
  structures this extra was added into this module. 

  Mark Koennecke, May-June 2000 
---------------------------------------------------------------------------*/
#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 13
#define MAXPAR 13

#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
/* lift for analyzer */
#define MOTAOZ 9
/* analyzer omega */
#define MOTAOM 10
/* detector 2 movement */
#define MOTC3Z  11

/* 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
/* base height of analyzer */
#define PARANA 8
/* distance of analyzer from sample */
#define PARADIS 9
/* flag analyzer calculation on/off */
#define ANAFLAG 10
/* constant for second detector */
#define PARDDD 11
/* constant part of AOM */
#define PARAOM 12

/*======================================================================
   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, fAOM, fAOZ, fC3Z, fconstAOM;
      double fAngle, fX, fZ, fZ2, 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 = 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

      if(ObVal(self->aParameter,ANAFLAG) > 0)
      {
        /* the analyzer height */
        fZ = ObVal(self->aParameter,PARADIS)*sin(fAngle); 
        fAOZ = fBase + fZ - ObVal(self->aParameter,PARANA);
        self->toStart[5].pMot = self->aEngine[MOTAOZ];
        strcpy(self->toStart[5].pName,self->aEngine[MOTAOZ]->name);
        self->toStart[5].fTarget = fAOZ;
        self->iStart = 6;

        /* analyzer omega */
        self->toStart[6].pMot = self->aEngine[MOTAOM];
        strcpy(self->toStart[6].pName,self->aEngine[MOTAOM]->name);
        self->toStart[6].fTarget = fNew/2. 
             + ObVal(self->aParameter,PARAOM);
        self->iStart = 7;

        /* C3Z */
        fZ2 = (ObVal(self->aParameter,PARDS) - ObVal(self->aParameter,
                 PARADIS))*sin(fAngle + (fNew/fPIR) );
                
        self->toStart[7].pMot = self->aEngine[MOTC3Z];
        strcpy(self->toStart[7].pName,self->aEngine[MOTC3Z]->name);
        self->toStart[7].fTarget = fBase + fZ + fZ2 - 
                    ObVal(self->aParameter,PARDDD) - 
              self->toStart[1].fTarget; 
        self->iStart = 8;
#ifdef DEBUG
        sprintf(pBueffel,"2T     AOZ    AOM    C3Z");
        SCWrite(pCon,pBueffel,eValue);
        sprintf(pBueffel,"%6.2f %6.2f %6.2f %6.2f",
             fNew, self->toStart[5].fTarget, self->toStart[6].fTarget,
             self->toStart[7].fTarget);
        SCWrite(pCon,pBueffel,eValue);         
#endif
       
      }      
      return 1; 
   }
/*=======================================================================
  Calculations for Analyzer two theta 
=========================================================================*/
   static int CalculateANA2T(pAmor2T self, SConnection *pCon, float fNew)
   {
      double fBase, fPIR;
      float fAOZ, fIncident, fSOM, fMOM, fDiffracted, fDistance, fX, fZ;
      int iRet;
#ifdef DEBUG
      char pBueffel[132];
#endif

      /* calculate base height of analyzer table */
      iRet = MotorGetSoftPosition(self->aEngine[MOTSOZ],pCon,&fAOZ);
      if(iRet != 1)
      {
        return iRet;
      }
      fBase = fAOZ + ObVal(self->aParameter,PARANA);
      fPIR = 180. / 3.1415926;

      /* Calculate the incident angle at the analyzer */
      iRet = MotorGetSoftPosition(self->aEngine[MOTSOM],pCon,&fSOM);
      if(iRet != 1)
      {
        return iRet;
      }
      iRet = MotorGetSoftPosition(self->aEngine[MOTMOM],pCon,&fMOM);
      if(iRet != 1)
      {
        return iRet;
      }
      fIncident = fMOM + 2. * fSOM;
           
      /* calculate the angle of the diffracted beam against the
         horizon  at the analyzer.  

         fDiffracted = fIncident - 2. * AOM.  
        
         There is a problem here. We should read AOM in order to get the 
         value. However in the context of an omega - two-theta scan on AOM
         and ana2t, it is fNew. 
      */
      fDiffracted = fIncident - fNew;

      
      /* calculation for detector */
      fDiffracted /= fPIR;  
      fDistance = ObVal(self->aParameter,PARDS) - 
            ObVal(self->aParameter, PARANA);
      fX = fDistance*cos(fDiffracted);
      fZ = fDistance*sin(fDiffracted);
      self->toStart[0].pMot = self->aEngine[MOTCOX];
      strcpy(self->toStart[0].pName,self->aEngine[MOTCOX]->name);
      self->toStart[0].fTarget = fX - fDistance;

      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 = -fDiffracted*fPIR;
      self->iStart = 3;

      /* calculation for diaphragm 5 */
      fZ = ObVal(self->aParameter,PARDD5) * sin(fDiffracted);
      self->toStart[3].pMot = self->aEngine[MOTD5B];
      strcpy(self->toStart[3].pName,self->aEngine[MOTD5B]->name);
      self->toStart[3].fTarget = fBase + fZ - 
                                 ObVal(self->aParameter,PARD5H);
      self->iStart = 4;

#ifdef DEBUG
      sprintf(pBueffel,"2T    COX    COZ   COM   D5B    ");
      SCWrite(pCon,pBueffel,eValue);
      sprintf(pBueffel,"%6.2f %6.2f %6.2f %6.2f %6.2f ",
             fNew, self->toStart[0].fTarget, self->toStart[1].fTarget,
             self->toStart[2].fTarget,self->toStart[3].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 long ANA2TSetValue(void *pData, SConnection *pCon, float fNew)
   {
      int i, iRet;
      pIDrivable pDriv = NULL;
      pAmor2T self = (pAmor2T) pData;

      assert(self);

      /* calculation */
      iRet = CalculateANA2T(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 ANA2TCheck(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 = CalculateANA2T(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 float ANA2TGetValue(void *pData, SConnection *pCon)
   {
     float fVal, fMOM, fResult;
     int iRet;
     pIDrivable pDriv = NULL;
     pAmor2T self = (pAmor2T) pData;
 
     assert(self);

     /* get AOM */
     pDriv =  self->aEngine[MOTAOM]->pDescriptor->GetInterface(
                                          self->aEngine[MOTAOM],DRIVEID);
     if(pDriv)
     {
         fVal = pDriv->GetValue(self->aEngine[MOTAOM],pCon);
         if(fVal < -9000)
	 {
           return fVal;
         }
     }

     return 2. * fVal;

   }
/*-----------------------------------------------------------------------*/
   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));
     fprintf(fd,"%s anah %f \n", name, ObVal(self->aParameter,PARANA));
     fprintf(fd,"%s anad %f \n", name, ObVal(self->aParameter,PARADIS));
     fprintf(fd,"%s anaflag %f \n", name, ObVal(self->aParameter,ANAFLAG));
     fprintf(fd,"%s c2h %f \n", name, ObVal(self->aParameter,PARDDD));
     fprintf(fd,"%s aomconst %f \n", name, ObVal(self->aParameter,PARAOM));
     return 1;
   }
/*------------------------------------------------------------------------*/
   static void A2TList(pAmor2T 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,PARDS));
      Tcl_DStringAppend(&tString,pBueffel,-1);
          sprintf(pBueffel,
            "%s.sampleh %f \n", name, ObVal(self->aParameter,PARDH));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.d4d %f \n", name, ObVal(self->aParameter,PARDD4));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.d5d %f \n", name, ObVal(self->aParameter,PARDD5));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.interrupt %f \n", name, ObVal(self->aParameter,PARINT));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.detectorh %f \n", name, ObVal(self->aParameter,PARDDH));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.d4h %f \n", name, ObVal(self->aParameter,PARD4H));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.d5h %f \n", name, ObVal(self->aParameter,PARD5H));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.anah %f \n", name, ObVal(self->aParameter,PARANA));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.anad %f \n", name, ObVal(self->aParameter,PARADIS));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.anaflag %f \n", name, ObVal(self->aParameter,ANAFLAG));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.c2h %f \n", name, ObVal(self->aParameter,PARDDD));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      sprintf(pBueffel,
            "%s.aomconst %f \n", name, ObVal(self->aParameter,PARAOM));
      Tcl_DStringAppend(&tString,pBueffel,-1);
      SCWrite(pCon,Tcl_DStringValue(&tString),eValue);
      Tcl_DStringFree(&tString);  
   }
/*------------------------------------------------------------------------*/
   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, pAOM = NULL;
       int i, iRet;
       char pBueffel[512];
       char *pMot = NULL;

       if(argc < 4)
       {
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          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);
          SCWrite(pCon,pBueffel,eError);
          A2TKill(pNew);
          return 0;
       }

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

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

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


       /* initialize parameters */
       ObParInit(pNew->aParameter,PARDS,"detectord",1400.,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);
       ObParInit(pNew->aParameter,PARANA,"anah",400.,usMugger);
       ObParInit(pNew->aParameter,PARADIS,"anad",600.,usMugger);
       ObParInit(pNew->aParameter,ANAFLAG,"anaflag",-1.,usMugger);
       ObParInit(pNew->aParameter,PARDDD,"c2h",100.,usMugger);
       ObParInit(pNew->aParameter,PARAOM,"aomconst",3.,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;

       /* copy data structure for second command for aom2t */
       pAOM = (pAmor2T)malloc(sizeof(Amor2T));
       if(!pAOM)
       {
          A2TKill(pNew);
          SCWrite(pCon,"ERROR: out of memory in Amor2TFactory",eError);
          return 0;
       }
       memcpy(pAOM,pNew,sizeof(Amor2T));
       pAOM->pDriv = CreateDrivableInterface();
       pAOM->pDes = CreateDescriptor("Amor2T");
       if(!pAOM->pDriv || !pAOM->pDes )
       {
          A2TKill(pNew);
          SCWrite(pCon,"ERROR: out of memory in Amor2TFactory",eError);
          return 0;
       }
       
       /* set modified interface functions */
       pAOM->pDes->GetInterface = A2TGetInterface;
       pAOM->pDriv->Halt = A2THalt;
       pAOM->pDriv->CheckLimits = ANA2TCheck;
       pAOM->pDriv->SetValue = ANA2TSetValue;
       pAOM->pDriv->GetValue = ANA2TGetValue;
       pAOM->pDriv->CheckStatus = A2TStatus;


       /* install commands */
       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;
       }
       iRet = AddCommand(pSics,argv[3],
                        Amor2TAction,free,pAOM);
       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]);
         /* deal with list */
         if(strcmp(argv[1],"list") == 0)
	 {
            A2TList(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
	 {
           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;
      }
   }