/**
 * This is the SICS driver for the simple indexing algorithm in simidx.c
 * 
 * copyright: GPL
 * 
 * Mark Koennecke, August 2008
 */
#include <stdlib.h>
#include  <math.h>
#include <sics.h>
#include <sicshipadaba.h>
#include <sicsobj.h>
#include "simidx.h"
#include "singlex.h"
#include "matrix/matrix.h"
#include "ubfour.h"

extern double nintd(double d);
/*----------------------------------------------------------------------*/
static void SicsOutFunc(void *data, char *text)
{
  SConnection *pCon = (SConnection *) data;
  if (pCon != NULL) {
    SCWrite(pCon, text, eLog);
  }
}
/*-----------------------------------------------------------------------*/
static hdbCallbackReturn SetSttLim(pHdb node, void *userData,
                                   pHdbMessage mm)
{
  pHdbDataMessage set = NULL;
  SConnection *pCon = NULL;
  hdbValue v;

  if ((set = GetHdbSetMessage(mm)) != NULL) {
    SimIdxSetSttLim(set->v->v.doubleValue);
    return hdbContinue;
  }
  return hdbContinue;
}

/*-----------------------------------------------------------------------*/
static hdbCallbackReturn SetAngLim(pHdb node, void *userData,
                                   pHdbMessage mm)
{
  pHdbDataMessage set = NULL;
  SConnection *pCon = NULL;
  hdbValue v;

  if ((set = GetHdbSetMessage(mm)) != NULL) {
    SimIdxSetAngLim(set->v->v.doubleValue);
    return hdbContinue;
  }
  return hdbContinue;
}
/*-----------------------------------------------------------------------*/
MATRIX calcUBForSolution(IndexSolution is, int *err)
{
	pSingleDiff diffi;
	pSICSOBJ reflist;
	int i;
	MATRIX UB;
	double hkl[3];
	
	diffi = SXGetDiffractometer();
	reflist = SXGetReflectionList();
	assert(reflist != NULL && diffi != NULL);
	for (i = 0; i < 3; i++) {
		if (is.originalID[i] != 999) {
	      hkl[0] = (double) is.h[i];
	      hkl[1] = (double) is.k[i];
	      hkl[2] = (double) is.l[i];
	      SetRefIndex(reflist, is.originalID[i], hkl);
	   }
	}

	if (is.originalID[2] == 999) {
		UB = diffi->calcUBFromTwo(diffi, GetRefName(reflist, is.originalID[0]),
	                              GetRefName(reflist, is.originalID[1]), err);
	} else {
		UB = diffi->calcUBFromThree(diffi,
	                                GetRefName(reflist, is.originalID[0]),
	                                GetRefName(reflist, is.originalID[1]),
	                                GetRefName(reflist, is.originalID[2]),
	                                err);
	}
	return UB;
}
/*-----------------------------------------------------------------------*/
static void filterSolutions(){
	int i, err;
	IndexSolution is;
	MATRIX UB;
	
	for(i = 0; i < SimIdxGetNSolutions(); i++){
		is = SimIdxGetSolution(i);
		if(is.originalID[0] != -999){
			UB = calcUBForSolution(is,&err);
			if(UB != NULL){
				mat_free(UB);
			} else {
				SimIdxRemoveSolution(i);
				i--;
			}
		}
	}
}
/*-------------------------------------------------------------------------*/
static int RunIndexCmd(pSICSOBJ self, SConnection * pCon, pHdb commandNode,
                       pHdb par[], int nPar)
{
  int i, j, status, err;
  pSingleDiff diffi;
  pSICSOBJ reflist;
  double ang[5], z1[3];
  IndexSolution is;
  MATRIX ub;
  pHdb nSol = NULL;

  SimIdxSetLambda(SXGetLambda()); 
  SimIdxSetCell((double *) SXGetCell());
  SimIdxSetSpacegroup(SXGetSpaceGroup());
  SimIdxOutput(pCon, SicsOutFunc, 10);

  reflist = SXGetReflectionList();
  diffi = SXGetDiffractometer();
  SimIdxClearReflection();
  for (i = 0; i < ReflectionListCount(reflist); i++) {
    GetRefAngles(reflist, i, ang);
    diffi->calcZ1(diffi, GetRefName(reflist, i), z1);
    SimIdxAddReflection(z1);
  }
 
  nSol = GetHipadabaNode(self->objectNode,"nsolutions");
  assert(nSol != NULL);
  
  status = SimIdxRun();
  filterSolutions(); 
  if (status == 1) {
    if (SimIdxGetNSolutions() < 1) {
      SCWrite(pCon, "No solutions were found", eLog);
      UpdateHipadabaPar(nSol,MakeHdbInt(0), pCon);
      return 0;
    }
    SCWrite(pCon, "Indexing Suggestions:", eLog);
    UpdateHipadabaPar(nSol,MakeHdbInt(SimIdxGetNSolutions()), pCon);
    for (i = 0; i < SimIdxGetNSolutions(); i++) {
      is = SimIdxGetSolution(i);
      SCPrintf(pCon, eLog, "Solution No : %d, GOF = %6.3f", i,
               is.diff * 100);
      for (j = 0; j < 3; j++) {
        if (is.originalID[j] != 999) {
          SCPrintf(pCon, eLog, "   Ref = %2d, HKL = %4d %4d %4d ",
                   is.originalID[j], is.h[j], is.k[j], is.l[j]);
        }
      }
      SCPrintf(pCon,eLog,"   UB:");
      ub = calcUBForSolution(is,&err);
      if(ub != NULL) { /* should never happen, as filtered */
    	  for(j = 0; j < 3; j++){
    		  SCPrintf(pCon, eLog,"  %8.5f %8.5f %8.5f", 
    				  ub[j][0], ub[j][1], ub[j][2]);
    	  }
      }
    }
  }
  
  return status;
}
/*----------------------------------------------------------------------*/
static void InternalOutFunc(void *data, char *text)
{
  pDynString tData = (pDynString) data;
  if (tData!= NULL) {
    DynStringConcat(tData, text);
  }
}
/*-------------------------------------------------------------------------*/
static int RunIntIndexCmd(pSICSOBJ self, SConnection * pCon, pHdb commandNode,
                       pHdb par[], int nPar)
{
  int i, j, status, err;
  pSingleDiff diffi;
  pSICSOBJ reflist;
  double ang[5], z1[3];
  IndexSolution is;
  MATRIX ub;
  pHdb nSol = NULL;
  pDynString tData;
  char fString[512];

  tData = CreateDynString(256,256);

  SimIdxSetLambda(SXGetLambda()); 
  SimIdxSetCell((double *) SXGetCell());
  SimIdxSetSpacegroup(SXGetSpaceGroup());
  SimIdxOutput(tData, InternalOutFunc, 10);

  reflist = SXGetReflectionList();
  diffi = SXGetDiffractometer();
  SimIdxClearReflection();
  for (i = 0; i < ReflectionListCount(reflist); i++) {
    GetRefAngles(reflist, i, ang);
    diffi->calcZ1(diffi, GetRefName(reflist, i), z1);
    SimIdxAddReflection(z1);
  }
 
  nSol = GetHipadabaNode(self->objectNode,"nsolutions");
  assert(nSol != NULL);
  
  status = SimIdxRun();
  filterSolutions(); 
  if (status == 1) {
    if (SimIdxGetNSolutions() < 1) {
      DynStringConcat(tData,"No solutions were found\n");
      UpdateHipadabaPar(nSol,MakeHdbInt(0), pCon);
      return 0;
    }
    DynStringConcat(tData, "Indexing Suggestions:\n");
    UpdateHipadabaPar(nSol,MakeHdbInt(SimIdxGetNSolutions()), pCon);
    for (i = 0; i < SimIdxGetNSolutions(); i++) {
      is = SimIdxGetSolution(i);
      
      snprintf(fString,sizeof(fString), "Solution No : %d, GOF = %6.3f\n", i,
               is.diff * 100);
      DynStringConcat(tData,fString);	       
      for (j = 0; j < 3; j++) {
        if (is.originalID[j] != 999) {
	  snprintf(fString,sizeof(fString),"   Ref = %2d, HKL = %4d %4d %4d \n",
                   is.originalID[j], is.h[j], is.k[j], is.l[j]);
          DynStringConcat(tData,fString);
        }
      }
      SCPrintf(pCon,eLog,"   UB:");
      DynStringConcat(tData,"   UB\n");
      ub = calcUBForSolution(is,&err);
      if(ub != NULL) { /* should never happen, as filtered */
    	  for(j = 0; j < 3; j++){
	    snprintf(fString,sizeof(fString),"  %8.5f %8.5f %8.5f\n", 
    				  ub[j][0], ub[j][1], ub[j][2]);
	    DynStringConcat(tData,fString);
    	  }
      }
    }
  }
  
  SCWrite(pCon,GetCharArray(tData),eValue);
  DeleteDynString(tData);
  return status;
}

/*-----------------------------------------------------------------------*/
static int ChooseCmd(pSICSOBJ self, SConnection * pCon, pHdb commandNode,
                     pHdb par[], int nPar)
{
  MATRIX UB;
  int err, solution, i;
  IndexSolution is;
  double  ub[9];

  if (nPar < 1) {
    SCWrite(pCon, "ERROR: need the solution number as argument", eError);
    return 0;
  }
  solution = par[0]->value.v.intValue;

  if (solution < 0 || solution >= SimIdxGetNSolutions()) {
    SCWrite(pCon, "ERROR: solution number out of range", eError);
    return 0;
  }

  is = SimIdxGetSolution(solution);
  UB = calcUBForSolution(is,&err);
  
  if (UB == 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 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;
  }
  for (i = 0; i < 3; i++) {
    ub[i] = UB[0][i];
    ub[i + 3] = UB[1][i];
    ub[i + 6] = UB[2][i];
  }
  SXSetUB(ub);
  mat_free(UB);
  SCSendOK(pCon);
  return 1;
}
/*-----------------------------------------------------------------------*/
static int DiraxCmd(pSICSOBJ self, SConnection * pCon, pHdb commandNode,
                    pHdb par[], int nPar)
{
  FILE *fd = NULL;
  int i;
  double uvw[3];
  pSICSOBJ reflist = NULL;
  pSingleDiff diffi = NULL;

  if (nPar < 1) {
    SCWrite(pCon, "ERROR: need filename parameter", eError);
    return 0;
  }

  reflist = SXGetReflectionList();
  assert(reflist != NULL);
  diffi = SXGetDiffractometer();
  assert(diffi != NULL);

  fd = fopen(par[0]->value.v.text, "w");
  if (fd == NULL) {
    SCWrite(pCon, "ERROR: failed to open dirax file", eError);
    return 0;
  }
  fprintf(fd, "  %f\n", SXGetLambda());
  for (i = 0; i < ReflectionListCount(reflist); i++) {
    diffi->calcZ1(diffi, GetRefName(reflist, i), uvw);
    fprintf(fd, " %f %f %f\n", uvw[0], uvw[1], uvw[2]);
  }

  fclose(fd);
  SCSendOK(pCon);
  return 1;
}

/*-----------------------------------------------------------------------*/
static int IndexCmd(pSICSOBJ self, SConnection * pCon, pHdb commandNode,
                    pHdb par[], int nPar)
{
  int i, j;
  double ang[5], hkl[3];
  const double *ub;
  pSICSOBJ reflist = NULL;
  pSingleDiff diffi = NULL;

  reflist = SXGetReflectionList();
  assert(reflist != NULL);
  diffi = SXGetDiffractometer();
  assert(diffi != NULL);
  for (i = 0; i < ReflectionListCount(reflist); i++) {
    GetRefAngles(reflist, i, ang);
    diffi->hklFromAnglesGiven(diffi, ang, hkl);
    for (j = 0; j < 3; j++) {
      hkl[j] = nintd(hkl[j]);
    }
    SetRefIndex(reflist, i, hkl);
  }

  SCSendOK(pCon);
  return 1;
}

/*-----------------------------------------------------------------------*/
void InitSimpleIndex(SConnection * pCon, SicsInterp * pSics)
{
  pSICSOBJ pNew = NULL;
  pHdb cmd;

  pNew = MakeSICSOBJ("simidx", "SimpleIndexer");
  if (!pNew) {
    return;
  }
  SimIdxInit();

  cmd = AddSICSHdbPar(pNew->objectNode, "sttlim", usUser,
                      MakeHdbFloat(.2));
  AppendHipadabaCallback(cmd, MakeHipadabaCallback(SetSttLim, NULL, NULL));
  SetHdbProperty(cmd, "__save", "true");

  cmd = AddSICSHdbPar(pNew->objectNode, "anglim", usUser,
                      MakeHdbFloat(.5));
  AppendHipadabaCallback(cmd, MakeHipadabaCallback(SetAngLim, NULL, NULL));
  SetHdbProperty(cmd, "__save", "true");

  cmd = AddSICSHdbPar(pNew->objectNode, "nsolutions", usUser,
                      MakeHdbInt(0));
  
  cmd = AddSICSHdbPar(pNew->objectNode, "run", usUser,
                      MakeSICSFunc(RunIndexCmd));

  cmd = AddSICSHdbPar(pNew->objectNode, "runint", usUser,
                      MakeSICSFunc(RunIntIndexCmd));

  cmd = AddSICSHdbPar(pNew->objectNode, "choose", usUser,
                      MakeSICSFunc(ChooseCmd));
  cmd = AddSICSHdbPar(cmd, "solution", usUser, MakeHdbInt(0));

  cmd = AddSICSHdbPar(pNew->objectNode, "dirax", usUser,
                      MakeSICSFunc(ChooseCmd));
  cmd = AddSICSHdbPar(cmd, "name", usUser, MakeHdbText("sics.drx"));

  cmd = AddSICSHdbPar(pNew->objectNode, "idxref", usUser,
                      MakeSICSFunc(IndexCmd));

  AddCommand(pSics, "simidx", InterInvokeSICSOBJ, KillSICSOBJ, pNew);

}