From 6b3bdef3a7d73417fc53c44e39e0c886c45c11e3 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Wed, 26 Apr 2017 00:07:57 +0200
Subject: [PATCH] IntHalfbandFilters: Optimized versions of
 workInterpolateCenter and myInterpolate

---
 sdrbase/dsp/inthalfbandfilter.h | 135 ++++++++++++++++++++++++++++----
 1 file changed, 121 insertions(+), 14 deletions(-)

diff --git a/sdrbase/dsp/inthalfbandfilter.h b/sdrbase/dsp/inthalfbandfilter.h
index ed7c2b67a..fc17a4ac5 100644
--- a/sdrbase/dsp/inthalfbandfilter.h
+++ b/sdrbase/dsp/inthalfbandfilter.h
@@ -26,7 +26,16 @@
 template<uint32_t HBFilterOrder>
 class SDRANGEL_API IntHalfbandFilter {
 public:
-	IntHalfbandFilter();
+	IntHalfbandFilter() :
+	    m_ptr(0),
+	    m_state(0)
+    {
+	    for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder + 1; i++)
+	    {
+	        m_samples[i][0] = 0;
+	        m_samples[i][1] = 0;
+	    }
+    }
 
 	// downsample by 2, return center part of original spectrum
 	bool workDecimateCenter(Sample* sample)
@@ -103,6 +112,40 @@ public:
         }
     }
 
+    /** Optimized upsampler by 2 not calculating FIR with inserted null samples */
+    bool workInterpolateCenterOptimized(Sample* sampleIn, Sample *SampleOut)
+    {
+        switch(m_state)
+        {
+        case 0:
+            // return the middle peak
+            SampleOut->setReal(m_samples[m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder/4) - 1][0]);
+            SampleOut->setImag(m_samples[m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder/4) - 1][1]);
+            m_state = 1;  // next state
+            return false; // tell caller we didn't consume the sample
+
+        default:
+            // calculate with non null samples
+            doInterpolateFIR(SampleOut);
+
+            // insert sample into ring double buffer
+            m_samples[m_ptr][0] = sampleIn->real();
+            m_samples[m_ptr][1] = sampleIn->imag();
+            m_samples[m_ptr + HBFIRFilterTraits<HBFilterOrder>::hbOrder/2][0] = sampleIn->real();
+            m_samples[m_ptr + HBFIRFilterTraits<HBFilterOrder>::hbOrder/2][1] = sampleIn->imag();
+
+            // advance pointer
+            if (m_ptr < (HBFIRFilterTraits<HBFilterOrder>::hbOrder/2) - 1) {
+                m_ptr++;
+            } else {
+                m_ptr = 0;
+            }
+
+            m_state = 0; // next state
+            return true; // tell caller we consumed the sample
+        }
+    }
+
 	bool workDecimateCenter(qint32 *x, qint32 *y)
 	{
 		// insert sample into ring-buffer
@@ -544,6 +587,29 @@ public:
         m_ptr = HBFIRFilterTraits<HBFilterOrder>::hbMod[m_ptr + 2 - 1];
     }
 
+    void myInterpolateOptimized(qint32 *x1, qint32 *y1, qint32 *x2, qint32 *y2)
+    {
+        // insert sample into ring double buffer
+        m_samples[m_ptr][0] = *x1;
+        m_samples[m_ptr][1] = *y1;
+        m_samples[m_ptr + HBFIRFilterTraits<HBFilterOrder>::hbOrder/2][0] = *x1;
+        m_samples[m_ptr + HBFIRFilterTraits<HBFilterOrder>::hbOrder/2][1] = *y1;
+
+        // advance pointer
+        if (m_ptr < (HBFIRFilterTraits<HBFilterOrder>::hbOrder/2) - 1) {
+            m_ptr++;
+        } else {
+            m_ptr = 0;
+        }
+
+        // first output sample calculated with the middle peak
+        *x1 = m_samples[m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder/4) - 1][0];
+        *y1 = m_samples[m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder/4) - 1][1];
+
+        // second sample calculated with the filter
+        doInterpolateFIR(x2, y2);
+    }
+
 protected:
 	qint32 m_samples[HBFIRFilterTraits<HBFilterOrder>::hbOrder + 1][2];     // Valgrind optim (from qint16)
 	qint16 m_ptr;
@@ -581,6 +647,48 @@ protected:
 		sample->setImag(qAcc >> (HBFIRFilterTraits<HBFilterOrder>::hbShift -1));
 	}
 
+	void doInterpolateFIR(Sample* sample)
+	{
+	    qint16 a = m_ptr;
+	    qint16 b = m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder / 2) - 1;
+
+        // go through samples in buffer
+        qint32 iAcc = 0;
+        qint32 qAcc = 0;
+
+        for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder / 4; i++)
+        {
+            iAcc += (m_samples[a][0] + m_samples[b][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
+            qAcc += (m_samples[a][1] + m_samples[b][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
+            a++;
+            b--;
+        }
+
+        sample->setReal(iAcc >> (HBFIRFilterTraits<HBFilterOrder>::hbShift -1));
+        sample->setImag(qAcc >> (HBFIRFilterTraits<HBFilterOrder>::hbShift -1));
+	}
+
+    void doInterpolateFIR(qint32 *x, qint32 *y)
+    {
+        qint16 a = m_ptr;
+        qint16 b = m_ptr + (HBFIRFilterTraits<HBFilterOrder>::hbOrder / 2) - 1;
+
+        // go through samples in buffer
+        qint32 iAcc = 0;
+        qint32 qAcc = 0;
+
+        for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder / 4; i++)
+        {
+            iAcc += (m_samples[a][0] + m_samples[b][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
+            qAcc += (m_samples[a][1] + m_samples[b][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
+            a++;
+            b--;
+        }
+
+        *x = iAcc >> (HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
+        *y = qAcc >> (HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
+    }
+
 	void doFIR(qint32 *x, qint32 *y)
 	{
 		// Coefficents. This is a sinc function:
@@ -621,18 +729,17 @@ protected:
 
 };
 
-template<uint32_t HBFilterOrder>
-IntHalfbandFilter<HBFilterOrder>::IntHalfbandFilter()
-{
-    for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder + 1; i++)
-    {
-        m_samples[i][0] = 0;
-        m_samples[i][1] = 0;
-    }
-
-    m_ptr = 0;
-
-    m_state = 0;
-}
+//template<uint32_t HBFilterOrder>
+//IntHalfbandFilter<HBFilterOrder>::IntHalfbandFilter()
+//{
+//    for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder + 1; i++)
+//    {
+//        m_samples[i][0] = 0;
+//        m_samples[i][1] = 0;
+//    }
+//
+//    m_ptr = 0;
+//    m_state = 0;
+//}
 
 #endif // INCLUDE_INTHALFBANDFILTER_H