/**
 * 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, qe, &angles);
	if(status < 0) {
		status = 0;
	} else {
		status = 1;
	}
	settings[0] = angles.a3;
	settings[1] = angles.sample_two_theta;
	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;
}