/*
 * singlebinb.c
 *
 * This is an implementation of the polymorphic single crystal calculation
 * system as defined if singlediff.h for a diffractometer which not only
 * uses an eulerian cradle for positioning the crystal but also detector
 * tilt. The aim is once more to fix the operation of a  diffractometer
 * which has been crippled in its movements by overly bulky cryostats.
 *
 * The tricky bit is in finding a solution to get the crystal into a diffraction
 * condition. The aim is to prefer the bisecting position above all. To this purpose
 * the crystal is rotated through psi and the position with the least difference in
 * chi and phi is noted. Where difference is the difference towards the limits
 * if the calculated chi and phi is outside of the limits. Then the closest chi and
 * phi is calculated. The scattering vector is rotated by this chi and phi and a
 * normal beam calculation attempted for the rotated scattering vector.
 *
 * copyright: see file COPYRIGHT
 *
 *  Created on: Feb 13, 2012
 *      Author: Mark Koennecke
 */
#include <stdlib.h>
#include <assert.h>
#include <sics.h>
#include "singlediff.h"
#include "ubfour.h"
#include "fourlib.h"
#include "motor.h"
#include "singlex.h"
#include "motorlist.h"
#include "lld.h"

#define ABS(x) (x < 0 ? -(x) : (x))

/*
 * from singlenb.c
 */
int checkNormalBeam(double om, double *gamma, double nu,
		float fSet[4], pSingleDiff self);
/*-----------------------------------------------------------------------*/
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 double testClosestLimit(pMotor mot, float pos, float *closestpos)
{
	double diff = .0;
	float lowlim, upperlim, zero, sign;

	*closestpos = pos;
	/*
	 * check for soft limit violation
	 */
	MotorGetPar(mot,"softlowerlim",&lowlim);
	MotorGetPar(mot,"softupperlim",&upperlim);

	if(pos > lowlim && pos < upperlim){
		diff = .0;
	} else {
		diff = ABS(lowlim - pos);
		*closestpos = lowlim;
		if(ABS(upperlim -pos) < diff){
			diff = ABS(upperlim - pos);
			*closestpos = upperlim;
		}
		return diff;
	}

	/**
	 * apply zero point and sign
	 */
	MotorGetPar(mot,"softzero",&zero);
	MotorGetPar(mot,"sign",&sign);
	zero = - zero;
	pos = pos - zero;
	pos *= sign;

	/**
	 * check hardlimits
	 */
	MotorGetPar(mot,"hardlowerlim",&lowlim);
	MotorGetPar(mot,"hardupperlim",&upperlim);

	if(pos > lowlim && pos < upperlim){
		diff = .0;
	} else {
		diff = ABS(lowlim - pos);
		*closestpos = lowlim*sign+zero;
		if(ABS(upperlim -pos) < diff){
			diff = ABS(upperlim - pos);
			*closestpos = upperlim*sign +zero;
		}
	}

	return diff;
}
/*---------------------------------------------------------------------*/
static double calculateDiff(pSingleDiff self, float fSet[4],
		float *closestchi, float *closestphi)
{
	double diff = .0, dTheta;
	float fHard;
	pMotor pOmega, pChi, pPhi;
	char pError[132];

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

	/* check two theta  */
	dTheta = fSet[0];
	if(checkTheta(self, &dTheta) != 1) {
		return 11000;
	}

	/* check omega */
	if(MotorCheckBoundary(pOmega, fSet[1], &fHard, pError, 131) != 1) {
		return 11000.;
	}

	diff += testClosestLimit(pChi, fSet[2],closestchi);
	diff += testClosestLimit(pPhi, fSet[3],closestphi);

	return diff;
}
/*---------------------------------------------------------------------*/
static double searchPSI(pSingleDiff self, float fSet[4], float bestSet[5])
{
	float fTest[4], closestchi, closestphi;
	double om, chi, phi, psi, ompsi, chipsi, phipsi, diff;
	double bestdiff = 200000;
	int i;

	/*
	 * do not scan psi if there is already bisecting happiness
	 */
	for(i = 0; i < 4; i++){
		bestSet[i] = fSet[i];
	}
	bestSet[4] = .0;
	if(calculateDiff(self,fSet,&closestchi, &closestphi) < .001){
		return 0;
	}

	om = fSet[1];
	chi = fSet[2];
	phi = fSet[3];
	for (psi = .0; psi < 360.; psi += .5) {
		rotatePsi(om, chi, phi, psi, &ompsi, &chipsi, &phipsi);
		fTest[0] = fSet[0];
		fTest[1] = ompsi;
		fTest[2] = circlify(chipsi);
		fTest[3] = circlify(phipsi);
		diff = calculateDiff(self, fTest, &closestchi, &closestphi);
		if(diff < bestdiff){
			bestSet[0] = fSet[0];
			bestSet[1] = ompsi;
			bestSet[2] = closestchi;
			bestSet[3] = closestphi;
			bestSet[4] = psi;
			bestdiff = diff;
		}
		if(bestdiff < .001){
			return diff;
		}
	}
	return diff;
}
/*---------------------------------------------------------------------*/
static int calculateBINBSettings(pSingleDiff self,
                               double *hkl, double *settings)
{
  MATRIX z1, chim, phim, rotmat, z1_2;
  double stt, om, chi, phi, psi, ompsi, chipsi, phipsi;
  int i, test, mask[4];
  double diff, gamma, nu;
  float fSet[4], bestSet[5];


  z1 = calculateScatteringVector(self, hkl);

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

  fSet[0] = stt;
  fSet[1] = om;
  fSet[2] = chi;
  fSet[3] = phi;
  diff = searchPSI(self, fSet, bestSet);
  for(i = 0; i < 4; i++){
	  settings[i] = bestSet[i];
  }
  settings[4] = .0;
  if(diff > 9000){
	  /* burning in stt or om */
	  return 0;
  } else if(diff < .01){
	  /* success in bisecting */
	  return 1;
  }

  /*
   * continue to apply normal beam too
   */

  /*
   * rotate z1 by chi and phi to z1_2
   */
  chim = mat_creat(3,3,ZERO_MATRIX);
  chimat(chim,bestSet[2]);
  phim = mat_creat(3,3,ZERO_MATRIX);
  phimat(phim,bestSet[3]);
  rotmat = mat_mul(chim,phim);
  z1_2 = mat_mul(rotmat,z1);
  mat_free(chim);
  mat_free(phim);
  mat_free(rotmat);

  /*
   * Calculate NB angles for z1-2
   */
  test = z1mToNormalBeam(self->lambda, z1_2, &gamma, &om, &nu);
  mat_free(z1_2);
  if(test != 1){
	  return 0;
  }
  settings[0] = gamma;
  settings[1] = om;
  settings[4] = nu;
  if(!checkNormalBeam(om,&gamma, nu, fSet,self)){
	  return 0;
  } else {
	  return 1;
  }
}
/*-------------------------------------------------------------------*/
static int settingsToBINBList(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]);
  setNewMotorTarget(self->motList, (char *) SXGetMotorName(Nu),
                    (float) settings[4]);
  return 1;
}
/*------------------------------------------------------------------------*/
static int hklFromBINBAngles(struct __SingleDiff *self, double *hkl)
{
  pIDrivable pDriv;
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, chi, phi, 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));
  chi = getListMotorPosition(self->motList, (char *) SXGetMotorName(Chi));
  phi = getListMotorPosition(self->motList, (char *) SXGetMotorName(Phi));
  nu = getListMotorPosition(self->motList, (char *) SXGetMotorName(Nu));
  z1FromAllAngles(self->lambda, om, stt, nu, 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 hklFromBINBAnglesGiven(struct __SingleDiff *self,
                                double *settings, double *hkl)
{
  MATRIX UBinv, z1m, rez;
  double z1[3], stt, om, chi, phi, nu;
  int i;

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

  stt = settings[0];
  om = settings[1];
  chi = settings[2];
  phi = settings[3];
  nu = settings[4];
  z1FromAllAngles(self->lambda, om, stt, nu, 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;
}
/*------------------------------------------------------------------
 * In order to make my life easier, I transform the full reflection into
 * a Bisecting one by recalculating angles. This allows me to use the
 * Bisecting UB matrix calculation routines.
 -------------------------------------------------------------------*/
static int getBINBReflection(char *id, reflection * r)
{
  pSICSOBJ refList;
  double hkl[3], angles[5], z1[3], stt, om, chi, phi, lambda;

  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];
    r->nu = angles[4];
  }
  lambda = SXGetLambda();
  z1FromAllAngles(lambda, r->om, r->s2t, r->nu, r->chi, r->phi, z1);
  z1ToBisecting(lambda, z1, &stt, &om, &chi, &phi);
  r->s2t = stt;
  r->om = om;
  r->chi = chi;
  r->phi = phi;
  r->nu = .0;
  return 1;
}
/*-------------------------------------------------------------------*/
MATRIX calcBINBUBFromTwo(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 (!getBINBReflection(refid1, &r1)) {
    *err = REFERR;
    return NULL;
  }
  if (!getBINBReflection(refid2, &r2)) {
    *err = REFERR;
    return NULL;
  }

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

  return newUB;
}

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

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

  newUB = calcUBFromThreeReflections(r1, r2, r3, self->lambda, err);
  return newUB;
}
/*--------------------------------------------------------------------*/
static int calcBINBZ1(pSingleDiff self, char *refid, double z1[3])
{
  reflection r1;

  if (!getBINBReflection(refid, &r1)) {
    return 0;
  }
  z1FromAngles(self->lambda, r1.s2t, r1.om, r1.chi, r1.phi, z1);
  return 1;
}
/*--------------------------------------------------------------------*/
void initializeSingleBINB(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);
  addMotorToList(diff->motList, (char *) SXGetMotorName(Nu), .0);

  diff->calculateSettings = calculateBINBSettings;
  diff->settingsToList = settingsToBINBList;
  diff->hklFromAngles = hklFromBINBAngles;
  diff->hklFromAnglesGiven = hklFromBINBAnglesGiven;
  diff->calcUBFromTwo = calcBINBUBFromTwo;
  diff->calcUBFromThree = calcBINBUBFromThree;
  diff->calcZ1 = calcBINBZ1;
}