feat: IIR filter impl

google-tests/test1.cpp

@@ -4,6 +4,7 @@
 #include <gtest/gtest.h>
 #include "library.h"
 #include "npy.hpp"
+#include <utility>
 #include <vector>
 #include <string>
 #include <filesystem>
@@ -37,3 +38,115 @@ TEST(HelloTest, Load_npy_matrix) {
     EXPECT_DOUBLE_EQ(data[2], expect_data[2]);
     EXPECT_DOUBLE_EQ(data[3], expect_data[3]);
 }
+
+TEST(HelloTest, Save_npy_matrix) {
+    const std::vector<double> data{1, 2, 3, 4, 5, 6};
+
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = data.data();
+    d.shape = {2, 3};
+    d.fortran_order = false;
+
+    const std::string path{"test1/data2.npy"};
+    npy::write_npy(path, d);
+}
+
+/** Shift register implemented as a circular buffer. */
+class Buf {
+protected:
+    std::vector<double> data;
+    size_t size;
+    size_t n;
+public:
+    Buf(size_t N): size(N), n(0) {
+        data.resize(N);
+        data.assign(N, 0.0);
+    }
+    void push(double val) {
+        data[n] = val;
+        n = (n+1) % size;
+    }
+};
+
+/** Running filter base. */
+class Filt : Buf {
+protected:
+    std::vector<double> taps;
+    size_t shift;
+    size_t offset;
+public:
+    Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps): Buf(N), shift(shift), offset(offset), taps(taps) {
+        if (taps.size() != N) throw std::invalid_argument("taps.size() != N");
+    }
+    double filter(double val) {
+        this->push(val);
+        return this->peek();
+    }
+    double peek() {
+        double sum = 0;
+        for (size_t i = offset; i < this->size; i++) {
+            //size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
+            size_t n = (this->size + this->n - i + shift - 1) % this->size;
+            //std::cout << " t[" << i << "] * v[" << n << "]" << std::endl;
+            sum += this->data[n] * this->taps[i];
+        }
+        return sum;
+    }
+    void push(double val) {
+        Buf::push(val);
+    }
+};
+
+class IirFilter {
+protected:
+    Filt y;
+    Filt x;
+public:
+    IirFilter(std::vector<double> b, std::vector<double> a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) {
+        if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()");
+    }
+    double filter(double val) {
+        //std::cout << "x.filter(" << val << ")" << std::endl;
+        double xv = x.filter(val);
+        //std::cout << "xv=" << xv << std::endl;
+        //std::cout << "y.peek()" << std::endl;
+        double yv = y.peek();
+        //std::cout << "yv=" << yv << std::endl;
+        //std::cout << "---" << std::endl;
+        double yo = xv - yv;
+        y.push(yo);
+        return yo;
+    }
+};
+
+// TODO: copy npy files from CMake output folder
+// TODO: add npy files to CMakeLists.txt so they are copied into output folder
+TEST(HelloTest, Test_IIR_1_Apply_IIR) {
+    npy::npy_data x = npy::read_npy<double>("test1/iir_t1_x.npy");
+    npy::npy_data y_e = npy::read_npy<double>("test1/iir_t1_y.npy");
+    size_t N = x.shape[0];
+    EXPECT_EQ(x.shape[0], y_e.shape[0]);
+
+    npy::npy_data a = npy::read_npy<double>("test1/iir_t1_a.npy");
+    npy::npy_data b = npy::read_npy<double>("test1/iir_t1_b.npy");
+
+    //EXPECT_EQ(6, b.data.size());
+
+    IirFilter filter(b.data, a.data);
+    std::vector<double> y;
+    y.resize(N);
+    for (size_t i = 0; i < N; i++) {
+        y[i] = filter.filter(x.data[i]);
+    }
+    // assert y == y_e, nb. upto 5 digits
+    double abs_err = 1e-5;
+    for (size_t i = 0; i < N; i++) {
+        ASSERT_NEAR(y_e.data[i], y[i], abs_err);
+    }
+
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = y.data();
+    d.shape = {(unsigned long)N};
+    const std::string path{"test1/iir_t1_y_out.npy"};
+    npy::write_npy(path, d);
+}