From f7bec2358b30aca6a2f00aa3e342cb0d7f148a0a Mon Sep 17 00:00:00 2001
From: Silvano Seva <silseva@fastwebnet.it>
Date: Mon, 23 Aug 2021 18:56:30 +0200
Subject: [PATCH] Class for implementation of FIR filters

---
 openrtx/include/dsp.h | 75 +++++++++++++++++++++++++++++++++++++------
 1 file changed, 65 insertions(+), 10 deletions(-)

diff --git a/openrtx/include/dsp.h b/openrtx/include/dsp.h
index ce966d2d..2098ace7 100644
--- a/openrtx/include/dsp.h
+++ b/openrtx/include/dsp.h
@@ -32,7 +32,10 @@ typedef int16_t audio_sample_t;
  */
 
 #ifdef __cplusplus
+
 #include <array>
+#include <cstddef>
+
 extern "C" {
 #endif
 
@@ -67,17 +70,69 @@ void dsp_invertPhase(audio_sample_t *buffer, uint16_t length);
 }
 
 /**
- * Applies a generic FIR filter on the audio buffer passed as parameter.
- * The buffer will be processed in place to save memory.
- *
- * @param buffer: the buffer to be used as both source and destination.
- * @param length: the length of the input buffer.
- * @param taps: an array of coefficients which defines the transfer function.
+ * Class for FIR filter with configurable coefficients.
+ * Adapted from the original implementation by Rob Riggs, Mobilinkd LLC.
  */
-template<size_t order>
-void dsp_applyFIR(audio_sample_t *buffer,
-                  uint16_t length,
-                  std::array<float, order> taps);
+template < size_t N >
+class Fir
+{
+public:
+
+    /**
+     * Constructor.
+     *
+     * @param taps: reference to a std::array of floating poing values representing
+     * the FIR filter coefficients.
+     */
+    Fir(const std::array< float, N >& taps) : taps(taps), pos(0)
+    {
+        reset();
+    }
+
+    /**
+     * Destructor.
+     */
+    ~Fir() { }
+
+    /**
+     * Perform one step of the FIR filter, computing a new output value given
+     * the input value and the history of previous input values.
+     *
+     * @param input: FIR input value for the current time step.
+     * @return FIR output as a function of the current and past input values.
+     */
+    float operator()(const float& input)
+    {
+        hist[pos++] = input;
+        if(pos >= N) pos = 0;
+
+        float  result = 0.0;
+        size_t index  = pos;
+
+        for(size_t i = 0; i < N; i++)
+        {
+            index   = (index != 0 ? index - 1 : N - 1);
+            result += hist[index] * taps[i];
+        }
+
+        return result;
+    }
+
+    /**
+     * Reset FIR history, clearing the memory of past values.
+     */
+    void reset()
+    {
+        hist.fill(0);
+        pos = 0;
+    }
+
+private:
+
+    const std::array< float, N >& taps;    ///< FIR filter coefficients.
+    std::array< float, N >        hist;    ///< History of past inputs.
+    size_t                        pos;     ///< Current position in history.
+};
 
 #endif // __cplusplus