/*----------------------------------------------------------------------
  UB calculation routines for four circle diffraction.
  This is the interpreter interface to functionality implemented
  in fourlib.c

  copyright: see file COPYRIGHT

  Mark Koennecke, March 2005
  
  Heavily reworked to fit into the new four circle system
  
  Mark Koennecke, July 2008

  Added UBfromCell

  Mark Koennecke, March 2013
-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <errno.h>
#include "tcl.h"
#include "fortify.h"
#include "sics.h"
#include "ubfour.h"
#include "ubcalc.h"
#include "motor.h"
#include "hkl.h"
#include "singlex.h"
#include "reflist.h"
#include "singlediff.h"
/*----------------------------------------------------------------------*/
static void killUBCALC(void *pData)
{
  pUBCALC self = (pUBCALC) pData;
  if (self == NULL) {
    return;
  }
  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
  }
  if (self->UB != NULL) {
    mat_free(self->UB);
  }
  free(self);
}

/*--------------------------------------------------------------------*/
static int SaveUBCalc(void *data, char *name, FILE * fd)
{
  pUBCALC self = (pUBCALC) data;
  if (self == NULL) {
    return 0;
  }
  fprintf(fd, "%s difftheta  %f\n", name, self->allowedDeviation);
  fprintf(fd, "%s maxindex  %d\n", name, self->indexSearchLimit);
  fprintf(fd, "%s maxlist  %d\n", name, self->maxSuggestions);
  return 1;
}

/*---------------------------------------------------------------------*/
static pUBCALC makeUBCALC(pHKL hkl)
{
  pUBCALC pNew = NULL;

  pNew = (pUBCALC) malloc(sizeof(UBCALC));
  if (pNew == NULL) {
    return NULL;
  }
  memset(pNew, 0, sizeof(UBCALC));
  pNew->pDes = CreateDescriptor("UBcalc");
  pNew->UB = mat_creat(3, 3, UNIT_MATRIX);
  if (pNew->pDes == NULL || pNew->UB == NULL) {
    return NULL;
  }
  pNew->pDes->SaveStatus = SaveUBCalc;
  pNew->hkl = hkl;
  pNew->allowedDeviation = .3;
  pNew->indexSearchLimit = 5;
  pNew->maxSuggestions = 10;
  return pNew;
}

/*----------------------------------------------------------------------*/
int CreateUBCalc(SConnection * pCon, SicsInterp * pSics, char *name,
                 char *hklname)
{
  pUBCALC pNew = NULL;
  int status;
  pHKL hkl = NULL;


  hkl = FindCommandData(pSics, hklname, "4-Circle-Calculus");
  if (hkl == NULL) {
    SCWrite(pCon, "ERROR: HKL object not found or wrong type", eError);
    return 0;
  }

  pNew = makeUBCALC(hkl);
  if (pNew == NULL) {
    SCWrite(pCon, "ERROR: out of memory creating UBCALC", eError);
    return 0;
  }
  status = AddCommand(pSics, name, UBCalcWrapper, killUBCALC, pNew);
  if (status != 1) {
    SCWrite(pCon, "ERROR: failed to create duplicate UBCALC module",
            eError);
  }
  return status;
}

/*----------------------------------------------------------------------*/
int MakeUBCalc(SConnection * pCon, SicsInterp * pSics, void *pData,
               int argc, char *argv[])
{

  if (argc < 3) {
    SCWrite(pCon,
            "ERROR: missing argument to MakeUBCalc: MakeUBCalc name hklobject",
            eError);
    return 0;
  }
  return CreateUBCalc(pCon, pSics, argv[1], argv[2]);
}

/*---------------------------------------------------------------------*/
static void listUB(SConnection * pCon, MATRIX UB)
{
  Tcl_DString list;
  char pBueffel[255];
  int i;


  Tcl_DStringInit(&list);
  if (UB == NULL) {
    Tcl_DStringAppend(&list, "NO UB", -1);
  } else {
    Tcl_DStringAppend(&list, "UB = ", -1);
    for (i = 0; i < 3; i++) {
      snprintf(pBueffel, 255, "%f %f %f\n", UB[i][0], UB[i][1], UB[i][2]);
      Tcl_DStringAppend(&list, pBueffel, -1);
    }
  }
  SCWrite(pCon, Tcl_DStringValue(&list), eValue);
  Tcl_DStringFree(&list);
}

/*---------------------------------------------------------------------*/
static int updateUBCALC(pUBCALC self, SConnection * pCon,
                        char *id1, char *id2, char *id3)
{
  const double *cell;
  double hkl[3], angles[5];
  pSICSOBJ refList;

  cell = SXGetCell();
  self->direct.a = cell[0];
  self->direct.b = cell[1];
  self->direct.c = cell[2];
  self->direct.alpha = cell[3];
  self->direct.beta = cell[4];
  self->direct.gamma = cell[5];

  refList = SXGetReflectionList();
  if (id1 != NULL) {
    if (!GetRefIndexID(refList, id1, hkl)) {
      SCPrintf(pCon, eError, "ERROR: reflection with id %s not found",
               id1);
      return 0;
    } else {
      self->r1.h = hkl[0];
      self->r1.k = hkl[1];
      self->r1.l = hkl[2];
      GetRefAnglesID(refList, id1, angles);
      self->r1.s2t = angles[0];
      self->r1.om = angles[1];
      self->r1.chi = angles[2];
      self->r1.phi = angles[3];
    }
  }

  if (id2 != NULL) {
    if (!GetRefIndexID(refList, id2, hkl)) {
      SCPrintf(pCon, eError, "ERROR: reflection with id %s not found",
               id2);
      return 0;
    } else {
      self->r2.h = hkl[0];
      self->r2.k = hkl[1];
      self->r2.l = hkl[2];
      GetRefAnglesID(refList, id2, angles);
      self->r2.s2t = angles[0];
      self->r2.om = angles[1];
      self->r2.chi = angles[2];
      self->r2.phi = angles[3];
    }
  }
  if (id3 != NULL) {
    if (!GetRefIndexID(refList, id3, hkl)) {
      SCPrintf(pCon, eError, "ERROR: reflection with id %s not found",
               id3);
      return 0;
    } else {
      self->r3.h = hkl[0];
      self->r3.k = hkl[1];
      self->r3.l = hkl[2];
      GetRefAnglesID(refList, id3, angles);
      self->r3.s2t = angles[0];
      self->r3.om = angles[1];
      self->r3.chi = angles[2];
      self->r3.phi = angles[3];
    }
  }
  return 1;
}

/*---------------------------------------------------------------------*/
static int calcUB(pUBCALC self, SConnection * pCon, char *ref1, char *ref2)
{
  MATRIX newUB = NULL;
  int err = 1;
  pSingleDiff single = NULL;

  if (!updateUBCALC(self, pCon, ref1, ref2, NULL)) {
    return 0;
  }

  single = SXGetDiffractometer();
  assert(single != NULL);

  newUB = single->calcUBFromTwo(single, ref1, ref2, &err);
  if (newUB == NULL) {
    switch (err) {
    case REFERR:
      SCWrite(pCon, "ERROR: one of reflections ID's is invalid", eError);
      return 0;
      break;
    case UBNOMEMORY:
      SCWrite(pCon, "ERROR: out of memory while calculating UB", eError);
      return 0;
      break;
    case REC_NO_VOLUME:
      SCWrite(pCon, "ERROR: bad cell constants", eError);
      return 0;
      break;
    default:
      SCWrite(pCon, "ERROR: unknown error on UB matrix calculation",
              eError);
      return 0;
    }
  } else {
    if (self->UB != NULL) {
      mat_free(self->UB);
    }
    self->UB = newUB;
    SCSendOK(pCon);
    return 1;
  }
}
/*---------------------------------------------------------------------*/
static int UBFromCell(pUBCALC self, SConnection *pCon)
{
  const double *cell;
  lattice mycell;
  MATRIX B;
  double ub[9];
  int i;

  cell = SXGetCell();
  mycell.a = cell[0];
  mycell.b = cell[1];
  mycell.c = cell[2];
  mycell.alpha = cell[3];
  mycell.beta = cell[4];
  mycell.gamma = cell[5];

  B = mat_creat(3,3,ZERO_MATRIX);
  if(B == NULL){
    SCWrite(pCon,"ERROR: out of memory in UBfromCell",eError);
    return 0;
  }

  calculateBMatrix(mycell,B);
  
  if(self->UB != NULL){
    mat_free(self->UB);
  }
  self->UB = B;
 
  SCSendOK(pCon);
  return 1;
}
/*---------------------------------------------------------------------*/
static int sendUBToHKL(SConnection * pCon, SicsInterp * pSics,
                       pHKL hkl, MATRIX UB)
{
  float ub[9];
  int i;

  assert(hkl != NULL);

  for (i = 0; i < 3; i++) {
    ub[i] = UB[0][i];
    ub[i + 3] = UB[1][i];
    ub[i + 6] = UB[2][i];
  }
  SetUB(hkl, ub);
  SCSendOK(pCon);
  return 1;
}

/*---------------------------------------------------------------------*/
static int setUBCalcParameters(pUBCALC self, SConnection * pCon,
                               char *name, char *value)
{

  if (strcmp(name, "difftheta") == 0) {
    if (!SCMatchRights(pCon, usUser)) {
      return 0;
    }
    self->allowedDeviation = atof(value);
    SCparChange(pCon);
    SCSendOK(pCon);
    return 1;
  } else if (strcmp(name, "maxindex") == 0) {
    if (!SCMatchRights(pCon, usUser)) {
      return 0;
    }
    self->indexSearchLimit = atoi(value);
    SCparChange(pCon);
    SCSendOK(pCon);
    return 1;
  } else if (strcmp(name, "maxlist") == 0) {
    if (!SCMatchRights(pCon, usUser)) {
      return 0;
    }
    self->maxSuggestions = atoi(value);
    SCparChange(pCon);
    SCSendOK(pCon);
    return 1;
  } else {
    SCWrite(pCon, "ERROR: subcommand not recognized", eError);
    return 0;
  }
}

/*---------------------------------------------------------------------*/
static int getUBCalcParameters(pUBCALC self, SConnection * pCon,
                               char *name)
{
  char pBueffel[255];
  int ret = 0;

  if (strcmp(name, "difftheta") == 0) {
    snprintf(pBueffel, 255, "ubcalc.difftheta = %f",
             self->allowedDeviation);
    ret = 1;
  } else if (strcmp(name, "maxindex") == 0) {
    snprintf(pBueffel, 255, "ubcalc.maxindex = %d",
             self->indexSearchLimit);
    ret = 1;
  } else if (strcmp(name, "maxlist") == 0) {
    snprintf(pBueffel, 255, "ubcalc.maxindex = %d", self->maxSuggestions);
    ret = 1;
  } else {
    snprintf(pBueffel, 255, "ERROR: subcommand not known");
  }
  SCWrite(pCon, pBueffel, eValue);
  return ret;
}

/*---------------------------------------------------------------------*/
static void listPar(pUBCALC self, char *name, SConnection * pCon)
{
  char pBueffel[255];

  snprintf(pBueffel, 255, "%s.difftheta = %f", name,
           self->allowedDeviation);
  SCWrite(pCon, pBueffel, eValue);
  snprintf(pBueffel, 255, "%s.maxindex = %d", name,
           self->indexSearchLimit);
  SCWrite(pCon, pBueffel, eValue);
  snprintf(pBueffel, 255, "%s.maxlist = %d", name, self->maxSuggestions);
  SCWrite(pCon, pBueffel, eValue);

}

/*---------------------------------------------------------------------*/
static int findIndex(pUBCALC self, SConnection * pCon, SicsInterp * pSics,
                     int argc, char *argv[])
{
  float two_theta;
  pMotor pMot = NULL;
  int status, numRef, i;
  refIndex *index = NULL;
  Tcl_DString list;
  char pLine[255];
  double lambda;

  if (argc > 2) {
    two_theta = atof(argv[2]);
  } else {
    pMot = SXGetMotor(TwoTheta);
    if (pMot == NULL) {
      SCWrite(pCon, "ERROR: cannot find stt motor", eError);
      return 0;
    }
    MotorGetSoftPosition(pMot, pCon, &two_theta);
  }
  lambda = SXGetLambda();
  updateUBCALC(self, pCon, NULL, NULL, NULL);

  numRef = self->maxSuggestions;
  index = (refIndex *) malloc(numRef * sizeof(refIndex));
  if (index == NULL) {
    SCWrite(pCon, "ERROR: out of memory allocating index list", eError);
    return 0;
  }
  memset(index, 0, numRef * sizeof(refIndex));

  status =
      searchIndex(self->direct, lambda, two_theta, self->allowedDeviation,
                  self->indexSearchLimit, index, numRef);

  if (status < 0) {
    if (status == UBNOMEMORY) {
      SCWrite(pCon, "ERROR: out of memory searching indices", eError);
      return 0;
    } else if (status == REC_NO_VOLUME) {
      SCWrite(pCon, "ERROR: bad cell parameters", eError);
      return 0;
    } else {
      SCWrite(pCon, "ERROR: unknown error code", eError);
      return 0;
    }
  }

  if (status < numRef) {
    numRef = status;
  }
  Tcl_DStringInit(&list);
  for (i = 0; i < numRef; i++) {
    snprintf(pLine, 255, " %3d %3d %3d %8.2f %8.2f %8.2f\r\n",
             (int) index[i].h, (int) index[i].k, (int) index[i].l,
             two_theta, index[i].t2calc, index[i].t2diff);
    Tcl_DStringAppend(&list, pLine, -1);
  }
  if (numRef == 0) {
    Tcl_DStringAppend(&list, "No suitable reflections found", -1);
  }
  SCWrite(pCon, Tcl_DStringValue(&list), eValue);

  Tcl_DStringFree(&list);
  free(index);
  return 1;
}

/*---------------------------------------------------------------------*/
static int calcUB3Ref(pUBCALC self, SConnection * pCon,
                      char *id1, char *id2, char *id3)
{
  double lambda;
  MATRIX newUB = NULL;
  int errCode = 1;
  char pBueffel[256];
  pSingleDiff single = NULL;

  lambda = SXGetLambda();
  if (!updateUBCALC(self, pCon, id1, id2, id3)) {
    return 0;
  }

  single = SXGetDiffractometer();
  assert(single != NULL);

  newUB = single->calcUBFromThree(single, id1, id2, id3, &errCode);
  if (newUB == NULL) {
    switch (errCode) {
    case REFERR:
      SCWrite(pCon, "ERROR: one of reflections ID's is invalid", eError);
      return 0;
      break;
    case UBNOMEMORY:
      SCWrite(pCon, "ERROR: out of memory calculating UB", eError);
      break;
    case INVALID_LAMBDA:
      SCWrite(pCon, "ERROR: bad value for wavelength", eError);
      break;
    case NOTRIGHTHANDED:
      SCWrite(pCon,
              "ERROR: reflections are coplanar or do not form a right handed system",
              eError);
      break;
    default:
      SCWrite(pCon, "ERROR: unknown error code from UB calculation",
              eError);
      break;
    }
    return 0;
  } else {
    if (self->UB != NULL) {
      mat_free(self->UB);
    }
    self->UB = newUB;
    SCSendOK(pCon);
  }
  return 1;
}

/*--------------------------------------------------------------------*/
static int cellFromUBWrapper(pUBCALC self, SConnection * pCon)
{
  int status;
  char pBueffel[256];
  lattice direct;
  const double *ub;
  MATRIX UB;
  int i;

  UB = mat_creat(3, 3, UNIT_MATRIX);
  ub = SXGetUB();
  for (i = 0; i < 3; i++) {
    UB[0][i] = ub[i];
    UB[1][i] = ub[i + 3];
    UB[2][i] = ub[i + 6];
  }

  status = cellFromUB(UB, &direct);
  mat_free(UB);

  if (status < 0) {
    switch (status) {
    case CELLNOMEMORY:
      SCWrite(pCon, "ERROR: out of memory while calculating cell", eError);
      break;
    default:
      SCWrite(pCon, "ERROR: unknown error calculating cell", eError);
      break;
    }
    return 0;
  } else {
    snprintf(pBueffel, 255, "%f %f %f %f %f %f",
             direct.a, direct.b, direct.c,
             direct.alpha, direct.beta, direct.gamma);
    SCWrite(pCon, pBueffel, eValue);
  }
  return 1;
}

/*----------------------------------------------------------------------*/
int UBCalcWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
                  int argc, char *argv[])
{
  pUBCALC self = (pUBCALC) pData;
  char pBuffer[512];

  assert(self);
  if (argc < 2) {
    SCWrite(pCon, "Insuffcient number of arguments to ubcalc", eError);
    return 0;
  }
  strtolower(argv[1]);

  if (strcmp(argv[1], "listub") == 0) {
    listUB(pCon, self->UB);
    return 1;
  } else if (strcmp(argv[1], "ub2ref") == 0) {
    if (argc < 4) {
      SCWrite(pCon, "Insuffcient number of arguments to ubcalc ub2ref",
              eError);
      return 0;
    }
    return calcUB(self, pCon, argv[2], argv[3]);
  } else if (strcmp(argv[1], "ub3ref") == 0) {
    if (argc < 5) {
      SCWrite(pCon, "Insuffcient number of arguments to ubcalc ub3ref",
              eError);
      return 0;
    }
    return calcUB3Ref(self, pCon, argv[2], argv[3], argv[4]);
  } else if (strcmp(argv[1], "cellub") == 0) {
    return cellFromUBWrapper(self, pCon);
  } else if (strcmp(argv[1], "list") == 0) {
    listUB(pCon, self->UB);
    listPar(self, argv[0], pCon);
    return 1;
  } else if (strcmp(argv[1], "activate") == 0) {
    return sendUBToHKL(pCon, pSics, self->hkl, self->UB);
  } else if (strcmp(argv[1], "index") == 0) {
    return findIndex(self, pCon, pSics, argc, argv);
  } else if (strcmp(argv[1], "fromcell") == 0) {
    return UBFromCell(self, pCon);
  } else {
    if (argc > 2) {
      return setUBCalcParameters(self, pCon, argv[1], argv[2]);
    } else {
      return getUBCalcParameters(self, pCon, argv[1]);
    }
  }
  return 1;
}

/*------------------------------------------------------------------------*/
reflection getReflection(void *ubcalc, int no)
{
  pUBCALC self = (pUBCALC) ubcalc;

  assert(self != NULL);

  switch (no) {
  case 0:
    return self->r1;
    break;
  case 1:
    return self->r2;
    break;
  case 2:
    return self->r3;
    break;
  default:
    assert(0);
    break;
  }
}