1 files changed, 53 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;
 }