/**
 * This is a little routine to calculate the beam center from SANS data 
 * in the detector. This is basically a COG or COC (center of contour) 
 * calculation above a  given threshold. Where we make an effor to 
 * find the threshold ourselves.
 * 
 * copyright: GPL
 * 
 * Mark Koennecke, July 2009
 */
#include <math.h>
#include <sics.h>
#include "HistMem.h"
#include "sicsobj.h"
#include "sicshipadaba.h"

extern float nintf(float x);
/*---------------------------------------------------------------------*/
static int calculateCOG(int *iData, int xsize, int ysize,
                 float *i, float *j, int *intensity, float contour, 
                 float level)
{
  int threshold, nCount;
  float cogTotal, cogX, cogY, conMin, conMax;
  int *iPtr, x, y;

  conMin = contour - (contour *level);
  conMax = contour + (contour *level);
  
  /*
     build the sums
   */
  nCount = 0;
  cogTotal = cogY = cogX = .0;
  for (y = 0; y < ysize; y++) {
    iPtr = iData + y * xsize;
    for (x = 0; x < xsize; x++) {
      if (iPtr[x] > conMin && iPtr[x] < conMax) {
        nCount++;
        cogTotal += iPtr[x];
        cogY += y * iPtr[x];
        cogX += x * iPtr[x];
      }
    }
  }
  if (cogTotal <= .0) {
    return 0;
  }

  *i = cogX / cogTotal;
  *j = cogY / cogTotal;
  *intensity = (int) cogTotal;

  return 1;
}
/*------------------------------------------------------------------*/
static int calculateCOC(int *iData, int xsize, int ysize,
                 float *i, float *j, int *intensity, float contour)
{
  int threshold, nCount;
  float cogTotal, cogX, cogY;
  int *iPtr, x, y;

  /*
     build the sums
   */
  nCount = 0;
  cogTotal = cogY = cogX = .0;
  for (y = 0; y < ysize; y++) {
    iPtr = iData + y * xsize;
    for (x = 0; x < xsize; x++) {
      if (iPtr[x] > contour) {
        nCount++;
        cogTotal += 1;
        cogY += y * 1;
        cogX += x * 1;
      }
    }
  }
  if (cogTotal <= .0) {
    return 0;
  }

  *i = cogX / cogTotal;
  *j = cogY / cogTotal;
  *intensity = (int) cogTotal;

  return 1;
}
/*-------------------------------------------------------------------------*/
static int COGCmd(pSICSOBJ self, SConnection *pCon, pHdb commandNode,
                  pHdb par[], int nPar)
{
	pHistMem hm = NULL;
	float contour, level, x = .0, y = .0;
	int length, intensity, dim[3];
		
	if(nPar < 3){
		SCWrite(pCon,"ERROR: need hm-name, contour and level for COG calculation", 
				eError);
		return 0;
	}
	
	hm  = FindCommandData(pServ->pSics, 
			par[0]->value.v.text, "HistMem");
	if(hm == NULL){
		SCPrintf(pCon,eError,"ERROR: hm %s not found", par[0]->value.v.text);
		return 0;
	}
	contour = par[1]->value.v.doubleValue;
	level = par[2]->value.v.doubleValue;
	
	
	GetHistDim(hm, dim, &length);
	if(length != 2) {
		SCPrintf(pCon,eError,"ERROR: hm %s is not 2D", par[0]->value.v.text);
		return 0;
	}
	
	calculateCOG(GetHistogramPointer(hm,pCon), 
			dim[0], dim[1], &x, &y, &intensity, contour, level);
	
	SCPrintf(pCon,eValue,"BC = %.2f,%.2f", x,y);
	return 1;
}
/*-------------------------------------------------------------------------*/
static int COCCmd(pSICSOBJ self, SConnection *pCon, pHdb commandNode,
                  pHdb par[], int nPar)
{
	pHistMem hm = NULL;
	float contour, x, y;
	int length, intensity, dim[3];
		
	if(nPar < 2){
		SCWrite(pCon,"ERROR: need hm-name and contour for COG calculation", 
				eError);
		return 0;
	}
	
	hm  = FindCommandData(pServ->pSics, 
			par[0]->value.v.text, "HistMem");
	if(hm == NULL){
		SCPrintf(pCon,eError,"ERROR: hm %s not found", par[0]->value.v.text);
		return 0;
	}
	contour = par[1]->value.v.doubleValue;
	
	GetHistDim(hm, dim, &length);
	if(length != 2) {
		SCPrintf(pCon,eError,"ERROR: hm %s is not 2D", par[0]->value.v.text);
		return 0;
	}
	
	calculateCOC(GetHistogramPointer(hm,pCon), 
			dim[0], dim[1], &x, &y, &intensity, contour);
	
	SCPrintf(pCon,eValue,"BC = %.2f,%.2f", x,y);
	return 1;
}
/*-------------------------------------------------------------------------*/
static int StatCmd(pSICSOBJ self, SConnection *pCon, pHdb commandNode,
                  pHdb par[], int nPar)
{
	pHistMem hm = NULL;
	long sum, max, min;
	double average;
	int dim[3], i, length;
	HistInt *data = NULL;
	
	if(nPar < 1){
		SCWrite(pCon,"ERROR: need hm-name for statistics", 
				eError);
		return 0;
	}
	
	hm  = FindCommandData(pServ->pSics, 
			par[0]->value.v.text, "HistMem");
	if(hm == NULL){
		SCPrintf(pCon,eError,"ERROR: hm %s not found", par[0]->value.v.text);
		return 0;
	}
	
	GetHistDim(hm, dim, &i);
	if(i != 2) {
		SCPrintf(pCon,eError,"ERROR: hm %s is not 2D", par[0]->value.v.text);
		return 0;
	}
	data = GetHistogramPointer(hm,pCon);
	if(data == NULL){
		SCPrintf(pCon,eError,"ERROR: failed to get data for hm %s", par[0]->value.v.text);
		return 0;
	}
	max =  -9999;
	min = 99999.99;
	length = dim[0]*dim[1];
	for(i = 0, sum = 0; i < length; i++){
		sum += data[i];
		if(data[i] > max){
			max = data[i];
		}
		if(data[i] < min ){
			min = data[i];
		}
	}
	average = (double)sum/(double)length;
	
	SCPrintf(pCon,eValue,"Stat:sum,max,min,av = %ld,%ld,%ld,%f", sum,max,min,average);
	return 1;
}

/*--------------------------------------------------------------------*/
int SansBCFactory(SConnection *pCon, SicsInterp *pSics, void *pData, 
		int argc, char *argv[])
{
	pSICSOBJ pNew = NULL;
	pHdb cmd = NULL, node;
	
	pNew = SetupSICSOBJ(pCon,pSics,pData,argc,argv);
	if(pNew == NULL){
		return 0;
	}
	
	cmd = AddSICSHdbPar(pNew->objectNode, "cog", usSpy, 
			MakeSICSFunc(COGCmd));
	SetHdbProperty(cmd,"type","command");
	SetHdbProperty(cmd,"priv","spy");
	node = MakeSICSHdbPar("hm",usSpy,MakeHdbText("banana"));
	AddHipadabaChild(cmd,node,NULL);
	node = MakeSICSHdbPar("contour",usSpy,MakeHdbFloat(100.));
	AddHipadabaChild(cmd,node,NULL);
	node = MakeSICSHdbPar("level",usSpy,MakeHdbFloat(.1));
	AddHipadabaChild(cmd,node,NULL);

	cmd = AddSICSHdbPar(pNew->objectNode, "coc", usSpy, 
			MakeSICSFunc(COCCmd));
	SetHdbProperty(cmd,"type","command");
	SetHdbProperty(cmd,"priv","spy");
	node = MakeSICSHdbPar("hm",usSpy,MakeHdbText("banana"));
	AddHipadabaChild(cmd,node,NULL);
	node = MakeSICSHdbPar("contour",usSpy,MakeHdbFloat(100.));
	AddHipadabaChild(cmd,node,NULL);

	cmd = AddSICSHdbPar(pNew->objectNode, "stat", usSpy, 
			MakeSICSFunc(StatCmd));
	SetHdbProperty(cmd,"type","command");
	SetHdbProperty(cmd,"priv","spy");
	node = MakeSICSHdbPar("hm",usSpy,MakeHdbText("banana"));
	AddHipadabaChild(cmd,node,NULL);
	
	return 1;
}
/*-------------------------------------------------------------------*/