/**
 * This is a library for calculating UB matrices for four circle diffraction.
 * The algorithm and settings definition is from:
 *
 *            Busing & Levy, Acta Cryst. (1967), 22, 457ff 
 *
 * Implemented:
 * - UB from cell cell constants and two reflections.
 * - Brute force index search
 *
 * Mark Koennecke, March 2005
 */
#include <math.h>
#include "ubfour.h"
#include <assert.h>
#include "vector.h"
#include "trigd.h"
#include "fourlib.h"
#include "lld.h"
#define ABS(x) (x < 0 ? -(x) : (x)) 
/*--------------------------------------------------------------------------------------*/
static MATRIX calcUVectorFromAngles(reflection r){
  MATRIX u;
  double om;

  u = makeVector();
  if(u == NULL){
    return NULL;
  }
  /*
   * the tricky bit is set again: Busing and Levy's omega is 0 in bisecting
   * position. This is why we have to correct for two_theta/2 here in order  
   * to arrive at the proper rotation around the omega axis.
   */
  om = r.om - r.s2t/2.;
  vectorSet(u,0,Cosd(om)*Cosd(r.chi)*Cosd(r.phi) - Sind(om)*Sind(r.phi));
  vectorSet(u,1,Cosd(om)*Cosd(r.chi)*Sind(r.phi) + Sind(om)*Cosd(r.phi));
  vectorSet(u,2,Cosd(om)*Sind(r.chi));
  return u;	    
}
/*--------------------------------------------------------------------------------------*/
static MATRIX reflectionToHC(reflection 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;
}
/*---------------------------------------------------------------------------------------*/
MATRIX calcUBFromCellAndReflections(lattice direct, reflection r1, 
				    reflection r2, int *errCode){
  MATRIX B, HT, UT, U, UB, HTT  ;
  MATRIX u1, u2, h1, h2;
  double ud[3];
  int status;
  reflection r;

  *errCode = 1;

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

  /*
    calculate U vectors and UT matrix
  */
  u1 = calcUVectorFromAngles(r1);
  u2 = calcUVectorFromAngles(r2);
  if(u1 == NULL || u2 == NULL){
    *errCode = UBNOMEMORY;
    return NULL;
  }
  UT = matFromTwoVectors(u1,u2);
  if(UT == NULL){
    *errCode = 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){
    *errCode = UBNOMEMORY;
    return NULL;
  }
  U = mat_mul(UT,HTT);
  if(U == NULL){
    *errCode = UBNOMEMORY; 
    return NULL;
  }
  UB = mat_mul(U,B);
  if(UB == NULL){
    *errCode = 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 void storeReflection(reflection r, double two_theta_obs, 
			    double two_theta_calc, int list){
  refIndex ri, test;
  int count = 0, status, pos = 0;

  ri.h = r.h;
  ri.k = r.k;
  ri.l = r.l;
  ri.t2obs = two_theta_obs;
  ri.t2calc = two_theta_calc;
  ri.t2diff = ABS(two_theta_obs - two_theta_calc);

  /*
    locate the last entry bigger then us
  */
  status = LLDnodePtr2First(list);
  while(status == 1){
    LLDnodeDataTo(list,&test);
    count++;
    if(test.t2diff == ri.t2diff){
      LLDnodeDataFrom(list,&ri);
      return;
    }
    if(test.t2diff > ri.t2diff){
      break;
    }
    status = LLDnodePtr2Next(list);
  }
  /*
    special case: empty list
  */
  if(count == 0){
    LLDnodeAppendFrom(list,&ri);
    return;
  }
  /*
    special case: append after last
  */
  LLDnodePtr2Last(list);
  LLDnodeDataTo(list,&test);
  if(ri.t2diff > test.t2diff){
    LLDnodeAppendFrom(list,&ri);
    return;
  }

  status = LLDnodePtr2First(list);
  pos = 0;
  while(status == 1){
    LLDnodeDataTo(list,&test);
    pos++;
    if(pos == count){
      LLDnodeInsertFrom(list,&ri);
      return;
    }
    status = LLDnodePtr2Next(list);
  }
}
/*----------------------------------------------------------------------------
  u_transform(i) = u(i)*(2*sin(theta)/lambda)
  -----------------------------------------------------------------------------*/ 
static void uToScatteringVector(MATRIX u, double theta, double lambda){
  double scale;
  int i;

  scale = (2. * Sind(theta))/lambda;
  for(i = 0; i < 3; i++){
    u[i][0] *= scale;
  }
}
/*----------------------------------------------------------------------------*/
static MATRIX buildHCHIMatrix(MATRIX u1, MATRIX u2, MATRIX u3){
  int i;
  MATRIX HCHI;

  HCHI = mat_creat(3,3,ZERO_MATRIX);
  if(HCHI == NULL){
    return NULL;
  }
  for(i = 0; i < 3; i++){
    HCHI[i][0] = u1[i][0];
    HCHI[i][1] = u2[i][0];
    HCHI[i][2] = u3[i][0];
  }
  return HCHI;
}
/*----------------------------------------------------------------------------*/
static MATRIX buildIndexMatrix(reflection r1, reflection r2, reflection r3){
  MATRIX HI;
  int i;

  HI = mat_creat(3,3,ZERO_MATRIX);
  if(HI == NULL){
    return NULL;
  }
  HI[0][0] = r1.h;
  HI[1][0] = r1.k;
  HI[2][0] = r1.l;

  HI[0][1] = r2.h;
  HI[1][1] = r2.k;
  HI[2][1] = r2.l;

  HI[0][2] = r3.h;
  HI[1][2] = r3.k;
  HI[2][2] = r3.l;

  return HI;
}
/*-----------------------------------------------------------------------------*/
MATRIX calcUBFromThreeReflections(reflection r1, reflection r2, reflection r3,
				  double lambda, int *errCode){
  MATRIX u1, u2, u3, HCHI, HI, HIINV,  UB;
  double det;

  if(lambda <= .1){
    *errCode = INVALID_LAMBDA;
    return NULL;
  }

  *errCode = 1;

  u1 = calcUVectorFromAngles(r1);
  u2 = calcUVectorFromAngles(r2);
  u3 = calcUVectorFromAngles(r3);
  uToScatteringVector(u1,r1.s2t/2.,lambda);
  uToScatteringVector(u2,r2.s2t/2.,lambda);
  uToScatteringVector(u3,r3.s2t/2.,lambda);
  
  HCHI = buildHCHIMatrix(u1,u2,u3);
  HI   = buildIndexMatrix(r1,r2,r3);
  if(HCHI == NULL || HI == NULL){
    *errCode = UBNOMEMORY;
    killVector(u1);
    killVector(u2);
    killVector(u3);
    return NULL;
  }

  HIINV = mat_inv(HI);
  if(HIINV == NULL){
    *errCode = NOTRIGHTHANDED;
    killVector(u1);
    killVector(u2);
    killVector(u3);
    mat_free(HI);
    mat_free(HCHI);
    return NULL;
  }
  UB = mat_mul(HCHI,HIINV);

  det = mat_det(UB);
  if(det < .0){
    mat_free(UB);
    UB = NULL;
    *errCode = NOTRIGHTHANDED;
  }

  killVector(u1);
  killVector(u2);
  killVector(u3);
  mat_free(HI);
  mat_free(HCHI);
  mat_free(HIINV);
  
  return UB;
}
/*---------------------------------------------------------------------------------*/
static int copyReflections(int list, refIndex index[], int maxIndex){
  int count = 0, status;
  refIndex ri;

  status = LLDnodePtr2First(list);
  while(status == 1 && count < maxIndex){
    LLDnodeDataTo(list,&ri);
    index[count] = ri;
    status = LLDnodePtr2Next(list);
    count++;
  }
  return count;
}
/*-----------------------------------------------------------------------------------
 - matching reflections will be entered in to a list in a sorted way. This list
   is copied into the index array.
 - There is some waste here in allocating and deallocating the HC vector in the 
   inner loop. I'am to lazy to resolve this.... May be I'am spared.....
   -----------------------------------------------------------------------------------*/ 
int searchIndex(lattice direct, double lambda, double two_theta, double max_deviation,
		int limit, refIndex index[], int maxIndex){
  int status, i, j, k, list;
  MATRIX B, HC;
  double theta, d;
  reflection r;

  B = mat_creat(3,3,UNIT_MATRIX);
  if(B == NULL) {
    return  UBNOMEMORY;
  }

  status = calculateBMatrix(direct,B);
  if(status < 0) {
    return status;
  }

  list = LLDcreate(sizeof(refIndex));
  if(list <0) {
    return  UBNOMEMORY;
  }
  
  for(i = -limit; i < limit; i++){
    r.h = (double)i;
    for(j = -limit; j < limit; j++){
      r.k = (double)j;
      for(k = -limit; k < limit; k++){
	r.l = (double)k;
	HC = reflectionToHC(r,B);
	status = calcTheta(lambda, HC, &d, &theta);
	if(status == 1){
	  if(ABS(two_theta - 2. * theta) <= max_deviation){
	    storeReflection(r,two_theta, theta * 2., list);
          }
	}
        killVector(HC);
      }
    }
  }
  mat_free(B);
  status = copyReflections(list,index,maxIndex);
  LLDdelete(list);
  return status;
} 
/*-------------------------------------------------------------------------*/
double angleBetweenReflections(MATRIX B, reflection r1, reflection r2){
    MATRIX chi1, chi2, h1, h2;
    double angle;
    
    h1 = makeVector();
    if(h1 == NULL){
        return -9999.99;
    }
    h1[0][0] = r1.h;
    h1[1][0] = r1.k;
    h1[2][0] = r1.l;

    h2 = makeVector();
    if(h2 == NULL){
        return -999.99;
    }
    h2[0][0] = r2.h;
    h2[1][0] = r2.k;
    h2[2][0] = r2.l;
    
    chi1 = mat_mul(B,h1);
    chi2 = mat_mul(B,h2);
    if(chi1 != NULL && chi2 != NULL){
        angle = angleBetween(chi1,chi2);
        killVector(chi1);
        killVector(chi2);
    }
    killVector(h1);
    killVector(h2);
    return angle;
}
/*---------------------------------------------------------------------------*/
MATRIX makeInstToConeVectorMatrix(reflection r,double lambda){
    double z1[3], alpha, beta;
    MATRIX mAlpha = NULL, mBeta = NULL, inst2CS = NULL;
    
    z1FromAngles(lambda,r.s2t,r.om,r.chi,r.phi,z1);
    alpha =  atan2(z1[1],z1[0]);
    beta  =  -atan2(z1[0],z1[2]);
/*     printf("alpha = %f, beta = %f\n", alpha*57.57, beta*57.57);*/
    
    mAlpha = mat_creat(3,3,ZERO_MATRIX);
    mBeta = mat_creat(3,3,ZERO_MATRIX);
    if(mAlpha == NULL || mBeta == NULL){
        return NULL;
    }
    mAlpha[0][0] = cos(alpha);
    mAlpha[0][1] = sin(alpha);
    mAlpha[1][0] = -sin(alpha);
    mAlpha[1][1] = cos(alpha);
    mAlpha[2][2] = 1.;
    
    mBeta[0][0] = cos(beta);
    mBeta[0][2] = sin(beta);
    mBeta[1][1] = 1.;
    mBeta[2][0] = -sin(beta);
    mBeta[2][2] =  cos(beta);
    
    inst2CS = mat_mul(mBeta,mAlpha);
    mat_free(mAlpha);
    mat_free(mBeta);
    
    return inst2CS;
}
/*--------------------------------------------------------------------------*/
MATRIX calcConeVector(double openingAngle, double coneAngle, 
    double length, MATRIX coneToPsi){
    MATRIX coneRot = NULL, nullVector = NULL, coneVector = NULL;
    MATRIX coneVectorScatter = NULL;
    double testAngle;
    MATRIX z;
    
    z = makeVector();
    z[2][0] =  1.;
 
    
    coneRot = mat_creat(3,3,ZERO_MATRIX);
    if(coneRot == NULL){
        return NULL;
    }
    coneRot[0][0] = Cosd(coneAngle);
    coneRot[0][1] = -Sind(coneAngle);
    coneRot[1][0] = Sind(coneAngle);
    coneRot[1][1] = Cosd(coneAngle);
    coneRot[2][2] = 1.0;
    
    nullVector = makeVector();
    if(nullVector == NULL){
        return NULL;
    }
    nullVector[0][0] = Sind(openingAngle);
    nullVector[1][0] = .0;
    nullVector[2][0] = Cosd(openingAngle);
    normalizeVector(nullVector);
    scaleVector(nullVector,length);
    testAngle = angleBetween(z,nullVector);
    
    coneVector = mat_mul(coneRot,nullVector);
    if(coneVector == NULL){
        return NULL;
    }
    testAngle = angleBetween(z,coneVector);
    
    coneVectorScatter = mat_mul(coneToPsi,coneVector);
    
    mat_free(coneRot);
    killVector(nullVector);
    killVector(coneVector);
    
    return coneVectorScatter;
}
/*---------------------------------------------------------------------------*/
double scatteringVectorLength(MATRIX B, reflection r){
    MATRIX h = NULL, psi = NULL;
    double length;
    
    h = makeVector();
    if(h == NULL){
        return -9999.9;
    }
    h[0][0] = r.h;
    h[1][0] = r.k;
    h[2][0] = r.l;
    
    psi = mat_mul(B,h);
    
    length = vectorLength(psi);
    killVector(h);
    killVector(psi);
    return  length;
}