/*----------------------------------------------------------------------------
                              S A N S W A V E 
                              
   Wavelength calculation for neutron velocity selector. Implements a 
   driveable interface.
   
   copyright: see copyright.h
   
   Mark Koennecke, October 1998
---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "motor.h"
#include "velo.h"
#include "sanswave.h"

#define NOTILT 801

/*-----------------------------------------------------------------------*/
   typedef struct __SANSwave {
                              pObjectDescriptor pDes;
                              pIDrivable        pDrivInt;
                              pVelSel           pSelector;
                             }SANSWave;

/*-----------------------------------------------------------------------*/
  static void CalculateCoefficients(float fTilt, float *fA, float *fB)
  {
     float fSQ, fTer, fQuat;
     
     fSQ = fTilt * fTilt;
     fTer = fSQ * fTilt;
     fQuat = fTer * fTilt;
     
     *fA = 0.01223 + (0.000360495 * fTilt) + (0.000313819 * fSQ) +
           (0.0000304937 * fTer) + (0.000000931533 * fQuat);
           
     *fB = 12721.11905 - (611.74127 * fTilt) - (12.44417 * fSQ) -
           (0.12411 * fTer) + (0.00583 * fQuat);
                 
  } 
/*-----------------------------------------------------------------------*/
  int CalculateLambda(float fRot, float fTilt, float *fLambda)
  {
     float fA, fB;
     
     if(fRot  < 10.0)
     {
        *fLambda = 0.0;
        return 1;
     }
     
     CalculateCoefficients(fTilt,&fA, &fB);
     
     *fLambda = fA + fB/fRot;
     return 1;
  } 
/*-------------------------------------------------------------------------*/
  static int SWHalt(void *pData)
  {
     pSANSWave self = NULL;
     pIDrivable pDriv = NULL;
     
     self = (pSANSWave)pData;
     assert(self);
     
     pDriv = GetDrivableInterface(self->pSelector);
     assert(pDriv);
     
     return pDriv->Halt(self->pSelector);
  }
/*----------------------------------------------------------------------*/
  static int SWLimits(void *pData, float fVal, char *error, int iErrLen)
  {
     pSANSWave self = NULL;
     pIDrivable pDriv = NULL;
     int iRet;
     float fTilt, fA, fB, fRot;
     
     self = (pSANSWave)pData;
     assert(self);
     
     pDriv = GetDrivableInterface(self->pSelector);
     assert(pDriv);
     
     /* get tilt */
     iRet = VSGetTilt(self->pSelector,&fTilt);
     if(!iRet)
     {
       strncpy(error,"Failed to obtain tilt angle",iErrLen);
       return 0;
     }

     /* Calculate rotation speed  */
     CalculateCoefficients(fTilt,&fA, &fB);
     fRot = fB/(fVal - fA);
     return pDriv->CheckLimits(self->pSelector,fRot,error, iErrLen);
  }
/*-------------------------------------------------------------------------*/
  static long SWSet(void *pData, SConnection *pCon, float fVal)
  {
     pSANSWave self = NULL;
     pIDrivable pDriv = NULL;
     int iRet, i;
     float fTilt, fA, fB, fRot;
     pDummy pDum = NULL;
     
     self = (pSANSWave)pData;
     assert(self);
     
     pDriv = GetDrivableInterface(self->pSelector);
     assert(pDriv);
     
     /* get tilt */
     fTilt = -910;
     for(i = 0; i < 3; i++)
     {
          iRet = VSGetTilt(self->pSelector,&fTilt);
          if(iRet)
          {
            break;
          }
          else
          {
            SCWrite(pCon,"WARNING: trouble reading tilt angle",eWarning);
          }
     }     
     if(fTilt < -900) /* failed to get tilt */
     {
        SCWrite(pCon,"ERROR: failed to read tilt angle 3 times",eError);
        return 0;
     }
     
     /* Calculate rotation speed  */
     CalculateCoefficients(fTilt,&fA, &fB);
     fRot = fB/(fVal - fA);
     return pDriv->SetValue(self->pSelector,pCon,fRot);
  }
/*------------------------------------------------------------------------*/
  static int SWStatus(void *pData,SConnection *pCon )
  {
     pSANSWave self = NULL;
     pIDrivable pDriv = NULL;
     
     self = (pSANSWave)pData;
     assert(self);
     
     pDriv = GetDrivableInterface(self->pSelector);
     assert(pDriv);
     
     return pDriv->CheckStatus(self->pSelector, pCon);
  }
/*-------------------------------------------------------------------------*/
  static float SWGetValue(void *pData,SConnection *pCon )
  {
     pSANSWave self = NULL;
     pIDrivable pDriv = NULL;
     float fRot, fTilt, fA, fB, fLambda;
     int i, iRet;
     
     self = (pSANSWave)pData;
     assert(self);
     
     pDriv = GetDrivableInterface(self->pSelector);
     assert(pDriv);
     
     /* get tilt */
     fTilt = -910;
     for(i = 0; i < 3; i++)
     {
          iRet = VSGetTilt(self->pSelector,&fTilt);
          if(iRet)
          {
            break;
          }
          else
          {
            SCWrite(pCon,"WARNING: trouble reading tilt angle",eWarning);
          }
     }     
     if(fTilt < -900) /* failed to get tilt */
     {
        SCWrite(pCon,"ERROR: failed to read tilt angle 3 times",eError);
        return -99999.99;
     }
     
     /* get rotation speed */
     iRet = VSGetRotation(self->pSelector,&fRot);    
     if(!iRet)
     {
        SCWrite(pCon,"ERROR: cannot reading rotation speed",eError);
        return -99999.99;
     }
   
      CalculateLambda(fRot,fTilt,&fLambda);
      return fLambda;
  }
/*--------------------------------------------------------------------------*/
  static void *SWGetInterface(void *pData, int iID)
  {
       pSANSWave self = NULL;
       
       self = (pSANSWave)pData;
       assert(self);

       if(iID == DRIVEID)
       {
         return self->pDrivInt;
       }
       return NULL;  
  }    
/*--------------------------------------------------------------------------*/
  static void KillSANSWave(void *pData)
  {
    pSANSWave self = NULL;
    
    self = (pSANSWave)pData;
    if(!self)
       return;
       
    if(self->pDes)
       DeleteDescriptor(self->pDes);
       
    if(self->pDrivInt)
       free(self->pDrivInt);
       
    free(self);         
  }       
/*--------------------------------------------------------------------------
   arguments: name, name of velocity selctor 
*/   
  int  MakeSANSWave(SConnection *pCon, SicsInterp *pSics, void *pData,
                    int argc, char *argv[])
  {
     pSANSWave pNew = NULL;
     CommandList *pCom = NULL;
     char pBueffel[512];
     int iRet;
     pDummy pDum = NULL;
     
     assert(pCon);
     assert(pSics);
     
     /* enough arguments ? */
     if(argc < 3)
     {
        SCWrite(pCon,"ERROR: not enough arguments to MakeSANSWave",eError);
        return 0;
     }
    
     /* allocate space */
     pNew = (pSANSWave)malloc(sizeof(SANSWave));
     if(!pNew)
     {
        SCWrite(pCon,"ERROR: out of memory in MakeSANSWave",eError);
        return 0;
     }
     memset(pNew,0,sizeof(SANSWave));
     
     
     /* the last arument must denote a velocity selector */
     pCom = FindCommand(pSics, argv[2]);
     if(!pCom)
     {
        sprintf(pBueffel,"ERROR: velocity selector %s NOT found",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        KillSANSWave(pNew);
        return 0;     
     } 
     pNew->pSelector = (pVelSel)pCom->pData;
     if(!pNew->pSelector)
     {
        sprintf(pBueffel,"ERROR: velocity selector %s is invalid",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        KillSANSWave(pNew);
        return 0;     
     }
     pDum = (pDummy)pNew->pSelector; 
     if(strcmp(pDum->pDescriptor->name,"VelocitySelector") != 0)
     {
        sprintf(pBueffel,"ERROR: velocity selector %s is invalid",argv[2]);
        SCWrite(pCon,pBueffel,eError);
        KillSANSWave(pNew);
        return 0;     
     } 

     /* initialise the rest of the data structure */
     pNew->pDes = CreateDescriptor("SANSWave");
     pNew->pDrivInt = CreateDrivableInterface();
     if( (!pNew->pDes) || (!pNew->pDrivInt) )
     {
        SCWrite(pCon,"ERROR: out of memory in MakeSANSWave",eError);
        KillSANSWave(pNew);
        return 0;
     } 
     pNew->pDes->GetInterface = SWGetInterface;
     
     pNew->pDrivInt->Halt          = SWHalt;
     pNew->pDrivInt->CheckLimits   = SWLimits;
     pNew->pDrivInt->SetValue      = SWSet;
     pNew->pDrivInt->CheckStatus   = SWStatus;
     pNew->pDrivInt->GetValue      = SWGetValue;
      
     /* install command */
     iRet = AddCommand(pSics,argv[1],SANSWaveAction,KillSANSWave,pNew);
     if(!iRet)
     {
        sprintf(pBueffel,"ERROR: duplicate command %s NOT created",
                argv[1]);
        SCWrite(pCon,pBueffel,eError);
        KillSANSWave(pNew);
        return 0;        
     } 
     return 1;
  }                          
/*---------------------------------------------------------------------------*/
  int SANSWaveAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                     int argc, char *argv[])
  {
    pSANSWave self = NULL;
    float fLambda, fTilt, fRot, fA, fB;
    double dNum;
    int iRet;
    char pBueffel[256];
    
    self = (pSANSWave)pData;
    assert(self);
    assert(pCon);
    assert(pSics);
    
    if(argc > 2)
    {
       strtolower(argv[1]);
       /* whatever we are asked to do, we need the current tilt angle */
       iRet = VSGetTilt(self->pSelector, &fTilt);
       if(!iRet)
       {
          SCWrite(pCon,"ERROR: failed to read tilt angle",eError);
          return 0;
       }
       /* the second argument must be a number */
       iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dNum);  
       if(iRet != TCL_OK)
       {
         sprintf(pBueffel,"ERROR: cannot convert %s to number",argv[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;
       }   
       if(strcmp(argv[1],"rot") == 0) /* calculate rotation for wl */ 
       {
          CalculateCoefficients(fTilt,&fA, &fB);
          fRot = fB/(dNum-fA);
          sprintf(pBueffel,"%f nm = %f RPM", (float)dNum, fRot);
          SCWrite(pCon,pBueffel,eValue);
          return 1;   
       }
       else if(strcmp(argv[1],"wl") == 0) /* calculate wl from rot */ 
       {
          CalculateLambda((float)dNum,fTilt,&fLambda);
          sprintf(pBueffel,"%f RPM = %f nm",(float)dNum,fLambda);
          SCWrite(pCon,pBueffel,eValue);
          return 1;
       }
       else
       {
         sprintf(pBueffel,"ERROR: subcommand %s NOT understood",argv[1]);
         SCWrite(pCon,pBueffel,eError);
         return 0;
       }
    }
    
    fLambda = self->pDrivInt->GetValue(self,pCon);
    if(fLambda < -90000)
    {
       return 0;
    }
    
    sprintf(pBueffel,"%s = %f", argv[0],fLambda);
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  }