From 4fbb64fc731c67cb96fe06bb471cb4a66d34d5b6 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Fri, 24 May 2019 18:10:01 +0000
Subject: [PATCH] - added hill climbing

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@456 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 gr-jay/lib/peak_detect_impl.cc | 72 +++++++++++++++++++++++++++++-----
 gr-jay/lib/peak_detect_impl.h  |  6 ++-
 2 files changed, 67 insertions(+), 11 deletions(-)

diff --git a/gr-jay/lib/peak_detect_impl.cc b/gr-jay/lib/peak_detect_impl.cc
index aeb1886..607745f 100644
--- a/gr-jay/lib/peak_detect_impl.cc
+++ b/gr-jay/lib/peak_detect_impl.cc
@@ -46,13 +46,13 @@ namespace gr {
 		, m_alpha_m(alpha_m)
 		, m_frameCount(0)
     {
-		m_pPeak = new float[vlen];
+		m_pPeakData = new float[vlen];
 		m_pX = new float[vlen];
 		m_pXs = new float[vlen];
 		m_pXm = new float[vlen];
 		m_pPeakPos = new int[vlen];
 		
-		memset(m_pPeak, 0, vlen*sizeof(int));
+		memset(m_pPeakData, 0, vlen*sizeof(int));
 		memset(m_pX, 0, vlen*sizeof(float));
 		memset(m_pXs, 0, vlen*sizeof(float));
 		memset(m_pXm, 0, vlen*sizeof(float));
@@ -71,7 +71,7 @@ namespace gr {
 		delete [] m_pXm;
 		delete [] m_pXs;
 		delete [] m_pX;
-		delete [] m_pPeak;
+		delete [] m_pPeakData;
     }
 
 	float peak_detect_impl::mean(float* pSrc, int length)
@@ -91,13 +91,22 @@ namespace gr {
 			pDst[i] = value;
 		}
 	}
+	int peak_detect_impl::threshold(float *pDst, float *pSrc, int length, float thresh)
+	{
+		for (int i=0; i < length; i++)
+		{
+			pDst[i] = (int)(pSrc[i] >= thresh);
+		}
+		
+		return length;
+	}
 	
-	int peak_detect_impl::findPeakPos(int* pDst, float* pSrc, int length, float thresh)
+	int peak_detect_impl::findPeakPos(int* pDst, float* pSrc, int length)
 	{
 		int count = 0;
 		for (int i=0; i < length; i++)
 		{
-			if (pSrc[i] > thresh)
+			if (pSrc[i] > 0)
 			{
 				pDst[count++] = i;
 			}
@@ -134,7 +143,6 @@ namespace gr {
 			for (int i = 0; i < nitems_per_block; i++)
 			{
 				float x = *(iptr++);
-				*(optr++) = x;
 				m_pX[i] = x;
 			}
 			
@@ -167,12 +175,58 @@ namespace gr {
 			// Peak detection
 			for (int i = 0; i < nitems_per_block; i++)
 			{
+				// Create peak distances by subtracting Xm
 				float peak_dist = m_pX[i]-m_pXm[i];
-				m_pPeak[i] = peak_dist;
+				m_pPeakData[i] = peak_dist;
+				*(optr++) = peak_dist;
 			}
 
 			// Create peak list
-			int numPeaks = findPeakPos(m_pPeakPos, m_pPeak, nitems_per_block, 10);
+			// Find peak candidates by thresholding
+			threshold(m_pPeakData, m_pPeakData, nitems_per_block, 20.f);
+			int numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
+			
+			// Search true maximum peak using hill climbing
+			int numPeaksLast = 0;
+			while(1)
+			{
+				int numPeaksClimbed = 0;
+				for (int i = 0; i < numPeaks; i++)
+				{
+					int pos = m_pPeakPos[i];
+					while(1)
+					{
+						float y0 = m_pX[pos];
+						int pos_p = std::min(nitems_per_block-1, pos+1);
+						int pos_n = std::max(0, pos-1);
+						if (m_pX[pos_p] > y0)
+						{
+							m_pPeakData[pos] = 0;
+							m_pPeakData[pos_n] = 0;
+							pos = pos_p;
+						}
+						else if (m_pX[pos_n] > y0)
+						{
+							m_pPeakData[pos] = 0;
+							m_pPeakData[pos_p] = 0;
+							pos = pos_n;
+						}
+						else
+						{
+							m_pPeakData[pos_n] = 0;
+							m_pPeakData[pos_p] = 0;
+							numPeaksClimbed++;
+							break;
+						}
+					}
+				}
+				if (numPeaksClimbed == numPeaksLast)
+				{
+					break;
+				}
+				numPeaksLast = numPeaksClimbed;
+			}
+			numPeaks = findPeakPos(m_pPeakPos, m_pPeakData, nitems_per_block);
 			for (int i = 0; i < numPeaks; i++)
 			{
 				// Create peak tags
@@ -180,7 +234,7 @@ namespace gr {
 				pmt::pmt_t tag_value = pmt::from_long(m_pPeakPos[i]);
 				add_item_tag(0, nitems_written(0) + m_pPeakPos[i], tag_key, tag_value, m_tag_id);
 			}
-			
+		
 			m_frameCount++;
 		}
 
diff --git a/gr-jay/lib/peak_detect_impl.h b/gr-jay/lib/peak_detect_impl.h
index 7b0b6d7..1d93ef4 100644
--- a/gr-jay/lib/peak_detect_impl.h
+++ b/gr-jay/lib/peak_detect_impl.h
@@ -32,7 +32,7 @@ namespace jay {
 		// Nothing to declare in this block.
 		float m_alpha_s;
 		float m_alpha_m;
-		float *m_pPeak;
+		float *m_pPeakData;
 		float *m_pX;
 		float *m_pXs;
 		float *m_pXm;
@@ -48,7 +48,9 @@ namespace jay {
 		int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
 		void fill(float *pDst, int length, float value);
 		float mean(float *pSrc, int length);
-		int findPeakPos(int *pDst, float *pSrc, int length, float thresh);
+		int threshold(float *pDst, float *pSrc, int length, float thresh);
+		int findPeakPos(int *pDst, float *pSrc, int length);
+		
     };
 
 } // namespace jay