/*----------------------------------------------------------------------
  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
-----------------------------------------------------------------------*/
#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[4];
	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 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(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;
    }
}