From ee1b97a1654e76c548bb3b16a3033c909a7f6309 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Tue, 4 Feb 2020 22:21:57 +0100
Subject: [PATCH] Better FIR Filter implementation, correct sample collecting

---
 ambed/cfirfilter.cpp | 23 +++++++++++++++--------
 1 file changed, 15 insertions(+), 8 deletions(-)

diff --git a/ambed/cfirfilter.cpp b/ambed/cfirfilter.cpp
index 9f93db8..920ffe3 100644
--- a/ambed/cfirfilter.cpp
+++ b/ambed/cfirfilter.cpp
@@ -34,7 +34,7 @@ CFIRFilter::CFIRFilter(const float* taps, int tapsLength)
 
     ::memcpy(m_taps, taps, tapsLength * sizeof(float));
     ::memset(m_buffer, 0, tapsLength * sizeof(float));
-    m_currentBufferPosition = m_tapsLength -1;
+    m_currentBufferPosition = 0;
 }
 
 CFIRFilter::~CFIRFilter()
@@ -45,23 +45,30 @@ CFIRFilter::~CFIRFilter()
 
 inline float CFIRFilter::Process(float inputSample)
 {
-    // Buffer latest sample
+    float output = 0.0f;
+    int iTaps = 0;
+
+    // Buffer latest sample into delay line
     m_buffer[m_currentBufferPosition] = inputSample;
 
-    float outputSample = 0.0f;
-    for(int i = 0; i < m_tapsLength; i++)
+    for(int i = m_currentBufferPosition; i >= 0; i--)
     {
-        outputSample += m_taps[i] * m_buffer[(m_currentBufferPosition + i) % m_tapsLength];
+        output += m_taps[iTaps++] * m_buffer[i];
     }
 
-    m_currentBufferPosition = MAX(0, m_currentBufferPosition - 1);
+    for(int i = m_tapsLength - 1; i > m_currentBufferPosition; i--)
+    {
+        output += m_taps[iTaps++] * m_buffer[i];
+    }
+    
+    m_currentBufferPosition = (m_currentBufferPosition + 1) % m_tapsLength;
 
-    return outputSample;
+    return output;
 }
 
 void CFIRFilter::Process(uint8* voice, int length)
 {
-    for(int i = 0; i < length; i++)
+    for(int i = 0; i < length; i+=2)
     {
         //Get the sample
         float input = (float)(short)MAKEWORD(voice[i+1], voice[i]);