From e82886ad0e13bb012e4a244df2ce6d84cd277763 Mon Sep 17 00:00:00 2001
From: Sylvain Munaut <tnt@246tNt.com>
Date: Fri, 16 Sep 2016 13:39:31 -0600
Subject: [PATCH] cxvec/math: Add helper to find the N highest energy values in
 vector

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
---
 include/osmocom/dsp/cxvec_math.h |  3 +++
 src/cxvec_math.c                 | 45 ++++++++++++++++++++++++++++++++
 2 files changed, 48 insertions(+)

diff --git a/include/osmocom/dsp/cxvec_math.h b/include/osmocom/dsp/cxvec_math.h
index 7e372c4..77e9ab7 100644
--- a/include/osmocom/dsp/cxvec_math.h
+++ b/include/osmocom/dsp/cxvec_math.h
@@ -103,6 +103,9 @@ osmo_cxvec_correlate(const struct osmo_cxvec *f, const struct osmo_cxvec *g,
 float complex
 osmo_cxvec_interpolate_point(const struct osmo_cxvec *cv, float pos);
 
+int
+osmo_cxvec_peaks_scan(const struct osmo_cxvec *cv, int *peaks_idx, int N);
+
 /*! \brief Various possible peak finding algorithms */
 enum osmo_cxvec_peak_alg {
 	/*! \brief Weigthed position for the max pwr window */
diff --git a/src/cxvec_math.c b/src/cxvec_math.c
index 9f81aa5..9dc6157 100644
--- a/src/cxvec_math.c
+++ b/src/cxvec_math.c
@@ -428,6 +428,51 @@ osmo_cxvec_interpolate_point(const struct osmo_cxvec *cv, float pos)
 	return val;
 }
 
+/*! \brief Find the index of the N highest energy (\f$|x|^2\f$) peaks
+ *  \param[in] cv Input complex vector
+ *  \param[out] peaks_idx Return array of the peak indexes
+ *  \param[in] Size of the \ref peaks_idx return array
+ *  \returns Number of peaks (will be N if there is enough points)
+ */
+int
+osmo_cxvec_peaks_scan(const struct osmo_cxvec *cv, int *peaks_idx, int N)
+{
+	int i, j;
+	float peaks_mag[N];
+
+	/* Pre-init */
+	for (i=0; i<N; i++) {
+		peaks_idx[i] = -1;
+		peaks_mag[i] = 0.0f;
+	}
+
+	/* Scan all */
+	for (i=0; i<cv->len; i++)
+	{
+		/* Magnitude */
+		float mag = osmo_normsqf(cv->data[i]);
+
+		/* Worth it ? */
+		if (mag < peaks_mag[N-1])
+			continue;
+
+		/* Find insertion point in sorted array and pre-move */
+		for (j=N-1; j>0; j--) {
+			if (mag < peaks_mag[j-1])
+				break;
+
+			peaks_mag[j] = peaks_mag[j-1];
+			peaks_idx[j] = peaks_idx[j-1];
+		}
+
+		/* Do the insert */
+		peaks_mag[j] = mag;
+		peaks_idx[j] = i;
+	}
+
+	return i < N ? i : N;
+}
+
 /*! \brief Find the maximum energy (\f$|x|^2\f$) peak in a sequence
  *  \param[in] cv Input complex vector
  *  \param[in] win_size Size of the window (for algorithms using windows)