/**
 * This is a library of functions and data structures for performing 
 * triple axis spectrometer angle calculations using the UB-matrix
 * formalism as described by Mark Lumsden.
 *
 * copyright: see file COPYRIGHT
 *
 * Mark Koennecke, April 2005
 */
#include <math.h>
#include "trigd.h"
#include "vector.h"
#include "tasublib.h"

#define ABS(x) (x < 0 ? -(x) : (x)) 
#define PI 3.141592653589793
#define ECONST 2.072
#define DEGREE_RAD      (PI/180.0)      /* Radians per degree */
/*============== monochromator/analyzer stuff =========================*/
double energyToK(double energy){
  double K;
  
  K = sqrt(energy/ECONST);
  return K;
}
/*---------------------------------------------------------------------*/
double KtoEnergy(double k){
  double energy;

  energy = ECONST*k*k;
  return energy;
}
/*--------------------------------------------------------------------*/
int maCalcAngles(maCrystal data, pmaAngles angles, double k){
  double fd;

  /* fd = k/(2.*data.dd); */
  fd = PI/(data.dd*k);
  if(fd > 1.0) {
    return ENERGYTOBIG;
  }
  angles->theta = Asind(fd)*data.ss;
  angles->two_theta = 2.*angles->theta;
  angles->horizontal_curvature = data.HB1 + data.HB2/Sind(angles->theta);
  angles->vertical_curvature   = data.VB1 + data.VB2/Sind(angles->theta);
  return 1;
}
/*--------------------------------------------------------------------*/
int maCalcK(maCrystal data, maAngles angles, double *k){
  
  *k = ABS(data.dd * Sind(angles.two_theta/2));
  *k = PI / *k;
  if(ABS(angles.two_theta/2. - angles.theta) > .1) {
    return BADSYNC;
  }
  return 1;
}
/*==================== reciprocal space ==============================*/
static MATRIX tasReflectionToHC(tasReflection r, MATRIX B){
  MATRIX h = NULL, hc = NULL;

  h = makeVector();
  if(h == NULL){
    return NULL;
  }
  vectorSet(h,0,r.h);
  vectorSet(h,1,r.k);
  vectorSet(h,2,r.l);

  hc = mat_mul(B,h);
  killVector(h);
  return hc;
}
/*------------------------------------------------------------------
 a quadrant dependent tangens
 ------------------------------------------------------------------*/
static double rtan(double y, double x){
  double val;

  if( (x == 0.) && (y == 0.) ) {
    return .0;
  }       
  if( x == 0.) {
      if(y < 0.){
         return -PI/2.;
      } else {
	return PI/2.;
     }
  }
  if(ABS(y) < ABS(x)) {
    val = atan(ABS(y/x));
    if(x < 0.) {
      val =  PI - val;
    }
    if(y < 0.){
        val = -val;
    } 
    return val;
  } else {
      val  = PI/2. - atan(ABS(x/y));
      if(x < 0.) {
      val = PI - val;
      }
      if( y < 0.) {
      val = - val;
      }
  }
  return val;
}
/*---------------------------------------------------------------*/
static double calcTheta(double ki, double kf, double two_theta){
  /**
   *               |ki| - |kf|cos(two_theta)
   * tan(theta) = --------------------------
   *                |kf|sin(two_theta)
   */
  return rtan(ABS(ki) - ABS(kf)*Cosd(two_theta),
	      ABS(kf)*Sind(two_theta))/DEGREE_RAD;
}
/*--------------------------------------------------------------------*/
static MATRIX uFromAngles(double om, double sgu, double sgl){
  MATRIX u;

  u = makeVector();
  if(u == NULL){
    return NULL;
  }
  vectorSet(u,0,-Cosd(sgl)*Cosd(om));
  vectorSet(u,1,Cosd(sgu)*Sind(om) - Sind(sgu)*Sind(sgl)*Cosd(om));
  vectorSet(u,2,-Sind(sgu)*Sind(om) - Cosd(sgu)*Sind(sgl)*Cosd(om));
  
  return u;	    
}
/*---------------------------------------------------------------*/
static MATRIX calcTasUVectorFromAngles(tasReflection r){
  double theta, om;

  theta = calcTheta(r.ki,r.kf,r.two_theta);
  om = r.a3 - theta;
  return uFromAngles(om,r.sgu, r.sgl);
}
/*-------------------------------------------------------------------*/
MATRIX calcPlaneNormal(tasReflection r1, tasReflection r2){
  MATRIX u1 = NULL, u2 = NULL, planeNormal = NULL;

  u1 = calcTasUVectorFromAngles(r1);
  u2 = calcTasUVectorFromAngles(r2);
  if(u1 != NULL && u2 != NULL){
    return vectorCrossProduct(u1,u2);
  } else {
    return NULL;
  }
}
/*--------------------------------------------------------------------*/
MATRIX calcTasUBFromTwoReflections(lattice cell, tasReflection r1,
				   tasReflection r2, int *errorCode){
  MATRIX B, HT, UT, U, UB, HTT  ;
  MATRIX u1, u2, h1, h2, planeNormal;
  double ud[3];
  int status;

  *errorCode = 1;

  /*
    calculate the B matrix and the HT matrix
  */
  B = mat_creat(3,3,ZERO_MATRIX);
  status = calculateBMatrix(cell,B);
  if(status < 0){
    *errorCode = status;
    return NULL;
  }
  h1 = tasReflectionToHC(r1,B);
  h2 = tasReflectionToHC(r2,B);
  if(h1 == NULL || h2 == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  HT = matFromTwoVectors(h1,h2);
  if(HT == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }

  /*
    calculate U vectors and UT matrix
  */
  u1 = calcTasUVectorFromAngles(r1);
  u2 = calcTasUVectorFromAngles(r2);
  if(u1 == NULL || u2 == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  UT = matFromTwoVectors(u1,u2);
  if(UT == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }

  /*
    debugging output
  printf("B-matrix\n");
  mat_dump(B);
  printf("HT-matrix\n");
  mat_dump(HT);
  printf("UT-matrix\n");
  mat_dump(UT);
  */

  /*
    UT = U * HT
  */
  HTT = mat_tran(HT);
  if(HTT == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  U = mat_mul(UT,HTT);
  if(U == NULL){
    *errorCode = UBNOMEMORY; 
    return NULL;
  }
  UB = mat_mul(U,B);
  if(UB == NULL){
    *errorCode = UBNOMEMORY;
  }

  /*
    clean up
  */
  killVector(h1);
  killVector(h2);
  mat_free(HT);
  mat_free(HTT);

  killVector(u1);
  killVector(u2);
  mat_free(UT);  

  mat_free(U);
  mat_free(B);

  return UB;
}
/*-----------------------------------------------------------------------------*/
static MATRIX buildTVMatrix(MATRIX U1V, MATRIX U2V){
  MATRIX T, T3V;
  int i;

  T3V = vectorCrossProduct(U1V,U2V);
  if(T3V == NULL){
    return NULL;
  }
  T = mat_creat(3,3,ZERO_MATRIX);
  if(T == NULL){
    killVector(T3V);
    return NULL;
  }
  for(i = 0; i < 3; i++){
    T[i][0] = U1V[i][0];
    T[i][1] = U2V[i][0];
    T[i][2] = T3V[i][0];
  }
  killVector(T3V);
  return T;
}
/*-----------------------------------------------------------------------------*/
static MATRIX tasReflectionToQC(ptasReflection r, MATRIX UB){
  MATRIX Q, QC;

  Q = makeVector();
  if(Q == NULL){
    return NULL;
  }
  vectorSet(Q,0,r->h);
  vectorSet(Q,1,r->k);
  vectorSet(Q,2,r->l);
  QC = mat_mul(UB,Q);
  killVector(Q);
  return QC;
}
/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
				     ptasReflection r){
  MATRIX U3V, U1V, U2V, TV, TVINV;

  U3V = planeNormal;
  
  U1V = tasReflectionToQC(r,UB);
  if(U1V == NULL){
    return NULL;
  }
  normalizeVector(U1V);

  U2V = vectorCrossProduct(U3V,U1V);
  if(U2V == NULL){
    killVector(U1V);
    return NULL;
  }

  TV = buildTVMatrix(U1V,U2V);
  if(TV == NULL){
    killVector(U1V);
    killVector(U2V);
    return NULL;
  }
  TVINV = mat_inv(TV);
  if(TVINV == NULL){
    killVector(U1V);
    killVector(U2V);
    mat_free(TV);
    return NULL;
  }
  
  killVector(U1V);
  killVector(U2V);
  mat_free(TVINV);

  return TVINV;
}
/*-------------------------------------------------------------------------------*/
int calcTasQAngles(MATRIX UB, MATRIX planeNormal, int ss,  ptasReflection r){
  MATRIX R, QC;
  double om, q, theta, cos2t, tmp;

  R = buildRMatrix(UB, planeNormal, r);
  if(R == NULL){
    return UBNOMEMORY;
  }

  om = Acosd(R[0][0]/sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0]));
  r->sgl = Acosd(sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0]));
  r->sgu = Asind(R[2][1]/sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0])); 

  /*
  r->sgl = Asind(-R[2][0]); 
  tmp = sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0]);
  if(tmp > .001){
    r->sgu = Acosd(R[2][2]/tmp);
  } else {
    return BADRMATRIX;
  }
  */

  
  QC = tasReflectionToQC(r,UB);
  if(QC == NULL){
    return UBNOMEMORY;
  }

  q = vectorLength(QC);
  q = 2.*PI*vectorLength(QC); 
  cos2t = (r->ki*r->ki + r->kf*r->kf - q*q)/(2. * ABS(r->ki) * ABS(r->kf));
  if(cos2t > 1.){
    return TRIANGLENOTCLOSED;
  }
  r->two_theta = ss*Acosd(cos2t);
  
  theta = calcTheta(r->ki, r->kf,r->two_theta);
  
  r->a3 = om + theta;

  killVector(QC);

  return 1;
}
/*------------------------------------------------------------------------*/
int calcTasQH(MATRIX UB, ptasReflection r){
  MATRIX UBINV = NULL, QV = NULL, Q = NULL;
  double q;
  int i;

  UBINV = mat_inv(UB);
  QV = calcTasUVectorFromAngles(*r);
  if(UBINV == NULL || QV == NULL){
    return UBNOMEMORY;
  }
  /*
    normalize the QV vector to be the length of the Q vector
    Thereby take into account the physicists magic fudge
    2PI factor
  */
  q = sqrt(r->ki*r->ki + r->kf*r->kf - 2.*r->ki*r->kf*Cosd(r->two_theta));
  q /= 2. * PI;
  for(i = 0; i < 3; i++){
    QV[i][0] *= q;
  }

  /*
  mat_dump(UB);
  mat_dump(UBINV);
  mat_dump(QV);
  */
  Q = mat_mul(UBINV,QV);
  if(Q == NULL){
    mat_free(UBINV);
    killVector(QV);
    return UBNOMEMORY;
  }
  r->h = Q[0][0];
  r->k = Q[1][0];
  r->l = Q[2][0];

  killVector(QV);
  killVector(Q);
  mat_free(UBINV);

  return 1;
}
/*---------------------------------------------------------------------*/
int calcAllTasAngles(ptasMachine machine, tasQEPosition qe,
		      ptasAngles angles){
  int status;
  tasReflection r;

  status = maCalcAngles(machine->monochromator,&angles->monochromator,
			qe.ki);
  if(status != 1){
    return status;
  }

  r.h = qe.qh;
  r.k = qe.qk;
  r.l = qe.ql;
  r.ki = qe.ki;
  r.kf = qe.kf;
  status = calcTasQAngles(machine->UB, machine->planeNormal,
			  machine->ss_sample, &r);
  if(status != 1){
    return status;
  }
  angles->a3 = r.a3;
  angles->sample_two_theta = r.two_theta;
  angles->sgu = r.sgu;
  angles->sgl = r.sgl;

  status = maCalcAngles(machine->analyzer,&angles->analyzer,
			qe.kf);
  if(status != 1){
    return status;
  }

  return 1;
}
/*----------------------------------------------------------------------*/
int calcTasQEPosition(ptasMachine machine, tasAngles angles,
		      ptasQEPosition qe){
  int status, retVal = 1;
  tasReflection r;
  double k;

  status = maCalcK(machine->monochromator,angles.monochromator,&k);
  if(status != 1){
    if(status != BADSYNC){
      retVal = BADSYNC;
    } else {
      return status;
    }
  }
  qe->ki = k;

  status = maCalcK(machine->analyzer,angles.analyzer,&k);
  if(status != 1){
    if(status != BADSYNC){
      retVal = BADSYNC;
    } else {
      return status;
    }
  }
  qe->kf = k;


  r.sgu = angles.sgu;
  r.sgl = angles.sgl;
  r.a3 = angles.a3;
  r.two_theta = angles.sample_two_theta;
  r.ki = qe->ki;
  r.kf = qe->kf;
  status = calcTasQH(machine->UB,&r);
  if(status != 1){
    return status;
  }
  qe->qh = r.h;
  qe->qk = r.k;
  qe->ql = r.l;

  return retVal;
}