sics/cone.c

/*----------------------------------------------------------------------
  SICS cone module for cone scans. Form more details see conescan.tex
  and cone.tex.
  
  COPYRIGHT: see file COPYRIGHT
  
  Mark Koennecke, March 2006
  
  Reworked for new four circle infrastructure.
   
  Mark Koennecke, August 2008
------------------------------------------------------------------------*/
#include <stdio.h>
#include <assert.h>
#include "cone.h"
#include  "hkl.i"
#include "vector.h"
#include "fourlib.h"
#include "singlex.h"
#include "sicsobj.h"
#include "sicshipadaba.h"
/*=================== Object Descriptor Interface ===================================================*/
static void *ConeGetInterface(void *pData, int iID)
{
  pConeData self = NULL;
  pSICSOBJ obj = (pSICSOBJ) pData;

  self = (pConeData) obj->pPrivate;
  if (self == NULL) {
    return NULL;
  }
  if (iID == DRIVEID) {
    return self->pDriv;
  }
  return NULL;
}

/*=================== Drivable Interface ============================================================*/
static int ConeHalt(void *pData)
{
  pSICSOBJ obj = pData;
  pConeData self = NULL;

  self = (pConeData) obj->pPrivate;
  assert(self != NULL);

  stopHKLMotors(self->pHkl);
  return 1;
}

/*-----------------------------------------------------------------------------------------------------*/
static int ConeCheckLimits(void *self, float fVal, char *error, int errLen)
{
  /*
     There is no meaningful implementation here. This gets called when starting the motor.
     At that stage not all other values may be known. If the calculation fails, this will die 
     at status check time.
   */
  return 1;
}

/*------------------------------------------------------------------------*/
static MATRIX makeCsToPsiMatrix(reflection center, double lambda)
{
  MATRIX psiToCs = NULL, csToPsi = NULL, t1, t2;
  double z1[3], u;

  psiToCs = makeInstToConeVectorMatrix(center, lambda);
  if (psiToCs == NULL) {
    return NULL;
  }
  csToPsi = mat_inv(psiToCs);
  /*
   * this is debugging code: remove together with variables
   */
  z1FromAngles(lambda, center.s2t, center.om, center.chi, center.phi, z1);
  t1 = makeVectorInit(z1);
  t2 = mat_mul(psiToCs, t1);
  normalizeVector(t2);
  t1[0][0] = .0;
  t1[1][0] = .0;
  t1[2][0] = 1.;
  u = angleBetween(t1, t2);

  mat_free(psiToCs);
  return csToPsi;
}

/*----------------------------------------------------------------------------------------------------
 * I am lazy in this function: I calculate anew from all the data. This saves
 * me a lot of trouble keeping track of parameter changes in UBCALC etc.
 * ---------------------------------------------------------------------------*/
static long ConeSetValue(void *pData, SConnection * pCon, float fVal)
{
  pSICSOBJ obj = pData, refList;
  pConeData self = NULL;
  double fSet[4];
  float ffSet[4];
  double openingAngle, length;
  MATRIX csToPsi = NULL, B = NULL, newScat = NULL;
  int status, i;
  reflection center, target;
  char buffer[131];
  const double *cell;
  double hkl[3], ang[4];
  lattice direct;
  hdbValue v;

  if (!SCMatchRights(pCon, usUser)) {
    return 0;
  }

  self = (pConeData) obj->pPrivate;
  assert(self != NULL);

  /*
   * calculate opening angle
   */
  B = mat_creat(3, 3, UNIT_MATRIX);
  cell = SXGetCell();
  direct.a = cell[0];
  direct.b = cell[1];
  direct.c = cell[2];
  direct.alpha = cell[3];
  direct.beta = cell[4];
  direct.gamma = cell[5];
  status = calculateBMatrix(direct, B);
  if (status < 0) {
    SCWrite(pCon, "ERROR: cell has no volume", eError);
    return 0;
  }
  /*
   * get center from the main reflection list
   */
  refList = SXGetReflectionList();
  SICSHdbGetPar(obj, pCon, "center", &v);
  if (!GetRefIndexID(refList, v.v.text, hkl)) {
    SCPrintf(pCon, eError, "ERROR: cannot find reflection with ID: %s",
             v.v.text);
    return 0;
  }
  center.h = hkl[0];
  center.k = hkl[1];
  center.l = hkl[2];
  GetRefAnglesID(refList, v.v.text, ang);
  center.s2t = ang[0];
  center.om = ang[1];
  center.chi = ang[2];
  center.phi = ang[3];

  SICSHdbGetPar(obj, pCon, "target", &v);
  target.h = v.v.floatArray[0];
  target.k = v.v.floatArray[1];
  target.l = v.v.floatArray[2];
  openingAngle = angleBetweenReflections(B, center, target);

  /*
   * calculate conversion matrix from cone system to PSI system
   */
  csToPsi = makeCsToPsiMatrix(center, SXGetLambda());
  if (csToPsi == NULL) {
    SCWrite(pCon,
            "ERROR: bad parameters: failed to generate conversion matrix",
            eError);
    return 0;
  }

  /*
   * calculate scattering vector on cone and make its length
   * match the length of the apropriate scattering vector
   */
  SICSHdbGetPar(obj, pCon, "target", &v);
  target.h = v.v.floatArray[0];
  target.l = v.v.floatArray[2];
  SICSHdbGetPar(obj, pCon, "qscale", &v);
  /*
   * calculate scattering vector on cone and make its length
   * match the length of the apropriate scattering vector
   */
  length = scatteringVectorLength(B, target) * v.v.doubleValue;
  newScat = calcConeVector(openingAngle, fVal, length, csToPsi);
  if (newScat == NULL) {
    SCWrite(pCon, "ERROR: fails to calculate cone vector", eError);
    return 0;
  }

  /*
   * try to find setting angles for this vector
   */
  status = findAllowedBisecting(SXGetLambda(), newScat, fSet,
                                hklInRange, self->pHkl);
  /*
   * clean up matrices
   */
  mat_free(B);
  mat_free(newScat);
  mat_free(csToPsi);
  if (status != 1) {
    SCWrite(pCon, "ERROR: cannot get cone vector into scattering position",
            eLogError);
    SCSetInterrupt(pCon, eAbortOperation);
    return 0;
  }
  self->lastConeAngle = fVal;
  /*
   * start motors
   */
  for (i = 0; i < 4; i++) {
    ffSet[i] = fSet[i];
  }
  return startHKLMotors(self->pHkl, pCon, ffSet);
}

/*---------------------------------------------------------------------------------------------------*/
static int checkMotors(pConeData self, SConnection * pCon)
{
  int status, i;
  pMotor pMot = NULL;
  MotorFunction mots[] = { TwoTheta, Omega, Chi, Phi };

  for (i = 0; i < 4; i++) {
    pMot = SXGetMotor(mots[i]);
    if (pMot != NULL) {
      status = pMot->pDrivInt->CheckStatus(pMot, pCon);
      if (status != HWIdle && status != OKOK) {
        return status;
      }
    }
  }
  return HWIdle;
}

/*-----------------------------------------------------------------------------------------------------*/
static int ConeCheckStatus(void *pData, SConnection * pCon)
{
  pSICSOBJ obj = pData;
  pConeData self = NULL;
  int status;

  self = (pConeData) obj->pPrivate;
  assert(self != NULL);
  return checkMotors(self, pCon);
}

/*-----------------------------------------------------------------------------------------------------*/
static float ConeGetValue(void *pData, SConnection * pCon)
{
  pSICSOBJ obj = pData;
  pConeData self = NULL;
  float fVal[3];
  int status;

  self = (pConeData) obj->pPrivate;
  assert(self != NULL);

  return self->lastConeAngle;
}

/*=============================== Live and Death ====================================*/
static pConeData MakeConeMot()
{
  pConeData self = NULL;

  self = (pConeData) malloc(sizeof(coneData));
  if (self == NULL) {
    return NULL;
  }
  memset(self, 0, sizeof(coneData));
  self->pDriv = CreateDrivableInterface();
  if (self->pDriv == NULL) {
    free(self);
    return NULL;
  }

  self->pDriv->Halt = ConeHalt;
  self->pDriv->CheckLimits = ConeCheckLimits;
  self->pDriv->SetValue = ConeSetValue;
  self->pDriv->CheckStatus = ConeCheckStatus;
  self->pDriv->GetValue = ConeGetValue;
  return self;
}

/*------------------------------------------------------------------------------------------*/
int MakeCone(SConnection * pCon, SicsInterp * pSics, void *pData, int argc,
             char *argv[])
{
  pSICSOBJ pNew = NULL;
  pConeData pMot = NULL;
  char pBuffer[131], pName[80];
  int status;
  pHdb cmd;

  if (argc > 1) {
    strlcpy(pName, argv[1],80);
  } else {
    strcpy(pName, "cone");
  }

  pNew = MakeSICSOBJ(pName, "Cone");
  pMot = MakeConeMot();
  if (pNew == NULL || pMot == NULL) {
    SCWrite(pCon, "ERROR: out of memory creating cone virtual motor",
            eError);
    return 0;
  }

  pNew->pDes->GetInterface = ConeGetInterface;
  pNew->pPrivate = pMot;
  pNew->KillPrivate = DefaultFree;

  cmd =
      AddSICSHdbPar(pNew->objectNode, "target", usUser,
                    makeHdbValue(HIPFLOATAR, 3));
  SetHdbProperty(cmd, "__save", "true");
  cmd =
      AddSICSHdbPar(pNew->objectNode, "qscale", usUser, MakeHdbFloat(1.));
  SetHdbProperty(cmd, "__save", "true");
  cmd =
      AddSICSHdbPar(pNew->objectNode, "center", usUser,
                    MakeHdbText("unknown"));
  SetHdbProperty(cmd, "__save", "true");


  if (argc > 2) {
    pMot->pHkl = FindCommandData(pSics, argv[2], "4-Circle-Calculus");
  } else {
    pMot->pHkl = FindCommandData(pSics, "hkl", "4-Circle-Calculus");
  }
  if (pMot->pHkl == NULL) {
    snprintf(pBuffer, 131, "ERROR: %s is no hkl object", argv[2]);
    SCWrite(pCon, pBuffer, eError);
    return 0;
  }

  status = AddCommand(pSics, pName, InterInvokeSICSOBJ, KillSICSOBJ, pNew);
  if (status != 1) {
    SCWrite(pCon, "ERROR: failed to create duplicate cone motor", eError);
  }
  return status;
}