update find_peaks_array() to return peaks in sorted order

2 files changed, 136 insertions, 9 deletions

diff --git a/src/find_peaks.c b/src/find_peaks.c
index e3a2fc6..05d8c15 100644
--- a/src/find_peaks.c
+++ b/src/find_peaks.c
@@ -7,6 +7,30 @@
 #include "optics.h"
 #include <math.h> /* for exp() */
 
+typedef struct peak {
+    size_t i;
+    size_t j;
+    double value;
+} peak;
+
+/* Compare two different peaks.
+ *
+ * Note: We return 1 if peak b is greater than peak b and -1 if peak a is
+ * greater than peak b. This is backwards from what you would normally expect,
+ * but it's because we want to keep the peaks sorted in *descending* order. */
+static int peak_compare(const void *a, const void *b)
+{
+    const peak *pa = (peak *) a;
+    const peak *pb = (peak *) b;
+
+    if (pa->value > pb->value)
+        return -1;
+    else if (pa->value < pb->value)
+        return 1;
+
+    return 0;
+}
+
 void find_peaks_array(double *x, size_t n, size_t m, size_t *imax, size_t *jmax, size_t *npeaks, size_t max_peaks, double threshold)
 {
     /* Find a maximum of `max_peaks` in the 2D array `x` indexed as:
@@ -17,37 +41,47 @@ void find_peaks_array(double *x, size_t n, size_t m, size_t *imax, size_t *jmax,
      *
      *  Peaks are defined as any array element which is greater than all 8 of
      *  it's neighbors. Peaks are also required to be at least max*threshold
-     *  times as high as the highest peak. */
+     *  times as high as the highest peak.
+     *
+     *  The returned peaks will always be the *highest* peaks and they will be
+     *  returned in sorted order from highest to lowest. */
     size_t i, j;
     double max;
+    peak *p;
 
     if (n*m == 0) {
         *npeaks = 0;
         return;
     }
 
+    p = malloc(sizeof(peak)*(max_peaks+1));
+
     /* First, find the highest value in the array (which is guaranteed to be a peak). */
     max = x[0];
-    imax[0] = 0;
-    jmax[0] = 0;
+    p[0].i = 0;
+    p[0].j = 0;
+    p[0].value = max;
     for (i = 0; i < n; i++) {
         for (j = 0; j < m; j++) {
             if (x[i*m + j] > max) {
                 max = x[i*m + j];
-                imax[0] = i;
-                jmax[0] = j;
+                p[0].i = i;
+                p[0].j = j;
+                p[0].value = max;
             }
         }
     }
 
     *npeaks = 1;
 
+    if (*npeaks >= max_peaks) goto end;
+
     /* Now we look for other peaks which are at least max*threshold high. */
     for (i = 0; i < n; i++) {
         for (j = 0; j < m; j++) {
             if (x[i*m + j] <= max*threshold) continue;
 
-            if (i == imax[0] && j == jmax[0]) continue;
+            if (i == p[0].i && j == p[0].j) continue;
 
             /* Check to see if it is actually a peak. */
             if (x[i*m + (j+1) % m] < x[i*m + j]               && /* 0 +1 */
@@ -58,16 +92,26 @@ void find_peaks_array(double *x, size_t n, size_t m, size_t *imax, size_t *jmax,
                 x[((i+n-1) % n)*m + j] < x[i*m + j]           && /* -1 0 */
                 x[((i+n-1) % n)*m + (j+1) % m] < x[i*m + j]   && /* -1 +1 */
                 x[((i+n-1) % n)*m + (j+m-1) % m] < x[i*m + j]) { /* -1 -1 */
-                imax[*npeaks] = i;
-                jmax[*npeaks] = j;
+                p[*npeaks].i = i;
+                p[*npeaks].j = j;
+                p[*npeaks].value = x[i*m+j];
                 *npeaks += 1;
 
-                if (*npeaks >= max_peaks) goto end;
+                qsort(p,*npeaks,sizeof(peak),peak_compare);
+
+                if (*npeaks >= max_peaks) *npeaks = max_peaks;
             }
         }
     }
 
 end:
+    for (i = 0; i < *npeaks; i++) {
+        imax[i] = p[i].i;
+        jmax[i] = p[i].j;
+    }
+
+    free(p);
+
     return;
 }
 
diff --git a/src/test.c b/src/test.c
index 6a0bc25..dcfc03c 100644
--- a/src/test.c
+++ b/src/test.c
@@ -1757,6 +1757,87 @@ int test_unique_vertices(char *err)
     return 0;
 }
 
+int test_find_peaks_highest(char *err)
+{
+    /* Tests that the find_peaks_array() function returns the *highest* n peaks
+     * assuming there are more than n total peaks. */
+    size_t imax[2], jmax[2], npeaks;
+
+    double x[10][10] = {
+        {0,0,2,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {4,0,0,0,0,0,0,0,0,5},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,0,0,0,0,0,0,0,0},
+        {0,0,10,0,0,0,0,0,0,10.1},
+    };
+
+    find_peaks_array(&x[0][0],10,10,imax,jmax,&npeaks,LEN(imax),0.1);
+
+    if (npeaks != 2) {
+        sprintf(err, "number of peaks = %zu, but expected %i", npeaks, 2);
+        return 1;
+    }
+
+    if (imax[0] != 9) {
+        sprintf(err, "imax[0] = %zu, but expected %i", imax[0], 9);
+        return 1;
+    }
+
+    if (jmax[0] != 9) {
+        sprintf(err, "jmax[0] = %zu, but expected %i", jmax[0], 9);
+        return 1;
+    }
+
+    if (imax[1] != 9) {
+        sprintf(err, "imax[0] = %zu, but expected %i", imax[1], 9);
+        return 1;
+    }
+
+    if (jmax[1] != 2) {
+        sprintf(err, "jmax[0] = %zu, but expected %i", jmax[1], 2);
+        return 1;
+    }
+
+    return 0;
+}
+
+int test_find_peaks_sorted(char *err)
+{
+    /* Tests that the find_peaks_array() function returns peaks in order from
+     * highest to lowest. */
+    size_t i, j, k;
+    size_t imax[10], jmax[10], npeaks;
+    double x[10][10];
+
+    init_genrand(0);
+
+    for (i = 1; i < 100; i++) {
+        /* Randomly initialize array. */
+        for (j = 0; j < 10; j++) {
+            for (k = 0; k < 10; k++) {
+                x[j][k] = genrand_real2();
+            }
+        }
+
+        find_peaks_array(&x[0][0],10,10,imax,jmax,&npeaks,LEN(imax),0.1);
+
+        /* Test that each peak is greater than the previous peak. */
+        for (j = 1; j < npeaks; j++) {
+            if (x[imax[j]][jmax[j]] > x[imax[j-1]][jmax[j-1]]) {
+                sprintf(err, "peak %zu is higher than peak %zu", j, j-1);
+                return 1;
+            }
+        }
+    }
+
+    return 0;
+}
+
 struct tests {
     testFunction *test;
     char *name;
@@ -1801,6 +1882,8 @@ struct tests {
     {test_ipow, "test_ipow"},
     {test_product, "test_product"},
     {test_unique_vertices, "test_unique_vertices"},
+    {test_find_peaks_highest, "test_find_peaks_highest"},
+    {test_find_peaks_sorted, "test_find_peaks_sorted"},
 };
 
 int main(int argc, char **argv)