/**
 * This is an implementation  of the polymorphic single crystal calculation
 * system defined in singlediff.h for a bisecting four circle diffractometer with
 * an eulerian cradle.
 * 
 * copyright: see file COPYRIGHT
 * 
 * Mark Koennecke, August 2008
 */
#include <stdlib.h>
#include <assert.h>
#include <sics.h>
#include "singlediff.h"
#include "fourlib.h"
#include "ubfour.h"
#include "motor.h"
#include "singlex.h"
#include "motorlist.h"
#include "lld.h"

/*
  the tolerance in chi we give before we allow to fix omega with phi
*/
#define CHITOLERANCE 3.
#define ABS(x) (x < 0 ? -(x) : (x))
/*--------------------------------------------------------------------*/
static int chiVertical(double chi)
{
  if (ABS(chi - .0) < CHITOLERANCE) {
    return 1;
  }
  if (ABS(chi - 180.0) < CHITOLERANCE) {
    return 1;
  }
  return 0;
}

/*-----------------------------------------------------------------------*/
static int checkTheta(pSingleDiff self, double *stt)
{
  char pError[132];
  int iTest;
  float fHard;
  pMotor pTheta;

  pTheta = SXGetMotor(TwoTheta);
  if (pTheta == NULL) {
    return 0;
  }

  iTest = MotorCheckBoundary(pTheta, (float) *stt, &fHard, pError, 131);
  if (!iTest) {
    return -1;
  }
  return iTest;
}

/*-----------------------------------------------------------------------*/
static int checkBisecting(pSingleDiff self,
                          double *stt, double om, double chi, double phi)
{
  int iTest;
  float fHard, fLimit;
  char pError[132];
  pMotor pMot;

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

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

  /* check chi and phi */
  pMot = SXGetMotor(Chi);
  if (pMot == NULL) {
    return 0;
  }
  iTest += MotorCheckBoundary(pMot, (float) chi, &fHard, pError, 131);
  pMot = SXGetMotor(Phi);
  if (pMot == NULL) {
    return 0;
  }
  iTest += MotorCheckBoundary(pMot, (float) phi, &fHard, pError, 131);
  if (iTest == 3) {             /* none of them burns */
    return 1;
  }
  return 0;
}

/*-----------------------------------------------------------------------
  tryOmegaTweak tries to calculate a psi angle in order to put an
  offending omega back into range.

  This routine also handles the special case when chi ~ 0 or chi ~ 180.
  Then it is possible to fix a omega problem by turing in phi.
  -----------------------------------------------------------------------*/
static int tryOmegaTweak(pSingleDiff self, MATRIX z1, double *stt,
                         double *om, double *chi, double *phi)
{
  int status;
  float fLower, fUpper, omTarget, omOffset, phiSign;
  double dumstt, offom, offchi, offphi, lambda;
  pMotor pOmega, pPhi;

  pOmega = SXGetMotor(Omega);
  pPhi = SXGetMotor(Phi);
  lambda = self->lambda;
  if (pOmega == NULL) {
    return 0;
  }

  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(pOmega, "softlowerlim", &fLower);
  MotorGetPar(pOmega, "softupperlim", &fUpper);
  if (*om < fLower) {
    omTarget = fLower;
  }
  if (*om > fUpper) {
    omTarget = fUpper;
  }
  if (omTarget < -7000) {
    return 0;
  }

  /*
     calculate omega offset
   */
  omOffset = *om - omTarget;
  omOffset = -omOffset;

  /*
     check for the special case of chi == 0 or chi == 180
   */
  if (chiVertical(*chi)) {
    dumstt = *stt;
    offom = omTarget;
    offchi = *chi;
    MotorGetPar(pPhi, "sign", &phiSign);
    offphi = *phi - omOffset * phiSign;
    if (checkBisecting(self, &dumstt, offom, offchi, offphi)) {
      *om = offom;
      *chi = offchi;
      *phi = offphi;
      return 1;
    }
  }

  /*
     calculate angles with omega offset 
   */
  status = z1ToAnglesWithOffset(lambda, 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 int hklInRange(void *data, double fSet[4], int mask[4])
{
  pSingleDiff self = (pSingleDiff) data;
  float fHard, fLimit;
  char pError[132];
  int i, test;
  double dTheta;
  pMotor pOmega, pChi, pPhi;

  pOmega = SXGetMotor(Omega);
  pChi = SXGetMotor(Chi);
  pPhi = SXGetMotor(Phi);
  if (pOmega == NULL || pChi == NULL || pPhi == NULL) {
    return 0;
  }

  /* check two theta  */
  dTheta = fSet[0];
  mask[0] = checkTheta(self, &dTheta);
  fSet[0] = dTheta;

  mask[1] = MotorCheckBoundary(pOmega, fSet[1], &fHard, pError, 131);
  mask[2] = MotorCheckBoundary(pChi, fSet[2], &fHard, pError, 131);
  mask[3] = MotorCheckBoundary(pPhi, fSet[3], &fHard, pError, 131);
  for (i = 0, test = 0; i < 4; i++) {
    test += mask[i];
  }
  if (test != 4) {
    return 0;
  } else {
    return 1;
  }
}


/*----------------------------------------------------------------------*/
static int hklInRangeOrion(void *data, double fSet[4], int mask[4])
{
  pSingleDiff self = (pSingleDiff) data;
  float fHard, fLimit;
  char pError[132];
  int i, test;
  double dTheta;
  pMotor pOmega, pChi, pPhi;

  pOmega = SXGetMotor(Omega);
  pChi = SXGetMotor(Chi);
  pPhi = SXGetMotor(Phi);
  if (pOmega == NULL || pChi == NULL || pPhi == NULL) {
    return 0;
  }

  /* check two theta  */
  dTheta = fSet[0];
  mask[0] = checkTheta(self, &dTheta);
  fSet[0] = dTheta;

  /* Orion is not upside down... */
  fSet[2] = -1.*(fSet[2]+180.) + 360. ;
  fSet[3] = circlify(fSet[3]+180.);

  mask[1] = MotorCheckBoundary(pOmega, fSet[1], &fHard, pError, 131);
  mask[2] = MotorCheckBoundary(pChi, fSet[2], &fHard, pError, 131);
  mask[3] = MotorCheckBoundary(pPhi, fSet[3], &fHard, pError, 131);
  for (i = 0, test = 0; i < 4; i++) {
    test += mask[i];
  }
  if (test != 4) {
    return 0;
  } else {
    return 1;
  }
}

/*-------------------------------------------------------------------*/
static int calculateBiSettings(pSingleDiff self,
                               double *hkl, double *settings)
{
  MATRIX z1;
  double stt, om, chi, phi, psi, ompsi, chipsi, phipsi, myPsi;
  int i, test, mask[4], iRetry;
  double lambda;

  myPsi = hkl[3];
  if (myPsi > 0.1) {
    iRetry = 1;
  } else {
    iRetry = 699;
  }

  z1 = calculateScatteringVector(self, hkl);

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

  settings[0] = stt;
  settings[1] = om;
  settings[2] = chi;
  settings[3] = phi;
  if (iRetry == 1) {
    rotatePsi(om, chi, phi, myPsi, &ompsi, &chipsi, &phipsi);
    settings[1] = ompsi;
    settings[2] = circlify(chipsi);
    settings[3] = circlify(phipsi);
    return 1;
  } else {
    if (hklInRange(self, settings, mask) == 1) {
      return 1;
    } else {
      if (tryOmegaTweak(self, z1, &stt, &om, &chi, &phi) == 1) {
        settings[0] = stt;
        settings[1] = om;
        settings[2] = chi;
        settings[3] = phi;
        return 1;
      } else {
        return findAllowedBisecting(self->lambda, z1, settings, hklInRange,
                                    self);
      }
    }
  }
  return 0;
}

/*-------------------------------------------------------------------*/
static int calculateBiSettingsOrion(pSingleDiff self,
                               double *hkl, double *settings)
{
  MATRIX z1;
  double stt, om, chi, phi, psi, ompsi, chipsi, phipsi, myPsi;
  int i, test, mask[4], iRetry;
  double lambda;

  myPsi = hkl[3];
  if (myPsi > 0.1) {
    iRetry = 1;
  } else {
    iRetry = 699;
  }

  z1 = calculateScatteringVector(self, hkl);

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

  settings[0] = stt;
  settings[1] = om;
  settings[2] = -1.*(180.+chi) + 360.;
  settings[3] = phi+180.;
  if (iRetry == 1) {
    rotatePsi(om, chi, phi, myPsi, &ompsi, &chipsi, &phipsi);
    settings[1] = ompsi;
    settings[2] = circlify(chipsi);
    settings[3] = circlify(phipsi);
    return 1;
  } else {
    if (hklInRange(self, settings, mask) == 1) {
      return 1;
    } else {
      if (tryOmegaTweak(self, z1, &stt, &om, &chi, &phi) == 1) {
        settings[0] = stt;
        settings[1] = om;
        settings[2] = chi;
        settings[3] = phi;
        return 1;
      } else {
        return findAllowedBisecting(self->lambda, z1, settings, hklInRangeOrion,
                                    self);
      }
    }
  }
  return 0;
}

/*-------------------------------------------------------------------*/
static int settingsToBiList(struct __SingleDiff *self, double *settings)
{

  setNewMotorTarget(self->motList, (char *) SXGetMotorName(TwoTheta),
                    (float) settings[0]);
  setNewMotorTarget(self->motList, (char *) SXGetMotorName(Omega),
                    (float) settings[1]);
  setNewMotorTarget(self->motList, (char *) SXGetMotorName(Chi),
                    (float) settings[2]);
  setNewMotorTarget(self->motList, (char *) SXGetMotorName(Phi),
                    (float) settings[3]);
  return 1;
}

/*--------------------------------------------------------------------*/
static int hklFromBiAngles(struct __SingleDiff *self, double *hkl)
{
  pIDrivable pDriv;
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, chi, phi;
  int i;

  pDriv = makeMotListInterface();
  pDriv->GetValue(&self->motList, pServ->dummyCon);

  UBinv = mat_inv(self->UB);
  if (UBinv == NULL) {
    return 0;
  }

  stt = getListMotorPosition(self->motList,
                             (char *) SXGetMotorName(TwoTheta));
  om = getListMotorPosition(self->motList, (char *) SXGetMotorName(Omega));
  chi = getListMotorPosition(self->motList, (char *) SXGetMotorName(Chi));
  phi = getListMotorPosition(self->motList, (char *) SXGetMotorName(Phi));
  z1FromAngles(self->lambda, stt, om, chi, phi, z1);
  z1m = vectorToMatrix(z1);
  rez = mat_mul(UBinv, z1m);
  for (i = 0; i < 3; i++) {
    hkl[i] = rez[i][0];
  }

  mat_free(UBinv);
  mat_free(z1m);
  mat_free(rez);
  return 1;
}

/*--------------------------------------------------------------------*/
static int hklFromBiAnglesGiven(struct __SingleDiff *self,
                                double *settings, double *hkl)
{
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, chi, phi;
  int i;

  UBinv = mat_inv(self->UB);
  if (UBinv == NULL) {
    return 0;
  }

  stt = settings[0];
  om = settings[1];
  chi = settings[2];
  phi = settings[3];

  z1FromAngles(self->lambda, stt, om, chi, phi, z1);
  z1m = vectorToMatrix(z1);
  rez = mat_mul(UBinv, z1m);
  for (i = 0; i < 3; i++) {
    hkl[i] = rez[i][0];
  }

  mat_free(UBinv);
  mat_free(z1m);
  mat_free(rez);
  return 1;
}

/*------------------------------------------------------------------*/
static int getBiReflection(char *id, reflection * r)
{
  pSICSOBJ refList;
  double hkl[3], angles[4];

  refList = SXGetReflectionList();
  if (!GetRefIndexID(refList, id, hkl)) {
    return 0;
  } else {
    r->h = hkl[0];
    r->k = hkl[1];
    r->l = hkl[2];
    GetRefAnglesID(refList, id, angles);
    r->s2t = angles[0];
    r->om = angles[1];
    r->chi = angles[2];
    r->phi = angles[3];
  }
  return 1;
}

/*-------------------------------------------------------------------*/
MATRIX calcBiUBFromTwo(pSingleDiff self,
                       char *refid1, char *refid2, int *err)
{
  MATRIX newUB;
  reflection r1, r2;
  lattice direct;

  direct.a = self->cell[0];
  direct.b = self->cell[1];
  direct.c = self->cell[2];
  direct.alpha = self->cell[3];
  direct.beta = self->cell[4];
  direct.gamma = self->cell[5];

  if (!getBiReflection(refid1, &r1)) {
    *err = REFERR;
    return NULL;
  }
  if (!getBiReflection(refid2, &r2)) {
    *err = REFERR;
    return NULL;
  }

  newUB = calcUBFromCellAndReflections(direct, r1, r2, err);

  return newUB;
}

/*-------------------------------------------------------------------*/
MATRIX calcBiUBFromThree(pSingleDiff self,
                         char *refid1, char *refid2, char *refid3,
                         int *err)
{
  MATRIX newUB;
  reflection r1, r2, r3;

  if (!getBiReflection(refid1, &r1)) {
    *err = REFERR;
    return NULL;
  }
  if (!getBiReflection(refid2, &r2)) {
    *err = REFERR;
    return NULL;
  }
  if (!getBiReflection(refid3, &r3)) {
    *err = REFERR;
    return NULL;
  }

  newUB = calcUBFromThreeReflections(r1, r2, r3, self->lambda, err);
  return newUB;
}

/*--------------------------------------------------------------------*/
static int calcBiZ1(pSingleDiff self, char *refid, double z1[3])
{
  reflection r1;

  if (!getBiReflection(refid, &r1)) {
    return 0;
  }
  z1FromAngles(self->lambda, r1.s2t, r1.om, r1.chi, r1.phi, z1);
  return 1;
}

/*--------------------------------------------------------------------*/
void initializeSingleBisecting(pSingleDiff diff)
{

  if (diff->motList >= 0) {
    LLDdelete(diff->motList);
  }
  diff->motList = LLDcreate(sizeof(MotControl));
  addMotorToList(diff->motList, (char *) SXGetMotorName(TwoTheta), .0);
  addMotorToList(diff->motList, (char *) SXGetMotorName(Omega), .0);
  addMotorToList(diff->motList, (char *) SXGetMotorName(Chi), .0);
  addMotorToList(diff->motList, (char *) SXGetMotorName(Phi), .0);

  diff->calculateSettings = calculateBiSettings;
  diff->settingsToList = settingsToBiList;
  diff->hklFromAngles = hklFromBiAngles;
  diff->hklFromAnglesGiven = hklFromBiAnglesGiven;
  diff->calcUBFromTwo = calcBiUBFromTwo;
  diff->calcUBFromThree = calcBiUBFromThree;
  diff->calcZ1 = calcBiZ1;
}
/*--------------------------------------------------------------------*/
void initializeSingleBisectingOrion(pSingleDiff diff)
{
  initializeSingleBisecting(diff);
  diff->calculateSettings = calculateBiSettingsOrion;
}