/**
 * A special module for doing SANSLI rebinning. This is a fudge to make 
 * the data from the new detector electronics look like the old one.
 * 
 * copyright: see file COPYRIGHT
 *
 * Mark Koennecke, May 2007
 */

#include <stdlib.h>
#include <assert.h>
#include <sics.h>
#include <splitter.h>
#include <math.h>
#include <sicsdata.h>
#include <motor.h>
#include "rebin.h"
/*---------------------------------------------------------------------------*/
static char correctionFilePath[512];
static float runde(float v)
{
  float ip, rund;

  rund = modff(v, &ip);
  if (rund > .5) {
    ip += 1.;
  }
  return ip;
}

/*---------------------------------------------------------------------------*/
static int loadCorrectionData(SConnection * pCon, double *xPos,
                              double *yPos, int length)
{
  pMotor dist = NULL;
  FILE *fd = NULL;
  float distance;
  char filename[1024];
  int i;

  dist = (pMotor) FindCommandData(pServ->pSics, "dz", "Motor");
  if (dist == NULL) {
    SCWrite(pCon, "ERROR: dz motor not found!, cannot correct", eError);
    return 0;
  }
  MotorGetSoftPosition(dist, pCon, &distance);
  snprintf(filename, 1023, "%s/%1.1dm-targetpos-final.txt",
           correctionFilePath, (int) runde(distance));
  fd = fopen(filename, "r");
  if (fd == NULL) {
    SCWrite(pCon, "ERROR: correction file not found!", eError);
    SCWrite(pCon, filename, eError);
    return 0;
  }
  for (i = 0; i < length; i++) {
    fscanf(fd, "%lf %lf", xPos + i, yPos + i);
  }
  fclose(fd);
  return 1;
}
/*---------------------------------------------------------------------------*/
static double sansround(double d)
{
	double frac, low;

	low = floor(d);
	frac = d - low;
	if(frac > .5){
		return low +1;
	} else {
		return low;
	}
}
/*---------------------------------------------------------------------------*/
typedef struct {
	int idx;
	double frac;
}MediSort;
/*---------------------------------------------------------------------------*/
static int MediCompare(const void *v1, const void *v2)
{
	MediSort *m1, *m2;
	
	m1 = (MediSort *)v1;
	m2 = (MediSort *)v2;
	if(m1->frac > m2->frac){
		return -1;
	} else if (m1->frac == m2->frac ){
		return 0;
	} else {
		return 1;
	}
}
/*---------------------------------------------------------------------------*/
int SansliRebin(SConnection * pCon, SicsInterp * pSics, void *pData,
                int argc, char *argv[])
{
  pSICSData target = NULL, hmData = NULL;
  pNXDS dataset = NULL, weights = NULL;
  int ix, iy, pos, ival, xDetDim[2], yDetDim[2], nDetX, nDetY,
      posIdx;
  int64_t iDim[2];
  long totalCounts = 0;
  double x, y, val, *xPos = NULL, *yPos = NULL, corrSum = .0, doubleCounts, sumFrac;
  double low, frac;
  float detectorDistance;
  MediSort *sortData;
  
  if (argc < 2) {
    SCWrite(pCon, "ERROR: Not enough arguments", eError);
    return 0;
  }

  xDetDim[0] = 108;
  xDetDim[1] = 413;
  nDetX = xDetDim[1] - xDetDim[0];
  yDetDim[0] = 155;
  yDetDim[1] = 365;
  nDetY = yDetDim[1] - yDetDim[0];

  hmData = (pSICSData) FindCommandData(pSics, argv[1], "SICSData");
  if (hmData == NULL) {
    SCWrite(pCon, "ERROR: histogram memory data not found", eError);
    return 0;
  }
  target = (pSICSData) FindCommandData(pSics, argv[2], "SICSData");
  if (target == NULL) {
    SCWrite(pCon, "ERROR: target sicsdata not found", eError);
    return 0;
  }
  iDim[0] = 128;
  iDim[1] = 128;
  dataset = createNXDataset(2, NX_FLOAT64, iDim);
  weights = createNXDataset(2, NX_FLOAT64, iDim);
  xPos = malloc(nDetX * nDetY * sizeof(double));
  yPos = malloc(nDetX * nDetY * sizeof(double));
  sortData = malloc(iDim[0]*iDim[1]*sizeof(MediSort));
  if (dataset == NULL || weights == NULL || xPos == NULL || yPos == NULL || sortData == NULL) {
    SCWrite(pCon, "ERROR: out of memory allocating temporary data",
            eError);
    return 0;
  }
  memset(xPos, 0, nDetX * nDetY * sizeof(double));
  memset(yPos, 0, nDetX * nDetY * sizeof(double));
  if (!loadCorrectionData(pCon, xPos, yPos, nDetX * nDetY)) {
    dropNXDataset(dataset);
    dropNXDataset(weights);
    free(xPos);
    free(yPos);
    return 0;
  }

  for (ix = xDetDim[0]; ix < xDetDim[1]; ix++) {
    for (iy = yDetDim[0]; iy < yDetDim[1]; iy++) {
      posIdx = (iy - yDetDim[0]) * nDetX + (ix - xDetDim[0]);
      x = yPos[posIdx];
      y = xPos[posIdx];
      getSICSDataInt(hmData, 512 * iy + ix, &ival);
      totalCounts += ival;
      val = (double) ival;
      rebinPoint2D(dataset, x, y, val, TRILINEAR);
      rebinPoint2D(weights, x, y, 1.0, TRILINEAR);
    }
  }

  pos = 0;
  clearSICSData(target);
  if (argc > 3) {
    setSICSDataInt(target, 0, 128);
    setSICSDataInt(target, 1, 128);
    pos = 2;
  }
  /*
   * apply weights
   */
  for (ix = 0; ix < 128 * 128; ix++) {
    if (weights->u.dPtr[ix] > .0) {
      dataset->u.dPtr[ix] /= weights->u.dPtr[ix];
    }
    corrSum += dataset->u.dPtr[ix];
  }
  doubleCounts = (double) totalCounts;
  
  /*
   * distribute counts
   */
  sumFrac = .0;
  for (ix = 0; ix < 128 * 128; ix++, pos++) {
    if (corrSum > .01) {
      val = dataset->u.dPtr[ix] * doubleCounts / corrSum;
      low = floor(val);
      frac = val - low;
      sumFrac += frac;
      val = low;
      sortData[ix].idx = pos;
      sortData[ix].frac = frac;
    } else {
      val = .0;
      frac = .0;
    }
    setSICSDataInt(target, pos, (int) val);
  }
  /*
   * apply median correction
   */
  qsort(sortData, iDim[0]*iDim[1], sizeof(MediSort), MediCompare);
  ix = 0;
  while(sumFrac > .0){
	  pos = sortData[ix].idx;
	  getSICSDataInt(target,pos,&ival);
	  setSICSDataInt(target,pos,ival+1);
	  ix++;
	  sumFrac -= 1.;
  }
  
  dropNXDataset(dataset);
  dropNXDataset(weights);
  free(xPos);
  free(yPos);
  free(sortData);
  SCSendOK(pCon);
  return 1;
}

/*---------------------------------------------------------------------------*/
int MakeSansliRebin(SConnection * pCon, SicsInterp * pSics, void *pData,
                    int argc, char *argv[])
{
  if (argc < 2) {
    SCWrite(pCon, "ERROR: need path to correction files as a parameter",
            eError);
    return 0;
  }
  strlcpy(correctionFilePath, argv[1], 512);
  AddCommand(pSics, "sanslirebin", SansliRebin, NULL, NULL);
  SCSendOK(pCon);
  return 1;
}