/**
 * This is an implementation  of the polymorphic single crystal calculation
 * system defined in singlediff.h for a diffractometer in normal beam geometry. 
 * This means the detector tilts out of the instrument plane upwards or 
 * downwards.
 * 
 * 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"

/*-----------------------------------------------------------------------*/
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;
}

/*-----------------------------------------------------------------------*/
int checkNormalBeam(double om, double *gamma, double nu,
                           double fSet[4], pSingleDiff self)
{
  int iTest;
  char pError[132];
  float fHard;
  pMotor pMot;

  fSet[0] = (float) *gamma;
  fSet[1] = (float) om;
  fSet[2] = (float) nu;

  /* check omega, gamma and nu  */
  pMot = SXGetMotor(Omega);
  if (pMot == NULL) {
    return 0;
  }
  iTest = MotorCheckBoundary(pMot, (float) om, &fHard, pError, 131);
  iTest += checkTheta(self, gamma);
  pMot = SXGetMotor(Nu);
  if (pMot == NULL) {
    return 0;
  }
  iTest += MotorCheckBoundary(pMot, (float) nu, &fHard, pError, 131);
  if (iTest == 3) {
    return 1;
  }
  return 0;
}

/*-------------------------------------------------------------------*/
static int calculateNBSettings(pSingleDiff self,
                               double *hkl, double *settings)
{
  MATRIX z1;
  double gamma, om, nu;
  int status;

  z1 = calculateScatteringVector(self, hkl);

  status = z1mToNormalBeam(self->lambda, z1, &gamma, &om, &nu);
  mat_free(z1);
  if (status != 1) {
    return 0;
  }
    
  if(om > 180.){
	  om -= 360;
  } else if (om < -180.){
	  om += 360.;
  }
  if (checkNormalBeam(om, &gamma, nu, settings, self)) {
    return 1;
  } else {
      return 0;
  }
  return 0;
}

/*-------------------------------------------------------------------*/
static int settingsToNBList(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(Nu),
                    (float) settings[2]);
  return 1;
}

/*--------------------------------------------------------------------*/
static int hklFromNBAngles(struct __SingleDiff *self, double *hkl)
{
  pIDrivable pDriv;
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, nu;
  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));
  nu = getListMotorPosition(self->motList, (char *) SXGetMotorName(Nu));

  z1FromNormalBeam(self->lambda, om, stt, nu, 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 hklFromNBAnglesGiven(struct __SingleDiff *self,
                                double *settings, double *hkl)
{
  pIDrivable pDriv;
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, nu;
  int i;

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

  stt = settings[0];
  om = settings[1];
  nu = settings[2];

  z1FromNormalBeam(self->lambda, om, stt, nu, 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;
}

/*-----------------------------------------------------------------------------
 * For UB matrix calculations I use a trick. I do now know how to do 
 * that properly for normal beam geometry. But I know that the UB for
 * bisecting and normal beam is the same. Thus I calculate the scattering 
 * vector z1 from the normal beam angles, then proceed to calculate 
 * bisecting angles from the scattering vector and use the bisecting 
 * geometry UB matrix calculation routines.
 * -------------------------------------------------------------------------*/
static int getNBReflection(pSingleDiff self, char *id, reflection * r)
{
  pSICSOBJ refList;
  double hkl[3], angles[4], z1[3];
  double omnb, gamma, nu, stt, om, chi, phi;


  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);
    gamma = angles[0];
    omnb = angles[1];
    nu = angles[2];
    z1FromNormalBeam(self->lambda, omnb, gamma, nu, z1);
    if (!z1ToBisecting(self->lambda, z1, &stt, &om, &chi, &phi)) {
      return 0;
    }
    r->s2t = stt;
    r->om = om;
    r->chi = chi;
    r->phi = phi;
  }
  return 1;
}
/*---------------------------------------------------------------------*/
static int getNBNBReflection(pSingleDiff self, char *id, reflection * r)
{
  pSICSOBJ refList;
  double hkl[3], angles[4], z1[3];
  double omnb, gamma, nu, stt, om, chi, phi;


  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);
    gamma = angles[0];
    omnb = angles[1];
    nu = angles[2];
    r->gamma  = gamma;
    r->omnb = omnb;
    r->nu = nu;
  }
  return 1;
}

/*-------------------------------------------------------------------*/
MATRIX calcNBUBFromTwo(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 (!getNBNBReflection(self, refid1, &r1)) {
    *err = REFERR;
    return NULL;
  }
  if (!getNBNBReflection(self, refid2, &r2)) {
    *err = REFERR;
    return NULL;
  }

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

  return newUB;
}

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

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

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

/*--------------------------------------------------------------------*/
static int calcNBZ1(pSingleDiff self, char *refid, double z1[3])
{
  reflection r1;
  pSICSOBJ refList;
  double angles[4];
  double omnb, gamma, nu;

  refList = SXGetReflectionList();
  if (!GetRefAnglesID(refList, refid, angles)) {
    return 0;
  } else {
    gamma = angles[0];
    omnb = angles[1];
    nu = angles[2];
    z1FromNormalBeam(self->lambda, omnb, gamma, nu, z1);
    return 1;
  }
  /*
  if (!getNBReflection(self, refid, &r1)) {
    return 0;
  }
  z1FromAngles(self->lambda, r1.s2t, r1.om, r1.chi, r1.chi, z1);
  return 1;
  */
}

/*--------------------------------------------------------------------*/
void initializeNormalBeam(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(Nu), .0);

  diff->calculateSettings = calculateNBSettings;
  diff->settingsToList = settingsToNBList;
  diff->hklFromAngles = hklFromNBAngles;
  diff->hklFromAnglesGiven = hklFromNBAnglesGiven;
  diff->calcUBFromTwo = calcNBUBFromTwo;
  diff->calcUBFromThree = calcNBFromThree;
  diff->calcZ1 = calcNBZ1;
}