/**
 * This is an implementation of the polymorphic single crystal calculation module 
 * for the triple axis mode. In this mode the tilt angles of a standard sample 
 * cradle are used to orient the sample. The math behind this has been decribed by 
 * Mark Lumsden in his paper.
 * 
 * copyright: see file COPYRIGHT
 * 
 * Mark Koennecke, November 2008
 */
#include <stdlib.h>
#include <assert.h>
#include <sics.h>
#include "singlediff.h"
#include "motor.h"
#include "singlex.h"
#include "motorlist.h"
#include "lld.h"
#include "tasublib.h"
#include "fourlib.h"

/*---------------------------------------------------------------------*/
static int calculateTASSettings(pSingleDiff self,
                                double *hkl, double *settings)
{
  tasQEPosition qe;
  tasAngles angles;
  double e, *normal;
  int status;
  MATRIX mn;
  char error[132];
  float fHard;

  e = 9.045 / self->lambda;
  e = e * e;
  qe.ki = energyToK(e);
  qe.kf = energyToK(e);
  qe.qh = hkl[0];
  qe.qk = hkl[1];
  qe.ql = hkl[2];

  normal = SXGetPlanenormal();
  if (normal == NULL) {
    return 0;
  }
  mn = vectorToMatrix(normal);
  if (mn == NULL) {
    return 0;
  }

  status = calcTasQAngles(self->UB, mn, 1,0, qe, &angles);
  if (status < 0) {
    status = 0;
  } else {
    status = 1;
  }
  settings[0] = angles.sample_two_theta;
  settings[1] = angles.a3;
  settings[2] = angles.sgu;
  settings[3] = angles.sgl;
  if (!MotorCheckBoundary(SXGetMotor(Omega), (float) settings[0], &fHard,
                          error, 132)) {
    status = 0;
  }
  if (!MotorCheckBoundary
      (SXGetMotor(TwoTheta), (float) settings[1], &fHard, error, 132)) {
    status = 0;
  }
  if (!MotorCheckBoundary(SXGetMotor(Sgu), (float) settings[2], &fHard,
                          error, 132)) {
    status = 0;
  }
  if (!MotorCheckBoundary(SXGetMotor(Sgl), (float) settings[3], &fHard,
                          error, 132)) {
    status = 0;
  }
  free(normal);
  mat_free(mn);
  return status;
}

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

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

/*--------------------------------------------------------------------*/
static int hklFromTasAngles(struct __SingleDiff *self, double *hkl)
{
  pIDrivable pDriv;
  double e, stt, om, sgu, sgl;
  tasQEPosition qe;
  tasAngles angles;
  int status = 1;

  e = 9.045 / self->lambda;
  e = e * e;
  qe.ki = energyToK(e);
  qe.kf = energyToK(e);

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

  stt = getListMotorPosition(self->motList,
                             (char *) SXGetMotorName(TwoTheta));
  om = getListMotorPosition(self->motList, (char *) SXGetMotorName(Omega));
  sgu = getListMotorPosition(self->motList, (char *) SXGetMotorName(Sgu));
  sgl = getListMotorPosition(self->motList, (char *) SXGetMotorName(Sgl));

  angles.a3 = om;
  angles.sample_two_theta = stt;
  angles.sgu = sgu;
  angles.sgl = sgl;

  status = calcTasQH(self->UB, angles, &qe);
  if (status < 0) {
    status = 0;
  }
  hkl[0] = qe.qh;
  hkl[1] = qe.qk;
  hkl[2] = qe.ql;

  return status;
}

/*-------------------------------------------------------------------------*/
static int hklFromTasAnglesGiven(struct __SingleDiff *self,
                                 double *settings, double *hkl)
{
  double e;
  tasQEPosition qe;
  tasAngles angles;
  int status = 1;

  e = 9.045 / self->lambda;
  e = e * e;
  qe.ki = energyToK(e);
  qe.kf = energyToK(e);

  angles.a3 = settings[0];
  angles.sample_two_theta = settings[1];
  angles.sgu = settings[2];
  angles.sgl = settings[3];

  status = calcTasQH(self->UB, angles, &qe);
  if (status < 0) {
    status = 0;
  }
  hkl[0] = qe.qh;
  hkl[1] = qe.qk;
  hkl[2] = qe.ql;

  return status;
}

/*------------------------------------------------------------------*/
static int getTasReflection(char *id, tasReflection * r)
{
  pSICSOBJ refList;
  double hkl[3], angles[4];
  double lambda, e;

  lambda = SXGetLambda();
  e = 9.045 / lambda;
  e = e * e;

  refList = SXGetReflectionList();
  if (!GetRefIndexID(refList, id, hkl)) {
    return 0;
  } else {
    r->qe.qh = hkl[0];
    r->qe.qk = hkl[1];
    r->qe.ql = hkl[2];
    r->qe.ki = energyToK(e);
    r->qe.kf = energyToK(e);
    GetRefAnglesID(refList, id, angles);
    r->angles.a3 = angles[0];
    r->angles.sample_two_theta = angles[1];
    r->angles.sgu = angles[2];
    r->angles.sgl = angles[3];
  }
  return 1;
}

/*--------------------------------------------------------------------------*/
MATRIX calcTasUBFromTwo(pSingleDiff self,
                        char *refid1, char *refid2, int *err)
{
  MATRIX ub = NULL, pl = NULL;
  tasReflection r1, r2;
  lattice direct;
  double p[3];

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

  ub = calcTasUBFromTwoReflections(direct, r1, r2, err);
  pl = calcPlaneNormal(r1, r2);
  if (pl != NULL) {
    matrixToVector(pl, p);
    SXSetPlanenormal(p);
    mat_free(pl);
  }
  return ub;
}

/*--------------------------------------------------------------------------*/
MATRIX calcTasUBFromThree(pSingleDiff self,
                          char *refid1, char *refid2, char *refid3,
                          int *err)
{
  /* 
   * not implemented yet
   * There is also a problem: if I have three reflections, which 
   * defines the plane normal? This also does not make much sense anyway:
   * we usually will not have a reflection out of plane: thus the three 
   * will be coplanar and the results may not be useful at all!
   */
  return NULL;
}

/*--------------------------------------------------------------------*/
static int calcTasZ1(pSingleDiff self, char *refid, double z1[3])
{
  tasReflection r1;
  MATRIX u = NULL;

  if (!getTasReflection(refid, &r1)) {
    return 0;
  }
  u = calcTasUVectorFromAngles(r1);
  if (u == NULL) {
    return 0;
  } else {
    matrixToVector(u, z1);
    mat_free(u);
  }
  return 1;
}

/*--------------------------------------------------------------------*/
void initializeSingleTas(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(Sgu), .0);
  addMotorToList(diff->motList, (char *) SXGetMotorName(Sgl), .0);

  diff->calculateSettings = calculateTASSettings;
  diff->settingsToList = settingsToTasList;
  diff->hklFromAngles = hklFromTasAngles;
  diff->hklFromAnglesGiven = hklFromTasAnglesGiven;
  diff->calcUBFromTwo = calcTasUBFromTwo;
  diff->calcUBFromThree = calcTasUBFromThree;
  diff->calcZ1 = calcTasZ1;
}