From c9453995e5dd834cf3ccd1f61348b83695dd267d Mon Sep 17 00:00:00 2001
From: Martijn <martijn@zimpa.nl>
Date: Wed, 28 Jul 2021 13:31:55 +0200
Subject: [PATCH] PolesFilter

---
 CMakeLists.txt            |  1 +
 src/utils/PolesFilter.cpp | 69 +++++++++++++++++++++++++++++++++++++++
 src/utils/PolesFilter.h   | 37 +++++++++++++++++++++
 3 files changed, 107 insertions(+)
 create mode 100644 src/utils/PolesFilter.cpp
 create mode 100644 src/utils/PolesFilter.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6cb4a9..a670c03 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,6 +35,7 @@ set(FILES
         src/utils/IIR_NOrder_BW_LH.cpp
         src/utils/MultiBiquad.cpp
         src/utils/PassFilter.cpp
+        src/utils/PolesFilter.cpp
         src/utils/TimeConstDelay.cpp
 )
 
diff --git a/src/utils/PolesFilter.cpp b/src/utils/PolesFilter.cpp
new file mode 100644
index 0000000..1e94ac5
--- /dev/null
+++ b/src/utils/PolesFilter.cpp
@@ -0,0 +1,69 @@
+//
+// Created by mart on 7/28/21.
+//
+
+#include "PolesFilter.h"
+#include "../constants.h"
+#include <cstring>
+#include <cmath>
+
+PolesFilter::PolesFilter() {
+    this->samplerate = DEFAULT_SAMPLERATE;
+    this->lower_freq = 160;
+    this->upper_freq = 8000;
+    UpdateCoeff();
+}
+
+void PolesFilter::Reset() {
+    UpdateCoeff();
+}
+
+void PolesFilter::UpdateCoeff() {
+    memset(&this->channels[0], 0, sizeof(channel));
+    memset(&this->channels[1], 0, sizeof(channel));
+
+    this->channels[0].lower_angle = ((float)this->lower_freq * M_PI / (float)this->samplerate);
+    this->channels[1].lower_angle = ((float)this->lower_freq * M_PI / (float)this->samplerate);
+    this->channels[0].upper_angle = ((float)this->upper_freq * M_PI / (float)this->samplerate);
+    this->channels[1].upper_angle = ((float)this->upper_freq * M_PI / (float)this->samplerate);
+}
+
+inline void DoFilterSide(channel* side, float sample, float* out1, float* out2, float* out3) {
+    float oldestSampleIn = this->in[2];
+    this->in[2] = this->in[1];
+    this->in[1] = this->in[0];
+    this->in[0] = sample;
+
+    channel->x[0] += channel->lower_angle * (sample - channel->x[0]);
+    channel->x[1] += channel->lower_angle * (channel->x[0] - channel->x[1]);
+    channel->x[2] += channel->lower_angle * (channel->x[1] - channel->x[2]);
+    channel->x[3] += channel->lower_angle * (channel->x[2] - channel->x[3]);
+
+    channel->y[0] += channel->upper_angle * (sample - channel->y[0]);
+    channel->y[1] += channel->upper_angle * (channel->y[0] - channel->y[1]);
+    channel->y[2] += channel->upper_angle * (channel->y[1] - channel->y[2]);
+    channel->y[3] += channel->upper_angle * (channel->y[2] - channel->y[3]);
+
+    *out1 = channel->x[3];
+    *out2 = oldestSampleIn - channel->y[3];
+    *out3 = channel->y[3] - channel->x[3];
+}
+
+void PolesFilter::DoFilterLeft(float sample, float *out1, float *out2, float *out3) {
+    DoFilterSide(&this->channels[0], sample, out1, out2, out3);
+}
+
+void PolesFilter::DoFilterRight(float sample, float *out1, float *out2, float *out3) {
+    DoFilterSide(&this->channels[1], sample, out1, out2, out3);
+}
+
+void PolesFilter::SetPassFilter(uint32_t lower_freq, uint32_t upper_freq) {
+    this->lower_freq = lower_freq;
+    this->upper_freq = upper_freq;
+    UpdateCoeff();
+}
+
+void PolesFilter::SetSamplingRate(uint32_t samplerate) {
+    this->samplerate = samplerate;
+    UpdateCoeff();
+}
\ No newline at end of file
diff --git a/src/utils/PolesFilter.h b/src/utils/PolesFilter.h
new file mode 100644
index 0000000..b0c90f8
--- /dev/null
+++ b/src/utils/PolesFilter.h
@@ -0,0 +1,37 @@
+//
+// Created by mart on 7/28/21.
+//
+
+#pragma once
+
+#include <cstdint>
+
+typedef struct {
+    float lower_angle;
+    float upper_angle;
+
+    float in[3];
+    float x[4];
+    float y[4];
+} channel;
+
+
+class PolesFilter {
+public:
+    PolesFilter();
+
+    void Reset();
+    void UpdateCoeff();
+    void DoFilterLeft(float sample, float* out1, float* out2, float* out3);
+    void DoFilterRight(float sample, float* out1, float* out2, float* out3);
+
+    void SetPassFilter(uint32_t lower_freq, uint32_t upper_freq);
+    void SetSamplingRate(uint32_t samplerate);
+
+    channel channels[2];
+    uint32_t lower_freq;
+    uint32_t upper_freq;
+    uint32_t samplerate;
+};
+
+