/*-------------------------------------------------------------------------
                   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) {
        (void)*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) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: histogram memory %s not found", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  if (!pCom->pData) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: histogram memory %s not found", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  *pHM = (pHistMem) pCom->pData;
  if (!iHasType(*pHM, "HistMem")) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "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("@"));
          }
          snprintf(pNum,sizeof(pNum)-1, "%d ", i);
          Tcl_DStringAppend(&result, pNum, strlen(pNum));
          snprintf(pNum,sizeof(pNum)-1, "%d ", j);
          Tcl_DStringAppend(&result, pNum, strlen(pNum));
          snprintf(pNum,sizeof(pNum)-1, "%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) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "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) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: expected number, got %s", argv[2]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    if (Tcl_GetInt(pSics->pTcl, argv[4], &j) != TCL_OK) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "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;
    }
    snprintf(pNum,sizeof(pNum)-1, "%d ", i);
    Tcl_DStringAppend(&result, pNum, strlen(pNum));
    snprintf(pNum,sizeof(pNum)-1, "%d ", j);
    Tcl_DStringAppend(&result, pNum, strlen(pNum));
    snprintf(pNum,sizeof(pNum)-1, "%d ", intensity);
    Tcl_DStringAppend(&result, pNum, strlen(pNum));
    snprintf(pNum,sizeof(pNum)-1, "%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) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "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) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: expected number, got %s", argv[2]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    if (Tcl_GetInt(pSics->pTcl, argv[4], &j) != TCL_OK) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: expected number, got %s", argv[2]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    if (Tcl_GetInt(pSics->pTcl, argv[5], &badMax) != TCL_OK) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "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);
    snprintf(pBueffel,sizeof(pBueffel)-1, "%5d", iRet);
    SCWrite(pCon, pBueffel, eValue);
    return 1;
  } else if (strcmp(argv[1], "stat") == 0) {
    if (argc < 3) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "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);
    snprintf(pBueffel,sizeof(pBueffel)-1, " %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) {
        snprintf(pBueffel,sizeof(pBueffel)-1, "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) {
        snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: parameter %s not found or wrong command",
                argv[1]);
        SCWrite(pCon, pBueffel, eError);
        return 0;
      }
      snprintf(pBueffel,sizeof(pBueffel)-1, "%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);
}