/*-------------------------------------------------------------------------
                   L o c a l     M a x i m u m

 This is a module for searching a local maximum in a 2D histogram as
 collected at TRICS.

 copyright: see copyright.h

 Mark Koennecke, November 2001
-------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <tcl.h>
#include <math.h>
#include "fortify.h"
#include "sics.h"
#include "lomax.h"
#include "lomax.i"
#include "HistMem.h"

#define WINDOW 0
#define THRESHOLD 1
#define STEEPNESS 2
#define COGWINDOW 3
#define COGCONTOUR 4

extern float nintf(float f);

/*-------------------------------------------------------------------*/
static int testBoundaries(int xsize, int ysize, int window, 
                          int i, int j)
{
  int half;

  /*
    if the window touches the data boundary the result is probably not
    very useful. Discarding these cases early on also protects us 
    against data array overrun problems.
  */ 

  half = window/2;

  if( i - half < 0 ||  i + half > xsize)
    return 0;

  if( j - half < 0 || j + half > ysize)
    return 0;

  return 1;
}
/*-------------------------------------------------------------------*/
static int testSteepness(int *iData, int xsize, int ysize,
			 int i, int j, int window, int steepness)
{
  int testValue, x, y, half;
  int *iPtr;
  

  testValue = iData[j * xsize + i] - steepness;
  half = window/2;

  /*
    test upper row
  */
  iPtr = iData + (j - half) * xsize + i - half;
  for(x = 0; x < window; x++)
  {
    if(iPtr[x] > testValue)
      return 0;
  }

  /*
    test lower row
  */
  iPtr = iData + (j + half) * xsize + i - half;
  for(x = 0; x < window; x++)
  {
    if(iPtr[x] > testValue)
      return 0;
  }

  /*
    test columns
  */
  for(y = j - half; y < j + half; y++)
  {
    /*
      left
    */
    if(iData[y*xsize + i - half] > testValue)
      return 0;
    /*
      right
    */
    if(iData[y*xsize + i + half] > testValue)
      return 0;
  }
 
  return 1;
}

/*--------------------------------------------------------------------*/
static int testMaximum(int *iData, int xsize, int ysize, 
		       int i, int j, int window)
{
  int testValue, x, y, half;
  int *iPtr;
  int equalCount = 0;

  testValue = iData[j * xsize + i];
  half = window/2;

  for(y = j - half; y < j + half; y++)
  {
    iPtr = iData + y * xsize + i - half;
    for(x = 0; x < window; x++)
    {
      if(iPtr[x] > testValue)
	return 0;
      if(iPtr[x] == testValue)
	equalCount++;
    }
  }
  /*
  if(equalCount > 3)
  {
    return 0;
  }
  */
  return 1;
} 
/*--------------------------------------------------------------------*/
int testLocalMaximum(int *iData, int xsize, int ysize, 
                     int i, int j, 
                     int window, int steepness, int threshold, 
                     int *intensity)
{

  if(!testBoundaries(xsize,ysize,window,i,j))
    return 0;


  if(!testMaximum(iData,xsize,ysize,i,j,window))
    return 0;

  if(iData[j * xsize + i] < threshold)
    return 0;

  if(!testSteepness(iData,xsize,ysize,i,j,window, steepness))
    return 0;

  *intensity = iData[j * xsize + i];

  return 1;
} 
/*-------------------------------------------------------------------*/
int calculateCOG(int *iData, int xsize, int ysize,
		 int *i, int *j, int *intensity,int *nCount,
                  int cogWindow, 
                  float contour)
{
  int x, xLow, xMax, y, yLow, yMax;
  int threshold;
  float cogTotal, cogX, cogY;
  int *iPtr;

  if(!testBoundaries(xsize,ysize,cogWindow,*i,*j))
    return 0;

  /*
    preparations
  */
  xLow = *i - cogWindow/2;
  if(xLow < 0)
    xLow = 0;
  xMax = *i + cogWindow/2;
  if(xLow >= xsize)
    xMax = xsize-1;

  yLow = *j - cogWindow/2;
  if(yLow < 0)
    yLow = 0;
  yMax = *j + cogWindow/2;
  if(yLow >= ysize)
    yMax = ysize-1;

  threshold = (int)(float)iData[*j * xsize + *i] * contour;

  /*
    build the sums
  */
  *nCount = 0;
  cogTotal = cogY = cogX = .0;
  for(y = yLow; y < yMax; y++)
  {
    iPtr = iData + y *xsize;
    for(x = xLow; x < xMax; x++)
    {
      if(iPtr[x] > threshold)
      {
	*nCount++;
	cogTotal += iPtr[x];
	cogY += y * iPtr[x];
        cogX += x * iPtr[x];
      }
    }
  }
  if(cogTotal <= .0)
  {
    return 0;
  }

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

  return 1;
}
/*-------------------------------------------------------------------*/
void calculateStatistics(int *iData, int xsize, int ysize,
			float *average, float *maximum)
{
  int i, iLength;
  int max = -999999999;
  long sum = 0;

  iLength = xsize*ysize;
  for(i = 0; i < iLength; i++)
  {
    sum += iData[i];
    if(iData[i] > max)
      max = iData[i];
  }
  *average = (float)sum/(float)iLength;
  *maximum = (float)max;
}
/*-------------------------------------------------------------------*/
int wellFormed(int *iData, int xsize, int ysize,
			 int i, int j, int window, float contour, 
                         int maxBad)
{
  int testValue, x, y, half;
  int *iPtr;
  int badCount = 0;  


  testValue = (int)((float)iData[j * xsize + i]*contour);
  half = window/2;

  /*
    test upper row
  */
  iPtr = iData + (j - half) * xsize + i - half;
  for(x = 0; x < window; x++)
  {
    if(iPtr[x] > testValue)
      badCount++;
  }

  /*
    test lower row
  */
  iPtr = iData + (j + half) * xsize + i - half;
  for(x = 0; x < window; x++)
  {
    if(iPtr[x] > testValue)
      badCount++;
  }

  /*
    test columns
  */
  for(y = j - half; y < j + half; y++)
  {
    /*
      left
    */
    if(iData[y*xsize + i - half] > testValue)
      badCount++;
    /*
      right
    */
    if(iData[y*xsize + i + half] > testValue)
      badCount++;
  }
 
  if(badCount > maxBad)
  {
    return 0;
  }

  return 1;
}

/*-------------------------------------------------------------------*/
static int checkHM(pHistMem *pHM, SicsInterp *pSics, SConnection *pCon,
                   char *name, int *iDim)
{
    CommandList *pCom = NULL;
    char pBueffel[256];
    int nDim;

    pCom = FindCommand(pSics, name);
    if(!pCom)
    {
      sprintf(pBueffel,"ERROR: histogram memory %s not found", name);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }        
    if(!pCom->pData)
    {
      sprintf(pBueffel,"ERROR: histogram memory %s not found", name);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }        
    *pHM = (pHistMem)pCom->pData;
    if(!iHasType(*pHM, "HistMem"))
    {
      sprintf(pBueffel,"ERROR: %s is no histogram memory!", name);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }    
    /*
      we now know that we have a histogram memory, now check Sizes
    */
    GetHistDim(*pHM,iDim,&nDim);
    if(nDim < 2)
    {
      sprintf(pBueffel,"ERROR: %s is not 2 dimensional!", name);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }

    return 1;
}
/*--------------------------------------------------------------------*/
int LoMaxAction(SConnection *pCon, SicsInterp *pSics,
		  void *pData, int argc, char *argv[])
{
  pLoMax self = NULL;
  char pBueffel[256], pNum[20];
  int iRet, i, j, intensity, count, badMax;
  int iDim[10], nDim;
  pHistMem pHM = NULL;
  CommandList *pCom = NULL;
  Tcl_DString result;
  int window;
  int *iData;
  double dVal;
  ObPar *ob = NULL;  
  float average, maximum;

  self = (pLoMax)pData;
  assert(pCon);
  assert(pSics);
  assert(self);

  /*
    we need arguments
  */
  if(argc < 2)
  {
    sprintf(pBueffel,"ERROR: insufficient number of arguments to %s",
	    argv[0]);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }

  /*
    interpret arguments
  */
  strtolower(argv[1]);
  if(strcmp(argv[1],"search") == 0)
  {
    if(argc < 3)
    {
      sprintf(pBueffel,"ERROR: insufficient number of arguments to %s.search",
	      argv[0]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    if(!checkHM(&pHM, pSics,pCon, argv[2],iDim))
    {
      return 0;
    }
    Tcl_DStringInit(&result);
    iData = GetHistogramPointer(pHM,pCon);
    window = (int)ObVal(self->pParam,WINDOW);
    count = 0;
    for(i = 0 + window/2; i < iDim[0] - window/2; i++)
    {
      for(j = 0 + window/2; j < iDim[1] - window/2; j++)
      {
	if(testLocalMaximum(iData,iDim[0], iDim[1],
			    i,j,
			    (int)ObVal(self->pParam,WINDOW),
			    (int)ObVal(self->pParam,STEEPNESS),
			    (int)ObVal(self->pParam,THRESHOLD),
                            &intensity))
	{
          if(count != 0)
	  {
	    Tcl_DStringAppend(&result,"@",strlen("@"));
          }
	  sprintf(pNum,"%d ", i);
	  Tcl_DStringAppend(&result,pNum,strlen(pNum));
	  sprintf(pNum,"%d ", j);
	  Tcl_DStringAppend(&result,pNum,strlen(pNum));
	  sprintf(pNum,"%d", intensity);
	  Tcl_DStringAppend(&result,pNum,strlen(pNum));
          count++;
        }
      } 
    }
    SCWrite(pCon,Tcl_DStringValue(&result),eValue);
    Tcl_DStringFree(&result);
    return 1;
  }
  else if(strcmp(argv[1],"cog") == 0) /* COG calculation */
  {
    if(argc < 5)
    {
      sprintf(pBueffel,"ERROR: insufficient number of arguments to %s.cog",
	      argv[0]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    if(!checkHM(&pHM, pSics,pCon, argv[2],iDim))
    {
      return 0;
    }
    if(Tcl_GetInt(pSics->pTcl,argv[3],&i)!= TCL_OK)
    {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
    }
    if(Tcl_GetInt(pSics->pTcl,argv[4],&j)!= TCL_OK)
    {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
    }
    Tcl_DStringInit(&result);
    iData = GetHistogramPointer(pHM,pCon);
    window = (int)ObVal(self->pParam,COGWINDOW);
    iRet = calculateCOG(iData,iDim[0], iDim[1], &i, &j, &intensity,&count,
			window, ObVal(self->pParam,COGCONTOUR));
    if(!iRet)
    {
      SCWrite(pCon,"ERROR: no intensity in data",eError);
      return 0;
    }
    sprintf(pNum,"%d ", i);
    Tcl_DStringAppend(&result,pNum,strlen(pNum));
    sprintf(pNum,"%d ", j);
    Tcl_DStringAppend(&result,pNum,strlen(pNum));
    sprintf(pNum,"%d ", intensity);
    Tcl_DStringAppend(&result,pNum,strlen(pNum));
    sprintf(pNum,"%d ", count);
    Tcl_DStringAppend(&result,pNum,strlen(pNum));
    SCWrite(pCon,Tcl_DStringValue(&result),eValue);
    Tcl_DStringFree(&result);
    return 1;
  }
  else if(strcmp(argv[1],"wellformed") == 0) /*  test for wellformedness  */
  {
    if(argc < 6)
    {
      sprintf(pBueffel,
       "ERROR: insufficient number of arguments to %s.wellformed",
	      argv[0]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    if(!checkHM(&pHM, pSics,pCon, argv[2],iDim))
    {
      return 0;
    }
    if(Tcl_GetInt(pSics->pTcl,argv[3],&i)!= TCL_OK)
    {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
    }
    if(Tcl_GetInt(pSics->pTcl,argv[4],&j)!= TCL_OK)
    {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
    }
    if(Tcl_GetInt(pSics->pTcl,argv[5],&badMax)!= TCL_OK)
    {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
    }
    iData = GetHistogramPointer(pHM,pCon);
    window = (int)ObVal(self->pParam,COGWINDOW);
    iRet = wellFormed(iData,iDim[0], iDim[1], i, j,
			window, ObVal(self->pParam,COGCONTOUR),
                        badMax);
    sprintf(pBueffel,"%5d", iRet);
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  }
  else if(strcmp(argv[1],"stat") == 0)
  {
    if(argc < 3)
    {
      sprintf(pBueffel,"ERROR: insufficient number of arguments to %s.search",
	      argv[0]);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }
    if(!checkHM(&pHM, pSics,pCon, argv[2],iDim))
    {
      return 0;
    }
    iData = GetHistogramPointer(pHM,pCon);
    calculateStatistics(iData,iDim[0],iDim[1],&average,&maximum);
    sprintf(pBueffel," %f %f", average, maximum);
    SCWrite(pCon,pBueffel,eValue);
    return 1;
  }
  else 
  {
    /* we are handling one of the parameter commands
     */
    if(argc > 2) /* set case */
    {
      if(Tcl_GetDouble(pSics->pTcl,argv[2],&dVal) != TCL_OK)
      {
	sprintf(pBueffel,"ERROR: expected number, got %s",argv[2]);
	SCWrite(pCon,pBueffel,eError);
	return 0;
      }
      return ObParSet(self->pParam,argv[0],argv[1],(float)dVal,pCon);
    }
    else /* read case */
    {
      ob = ObParFind(self->pParam,argv[1]);
      if(!ob)
      {
	sprintf(pBueffel,"ERROR: parameter %s not found or wrong command",
		argv[1]);
	SCWrite(pCon,pBueffel,eError);
        return 0;
      }
      sprintf(pBueffel,"%s.%s = %f",argv[0],argv[1],ob->fVal);
      SCWrite(pCon,pBueffel,eError);
      return 1;
    }
  }
  /*
    not reached
  */
  assert(0);
  return 0;
}
/*-------------------------------------------------------------------*/
static void KillLoMax(void *pData)
{
  pLoMax self = (pLoMax)pData;
  if(!self)
    return;

  if(self->pDes)
    DeleteDescriptor(self->pDes);
  if(self->pParam)
    ObParDelete(self->pParam);
  free(self);
}
/*-------------------------------------------------------------------*/
int LoMaxFactory(SConnection *pCon, SicsInterp *pSics,
		  void *pData, int argc, char *argv[])
{
  pLoMax pNew = NULL;

  if(argc < 2)
  {
    SCWrite(pCon,"ERROR: Insufficient number of arguments to LoMaxfactory",
	    eError);
    return 0;
  }
  
  pNew = (pLoMax)malloc(sizeof(LoMax));
  if(!pNew)
  {
    SCWrite(pCon,"ERROR: out of memory creating local maximum searcher",
            eError);
    return 0;
  }
  memset(pNew,0,sizeof(LoMax));

  /*
    create Descriptor
  */
  pNew->pDes = CreateDescriptor("Local Maximum Detector");
  if(!pNew->pDes)
  {
    KillLoMax(pNew);
    SCWrite(pCon,"ERROR: out of memory creating local maximum searcher",
            eError);
    return 0;
  }

  /*
    create and install parameters
  */
  pNew->pParam = ObParCreate(5);
  if(!pNew->pParam)
  {
    KillLoMax(pNew);
    SCWrite(pCon,"ERROR: out of memory creating local maximum searcher",
            eError);
    return 0;
  }
  ObParInit(pNew->pParam,WINDOW,"window",10,usUser);
  ObParInit(pNew->pParam,THRESHOLD,"threshold",30,usUser);
  ObParInit(pNew->pParam,STEEPNESS,"steepness",5,usUser);
  ObParInit(pNew->pParam,COGWINDOW,"cogwindow",50,usUser);
  ObParInit(pNew->pParam,COGCONTOUR,"cogcontour",.2,usUser);

  return AddCommand(pSics,
		    argv[1],
		    LoMaxAction,
		    KillLoMax,
		    pNew);
}