/** @file  pmPeaks.c
 *
 *  This file defines functions to detect and manipulate peaks in images
 *
 *  @author GLG, MHPCC
 *  @author EAM, IfA: significant modifications.
 *
 *  @version $Revision: 1.1.2.2 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-03-09 03:14:23 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 */

#include <stdio.h>
#include <math.h>
#include <string.h>
#include "pslib.h"
#include "pmPeaks.h"

/******************************************************************************
myListAddPeak(): A private function which allocates a psArray, if the list
argument is NULL, otherwise it adds the peak to that list.
XXX EAM : changed the output to psArray
XXX EAM : Switched row, col args
XXX EAM : NOTE: this was changed in the call, so the new code is consistent
*****************************************************************************/
static psArray *myListAddPeak(psArray *list,
                              psS32 row,
                              psS32 col,
                              psF32 counts,
                              pmPeakType type)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    pmPeak *tmpPeak = pmPeakAlloc(col, row, counts, type);

    if (list == NULL) {
        list = psArrayAlloc(100);
        list->n = 0;
    }
    psArrayAdd(list, 100, tmpPeak);
    psFree (tmpPeak);
    // XXX EAM : is this free appropriate?  (does psArrayAdd increment memory counter?)

    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return(list);
}

/******************************************************************************
getRowVectorFromImage(): a private function which simply returns a
psVector containing the specified row of data from the psImage.
 
XXX: Is there a better way to do this?  
XXX EAM: does this really need to alloc a new vector???
*****************************************************************************/
static psVector *getRowVectorFromImage(psImage *image,
                                       psU32 row)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_IMAGE_NON_NULL(image, NULL);
    PS_ASSERT_IMAGE_TYPE(image, PS_TYPE_F32, NULL);

    psVector *tmpVector = psVectorAlloc(image->numCols, PS_TYPE_F32);
    for (psU32 col = 0; col < image->numCols ; col++) {
        tmpVector->data.F32[col] = image->data.F32[row][col];
    }
    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return(tmpVector);
}

/******************************************************************************
isItInThisRegion(): a private function which simply returns a
boolean denoting if specified coordinate is in the region.
XXX: Macro this.
*****************************************************************************/
static bool isItInThisRegion(const psRegion valid,
                             psS32 x,
                             psS32 y)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    if ((x >= valid.x0) &&
            (x <= valid.x1) &&
            (y >= valid.y0) &&
            (y <= valid.y1)) {
        psTrace(__func__, 4, "---- %s(true) end ----\n", __func__);
        return(true);
    }
    psTrace(__func__, 4, "---- %s(false) end ----\n", __func__);
    return(false);
}

/******************************************************************************
pmPeakAlloc(): Allocate the pmPeak data structure and set appropriate members.
*****************************************************************************/
pmPeak *pmPeakAlloc(psS32 x,
                    psS32 y,
                    psF32 counts,
                    pmPeakType type)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    pmPeak *tmp = (pmPeak *) psAlloc(sizeof(pmPeak));
    tmp->x = x;
    tmp->y = y;
    tmp->counts = counts;
    tmp->type = type;

    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(tmp);
}

// psSort comparison function for peaks
int pmPeaksCompareAscend (const void **a, const void **b)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    pmPeak *A = *(pmPeak **)a;
    pmPeak *B = *(pmPeak **)b;

    psF32 diff;

    diff = A->counts - B->counts;
    if (diff < FLT_EPSILON) {
        psTrace(__func__, 3, "---- %s(-1) end ----\n", __func__);
        return (-1);
    } else if (diff > FLT_EPSILON) {
        psTrace(__func__, 3, "---- %s(+1) end ----\n", __func__);
        return (+1);
    }
    psTrace(__func__, 3, "---- %s(0) end ----\n", __func__);
    return (0);
}

// psSort comparison function for peaks
int pmPeaksCompareDescend (const void **a, const void **b)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    pmPeak *A = *(pmPeak **)a;
    pmPeak *B = *(pmPeak **)b;

    psF32 diff;

    diff = A->counts - B->counts;
    if (diff < FLT_EPSILON) {
        psTrace(__func__, 3, "---- %s(+1) end ----\n", __func__);
        return (+1);
    } else if (diff > FLT_EPSILON) {
        psTrace(__func__, 3, "---- %s(-1) end ----\n", __func__);
        return (-1);
    }
    psTrace(__func__, 3, "---- %s(0) end ----\n", __func__);
    return (0);
}

/******************************************************************************
pmFindVectorPeaks(vector, threshold): Find all local peaks in the given vector
above the given threshold.  Returns a vector of type PS_TYPE_U32 containing
the location (x value) of all peaks.
 
XXX: What types should be supported?  Only F32 is implemented.
 
XXX: We currently step through the input vector twice; once to determine the
size of the output vector, then to set the values of the output vector.
Depending upon actual use, this may need to be optimized.
*****************************************************************************/
psVector *pmFindVectorPeaks(const psVector *vector,
                            psF32 threshold)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_VECTOR_NON_NULL(vector, NULL);
    PS_ASSERT_VECTOR_NON_EMPTY(vector, NULL);
    PS_ASSERT_VECTOR_TYPE(vector, PS_TYPE_F32, NULL);
    int count = 0;
    int n = vector->n;

    //
    // Special case: the input vector has a single element.
    //
    if (n == 1) {
        psVector *tmpVector = NULL;
        ;
        if (vector->data.F32[0] > threshold) {
            tmpVector = psVectorAlloc(1, PS_TYPE_U32);
            tmpVector->data.U32[0] = 0;
        } else {
            tmpVector = psVectorAlloc(0, PS_TYPE_U32);
        }
        psTrace(__func__, 3, "---- %s() end ----\n", __func__);
        return(tmpVector);
    }

    //
    // Determine if first pixel is a peak
    //
    if ((vector->data.F32[0] > vector->data.F32[1]) &&
            (vector->data.F32[0] > threshold)) {
        count++;
    }

    //
    // Determine if interior pixels are peaks
    //
    for (psU32 i = 1; i < n-1 ; i++) {
        if ((vector->data.F32[i] > vector->data.F32[i-1]) &&
                (vector->data.F32[i] > vector->data.F32[i+1]) &&
                (vector->data.F32[i] > threshold)) {
            count++;
        }
    }

    //
    // Determine if last pixel is a peak
    //
    if ((vector->data.F32[n-1] > vector->data.F32[n-2]) &&
            (vector->data.F32[n-1] > threshold)) {
        count++;
    }

    //
    // We know how many peaks exist, so we now allocate a psVector to store
    // those peaks.
    //
    psVector *tmpVector = psVectorAlloc(count, PS_TYPE_U32);
    count = 0;

    //
    // Determine if first pixel is a peak
    //
    if ((vector->data.F32[0] > vector->data.F32[1]) &&
            (vector->data.F32[0] > threshold)) {
        tmpVector->data.U32[count++] = 0;
    }

    //
    // Determine if interior pixels are peaks
    //
    for (psU32 i = 1; i < (n-1) ; i++) {
        if ((vector->data.F32[i] > vector->data.F32[i-1]) &&
                (vector->data.F32[i] > vector->data.F32[i+1]) &&
                (vector->data.F32[i] > threshold)) {
            tmpVector->data.U32[count++] = i;
        }
    }

    //
    // Determine if last pixel is a peak
    //
    if ((vector->data.F32[n-1] > vector->data.F32[n-2]) &&
            (vector->data.F32[n-1] > threshold)) {
        tmpVector->data.U32[count++] = n-1;
    }

    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(tmpVector);
}


/******************************************************************************
pmFindImagePeaks(image, threshold): Find all local peaks in the given psImage
above the given threshold.  Returns a psArray containing location (x/y value)
of all peaks.
 
XXX: I'm not convinced the peak type definition in the SDRS is mutually
exclusive.  Some peaks can have multiple types.  Edges for sure.  Also, a
digonal line with the same value at each point will have a peak for every
point on that line.
 
XXX: This does not work if image has either a single row, or a single column.
 
XXX: In the output psArray elements, should we use the image row/column offsets?
     Currently, we do not.
XXX EAM : this function needs to return peaks in *parent* coords
 
XXX: Merge with CVS 1.20.  This had the proper code for images with a single
row or column.
 
*****************************************************************************/
psArray *pmFindImagePeaks(const psImage *image,
                          psF32 threshold)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_IMAGE_NON_NULL(image, NULL);
    PS_ASSERT_IMAGE_TYPE(image, PS_TYPE_F32, NULL);
    if ((image->numRows == 1) || (image->numCols == 1)) {
        psError(PS_ERR_UNKNOWN, true, "Currently, input image must have at least 2 rows and 2 columns.");
        psTrace(__func__, 3, "---- %s(NULL) end ----\n", __func__);
        return(NULL);
    }
    psVector *tmpRow = NULL;
    psU32 col = 0;
    psU32 row = 0;
    psArray *list = NULL;

    psU32 col0 = image->col0;
    psU32 row0 = image->row0;

    //
    // Find peaks in row 0 only.
    //
    row = 0;
    tmpRow = getRowVectorFromImage((psImage *) image, row);
    psVector *row1 = pmFindVectorPeaks(tmpRow, threshold);
    // pmFindVectorPeaks returns coords in the vector, not corrected for col0

    for (psU32 i = 0 ; i < row1->n ; i++ ) {
        col = row1->data.U32[i];
        //
        // Determine if pixel (0,0) is a peak.
        //
        if (col == 0) {
            if ( (image->data.F32[row][col] >  image->data.F32[row][col+1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row+1][col]) &&
                    (image->data.F32[row][col] >= image->data.F32[row+1][col+1])) {

                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }
        } else if (col < (image->numCols - 1)) {
            if ( (image->data.F32[row][col] >= image->data.F32[row][col-1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row][col+1]) &&
                    (image->data.F32[row][col] >= image->data.F32[row+1][col-1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row+1][col]) &&
                    (image->data.F32[row][col] >= image->data.F32[row+1][col+1])) {
                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }

        } else if (col == (image->numCols - 1)) {
            if ( (image->data.F32[row][col] >= image->data.F32[row][col-1]) &&
                    (image->data.F32[row][col] > image->data.F32[row+1][col]) &&
                    (image->data.F32[row][col] >= image->data.F32[row+1][col-1])) {
                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }

        } else {
            psError(PS_ERR_UNKNOWN, true, "peak specified valid column range.");
        }
    }
    psFree (tmpRow);
    psFree (row1);

    //
    // Exit if this image has a single row.
    //
    if (image->numRows == 1) {
        psTrace(__func__, 3, "---- %s() end ----\n", __func__);
        return(list);
    }

    //
    // Find peaks in interior rows only.
    //
    for (row = 1 ; row < (image->numRows - 1) ; row++) {
        tmpRow = getRowVectorFromImage((psImage *) image, row);
        row1 = pmFindVectorPeaks(tmpRow, threshold);

        // Step through all local peaks in this row.
        for (psU32 i = 0 ; i < row1->n ; i++ ) {
            pmPeakType myType = PM_PEAK_UNDEF;
            col = row1->data.U32[i];

            if (col == 0) {
                // If col==0, then we can not read col-1 pixels
                if ((image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                        (image->data.F32[row][col] >= image->data.F32[row-1][col+1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row][col+1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col+1])) {
                    myType = PM_PEAK_EDGE;
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], myType);
                }
            } else if (col < (image->numCols - 1)) {
                // This is an interior pixel
                if ((image->data.F32[row][col] >= image->data.F32[row-1][col-1]) &&
                        (image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                        (image->data.F32[row][col] >= image->data.F32[row-1][col+1]) &&
                        (image->data.F32[row][col] > image->data.F32[row][col-1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row][col+1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col-1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col+1])) {
                    if (image->data.F32[row][col] > threshold) {
                        if ((image->data.F32[row][col] > image->data.F32[row-1][col-1]) &&
                                (image->data.F32[row][col] > image->data.F32[row-1][col]) &&
                                (image->data.F32[row][col] > image->data.F32[row-1][col+1]) &&
                                (image->data.F32[row][col] > image->data.F32[row][col-1]) &&
                                (image->data.F32[row][col] > image->data.F32[row][col+1]) &&
                                (image->data.F32[row][col] > image->data.F32[row+1][col-1]) &&
                                (image->data.F32[row][col] > image->data.F32[row+1][col]) &&
                                (image->data.F32[row][col] > image->data.F32[row+1][col+1])) {
                            myType = PM_PEAK_LONE;
                        }

                        if ((image->data.F32[row][col] == image->data.F32[row-1][col-1]) ||
                                (image->data.F32[row][col] == image->data.F32[row-1][col]) ||
                                (image->data.F32[row][col] == image->data.F32[row-1][col+1]) ||
                                (image->data.F32[row][col] == image->data.F32[row][col-1]) ||
                                (image->data.F32[row][col] == image->data.F32[row][col+1]) ||
                                (image->data.F32[row][col] == image->data.F32[row+1][col-1]) ||
                                (image->data.F32[row][col] == image->data.F32[row+1][col]) ||
                                (image->data.F32[row][col] == image->data.F32[row+1][col+1])) {
                            myType = PM_PEAK_FLAT;
                        }

                        list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], myType);
                    }
                }
            } else if (col == (image->numCols - 1)) {
                // If col==numCols - 1, then we can not read col+1 pixels
                if ((image->data.F32[row][col] >= image->data.F32[row-1][col-1]) &&
                        (image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                        (image->data.F32[row][col] > image->data.F32[row][col-1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row][col+1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col-1]) &&
                        (image->data.F32[row][col] >= image->data.F32[row+1][col])) {
                    myType = PM_PEAK_EDGE;
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], myType);
                }
            } else {
                psError(PS_ERR_UNKNOWN, true, "peak specified outside valid column range.");
            }

        }
        psFree (tmpRow);
        psFree (row1);
    }

    //
    // Find peaks in the last row only.
    //
    row = image->numRows - 1;
    tmpRow = getRowVectorFromImage((psImage *) image, row);
    row1 = pmFindVectorPeaks(tmpRow, threshold);
    for (psU32 i = 0 ; i < row1->n ; i++ ) {
        col = row1->data.U32[i];
        if (col == 0) {
            if ( (image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                    (image->data.F32[row][col] >= image->data.F32[row-1][col+1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row][col+1])) {
                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }
        } else if (col < (image->numCols - 1)) {
            if ( (image->data.F32[row][col] >= image->data.F32[row-1][col-1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                    (image->data.F32[row][col] >= image->data.F32[row-1][col+1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row][col-1]) &&
                    (image->data.F32[row][col] >= image->data.F32[row][col+1])) {
                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }

        } else if (col == (image->numCols - 1)) {
            if ( (image->data.F32[row][col] >= image->data.F32[row-1][col-1]) &&
                    (image->data.F32[row][col] >  image->data.F32[row-1][col]) &&
                    (image->data.F32[row][col] >  image->data.F32[row][col-1])) {
                if (image->data.F32[row][col] > threshold) {
                    list = myListAddPeak(list, row + row0, col + col0, image->data.F32[row][col], PM_PEAK_EDGE);
                }
            }
        } else {
            psError(PS_ERR_UNKNOWN, true, "peak specified outside valid column range.");
        }
    }
    psFree (tmpRow);
    psFree (row1);
    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(list);
}


/******************************************************************************
psCullPeaks(peaks, maxValue, valid): eliminate peaks from the psArray that have
a peak value above the given maximum, or fall outside the valid region.
 
XXX: Should the sky value be used when comparing the maximum?
 
XXX: warning message if valid is NULL?
 
XXX: changed API to create a NEW output psArray (should change name as well)
 
XXX: Do we free the psList elements of those culled peaks?
 
XXX EAM : do we still need pmCullPeaks, or only pmPeaksSubset?
*****************************************************************************/
psList *pmCullPeaks(psList *peaks,
                    psF32 maxValue,
                    const psRegion valid)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(peaks, NULL);

    psListElem *tmpListElem = (psListElem *) peaks->head;
    psS32 indexNum = 0;

    //    printf("pmCullPeaks(): list size is %d\n", peaks->size);
    while (tmpListElem != NULL) {
        pmPeak *tmpPeak = (pmPeak *) tmpListElem->data;
        if ((tmpPeak->counts > maxValue) ||
                (true == isItInThisRegion(valid, tmpPeak->x, tmpPeak->y))) {
            psListRemoveData(peaks, (psPtr) tmpPeak);
        }

        indexNum++;
        tmpListElem = tmpListElem->next;
    }

    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(peaks);
}

// XXX EAM: I changed this to return a new, subset array
//          rather than alter the existing one
// XXX: Fix the *valid pointer.
psArray *pmPeaksSubset(
    psArray *peaks,
    psF32 maxValue,
    const psRegion valid)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(peaks, NULL);

    psArray *output = psArrayAlloc (200);
    output->n = 0;

    psTrace (".pmObjects.pmCullPeaks", 3, "list size is %d\n", peaks->n);

    for (int i = 0; i < peaks->n; i++) {
        pmPeak *tmpPeak = (pmPeak *) peaks->data[i];
        if (tmpPeak->counts > maxValue)
            continue;
        if (isItInThisRegion(valid, tmpPeak->x, tmpPeak->y))
            continue;
        psArrayAdd (output, 200, tmpPeak);
    }
    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(output);
}
