/**
 * This is a library for calculating UB matrices for four circle diffraction.
 * The algorithm and setting definitions is from:
 *
 *            Busing & Levy, Acta Cryst. (1967), 22, 457ff 
 *
 * Implemented:
 * - UB from cell cell constants and two reflections.
 *
 * Mark Koennecke, March 2005
 */
#include "ubfour.h"
#include <assert.h>
#include "vector.h"
#include "trigd.h"
/*--------------------------------------------------------------------------------------*/
static MATRIX calcUVectorFromAngles(reflection r){
  MATRIX u;

  u = makeVector();
  if(u == NULL){
    return NULL;
  }
  vectorSet(u,0,Cosd(r.om)*Cosd(r.chi)*Cosd(r.phi) - Sind(r.om)*Sind(r.phi));
  vectorSet(u,1,Cosd(r.om)*Cosd(r.chi)*Sind(r.phi) + Sind(r.om)*Cosd(r.phi));
  vectorSet(u,2,Cosd(r.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;

  *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;
}