/*---------------------------------------------------------------------------
                         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
   
-----------------------------------------------------------------------------*/
#include <math.h>
#include <ctype.h>
#include "sics.h"
#include "fortify.h"
#include "motor.h"
#include "selector.h"
#include "selvar.h"
#include "matrix/matrix.h"
#include "hkl.h"
#include "hkl.i"
/*-------------------------------------------------------------------------*/
   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]);
      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->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 hamth[8] = {0,0,1,1,0,0,1,1};
     static int hamom[8] = {0,0,1,1,0,0,1,1};
     static int hamch[8] = {0,1,1,1,1,1,1,0};
     static int hamph[8] = {0,1,0,1,1,0,1,0};
     static int hamhkl[8] = {0,1,0,1,0,1,0,1};
     static const float RD = 57.2957795, pi = 3.1415926;
       
#define ABS(x) (x < 0 ? -(x) : (x))    

/*-------------------------------------------------------------------------*/
  static void rtan(double y, double x, double *rt)
  {
    if( (x == 0.) && (y == 0.) )
    {
      *rt = 0.;
      return;
    }       
    if( x == 0.)
    {
      *rt = pi/2.;
      if(y < 0.)
      {
        *rt = -*rt;
      }
      return;
    }
    if(ABS(y) < ABS(x))
    {
      *rt = atan(ABS(y/x));
      if(x < 0.)
      {
        *rt = pi - *rt;
      }
      if(y < 0.)
      {
          *rt = - *rt;
      }
      return;
    }
    else
    {
      *rt = pi/2 - atan(ABS(x/y));
    }
    if(x < 0.)
    {
      *rt = pi - *rt;
    }
    if( y < 0.)
    {
      *rt = - *rt;
    }
    return;
  }
/*------------------------------------------------------------------------*/
  static double fsign(double a, double a2)
  {
    if(a2 >= 0.)
    {
      return ABS(a);
    }
    else
    {
      return -ABS(a);
    }
  }  
/*------------------------------------------------------------------------*/
  static void sincos(double *sn, double *cs, double *loc)
  {
    if(ABS(*sn) >= 1.)
    {
      *sn = fsign(1.0,*sn);
      *cs = 0.;
      return;
    }
    else
    {
      *cs = sqrt(1. - *sn * *sn);
      return;
    }
  }
/*-------------------------------------------------------------------------*/
  static int ICAL(pHKL self, float fHKL[3], float fPsi, int iHamil, 
                  int iQuad, float fSet[4], float fDelom)
  {
     double z[3],tang[2],r,r1,phi,delomr,t,dstar,v,theta,omega,chi,cosp;
     double sinp,d,q,zer,d2,all,den1,den2,cosps,a,sinc,cosc,b,c,chibi;
     double sinps,sinchi,extrap, psi, loc;
     int i,ip,ivariz,ipara,nh,nhamil,isign,icry,ii;
     
     delomr = fDelom/(RD * -1);
     iHamil -= 1; /* to get iHamil from 1 -- 8 & negative if not requested*/
     psi = fPsi;
     
     /* HKl --> Lab Axis through UB */
     for(i = 0; i < 3; i++)
     {
       ii = 3*i;
       z[i] = self->fUB[ii]*fHKL[0] + self->fUB[ii+1]*fHKL[1] +
                self->fUB[ii+2]*fHKL[2]; 
     }

     /* bissecting calculation! */     
     if(!self->iNOR)
     {
       /* four circle calculation */
       t = z[0]*z[0] + z[1]*z[1] + z[2]*z[2];
       dstar = sqrt(t);
       for(i = 0; i < 3; i++)
       {
         z[i] /= dstar;
       }
       v = self->fLambda *.5 * dstar;
       if(v > 1.)
       {
         return -1;
       }
       theta = 2 * asin(v);
       omega = .5 * theta - delomr;
       
       sinchi = -z[2]/cos(delomr);
       if(ABS(sinchi) > 1.)
       {
         return -1;
       } 
       chi = asin(sinchi);
       
       if(iQuad == 1)
       {
         chi = pi - chi;
         if(chi > pi)
         {
           chi = chi -(2.*pi);
         }
       }
       d = delomr;
       
       q = sin(d);
       r = cos(d) * cos(chi);
       cosp = (q*z[0] + r*z[1])/(q*q + r*r);
       sinp = (z[0] - q*cosp)/r;
       rtan(cosp,sinp,&phi);
       
       chi = -chi;
       
       if(fDelom != 0.)
       {
         /* recalculate psi */
         r1 = -z[2];
         if(r1 == 1.)
         {
           psi = phi;
         } 
         else 
         {
           chibi = asin(r1);
           if(iQuad == 1)
           {
             chibi = pi - chibi;
             if(chibi > pi)
             {
               chibi = chibi -(2*pi);
             }
           }
           sinps = tan(chibi)*tan(delomr);
           cosps = (cos(chibi) -sinps*sin(delomr)*sinchi)/cos(chi);
           rtan(sinps,cosps,&psi);
           psi = -psi;
         }
         psi = RD*psi;
         goto hamil;   
       }
       if(psi != 0.)
       { 
          /* non zero psi */
          a = psi/RD;
          sinps = sin(a);
          cosps = cos(a);
          sinc = sin(chi);
          cosc = cos(chi);
          sinp = sin(phi);
          cosp = cos(phi);
       
          a = sinps*sinp - sinc*cosp*cosps;
          b = -sinps*cosp - sinc*sinp*cosps;
          /* do chi */
          c = sqrt(a*a + b*b);
          d = cosps*cosc;
          rtan(c,d,&chi);
          /* do phi */
          rtan(-b,-a,&phi);
          /* do omega */
          a = -sinps *cosc;
          rtan(a,sinc,&omega);
          omega = omega + theta *.5;
       }   
 hamil:
          theta = theta *RD;
          omega = omega *RD;
          chi   = chi*RD;
          phi   = phi*RD;
          
          if(iHamil >= 0)
          {
            /* hamilton logic */
            if(hamth[iHamil]) theta = theta * -1;
            if(hamom[iHamil]) omega = omega -ABS(theta);
            if(hamph[iHamil])
            {
              phi += 180.;
              if(phi > 180.)
              {
                phi -= 360;
              }
            } 
            if(hamch[iHamil])
            {
              if( (iHamil == 1) || (iHamil == 6) )
              {
                chi *= -1;
              
              }else if((iHamil == 1) || (iHamil == 5))
              {
                chi -= 180.;
                if(chi < -180.)
                {
                  chi += 360.;
                }
              }
              else
              {
                chi = chi* -1 - 180;
                if(chi > 180.)
                { 
                  chi -= 360;
                }
              }
            }
          }              
          
          /* hamhkl left out */
          fSet[0] = theta;
          fSet[1] = omega;
          fSet[2] = chi;
          fSet[3] = phi;
          return 1;

     } /* end four circle */
     else
     {
        /* start normal bissecting calculation */
        /* ignore psi values */
        d2 = 0.;
        for(i = 0; i < 3; i++)
        {
          d2 += z[i]*z[i];
        }
        d = sqrt(d2);
        d2 *= self->fLambda*self->fLambda;
        
        tang[0] = z[2]*self->fLambda;
        all = tang[0]*tang[0];
        den1 = sqrt(d2-all);
        if(all > 1.)
        {
          return -1;
        }
        den2 = sqrt(1. - all);
        if(!((den1+den2 > 1.) && (ABS(den2-den1) < 1.)))
        {
          return -1;
        }
        
        isign = 1;
        fSet[3] = 0.;
        sincos(&tang[0],&tang[1],&loc);
        fSet[2] = RD*atan2(tang[0],tang[1]);
        tang[1] = (2-d2)/(2*den2);
        sincos(&tang[1],&tang[0],&loc);
        fSet[0] = RD*atan2(tang[0],tang[1])*isign;
        tang[1] = d2/(2*den1);
        sincos(&tang[1],&tang[0],&loc);
        all = atan2(tang[0],tang[1]);
        all -= isign*atan2(z[1],z[0]);
        fSet[1] = 180. - RD*all -90.;
        if(fSet[1] < 0.)
	{
          fSet[1] = 360. + fSet[1];
        }

        return 1;
     }
  }        
/*-------------------------------------------------------------------------*/
/* This may become dead code if the looping show does not work */
  int CulculuteSettings(pHKL self, float fHKL[3], float fPsi, int iHamil,
                       float fSet[4], SConnection *pCon)
  {
     int iRet, iSuccess = 0, iRes = 1, iRetry = 1;
     int iQuad = 0;
     int iTest;
     float fDelom = 0.;
     char pError[132];
     char pBueffel[512];
     float fHard;
     float fVal;
     
     /* 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;         
     }
     
     /* no retries if normal beam calculation or specific Hamilton requested */
     if( (self->iNOR) || (iHamil != 0) )
     {
       iRetry = 0;
     }
     
     /* loop till success */
     while(!iSuccess)
     {
       /* just try it*/
       iRet = ICAL(self,fHKL, fPsi, iHamil, self->iQuad,fSet,fDelom);
       if(iRet < 0 ) /* could not do it */
       {
          sprintf(pBueffel,"ERROR: cannot calculate %4.1f %4.1f %4.1f",
                  fHKL[0], fHKL[1], fHKL[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;        
       }
       
       /* check two theta  */
       iTest = MotorCheckBoundary(self->pTheta,fSet[0], &fHard,pError,131);
       if(!iTest)
       {
         /* this cannot be fixed */
         sprintf(pBueffel,
           "ERROR: %4.1f, %4.1f, %4.1f, %5.2f violates two theta limits",
                 fSet[0], fHKL[0], fHKL[1],fHKL[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;        
       } 
       
       /* check omega */
       iTest = MotorCheckBoundary(self->pOmega,fSet[1], &fHard,pError,131);
       if(!iTest)
       {
         if((iRetry) && (ABS(fPsi) <= 0.) )
         {
            /* try tweaking omega a bit */
            MotorGetPar(self->pOmega,"softupperlim",&fVal);
            if(fSet[1] > fVal) /* upper limit violated */
            {
              fDelom = -(fSet[1] - fVal - .5);
              iRetry = 0; /* do not try a second time */
            }
            MotorGetPar(self->pOmega,"softlowerlim",&fVal);
            if(fSet[1] < fVal)
            {
              fDelom = (fVal +.5) -fSet[1];
              iRetry = 0;
            }
            continue;
         }  
         else
         {
           /* this cannot be fixed */
           sprintf(pBueffel,
             "ERROR: %4.1f, %4.1f, %4.1f, %5.2f violates omega limits",
                   fSet[1], fHKL[0], fHKL[1],fHKL[2]);
           SCWrite(pCon,pBueffel,eError);
           return 0;        
         }
       }
       
       /* check nu if normal beam */
       if(self->iNOR)
       {
         iTest = MotorCheckBoundary(self->pNu,fSet[2], &fHard,pError,131);
         if(!iTest)
	 {
           sprintf(pBueffel,
       "ERROR: %4.1f, %4.1f, %4.1f, %5.2f %5.2f %5.2f violates nu limits",
                     fHKL[0], fHKL[1],fHKL[2],fSet[0], fSet[1],fSet[2]);
             SCWrite(pCon,pBueffel,eError);
           return 0;
         }
         else
	 {
            return 1;
         }
       }

       /* check chi and phi, but first put into 0-360 degrees */
       if(fSet[2] < 0.0)
       {
          fSet[2] = 360.0 + fSet[2];
       }
       if(fSet[3] < 0.0)
       {
          fSet[3] = 360.0 + fSet[3];
       }
       iTest = MotorCheckBoundary(self->pChi,fSet[2], &fHard,pError,131);
       iTest += MotorCheckBoundary(self->pPhi,fSet[4], &fHard,pError,131);
       if(iTest < 2) /* one of them burns */
       {
           if(iRetry)
           {
             /* try other quadrant */
             if(self->iQuad == 1)
             {
               iQuad = 0;
             }
             else
             {
               iQuad = 1;
             }  
             iRetry = 0;
             continue;
           }
           else
           {
             sprintf(pBueffel,
       "ERROR: %4.1f, %4.1f, %4.1f, %5.2f %5.2f %5.2f %5.2f violates chi, phi limits",
                     fHKL[0], fHKL[1],fHKL[2],fSet[0], fSet[1],fSet[2],fSet[3]);
             SCWrite(pCon,pBueffel,eError);
             return 0;        
             
           }
       }
       iSuccess = 1; /* end loop, we got valid data */
     }
     return 1;
  }          
/*-------------------------------------------------------------------------
  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 succed.
*/
  int CalculateSettings(pHKL self, float fHKL[3], float fPsi, int iHamil,
                       float fSet[4], SConnection *pCon)
  {
     int iRet,iRetry, i;
     int iQuad = 0;
     int iTest;
     float fDelom = 0.;
     char pError[132];
     char pBueffel[512];
     float fHard;
     float fVal;
     float myPsi = fPsi;
     
     /* 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.......... */
     if(myPsi > 360.)
     {
       myPsi -= 360;
     }
     if(myPsi < .0)
     {
       myPsi += 360.;
     }
     
     /* no retries if normal beam calculation 
        or specific Hamilton or specific 
        psi requested */
     if( (self->iNOR) || (iHamil != 0) || (myPsi > 0.1) )
     {
       iRetry = 1;
     }
     else
     {
        iRetry = 35;
     }
     
     /* loop till success */
     for(i = 0, myPsi = fPsi; i < iRetry; i++, myPsi += 10.)
     {
       /* just try it*/
       iRet = ICAL(self,fHKL, myPsi, iHamil, self->iQuad,fSet,fDelom);
       if(iRet < 0 ) /* could not do it */
       {
          sprintf(pBueffel,"ERROR: cannot calculate %4.1f %4.1f %4.1f",
                  fHKL[0], fHKL[1], fHKL[2]);
          SCWrite(pCon,pBueffel,eError);
          return 0;        
       }
       
       /* check two theta  */
       iTest = MotorCheckBoundary(self->pTheta,fSet[0], &fHard,pError,131);
       if(!iTest)
       {
         /* this cannot be fixed */
         sprintf(pBueffel,
           "ERROR: %4.1f, %4.1f, %4.1f, %5.2f violates two theta limits",
                 fSet[0], fHKL[0], fHKL[1],fHKL[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;        
       } 
       
       /* check nu and omega if normal beam */
       if(self->iNOR)
       {
         /* check omega */
         iTest = MotorCheckBoundary(self->pOmega,fSet[1], &fHard,pError,131);
         iTest += MotorCheckBoundary(self->pNu,fSet[2], &fHard,pError,131);
         if(iTest != 2)
	 {
           sprintf(pBueffel,
       "ERROR: %4.1f, %4.1f, %4.1f, %5.2f %5.2f %5.2f violates nu limits",
                     fHKL[0], fHKL[1],fHKL[2],fSet[0], fSet[1],fSet[2]);
             SCWrite(pCon,pBueffel,eError);
           return 0;
         }
         else
	 {
            return 1;
         }
       }


       /* check chi and phi and omega, but first put into 0-360 degrees */
       if(fSet[2] < 0.0)
       {
          fSet[2] = 360.0 + fSet[2];
       }
       /*
       if(fSet[3] < 0.0)
       {
          fSet[3] = 360.0 + fSet[3];
       }
       */
       iTest = MotorCheckBoundary(self->pOmega,fSet[1], &fHard,pError,131);
       iTest += MotorCheckBoundary(self->pChi,fSet[2], &fHard,pError,131);
       iTest += MotorCheckBoundary(self->pPhi,fSet[3], &fHard,pError,131);
       if(iTest == 3) /* none of them burns */
       {
           sprintf(pBueffel,"Solution found at psi = %4.1f",myPsi);
           SCWrite(pCon,pBueffel,eWarning);
           return 1;
       }
     }
     if(iRetry != 1)
     {
        sprintf(pBueffel,
         "ERROR: failed to find a possible setting for %4.1f %4.1f %4.1f %s",
          fHKL[0], fHKL[1], fHKL[2], "\n Even tried 36 psi settings");
        SCWrite(pCon,pBueffel,eError);       
        return 0;
     }
     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])  
  {
     double dTh, dOm, dChi, dPhi, dHM;
     MATRIX res, rez;
     int i;

     /* conversion to radians */
     dTh = tth/(2*RD);
     dOm = (om - tth/2.)/RD;
     dChi = chi/RD;
     dPhi = phi/RD;
     dHM = 2* sin(dTh)/self->fLambda;

     /* angles to XYZ, stolen from DIFRAC code in PRPXYZ */
     res = mat_creat(3,1,ZERO_MATRIX);
     res[0][0] =  dHM*(cos(dChi)*cos(dPhi)*cos(dOm) -
                    sin(dPhi)*sin(dOm));
     res[1][0] = dHM*(cos(dChi)*sin(dPhi)*cos(dOm) +
                 cos(dPhi)*sin(dOm));
     res[2][0] = dHM*sin(dChi)*cos(dOm);

     /* multiply with UBinv in order to yield HKL */
     rez = mat_mul(self->UBinv,res);
     for(i = 0; i < 3; i++)
     {
        fHKL[i] = rez[i][0];
     }  
     mat_free(res);
     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", 
                self->fLambda, self->iNOR, self->iQuad);
        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;
          }
          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;
     }
/*------------- 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);
       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 */ 
  }