/*----------------------------------------------------------------------
  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
-----------------------------------------------------------------------*/
#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"
/*----------------------------------------------------------------------*/
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 cell %f %f %f %f %f %f\n", name, self->direct.a,
	  self->direct.b, self->direct.c, self->direct.alpha,
	  self->direct.beta, self->direct.gamma);
  fprintf(fd,"%s ref1 %f %f %f  %f %f %f %f\n",name,
	  self->r1.h, self->r1.k, self->r1.l,
	  self->r1.s2t, self->r1.om, self->r1.chi, self->r1.phi);
  fprintf(fd,"%s ref2 %f %f %f  %f %f %f %f\n",name,
	  self->r2.h, self->r2.k, self->r2.l,
	  self->r2.s2t, self->r2.om, self->r2.chi, self->r2.phi);
  fprintf(fd,"%s ref3 %f %f %f  %f %f %f %f\n",name,
	  self->r3.h, self->r3.k, self->r3.l,
	  self->r3.s2t, self->r3.om, self->r3.chi, self->r3.phi);
  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 MakeUBCalc(SConnection *pCon, SicsInterp *pSics, void *pData,		
	       int argc, char *argv[]){
  pUBCALC pNew = NULL;
  int status;
  pHKL hkl = NULL;

  if(argc < 3){
    SCWrite(pCon,"ERROR: missing argument to MakeUBCalc: MakeUBCalc name hklobject",eError);
    return 0;
  }

  hkl = FindCommandData(pSics,argv[2],"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,argv[1],UBCalcWrapper,killUBCALC,pNew);
  if(status != 1){
    SCWrite(pCon,"ERROR: failed to create duplicate UBCALC module",eError);
  }
  return status;
}
/*---------------------------------------------------------------------*/
static int readCell(SConnection *pCon, pUBCALC self, int argc, char *argv[]){
  int status;
  Tcl_Interp *pTcl = InterpGetTcl(pServ->pSics);
  char pBueffel[256];

  if(argc < 8){
    SCWrite(pCon,"ERROR: insufficient number of arguments to ubcalc cell",
	    eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[2],&self->direct.a);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[3],&self->direct.b);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[4],&self->direct.c);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[5],&self->direct.alpha);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[6],&self->direct.beta);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[6]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[7],&self->direct.gamma);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[7]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  SCparChange(pCon);
  SCSendOK(pCon);
  return 1;
}
/*--------------------------------------------------------------------*/
static int readRefMotors(SConnection *pCon, SicsInterp *pSics, 
			 reflection *r){
  pMotor pMot = NULL;
  float val;

  pMot = FindMotor(pSics,"stt");
  if(pMot == NULL){
    SCWrite(pCon,"ERROR: cannot find stt motor",eError);
    return 0;
  }
  MotorGetSoftPosition(pMot,pCon,&val);
  r->s2t = val;

  pMot = FindMotor(pSics,"om");
  if(pMot == NULL){
    SCWrite(pCon,"ERROR: cannot find om motor",eError);
    return 0;
  }
  MotorGetSoftPosition(pMot,pCon,&val);
  r->om = val;

  pMot = FindMotor(pSics,"chi");
  if(pMot == NULL){
    SCWrite(pCon,"ERROR: cannot find chi motor",eError);
    return 0;
  }
  MotorGetSoftPosition(pMot,pCon,&val);
  r->chi = val;

  pMot = FindMotor(pSics,"phi");
  if(pMot == NULL){
    SCWrite(pCon,"ERROR: cannot find phi motor",eError);
    return 0;
  }
  MotorGetSoftPosition(pMot,pCon,&val);
  r->phi = val;

  SCparChange(pCon);
  SCSendOK(pCon);
  return 1;
}
/*---------------------------------------------------------------------*/
static int readReflection(SConnection *pCon, SicsInterp *pSics, reflection *r,
			  int argc, char *argv[]){
  char pBueffel[255];
  int status;
  Tcl_Interp *pTcl = InterpGetTcl(pSics);

  if(argc < 5){
    SCWrite(pCon,"ERROR: not enough arguments to ubcalc ref1,2",eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[2],&r->h);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[2]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[3],&r->k);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[3]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }
  status = Tcl_GetDouble(pTcl,argv[4],&r->l);
  if(status != TCL_OK){
    snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[4]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }

  if(argc >= 9){
    status = Tcl_GetDouble(pTcl,argv[5],&r->s2t);
    if(status != TCL_OK){
      snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[5]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    status = Tcl_GetDouble(pTcl,argv[6],&r->om);
    if(status != TCL_OK){
      snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[6]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    status = Tcl_GetDouble(pTcl,argv[7],&r->chi);
    if(status != TCL_OK){
      snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[7]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    status = Tcl_GetDouble(pTcl,argv[8],&r->phi);
    if(status != TCL_OK){
      snprintf(pBueffel,255,"ERROR: failed to convert %s to number",argv[8]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    SCparChange(pCon);
    SCSendOK(pCon);
    return 1;
  } else {
    return readRefMotors(pCon,pSics,r);
  }
} 
/*---------------------------------------------------------------------*/
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 calcUB(pUBCALC self, SConnection *pCon){
  MATRIX newUB = NULL;
  int err = 1;

  newUB = calcUBFromCellAndReflections(self->direct, self->r1, self->r2, &err);
  if(newUB == NULL){
    switch(err){
    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 void listCell(SConnection *pCon, char *name, lattice direct){
  char pBueffel[255];

  snprintf(pBueffel,255,"%s.cell = %f %f %f %f %f %f",
	   name,direct.a, direct.b,direct.c,
	   direct.alpha,direct.beta,direct.gamma);
  SCWrite(pCon,pBueffel,eValue);
  
}
/*---------------------------------------------------------------------*/
static void listReflection(SConnection *pCon, char *name, 
			   char *refName, reflection r){
  char pBueffel[255];

  snprintf(pBueffel,255,"%s.%s = %f %f %f   %f %f %f %f",
	   name,refName, r.h, r.k, r.l, r.s2t, r.om, r.chi, r.phi);
  SCWrite(pCon,pBueffel,eValue);
}
/*---------------------------------------------------------------------*/
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, lambda;
  pMotor pMot = NULL;
  int status, numRef, i;
  refIndex *index = NULL;
  Tcl_DString list;
  char pLine[255];

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

  GetLambda(self->hkl,&lambda);

  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){
  float lambda;
  MATRIX newUB = NULL;
  int errCode = 1;
  char pBueffel[256];

  GetLambda(self->hkl,&lambda);
  
  newUB = calcUBFromThreeReflections(self->r1, self->r2, self->r3,
				     lambda, &errCode);
  if(newUB == NULL){
    switch(errCode){
    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;

  status = cellFromUB(self->UB,&direct);
  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," %8.4f %8.4f %8.4f %6.2f %6.2f %6.2f",
	     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],"cell") == 0){
    if(argc > 3){
        return readCell(pCon, self, argc, argv);
    } else {
        snprintf(pBuffer,511,"ubcalc cell = %f %f %f %f %f %f",
            self->direct.a, self->direct.b, self->direct.c,
            self->direct.alpha, self->direct.beta,self->direct.gamma);
        SCWrite(pCon,pBuffer,eValue);
        return 1;          
    }
  } else if(strcmp(argv[1],"ref1") == 0){
    return readReflection(pCon,pSics, &self->r1,argc,argv);
  }  else if(strcmp(argv[1],"ref2") ==0){
    return readReflection(pCon,pSics, &self->r2,argc,argv);
  } else if(strcmp(argv[1],"ref3") == 0){
    return readReflection(pCon,pSics, &self->r3,argc,argv);
  } else if(strcmp(argv[1],"listub") == 0){
    listUB(pCon,self->UB);
    return 1;
  } else if(strcmp(argv[1],"ub2ref") == 0){
    return calcUB(self,pCon);
  } else if(strcmp(argv[1],"ub3ref") == 0){
    return calcUB3Ref(self,pCon);
  } else if(strcmp(argv[1],"cellub") == 0){
    return cellFromUBWrapper(self,pCon);
  } else if(strcmp(argv[1],"listcell") == 0){
    listCell(pCon,argv[0],self->direct);
    return 1;
  } else if(strcmp(argv[1],"listref1") == 0){
    listReflection(pCon,argv[0],"ref1",self->r1);
    return 1;
  } else if(strcmp(argv[1],"listref2") == 0){
    listReflection(pCon,argv[0],"ref2",self->r2);
    return 1;
  } else if(strcmp(argv[1],"listref3") == 0){
    listReflection(pCon,argv[0],"ref3",self->r3);
    return 1;
  } else if(strcmp(argv[1],"list") == 0){
    listCell(pCon,argv[0],self->direct);
    listReflection(pCon,argv[0],"ref1",self->r1);
    listReflection(pCon,argv[0],"ref2",self->r2);
    listReflection(pCon,argv[0],"ref3",self->r3);
    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;
    }
}