/*---------------------------------------------------------------------------
                         H K L
                         
   Implementation of the SICS object for doing four circle diffractometer
   angle calculations. The actual transformation routines were recoded
   in C from F77 routines provided by J. Allibon, ILL with the MAD
   system.
   
   copyright: see copyright.h
   
   Mark Koennecke, February 1998
   
   Updated to use fourlib.

   Mark Koennecke, December 2001

   Introduced HM mode in order to cope with with the fact that TRICS has 
   three detectors.

   Mark Koennecke, May 2002
-----------------------------------------------------------------------------*/
#include <math.h>
#include <ctype.h>
#include "sics.h"
#include "fortify.h"
#include "motor.h"
#include "selector.h"
#include "selvar.h"
#include "fourlib.h"
#include "matrix/matrix.h"
#include "hkl.h"
#include "hkl.i"

/*
  the space we leave in omega in order to allow for a scan to be done 
*/
#define SCANBORDER 3.
/*-------------------------------------------------------------------------*/
   static int HKLSave(void *pData, char *name, FILE *fd)
   {
      pHKL self = NULL;

      self = (pHKL)pData;
      if( (self == NULL) || (fd == NULL) )
      {
        return 1;
      }
      fprintf(fd,"#Crystallographic Settings\n");
      fprintf(fd,"%s lambda %f\n",name, self->fLambda);
      fprintf(fd,
      "%s setub %8.6f %8.6f %8.6f %8.6f %8.6f %8.6f %8.6f %8.6f %8.6f\n",
         name, 
         self->fUB[0], self->fUB[1], self->fUB[2], self->fUB[3], self->fUB[4],
         self->fUB[5], self->fUB[6], self->fUB[7], self->fUB[8]);
      fprintf(fd,"%s hm %d\n",name, self->iHM);
      return 1;
   }

/*---------------------------------------------------------------------------*/
  pHKL CreateHKL(pMotor pTheta, pMotor pOmega, pMotor pChi, 
                 pMotor pPhi, pMotor pNu)
  {
     pHKL pNew = NULL;
   
     assert(pTheta);
     assert(pOmega);
     assert(pChi);
     assert(pPhi);
     
     /* allocate memory */
     pNew = (pHKL)malloc(sizeof(HKL));
     if(!pNew)
     {
       return NULL;
     }
     memset(pNew,0,sizeof(HKL));
     
     /* create object descriptor */
     pNew->pDes = CreateDescriptor("4-Circle-Calculus");
     if(!pNew->pDes)
     {
       free(pNew);
       return NULL;
     }
     pNew->pDes->SaveStatus =  HKLSave; 

     pNew->pTheta  = pTheta;
     pNew->pOmega  = pOmega;
     pNew->pChi    = pChi;
     pNew->pPhi    = pPhi;
     pNew->pNu     = pNu;
     pNew->fLambda = 1.38;
     pNew->iManual = 1;
     pNew->iQuad   = 1;
     pNew->iHM = 0;
     pNew->fUB[0] = 1.;
     pNew->fUB[4] = 1.;
     pNew->fUB[8] = 1.;
     pNew->UBinv = NULL;
     
     return pNew;
  }
/*--------------------------------------------------------------------------*/
  void DeleteHKL(void *pData)
  {
    pHKL self = NULL;
    
    self = (pHKL)pData;
    if(!self)
    {
      return;
    }
    
    if(self->pDes)
    {
      DeleteDescriptor(self->pDes);
    }
    
    free(self);
  }           
/*---------------------------------------------------------------------------*/
  int HKLFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                 int argc, char *argv[])
  {
     pMotor pTheta = NULL, pOmega = NULL, pChi = NULL, 
            pPhi = NULL, pNu = NULL;
     pHKL self = NULL;
     int iRet;
     
     /* check no of arguments */
     if(argc < 5)
     {
       SCWrite(pCon,"ERROR: Insufficient number of arguments to HKLFactory",
               eError);
       return 0;        
     } 
     
     /* check motors */
     pTheta = FindMotor(pSics,argv[1]);
     if(!pTheta)
     {
       SCWrite(pCon,"ERROR: cannot find two theta motor",eError);
       return 0;
     }
     pOmega = FindMotor(pSics,argv[2]);
     if(!pOmega)
     {
       SCWrite(pCon,"ERROR: cannot find omega motor",eError);
       return 0;
     }
     pPhi = FindMotor(pSics,argv[3]);
     if(!pPhi)
     {
       SCWrite(pCon,"ERROR: cannot find phi motor",eError);
       return 0;
     }
     pChi = FindMotor(pSics,argv[4]);
     if(!pChi)
     {
       SCWrite(pCon,"ERROR: cannot find chi motor",eError);
       return 0;
     }

     if(argc >= 6)
     {
        pNu = FindMotor(pSics,argv[5]);
        if(!pNu)
	{
           SCWrite(pCon,"WARNING: cannot find nu motor",eWarning);
        }
     }
     
     /* make a new structure */
     self = CreateHKL(pTheta, pOmega, pPhi, pChi, pNu);
     if(!self)
     {
       SCWrite(pCon,"ERROR: cannot allocate HKL data structure",eError);
       return 0;
     }
     
     /* install a command */
     iRet = AddCommand(pSics,
                       "HKL",
                       HKLAction,
                       DeleteHKL,
                       self);
      if(!iRet)
      {
        SCWrite(pCon,"ERROR: duplicate command HKL not created",eError);
        DeleteHKL(self);
        return 0;
      }         
      return 1;        
  }           
#include "selvar.i"
/*---------------------------------------------------------------------------*/

  static int HKLCallback(int iEvent, void *pEvent, void *pUser)
  {
    pHKL self = NULL;
    pSelVar pVar = NULL;
    float fVal;
    
    if(iEvent == WLCHANGE)
    {
      self = (pHKL)pUser;
      pVar = (pSelVar)pEvent;
      assert(self);
      assert(pVar);
      
      if(pVar->pCon != NULL)
      {
         fVal = GetSelValue(pVar, pVar->pCon);
         if(fVal > -900.)
         {
           self->fLambda = fVal;
           return 1;
         }
      }          
    }
    return 1;
  }         
/*--------------------------------------------------------------------------*/
  int SetWavelengthVariable(SConnection *pCon, pHKL self, pSelVar pVar)
  {
    pICallBack pCall = NULL, pCall2 = NULL;
    pDummy pDum = NULL;
    float fVal;
    
    assert(pCon);
    assert(self);
    assert(pVar);

    /* try to get callback interface of the new mono variable */
    pDum = (pDummy)pVar;
    pCall2 = pDum->pDescriptor->GetInterface(pDum,CALLBACKINTERFACE);
    if(!pCall2)
    {
      return 0;
    }
    
    /* clear old stuff, if apropriate */
    if(self->pMono)
    {
      pDum = (pDummy)self->pMono;
      pCall = pDum->pDescriptor->GetInterface(pDum,CALLBACKINTERFACE);
      if(pCall)
      {
        RemoveCallback(pCall,self->lID);
        self->lID = 0;
        self->pMono = NULL;
        self->iManual = 1;
      }
    }
    
    /* install new callback */
    self->lID = RegisterCallback(pCall2, 
                                 WLCHANGE,
                                 HKLCallback,
                                 self,
                                 NULL);
    self->pMono =  pVar;
    self->iManual = 0;
    
    /* update the current value */
    fVal = GetSelValue(pVar,pCon);
    if(fVal > -900.)
    {
      self->fLambda = fVal;
    } 
    else
    {
      return 0;
    }    
    return 1;                         
  }  
/*-------------------------------------------------------------------------*/
  int SetWavelengthManual(pHKL self, float fVal)
  {
    pICallBack pCall = NULL;
    pDummy pDum = NULL;
    
    assert(self);

    /* clear old stuff, if apropriate */
    if(self->pMono)
    {
      pDum = (pDummy)self->pMono;
      pCall = pDum->pDescriptor->GetInterface(pDum,CALLBACKINTERFACE);
      if(pCall)
      {
        RemoveCallback(pCall,self->lID);
        self->lID = 0;
        self->pMono = NULL;
        self->iManual = 1;
      }
    }
    
    self->fLambda = fVal;
    return 1;
  }
/*------------------------------------------------------------------------*/
  int GetLambda(pHKL self, float *fVal)
  {
     assert(self);
     
     *fVal = self->fLambda;
     return 1;
  }  
/*-------------------------------------------------------------------------*/
  int GetCurrentHKL(pHKL self, float fHKL[3])
  {
     int i;
     
     assert(self);
     
     for(i = 0; i < 3; i++)
     {
       fHKL[i] = self->fLastHKL[i];
     }
     return 1;
  }  
/*-------------------------------------------------------------------------*/
  int SetUB(pHKL self, float fUB[9])
  {
    int i;
    MATRIX m;
    
    assert(self);
    
    for(i = 0; i < 9; i++)
    {
      self->fUB[i] = fUB[i];
    }
    /* invert UB matrix for use in backwards calculation */
    if(self->UBinv != NULL)
    {
         mat_free(self->UBinv);
    }  
    m = mat_creat(3,3,ZERO_MATRIX);
    for(i = 0; i < 3; i++)
    {
       m[0][i] = self->fUB[i];
       m[1][i] = self->fUB[3+i];
       m[2][i] = self->fUB[6+i];
    }  
    self->UBinv = mat_inv(m);
    mat_free(m);

    return 1;
  }    
/*-------------------------------------------------------------------------*/
  int GetUB(pHKL self, float fUB[9])
  {
    int i;
    
    assert(self);
    
    for(i = 0; i < 9; i++)
    {
      fUB[i] = self->fUB[i];
    }
    return 1;
  }    
  
/*--------------------------------------------------------------------------*/
  int SetNOR(pHKL self, int iNOB)
  {
    assert(self);
    
    /* cannot set normal beam geometry if no nu motor */
    if( (iNOB == 1) && (self->pNu == NULL))
       return 0;

    self->iNOR = iNOB;
    return 1;
  }   
/*-----------------------------------------------------------------------*/
static int checkTheta(pHKL self, double *stt){
  char pError[132];
  int iTest;
  float fHard;

   iTest = MotorCheckBoundary(self->pTheta,(float)*stt, &fHard,pError,131);
   if(!iTest)
   {
     /*
       check if it is on the other detectors
     */
     if(self->iHM){
       iTest = MotorCheckBoundary(self->pTheta,(float)*stt-45., 
				  &fHard,pError,131);
       if(iTest){
	 *stt -= 45.;
       } else {
	 iTest = MotorCheckBoundary(self->pTheta,(float)*stt-90., 
				  &fHard,pError,131);
	 if(iTest) {
	   *stt -= 90.;
         }
       }
     }
     if(!iTest){
       return -1;
     }
   }
   return 1;
}
/*-----------------------------------------------------------------------*/
static int checkBisecting(pHKL self,
			  double *stt, double om, double chi, double phi)
{
   int iTest;
   float fHard, fLimit;
   char pError[132];

   /* check two theta  */
   iTest = checkTheta(self, stt);
   if(!iTest){
     return -1;
   }

   /* for omega check against the limits +- SCANBORDER in order to allow for
      a omega scan
   */
   MotorGetPar(self->pOmega,"softlowerlim",&fLimit);
   if((float)om < fLimit + SCANBORDER){
     iTest = 0;
   } else {
     iTest = 1;
     MotorGetPar(self->pOmega,"softupperlim",&fLimit);
     if((float)om > fLimit - SCANBORDER){
       iTest = 0;
     } else {
       iTest = 1;
     }
   }

   /* check chi and phi*/
   iTest += MotorCheckBoundary(self->pChi,(float)chi, &fHard,pError,131);
   iTest += MotorCheckBoundary(self->pPhi,(float)phi, &fHard,pError,131);
   if(iTest == 3) /* none of them burns */
   {
     return 1;
   }
   return 0;
}
/*-----------------------------------------------------------------------*/
static int checkNormalBeam(double om, double *gamma, double nu,
			   float fSet[4], SConnection *pCon, pHKL self)
{
  int iTest;
  char pError[132];
  float fHard;

   /* check omega, gamma and nu  */
   iTest = MotorCheckBoundary(self->pOmega,(float)om, &fHard,pError,131);
   iTest += checkTheta(self,gamma);
   iTest += MotorCheckBoundary(self->pNu,(float)nu, &fHard,pError,131);
   if(iTest == 3) /* none of them burns */
   {
     fSet[0] = (float)*gamma;
     fSet[1] = (float)om;
     fSet[2] = (float)nu;
     return 1;
   }
   return 0;
}
/*-----------------------------------------------------------------------
  tryOmegaTweak tries to calculate a psi angle in order to put an
  offending omega back into range.
  -----------------------------------------------------------------------*/
static int tryOmegaTweak(pHKL self, MATRIX z1, double *stt, double *om, 
			 double *chi, double *phi){
  int status;
  float fLower, fUpper, omTarget, omOffset;
  double dumstt, offom, offchi, offphi;


  status = checkBisecting(self,stt,*om,*chi,*phi);
  if(status < 0){
    return 0; /* stt is burning */
  } else if(status == 1){
    return 1;
  }

  /*
    Is omega really the problem? 
  */
  omTarget = -9999;
  MotorGetPar(self->pOmega,"softlowerlim",&fLower);
  MotorGetPar(self->pOmega,"softupperlim",&fUpper);
  if(*om < fLower + SCANBORDER) {
    omTarget = fLower + SCANBORDER + .5;
  }
  if(*om > fUpper - SCANBORDER){
    omTarget = fUpper - SCANBORDER - .5;
  }
  if(omTarget < -7000){
    return 0;
  }
    
  /*
    calculate omega offset
  */
  omOffset = *om - omTarget;
  omOffset = -omOffset;
  
  /*
    calculate angles with omega offset 
  */
  status = z1ToAnglesWithOffset(self->fLambda,z1, omOffset, &dumstt,
				&offom, &offchi, &offphi);
  if(!status){
    return 0;
  }
   
  if(checkBisecting(self,&dumstt,offom,offchi,offphi)){
    *om = offom;
    *chi = offchi;
    *phi = offphi;
    return 1;
  }
  return 0;
}
/*-----------------------------------------------------------------------*/
static MATRIX calculateScatteringVector(pHKL self, float fHKL[3])
{
  MATRIX z1, hkl, ubm;
  int i;

   hkl = mat_creat(3,1,ZERO_MATRIX);
   ubm = mat_creat(3,3,ZERO_MATRIX);
   for(i = 0; i < 3; i++)
   {
      hkl[i][0] = fHKL[i];
      ubm[0][i] = self->fUB[i];
      ubm[1][i] = self->fUB[i+3];
      ubm[2][i] = self->fUB[i+6];
    }
    z1 = mat_mul(ubm,hkl);
    mat_free(ubm);
    mat_free(hkl);

    return z1;
}
/*---------------------------------------------------------------------*/
static int calculateBisecting(MATRIX z1, pHKL self, SConnection *pCon,
			      float fSet[4], double myPsi, int iRetry)
{
  double stt, om, chi, phi, psi, ompsi, chipsi, phipsi;
  int i, test;

  /* 
     just the plain angle calculation
  */
  if(!z1mToBisecting(self->fLambda,z1,&stt,&om,&chi,&phi))
  {
    return 0;        
  }

  /*
    check if angles in limits. If omega problem: try to tweak
  */
  chi = circlify(chi);
  phi = circlify(phi);
  if(iRetry > 1)
  {
    if(tryOmegaTweak(self,z1,&stt,&om,&chi,&phi)){
	 fSet[0] = (float)stt;
	 fSet[1] = (float)om;
	 fSet[2] = (float)circlify(chi);
	 fSet[3]= (float)circlify(phi);
	 return 1;
     }
   }
  /*
    if this does not work, try rotating through psi in order to
    find a useful setting
  */
  for(i = 0; i < iRetry; i++)
  {
    if(iRetry > 1)
    {
      psi = i*10.;
    }
    else
    {
      psi = myPsi;
    }
    rotatePsi(om,chi,phi,psi,&ompsi,&chipsi,&phipsi);
    chipsi = circlify(chipsi);
    phipsi = circlify(phipsi);
    test = checkBisecting(self,&stt,ompsi,chipsi,phipsi);
    if(test == 1)
    {
      fSet[0] = (float)stt;
      fSet[1] = (float)ompsi;
      fSet[2] = (float)chipsi;
      fSet[3]= (float)phipsi;
      return 1;
    }
  }

  /*
    giving up!
  */
  for(i = 0; i < 4; i++)
  {
    fSet[i] = .0;
  }
  return 0;
}
/*----------------------------------------------------------------------*/
#define ABS(x) (x < 0 ? -(x) : (x)) 
/*-----------------------------------------------------------------------*/
static int calculateNormalBeam(MATRIX z1, pHKL self, SConnection *pCon,
			      float fSet[4], double myPsi, int iRetry)
{
  int i, iTest;
  double stt, om, chi, phi, gamma, nu, psi;
  float currentPhi, currentChi;
  double ompsi, chipsi, phipsi;
  MATRIX chim, phim, z4, z3;

  /*
    The usual condition for normal beam calculations is that both chi
    and phi are 0. This is not the case at TRICS. Therefore we have to
    multiply the scattering vector first with the chi and phi rotations
    before we start. 
  */
  iTest = MotorGetSoftPosition(self->pChi,pCon,&currentChi);
  if(iTest != 1)
  {
    return 0;
  }
  iTest = MotorGetSoftPosition(self->pPhi,pCon,&currentPhi);
  if(iTest != 1)
  {
    return 0;
  }
  phim = mat_creat(3,3,ZERO_MATRIX);
  phimat(phim,(double)currentPhi);
  z4 = mat_mul(phim,z1);
  chim = mat_creat(3,3,ZERO_MATRIX);
  chimat(chim,(double)currentChi);
  z3 = mat_mul(chim,z4);
  mat_free(phim);
  mat_free(chim);
  mat_free(z4);

  /*
    do the bisecting angles first
  */
  if(!z1mToBisecting(self->fLambda,z3,&stt,&om,&chi,&phi))
  {
    return 0;        
  }

  if(ABS(chi -90.) < .001 && ABS(phi-180.) < .001)
  {
    chi = .0;
    phi = .0;
  }

  /*
    in order to cope with all those limitations: rotate through psi
  */
  for(i = 0; i < iRetry; i++)
  {
    if(iRetry > 1)
    {
      psi = 10. *i;
    }
    else
    {
      psi = myPsi;
    }
    rotatePsi(om,chi,phi,psi,&ompsi,&chipsi,&phipsi);
    if(bisToNormalBeam(stt,ompsi,chipsi,phipsi,
		       &om, &gamma, &nu))
    {
      if(checkNormalBeam(om, &gamma, nu,fSet,pCon,self))
      {
	return 1;
      }
    }
  }

  return 0;
}
/*---------------------------------------------------------------------*/
static int calculateNormalBeamOmega(MATRIX z1, pHKL self, 
				    SConnection *pCon,
			            float fSet[4], double omOffset)
{
  int iTest;
  double stt, om, chi, phi, gamma, nu, psi;
  float currentPhi, currentChi;
  double ompsi, chipsi, phipsi;
  MATRIX chim, phim, z4, z3;

  /*
    The usual condition for normal beam calculations is that both chi
    and phi are 0. This is not the case at TRICS. Therefore we have to
    multiply the scattering vector first with the chi and phi rotations
    before we start. 
  */
  iTest = MotorGetSoftPosition(self->pChi,pCon,&currentChi);
  if(iTest != 1)
  {
    return 0;
  }
  iTest = MotorGetSoftPosition(self->pPhi,pCon,&currentPhi);
  if(iTest != 1)
  {
    return 0;
  }
  phim = mat_creat(3,3,ZERO_MATRIX);
  phimat(phim,(double)currentPhi);
  z4 = mat_mul(phim,z1);
  chim = mat_creat(3,3,ZERO_MATRIX);
  chimat(chim,(double)currentChi);
  z3 = mat_mul(chim,z4);
  mat_free(phim);
  mat_free(chim);
  mat_free(z4);
  

  /*
    do the bisecting angles first
  */
  if(!z1ToAnglesWithOffset(self->fLambda,z3, omOffset, &stt,
			   &ompsi, &chi, &phi))
  {
    return 0;        
  }

  if(ABS(chi -90.) < .001 && ABS(phi-180.) < .001)
  {
    chi = .0;
    phi = .0;
  }

  if(bisToNormalBeam(stt,ompsi,chi,phi,
		       &om, &gamma, &nu))
  {
      if(checkNormalBeam(om, &gamma, nu,fSet,pCon,self))
      {
	return 1;
      }
  }
  return 0;
}
/*-------------------------------------------------------------------------
  calculates the four circle settings. If the position can not be reached
  because of a limit violation, then psi is rotated in 10 degree steps
  until either the loop ends or we finally succeed. If there is a omega
  violation we first try to calculate a delta omega which puts omega
  into the right range. This is a fix because the omega movement is quite
  often restricted due to the cryogenic garbage around the sample.
*/
  int CalculateSettings(pHKL self, float fHKL[3], float fPsi, int iHamil,
                       float fSet[4], SConnection *pCon)
  {
     char pError[132];
     char pBueffel[512];
     float fHard, fVal, fUpper, fLower;
     double myPsi = fPsi;
     MATRIX z1;
     double stt, om, chi, phi, ompsi, chipsi, phipsi;
     int i,iRetry, status;
     
     /* catch shitty input */
     if( (fHKL[0] == 0.) && (fHKL[1] == 0.) && (fHKL[2] == 0.))
     {
       SCWrite(pCon,"ERROR: I will not calculate angles for HKL = (0,0,0) ",
               eError);
       return 0;         
     }
     
     /* some people are stupid.......... */
     myPsi = circlify(myPsi);

     /* 
	no retries if specific psi requested.
     */
     if((myPsi > 0.1) )
     {
       iRetry = 1;
     }
     else
     {
        iRetry = 35;
     }


     z1 = calculateScatteringVector(self,fHKL);

     if(self->iNOR == 0)
     {
       status = calculateBisecting(z1,self,pCon,fSet, myPsi, iRetry);
     } 
     else if(self->iNOR == 1)
     {
       status = calculateNormalBeam(z1,self,pCon,fSet, myPsi, iRetry);
     }
     else
     {
       myPsi = fPsi;
       status = calculateNormalBeamOmega(z1,self,pCon,fSet, myPsi);
     }


     if(!status)
     { 
       sprintf(pBueffel,"ERROR: cannot calculate %4.1f %4.1f %4.1f",
                  fHKL[0], fHKL[1], fHKL[2]);
        SCWrite(pCon,pBueffel,eError);
     }
     mat_free(z1);

     return status;

     return 0;        
  }          
/*-------------------------------------------------------------------------*/
  int RunHKL(pHKL self, float fHKL[3], 
             float fPsi, int iHamil, SConnection  *pCon)
  {
     float fSet[4];
     int iRet,i;
     char pBueffel[512];
     pDummy pDum;

     assert(self);
     iRet = CalculateSettings(self,fHKL,fPsi,iHamil,fSet,pCon);
     if(!iRet)
     {
       SCWrite(pCon,"ERROR: NOT started",eError);
       return 0;
     }

     /* start all the motors */
     pDum = (pDummy)self->pTheta;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[0]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start two theta motor",eError);
        StopExe(pServ->pExecutor,"all");
        return 0;
     }           
     
     pDum = (pDummy)self->pOmega;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[1]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start omega motor",eError);
        StopExe(pServ->pExecutor,"all");
        return 0;
     }           

     /* special case: normal beam */
     if(self->iNOR)
     {
        pDum = (pDummy)self->pNu;
        iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[2]);
        if(!iRet)
        {
            StopExe(pServ->pExecutor,"all");
            SCWrite(pCon,"ERROR: cannot start nu motor",eError);
            return 0;
        }
        else
	{
           for(i = 0; i < 3; i++)
           {
               self->fLastHKL[i] = fHKL[i];
           }               
           return 1;
        }           
     }

     pDum = (pDummy)self->pChi;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[2]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start chi motor",eError);
        StopExe(pServ->pExecutor,"all");
        return 0;
     }           
     pDum = (pDummy)self->pPhi;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[3]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start phi",eError);
        StopExe(pServ->pExecutor,"all");
        return 0;
     }        
     for(i = 0; i < 3; i++)
     {
       self->fLastHKL[i] = fHKL[i];
     }    
     return 1;
             
  }        
/*-------------------------------------------------------------------------*/
  int DriveSettings(pHKL self, float fSet[4], SConnection *pCon)
  {
     int iRet,i, iReturn;
     char pBueffel[512];
     pDummy pDum;

     iReturn = 1;

     /* start all the motors */
     pDum = (pDummy)self->pTheta;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[0]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start two theta motor",eError);
        StopExe(pServ->pExecutor,"all");
        iReturn = 0;
        goto ente;
     }           
     
     pDum = (pDummy)self->pOmega;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[1]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start omega motor",eError);
        StopExe(pServ->pExecutor,"all");
        iReturn = 0;
        goto ente;
     }           

     /* special case: normal beam */
     if(self->iNOR)
     {
        pDum = (pDummy)self->pNu;
        iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[2]);
        if(!iRet)
        {
            SCWrite(pCon,"ERROR: cannot start nu motor",eError);
            StopExe(pServ->pExecutor,"all");
            iReturn = 0;
        }
        goto ente;
     }

     pDum = (pDummy)self->pChi;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[2]);
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot start chi motor",eError);
        StopExe(pServ->pExecutor,"all");
        iReturn = 0;
        goto ente;
     }           
     pDum = (pDummy)self->pPhi;
     iRet = StartDevice(pServ->pExecutor, "HKL",
                        pDum->pDescriptor, pDum, pCon,fSet[3]);
     if(!iRet)
     {
        StopExe(pServ->pExecutor,"all");
        iReturn = 0;
        SCWrite(pCon,"ERROR: cannot start phi",eError);
     }        

ente:
      /* wait for end */
      iRet = Wait4Success(pServ->pExecutor);
      switch(iRet)
      {
         case DEVINT:
                 if(SCGetInterrupt(pCon) == eAbortOperation)
                 {
                       SCSetInterrupt(pCon,eContinue);
                       SCSetError(pCon,OKOK);
                       SCWrite(pCon,"Driving to HKL Aborted",eStatus);
                 }
                 return 0;
                 break;
          case DEVDONE:
                 SCWrite(pCon,"Driving to Reflection done",eStatus);
                 break;
          default:
                 SCWrite(pCon,"WARNING: driving to HKL finished with problems",
                         eWarning);
                 if(SCGetInterrupt(pCon) != eContinue)
		 {
                    return 0;
                 }
                 break;                          
     }   
     return iReturn;
  }    
/*--------------------------------------------------------------------------*/
  int DriveHKL(pHKL self, float fHKL[3], 
             float fPsi, int iHamil, SConnection  *pCon)
  {
      int iRet, iReturn;
      long lID;
      char pBueffel[132];
      
      assert(self);
      
      /* start running */
      iReturn = 1;
      iRet = RunHKL(self,fHKL,fPsi,iHamil,pCon);
      if(!iRet)
      {
        StopExe(pServ->pExecutor,"all");
        iReturn = 0;
      }    

      /* wait for end */
      iRet = Wait4Success(pServ->pExecutor);
      switch(iRet)
      {
         case DEVINT:
                 if(SCGetInterrupt(pCon) == eAbortOperation)
                 {
                       SCSetInterrupt(pCon,eContinue);
                       SCSetError(pCon,OKOK);
                       SCWrite(pCon,"Driving to HKL Aborted",eStatus);
                 }
                 return 0;
                 break;
          case DEVDONE:
                 sprintf(pBueffel,"Driving to %8.4f %8.4f %8.4f done",
                         fHKL[0], fHKL[1], fHKL[2]);
                 SCWrite(pCon,pBueffel,eStatus);
                 break;
          default:
                 SCWrite(pCon,"WARNING: driving to HKL finished with problems",
                         eWarning);
                 if(SCGetInterrupt(pCon) != eContinue)
		 {
                    return 0;
                 }
                 break;                          
      }   
      return iReturn;
  }    
/*-----------------------------------------------------------------------*/
  int GetCurrentPosition(pHKL self, SConnection *pCon, float fPos[4])
  {
     float fVal;
     int iRet, iResult;

     assert(self);
     assert(pCon);
     
     iResult = 1;
     iRet = MotorGetSoftPosition(self->pTheta,pCon, &fVal);
     if(iRet == 1)
     {
        fPos[0] = fVal;
     }
     else
     {
       iResult = 0;
     }
     iRet = MotorGetSoftPosition(self->pOmega,pCon, &fVal);
     if(iRet == 1)
     {
        fPos[1] = fVal;
     }
     else
     {
       iResult = 0;
     }

     /* normal beam geometry */
     if(self->iNOR == 1)
     {
        iRet = MotorGetSoftPosition(self->pNu,pCon, &fVal);
        if(iRet == 1)
        {
            fPos[2] = fVal;
        }
        else
        {
          iResult = 0;
        }
        return iResult;
     }

     /* bissecting geometry */
     iRet = MotorGetSoftPosition(self->pChi,pCon, &fVal);
     if(iRet == 1)
     {
        fPos[2] = fVal;
     }
     else
     {
       iResult = 0;
     }
     iRet = MotorGetSoftPosition(self->pPhi,pCon, &fVal);
     if(iRet == 1)
     {
        fPos[3] = fVal;
     }
     else
     {
       iResult = 0;
     }

     return iResult;
  }
/*-------------------------------------------------------------------------
  For the conversion from angles to HKL.
  -------------------------------------------------------------------------*/
  static int angle2HKL(pHKL self ,double tth, double om, 
                          double chi, double phi, float fHKL[3])  
  {
     MATRIX rez, z1m;
     double z1[3];
     int i;

     z1FromAngles(self->fLambda,tth,om,chi,phi,z1);
     z1m = vectorToMatrix(z1);
     /* multiply with UBinv in order to yield HKL */
     rez = mat_mul(self->UBinv,z1m);
     for(i = 0; i < 3; i++)
     {
       fHKL[i] = (float)rez[i][0];
     }
     mat_free(z1m);
     mat_free(rez);
  
     return 1;
  }
/*--------------------------------------------------------------------------*/
  int isNumeric(char *pText)
  {
    int i, ii, iGood;
    static char pNum[13] = {"1234567890.+-"};
    
    for(i = 0; i < strlen(pText); i++)
    {
      for(ii = 0; ii < 13; ii++)
      {
        iGood = 0;
        if(pText[i] == pNum[ii])
        {
          iGood = 1;
          break;
        }
      }
      if(!iGood)
      {
          return 0;
      }
    }    
    return 1;
  }  
                                  
/*--------------------------------------------------------------------------*/
  static int GetCommandData(int argc, char *argv[], float fHKL[3], 
                            float *fPsi, int *iHamil, SConnection *pCon)
  {
     int iRet, i;
     double d;
     char pBueffel[512];
     
     if(argc < 3)
     {
        SCWrite(pCon,
              "ERROR: Insufficient reflection data specified for calculation",
              eError);
        return 0;      
     }

     /* get the reflection */
     for(i = 0; i < 3; i++)
     {
        if(!isNumeric(argv[i]))
        {
           sprintf(pBueffel,"ERROR: %s is NOT recognized as a number",argv[i]);
           SCWrite(pCon,pBueffel,eError);
           return 0;
        }
        fHKL[i] = atof(argv[i]);
     }
     
     *fPsi = 0.;
     *iHamil = 0;
     
     /* has psi been specicifed ? */
     if(argc > 3)
     {
        if(!isNumeric(argv[3]))
        {
           sprintf(pBueffel,"ERROR: %s is NOT recognized as a number",argv[3]);
           SCWrite(pCon,pBueffel,eError);
        }
        *fPsi = atof(argv[3]);
     }
     
     /* has iHamil been specified ? */
     if(argc > 4)
     {
        if(!isNumeric(argv[4]))
        {
           sprintf(pBueffel,"ERROR: %s is NOT recognized as a number",argv[4]);
           SCWrite(pCon,pBueffel,eError);
        }
        *iHamil = atof(argv[4]);
     }
     return 1;
  }
/*--------------------------------------------------------------------------*/
  int HKLAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                int argc, char *argv[])
  {
     int iRet,i, iHamil;
     char pBueffel[512];
     float fUB[9], fPsi, fVal;
     float fHKL[3], fSet[4];
     pHKL self = NULL;
     CommandList *pCom = NULL;
     pDummy pDum = NULL;
     
     assert(pCon);
     assert(pSics);
     
     self = (pHKL)pData;
     assert(self);
     
     /* enough arguments ? */
     if(argc < 2)
     {
       SCWrite(pCon,"Insufficient number of arguments to HKL",eError);
       return 0;       
     }
     
/*-------- handle list */
     strtolower(argv[1]);
     if(strcmp(argv[1],"list") == 0 )
     {
        sprintf(pBueffel,
           "lambda = %f   Normal Beam = %d  Quadrant = %d HM = %d", 
                self->fLambda, self->iNOR, self->iQuad,self->iHM);
        SCWrite(pCon,pBueffel,eValue);
        sprintf(pBueffel,"UB = { %f %f %f",
                self->fUB[0], self->fUB[1],self->fUB[2]);
        SCWrite(pCon,pBueffel,eValue);
        sprintf(pBueffel,"       %f %f %f",
                self->fUB[3], self->fUB[4],self->fUB[5]);
        SCWrite(pCon,pBueffel,eValue);
        sprintf(pBueffel,"       %f %f %f }",
                self->fUB[6], self->fUB[7],self->fUB[8]);
        SCWrite(pCon,pBueffel,eValue);
        return 1;                        
        sprintf(pBueffel,"Last HKL:  %f %f %f ",
                self->fLastHKL[0], self->fLastHKL[1],self->fLastHKL[2]);
        SCWrite(pCon,pBueffel,eValue);
        return 1;                        
     } 
/*----------- current */     
     else if(strcmp(argv[1],"current") == 0)
     {
        if(self->iNOR)
	{
           sprintf(pBueffel,"Last HKL:  %8.4f %8.4f %8.4f ",
                   self->fLastHKL[0], self->fLastHKL[1],self->fLastHKL[2]);
           SCWrite(pCon,pBueffel,eValue);
           return 1;
        } 
        else 
	{
	  /* do a serious calculation based on angles */
          iRet = GetCurrentPosition(self,pCon,fSet);
          if(iRet == 0)
	  {
            return 0;
          }
          angle2HKL(self,(double)fSet[0],(double)fSet[1],
                    (double)fSet[2],(double)fSet[3],fHKL);
          sprintf(pBueffel,"Current HKL:  %8.4f %8.4f %8.4f ",
                   fHKL[0], fHKL[1],fHKL[2]);
           SCWrite(pCon,pBueffel,eValue);
           return 1;
        }                        
     }
/*------------- lambda */
     else if(strcmp(argv[1],"lambda") == 0)
     {
        if(argc < 3)
        {
          SCWrite(pCon,"ERROR: Insufficient number of arguments to HKL lambda",eError);
          return 0;       
        }
        if(!SCMatchRights(pCon,usUser))
        {
          return 0;
        }
        if(!isNumeric(argv[2]))
        {
          sprintf(pBueffel,"ERROR: %s was not recognized as a number", argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        fVal = atof(argv[2]);
        iRet = SetWavelengthManual(self,fVal);
        if(!iRet)
        {
          SCWrite(pCon,"ERROR: cannot set wavelength",eError);
          return 0; 
        }
        SCSendOK(pCon);
        return 1;
     }
/*------- lambdavar*/
     else if(strcmp(argv[1],"lambdavar") == 0)
     {
        if(argc < 3)
        {
          SCWrite(pCon,"ERROR: Insufficient number of arguments to HKL lambdavar",eError);
          return 0;       
        }
        if(!SCMatchRights(pCon,usUser))
        {
          return 0;
        }
        pCom = FindCommand(pSics,argv[2]);
        if(!pCom)
        {
          sprintf(pBueffel,"ERROR: cannot find variable --> %s <--",argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        pDum = (pDummy)pCom->pData;
        if(!pDum)
        {
          sprintf(pBueffel,"ERROR: cannot find variable --> %s <--",argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        if(strcmp(pDum->pDescriptor->name,"SicsSelVar") != 0)
        {
          sprintf(pBueffel,"ERROR: variable --> %s <-- has nothing to do with wavelength",argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        iRet = SetWavelengthVariable(pCon,self,(pSelVar)pDum);
        if(!iRet)
        {
          SCWrite(pCon,"ERROR: cannot set wavelength variable",eError);
          return 0; 
        }
        SCSendOK(pCon);
        return 1;
     }
/*------------ UB */
     else if(strcmp(argv[1],"setub") == 0)
     {
       if(argc < 11)
       {
          SCWrite(pCon,"ERROR: Insufficient number of arguments to HKL setUB",eError);
          return 0;       
       }
       if(!SCMatchRights(pCon,usUser))
       {
          return 0;
       }
       for(i = 2; i < 11; i++)
       {
          if(!isNumeric(argv[i]))
          {
            sprintf(pBueffel,"ERROR: %s was not recognized as a number", argv[i]);
            SCWrite(pCon,pBueffel,eError);
            return 0;
          }
          fUB[i-2] = atof(argv[i]);
       }
       iRet = SetUB(self,fUB);
       if(!iRet)
       {
          SCWrite(pCon,"ERROR: cannot set UB Matrix",eError);
          return 0; 
       }
        SCSendOK(pCon);
        return 1;
     }
/*------------- HM mode */
     else if(strcmp(argv[1],"hm") == 0)
     {
        if(argc < 3)
        {
	  sprintf(pBueffel,"%s.hm = %d", argv[0],self->iHM);
          SCWrite(pCon,pBueffel,eValue);
          return 1;       
        }
        if(!SCMatchRights(pCon,usUser))
        {
          return 0;
        }
        if(!isNumeric(argv[2]))
        {
          sprintf(pBueffel,"ERROR: %s was not recognized as a number", argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        self->iHM = atoi(argv[2]); 
        SCSendOK(pCon);
        return 1;
     }
/*------------- normal beam */
     else if(strcmp(argv[1],"nb") == 0)
     {
        if(argc < 3)
        {
          SCWrite(pCon,"ERROR: Insufficient number of arguments to HKL nb",eError);
          return 0;       
        }
        if(!SCMatchRights(pCon,usUser))
        {
          return 0;
        }
        if(!isNumeric(argv[2]))
        {
          sprintf(pBueffel,"ERROR: %s was not recognized as a number", argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        iRet = SetNOR(self,atoi(argv[2])); 
        if(!iRet)
        {
           SCWrite(pCon,
        "ERROR: cannot set Normal Beam geometry, probably nu motor undefined",
             eError);
           return 0; 
        }
        SCSendOK(pCon);
        return 1;
     }
/*------------- quadrant  */
     else if(strcmp(argv[1],"quadrant") == 0)
     {
        if(argc < 3)
        {
          SCWrite(pCon,"ERROR: Insufficient number of arguments to HKL quadrant",eError);
          return 0;       
        }
        if(!SCMatchRights(pCon,usUser))
        {
          return 0;
        }
        if(!isNumeric(argv[2]))
        {
          sprintf(pBueffel,"ERROR: %s was not recognized as a number", argv[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;
        }
        iRet = atoi(argv[2]);
        if(!( (iRet == 1) || (iRet == 0)) )
        {
          sprintf(pBueffel,"ERROR: Invalid parameter %d for quadrant, posiible: 0,1",
                  iRet);
          SCWrite(pCon,pBueffel,eError);
          return 0;        
        }
        self->iQuad = iRet;
        SCSendOK(pCon);
        return 1;
     }
/*------------- calculate */     
     else if(strcmp(argv[1],"calc") == 0)
     {
       iRet = GetCommandData(argc-2,&argv[2],fHKL, &fPsi, &iHamil,pCon);
       if(!iRet)
       { 
         return 0;
       }
       iRet = CalculateSettings(self,fHKL, fPsi, iHamil, fSet,pCon);
       if(self->iNOR)
       {
            sprintf(pBueffel," gamma = %f, omega = %f, nu = %f",
               fSet[0], fSet[1], fSet[2]);
            SCWrite(pCon,pBueffel,eValue);
       }
       else
       {
         sprintf(pBueffel," theta = %f, omega = %f, chi = %f, phi = %f",
               fSet[0], fSet[1], fSet[2],fSet[3]);
          SCWrite(pCon,pBueffel,eValue);
       }
       if(!iRet)
       {
         SCWrite(pCon,
           "WARNING: Settings violate motor limits or cannot be calculated",
           eWarning); 
         return 0;
       }
       return 1;        
     }
/*------------------ run */
     else if(strcmp(argv[1],"run") == 0)
     {
       if(!SCMatchRights(pCon,usUser))
       {
          return 0;
       }
       iRet = GetCommandData(argc-2,&argv[2],fHKL, &fPsi, &iHamil,pCon);
       if(!iRet)
       { 
         return 0;
       }
       iRet = RunHKL(self,fHKL,fPsi, iHamil, pCon);
       if(!iRet)
       {
         return 0;
       }
       else
       {
         SCSendOK(pCon);
         return 1;
       }
     }
/*------------------ drive */
     else if(strcmp(argv[1],"drive") == 0)
     {
       if(!SCMatchRights(pCon,usUser))
       {
          return 0;
       }
       iRet = GetCommandData(argc-2,&argv[2],fHKL, &fPsi, &iHamil,pCon);
       if(!iRet)
       { 
         return 0;
       }
       iRet = DriveHKL(self,fHKL,fPsi, iHamil, pCon);
       if(!iRet)
       {
         return 0;
       }
       else
       {
         SCSendOK(pCon);
         return 1;
       }
     }
     else
     {
       sprintf(pBueffel,"ERROR: subcommand --> %s <-- not recognized",
               argv[1]);
       SCWrite(pCon,pBueffel,eError);
       return 0;        
     }         
     return 0; /* not reached */ 
  }               





