/*----------------------------------------------------------------------------
                              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"
#include "sicshipadaba.h"

#define NOTILT 801

static int  SWGetTilt(void *data, float *fTilt)
{
	pDummy pDum = (pDummy)data;
	pHdb node = NULL;
	
	if(strcmp(pDum->pDescriptor->name,"VelocitySelector") == 0) {
		return VSGetTilt((pVelSel)data, fTilt);
	} else if(strcmp(pDum->pDescriptor->name,"NVS") == 0){
		node = GetHipadabaNode(pDum->pDescriptor->parNode,"tilt");
		assert(node != NULL);
		*fTilt = (float)node->value.v.doubleValue;
		return 1;
	} else {
		assert(0);
	}
	return 0;
}
/*-----------------------------------------------------------------------*/
typedef struct __SANSwave {
  pObjectDescriptor pDes;
  pIDrivable pDrivInt;
  void  *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 = SWGetTilt(self->pSelector, &fTilt);
  if (!iRet) {
    strlcpy(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 = SWGetTilt(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 = SWGetTilt(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 */
  fRot = pDriv->GetValue(self->pSelector, pCon);
  if (fRot < -9999.) {
    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) {
    snprintf(pBueffel,511, "ERROR: velocity selector %s NOT found", argv[2]);
    SCWrite(pCon, pBueffel, eError);
    KillSANSWave(pNew);
    return 0;
  }
  pNew->pSelector = pCom->pData;
  if (!pNew->pSelector) {
    snprintf(pBueffel,511, "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 
		  && strcmp(pDum->pDescriptor->name,"NVS") != 0) {
    snprintf(pBueffel,511, "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) {
    snprintf(pBueffel,511, "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 = SWGetTilt(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) {
      snprintf(pBueffel,255, "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 {
      snprintf(pBueffel,255, "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;
}