git: add submodule libnpy

move StepDetector from lockstep android to libpasada

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "google-tests/libnpy"]
+	path = google-tests/libnpy
+	url = https://github.com/llohse/libnpy.git

google-tests/CMakeLists.txt

@@ -8,9 +8,26 @@ include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} libnpy/inclu
 
 # 'Google_Tests_run' is the target name
 # 'test1.cpp test2.cpp' are source files with tests
-add_executable(Google_Tests_run test1.cpp)
+add_executable(Google_Tests_run
+        test_helpers.cpp
+        test1.cpp
+        test2.cpp
+        test3.cpp
+        test4.cpp
+)
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_b.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_x.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_y.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+
+file(COPY test2/ssf_t2_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
+file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
+
+file(COPY test3/ssf_t3_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test3)
+
+file(COPY test4/step_150a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test4)
 
 target_link_libraries(Google_Tests_run pasada)
 #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include")

google-tests/test1.cpp

@@ -3,6 +3,7 @@
 //
 #include <gtest/gtest.h>
 #include "library.h"
+#include "iir_filter.h"
 #include "npy.hpp"
 #include <utility>
 #include <vector>
@@ -51,76 +52,6 @@ TEST(HelloTest, Save_npy_matrix) {
     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");

google-tests/test2.cpp

@@ -0,0 +1,172 @@
+//
+// Created by david on 02.03.2026.
+//
+#include <gtest/gtest.h>
+#include "npy.hpp"
+#include <vector>
+#include "iir_filter.h"
+#include "ssf_filter.h"
+#include "test_helpers.h"
+#include <cmath>
+#include <limits>
+
+#define FPS 60
+#define MAX_BPM 300
+
+TEST(HelloTest, Zong_SSF_Stage1) {
+    npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
+    npy::npy_data y_ref = npy::read_npy<double>("test2/ssf_t2_y_ref.npy");
+
+    std::vector<double> signal = fetch_y_axis(acc);
+    const size_t N = signal.size();
+
+    ASSERT_NEAR(1.7, signal[0], 1e-5);
+    ASSERT_NEAR(3.6, signal[1], 1e-5);
+    ASSERT_NEAR(4.3, signal[2], 1e-5);
+
+    // # de-trending using a high-pass has helped the SSF be less noisy!
+    // # but it adds delay...
+    // # <- we try reducing that to 100 ms delay.
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+    // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+    std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+    std::vector a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+    IirFilter filter(b, a);
+
+    //
+    // apply high-pass filter
+    //
+    std::vector<double> y;
+    y.resize(N);
+    for (int i = 0; i < N; i++) {
+        y[i] = filter.filter(signal[i]);
+    }
+    // see: http://localhost:8888/notebooks/2026-02-25%20Accelero1/2026-03-01%20SSF3.ipynb
+
+    // check results
+    for (int i = 0; i < N; i++) {
+        //double rel_error = std::abs((y[i] - y_ref.data[i]) / y_ref.data[i]);
+        //ASSERT_NEAR(0, rel_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg
+        double abs_error = (y[i] - y_ref.data[i]);
+        //ASSERT_NEAR(0, abs_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg
+        ASSERT_NEAR(0, abs_error, 0.1 + ((double) i) * 1e-9); // hack: put in the index into error msg
+    }
+
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = y.data();
+    d.shape = {(unsigned long) N};
+    const std::string path{"test2/ssf_t2_y_out.npy"};
+    npy::write_npy(path, d);
+}
+
+TEST(HelloTest, Filter_Delta_U) {
+    Filt f_delta_u(2, 0, 0, std::vector {1.0, -1.0});
+    std::vector x { 1.0, 3.0, 2.0 };
+    std::vector y_ref { 1.0, 2.0, -1.0 };
+    std::vector<double> y;
+    y.resize(x.size());
+    for (int i = 0; i < x.size(); i++) {
+        y[i] = f_delta_u.filter(x[i]);
+    }
+    for (int i = 0; i < x.size(); i++) {
+        ASSERT_NEAR(y_ref[i], y[i], 1e-5);
+    }
+}
+
+// NOTE: later SSF must be fed -u, not u
+TEST(HelloTest, Filter_SSF) {
+    SsfFilter f_ssf(3);
+    std::vector x { 1.0, 3.0,  2.0, 5.0,  1.0, 1.5 };
+    //         du { 1.0, 2.0, -1.0, 3.0, -4.0, 0.5 }
+    //        duc { 1.0, 2.0,  0.0, 3.0,  0.0, 0.5 }
+    //        ssf { 1.0, 3.0,  3.0, 5.0,  3.0, 3.5 }
+    std::vector ssf_ref { 1.0, 3.0,  3.0, 5.0,  3.0, 3.5 };
+    std::vector<double> ssf;
+    ssf.resize(x.size());
+    for (int i = 0; i < x.size(); i++) {
+        ssf[i] = f_ssf.filter(x[i]);
+    }
+    for (int i = 0; i < x.size(); i++) {
+        ASSERT_NEAR(ssf_ref[i], ssf[i], 1e-5);
+    }
+}
+
+TEST(HelloTest, Zong_SSF_Stage2) {
+    npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
+
+    std::vector<double> signal = fetch_y_axis(acc);
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+    // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+    std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+    std::vector a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+    IirFilter filter(b, a);
+
+    // Stage 1: high-pass
+    auto y = apply_filter(filter, signal);
+
+    Filt f_neg(1, 0, 0, std::vector {-1.0});
+    auto y_neg = apply_filter(f_neg, y);
+
+    // Stage 2: sum slope function
+    const size_t upslope_width = 4;
+    SsfFilter f_ssf(upslope_width);
+    auto ssf = apply_filter(f_ssf, y_neg);
+
+    npy_save("test2/ssf_t2_ssf.npy", ssf);
+}
+
+TEST(HelloTest, Zong_SSF_Stage3) {
+    npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
+
+    std::vector<double> signal = fetch_y_axis(acc);
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+    // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+    std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+    std::vector a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+    IirFilter filter(b, a);
+
+    //std::cerr << "before stage 1" << std::endl;
+
+    // Stage 1: high-pass
+    auto y = apply_filter(filter, signal);
+
+    Filt f_neg(1, 0, 0, std::vector {-1.0});
+    auto y_neg = apply_filter(f_neg, y);
+
+    //std::cerr << "before stage 2" << std::endl;
+
+    // Stage 2: sum slope function
+    const size_t upslope_width = 4;
+    SsfFilter f_ssf(upslope_width);
+    auto ssf = apply_filter(f_ssf, y_neg);
+
+    //std::cerr << "before stage 3" << std::endl;
+
+    // Stage 3: threshold detection
+    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
+    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
+
+    //std::cerr << "before writing results 1 and doing step detection" << std::endl;
+
+    npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold);
+
+    SsfStepDetector f_ssd(len_refr);
+    auto steps = apply_filter(f_ssd, ssf);
+
+    //std::cerr << "before writing results 2" << std::endl;
+
+    npy_save("test2/ssf_t2_steps.npy", steps);
+}

google-tests/test3.cpp

@@ -0,0 +1,228 @@
+//
+// Created by david on 04.03.2026.
+//
+#include <gtest/gtest.h>
+#include "npy.hpp"
+//#include <utility>
+#include <deque>
+#include <iomanip>
+
+#include "pd_signal.h"
+#include "ssf_filter.h"
+#include "test_helpers.h"
+
+using namespace pd_signal;
+
+TEST(SignalTest, interp_t1) {
+    //EXPECT_EQ();
+    std::vector<double> xp     { 1.0,      2.0,      4.0, 5.0, 5.01, 6.0 };
+    std::vector<double> fp     { 1.0,      2.0,      4.0, 5.0, 5.01, 7.0 };
+
+    std::vector<double> x { 0.9, 1.0, 1.5, 2.0, 3.9,    4.1, 5.0, 5.5, 6.0, 6.1 };
+    std::vector<double>y_e{ 1.0, 1.0, 1.5, 2.0, 3.9,    4.1, 5.0, 5.99494949495, 7.0, 7.0 };
+    size_t N = x.size();
+    // 5.99494949495 = (5.5-5.01)/0.99*(7-5.01)+5.01
+
+    std::vector<double> y;
+    interp(y, x, xp, fp);
+
+    // 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[i], y[i], abs_err + 1e-9 * i);
+    }
+}
+
+TEST(SignalTest, cross_corr_t1) {
+    std::vector<double> sig {0.9, 1.5, 2.0, 3.0, 5.0, 4.0, 1.0, 0.5, 0.3, 0.2};
+    double corr = pd_signal::crossCorr(sig, sig);
+    ASSERT_NEAR(1.0, corr, 1e-7);
+}
+
+TEST(SignalTest, cross_corr_t2) {
+    std::vector<double> x {0.9, 1.5, 2.0, 3.0, 5.0, 4.0, 1.0, 0.5, 0.3, 0.2};
+    std::vector<double> y {0.4, 0.7, 0.9, 1.5, 2.5, 2.0, 0.5, 0.25, 0.15, 0.1};
+    double corr = pd_signal::crossCorr(x, y);
+    ASSERT_NEAR(0.999, corr, 1e-3);
+}
+
+TEST(SignalTest, resample_t1) {
+    std::vector<double> x   {0.9,      1.5,       2.0,      3.0,      5.0,      4.0,      1.0,       0.5,      0.3,       0.2};
+    std::vector<double> y_e {0.9, 1.2, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0, 5.0, 4.5, 4.0, 2.5, 1.0, 0.75, 0.5, 0.4, 0.3, 0.25, 0.2};
+
+    std::vector<double> t;
+    linspace(t, 0, (double) x.size()-1, y_e.size(), false);
+    // interp t=0.000 0.500 1.000 1.500 2.000 2.500 3.000 3.500 4.000 4.500 5.000 5.500 6.000 6.500 7.000 7.500 8.000 8.500 9.000
+    /*
+    std::cout << "interp t=";
+    for (size_t n = 0; n < t.size(); n++) {
+        std::cout << std::fixed << std::setw(5) << std::setprecision(3) << t[n] << " ";
+    }
+    std::cout << std::endl;
+    */
+
+    std::vector<double> y;
+    pd_signal::resample(y, x, y_e.size());
+
+    double corr = pd_signal::crossCorr(y, y_e);
+    ASSERT_NEAR(1.0, corr, 1e-3);
+}
+
+TEST(SignalTest, ranges) {
+    const double abs_error = 1e-5;
+    std::vector<double> i;
+    size_t N = 3;
+    linspace(i, 0, (int) (N-1), (int) N, false);
+    ASSERT_NEAR(0.0, i[0], abs_error);
+    ASSERT_NEAR(1.0, i[1], abs_error);
+    ASSERT_NEAR(2.0, i[2], abs_error);
+}
+
+class DebugRunningQuality : public RunningQuality {
+protected:
+    virtual void dispatchLocked() { locked = true; }
+    virtual void dispatchBeat(int idx, bool good, double posCorr) {
+        if (locked && lockedAt == -1)
+            lockedAt = idx;
+        goods.push_back(good);
+        corrs.push_back(posCorr);
+    }
+
+    int lockedAt;
+    bool locked;
+    std::vector<double> corrs;
+    std::vector<bool> goods;
+
+public:
+    DebugRunningQuality(): lockedAt(-1), locked(false) {}
+    explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
+    virtual ~DebugRunningQuality() {}
+    bool isLocked() { return locked; }
+    std::vector<double> getCorrs() { return corrs; }
+    std::vector<bool> getGoods() { return goods; }
+    int getLockedAt() { return lockedAt; }
+    std::vector<double> getBeatTemplate() { return this->beatTemplate; }
+};
+
+/*
+TEST(SignalTest, resample_same_len) {
+    std::vector<double> rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
+    std::vector<double> beat;
+    resample(beat, rawBeat, 7);
+    // TODO
+    ASSERT_NEAR(0.3, beat[1], 1e-6);
+}
+*/
+/*
+TEST(SignalTest, resample_same_len) {
+    std::vector<double> rawBeat {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
+    std::vector<double> beat;
+    resample(beat, rawBeat, 7);
+    // TODO
+    //ASSERT_NEAR(0.3, beat[1], 1e-6);
+    for (int i = 0; i < 7; i++)
+        std::cout << "b[" << i << "]=" << beat[i] << std::endl;
+}
+*/
+
+TEST(SignalTest, RunningQuality_t1) {
+    DebugRunningQuality sqi(true);
+    std::vector a {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
+    std::vector b {0.0, 0.3, 0.9, 1.0, 0.5, 0.5, 0.1};
+    std::vector c {0.0, 0.3, 0.9, 1.0, 0.9, 0.5, 0.1};
+    std::vector d {0.0, 0.3, 0.9, 1.0, 0.7, 0.4, 0.1};
+    sqi.append(a, a);
+    sqi.append(b, b);
+    sqi.append(c, c);
+    EXPECT_FALSE(sqi.isLocked());
+    sqi.append(d, d);
+    EXPECT_TRUE(sqi.isLocked());
+    ASSERT_EQ(1, sqi.getCorrs().size());
+    double norm = sqrt((0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.5*0.5 + 0.1*0.1) // \sum x_i^2
+                        * (0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.4*0.4 + 0.1*0.1)); // \sum y_i^2
+    double num =          (0.3*0.3 + 0.9*0.9 + 1.0 + 0.7*0.7 + 0.5*0.4 + 0.1*0.1); // \sum x_i * y_i
+    //ASSERT_NEAR(0.3, sqi.getBeatTemplate()[1], 1e-6);
+    //ASSERT_NEAR(0.7, sqi.getBeatTemplate()[4], 1e-6); // nb. resampled!
+    ASSERT_NEAR(num/norm, sqi.getCorrs()[0], 1e-3);
+}
+
+TEST(SignalTest, RunningQuality_t2) {
+    npy::npy_data acc = npy::read_npy<double>("test3/ssf_t3_acc.npy");
+
+    std::vector<double> signal = fetch_y_axis(acc);
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+    // TODO: SQI: cehck input file
+    // TODO: SQI: print debug values corr,idx, checkedSsf
+
+    // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+    std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+    std::vector a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+    IirFilter filter(b, a);
+
+    //std::cerr << "before stage 1" << std::endl;
+
+    // Stage 1: high-pass
+    auto y = apply_filter(filter, signal);
+
+    Filt f_neg(1, 0, 0, std::vector {-1.0});
+    auto y_neg = apply_filter(f_neg, y);
+
+    //std::cerr << "before stage 2" << std::endl;
+
+    // Stage 2: sum slope function
+    const size_t upslope_width = 4;
+    SsfFilter f_ssf(upslope_width);
+    auto ssf = apply_filter(f_ssf, y_neg);
+
+    //std::cerr << "before stage 3" << std::endl;
+
+    // Stage 3: threshold detection
+    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
+    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
+
+    //std::cerr << "before writing results 1 and doing step detection" << std::endl;
+
+    npy_save("test2/ssf_t3_ssf_threshold.npy", ssf_threshold);
+
+    SsfStepDetector f_ssd(len_refr);
+    auto steps = apply_filter(f_ssd, ssf);
+
+    //std::cerr << "before writing results 2" << std::endl;
+
+    npy_save("test2/ssf_t3_steps.npy", steps);
+
+    // Debug SQI
+
+    DebugRunningQuality sqi;
+
+    std::vector<double> beat_buf;
+    std::vector<double> ssf_buf;
+    // y, ssf
+    size_t N = y.size();
+    for (size_t i = 0; i < N; i++) {
+        if (steps[i] == 1.0) {
+            sqi.append(beat_buf, ssf_buf);
+            beat_buf.clear();
+            ssf_buf.clear();
+        }
+        beat_buf.push_back(y[i]);
+        ssf_buf.push_back(ssf[i]);
+    }
+
+    EXPECT_TRUE(sqi.isLocked());
+    EXPECT_TRUE(sqi.getCorrs().size() > 50);
+
+    EXPECT_TRUE(sqi.getLockedAt() < 10);
+    std::cout << "lockedAt=" << sqi.getLockedAt() << std::endl;
+
+    std::vector<double> corrs(sqi.getCorrs());
+    npy_save("test3/ssf_t3_sqi_corrs.npy", corrs);
+
+    std::vector<bool> goods(sqi.getGoods());
+    npy_save("test3/ssf_t3_sqi_goods.npy", goods);
+}

google-tests/test4.cpp

@@ -0,0 +1,38 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#include <gtest/gtest.h>
+
+#include "step_detector.h"
+#include "npy.hpp"
+#include "test_helpers.h"
+
+TEST(StepDetector, t1_sub_sample_resolution) {
+    npy::npy_data s = npy::read_npy<double>("test4/step_150a.npy");
+
+    std::vector<double> signal = fetch_y_axis(s);
+    const size_t N = signal.size();
+    const size_t N_INIT = SsfStepDetector::initial_samples();
+
+    StepDetector det(nullptr, true);
+
+    // initialize: feed for priming the filters
+    det.primeFilters(signal);
+
+    // feed for actual test
+    for (size_t i = 0; i < N; i++) {
+        const auto a_i = static_cast<float>(signal[i]);
+        det.filter(std::vector<float> {0.0f, a_i, 0.0f});
+    }
+
+    std::vector<double> ssd = det.getBufSsd(); // raw SsfStepDetector
+    std::vector<double> sqi = det.getBufSqi(); // SQI - RunningQuality beat correlations
+    std::vector<double> out = det.getBufOut(); // steps where SQI is OK
+
+    npy_save("test4/t1_ssd.npy", ssd);
+    npy_save("test4/t1_sqi.npy", sqi);
+    npy_save("test4/t1_out.npy", out);
+
+    // http://localhost:8888/notebooks/2026-03-10%20step%20interpolate%2F2026-03-15%20synth.ipynb
+}

google-tests/test_helpers.cpp

@@ -0,0 +1,53 @@
+//
+// Created by david on 11.03.2026.
+//
+
+#include "test_helpers.h"
+
+void npy_save(std::string path, std::vector<double>& x) {
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = x.data();
+    d.shape = {(unsigned long) x.size()};
+    npy::write_npy(path, d);
+}
+void npy_save(std::string path, std::vector<bool>& x) {
+    npy::npy_data_ptr<int> d;
+    std::vector<int> xx(x.begin(), x.end());
+    d.data_ptr = xx.data();
+    d.shape = {(unsigned long) x.size()};
+    npy::write_npy(path, d);
+}
+
+std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
+    // TODO: later on, we should use a vector projection towards gravity
+    std::vector<double> signal;
+    const size_t rows_real = acc.shape[0];
+#if DEBUG_IIR == 1
+    const size_t rows = 5;
+#else
+    const size_t rows = acc.shape[0];
+#endif
+    int stride = 3;
+    int offset = 1; // [x,y,z] per row - fetch y
+    signal.resize(rows);
+    if (acc.fortran_order) {
+        stride = 1;
+        offset = (int) rows_real;
+    }
+    /*
+    std::cout << "is_fortran=" << acc.fortran_order << std::endl;
+    for (size_t i = 0; i < 10; i++) {
+        std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl;
+    }
+    */
+    for (int i = 0; i < rows; i++) {
+        signal[i] = acc.data[i * stride + offset];
+    }
+    return signal;
+}
+
+DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {}
+double DebugSsfStepDetectorThreshold::filter(double val) {
+    this->SsfStepDetector::filter(val);
+    return peek_threshold();
+}

google-tests/test_helpers.h

@@ -0,0 +1,34 @@
+//
+// Created by david on 11.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_TEST_HELPERS_H
+#define PASADASUPERPROJECT_TEST_HELPERS_H
+
+#include "npy.hpp"
+#include "ssf_filter.h"
+#include <string>
+#include <vector>
+
+template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
+    std::vector<double> y;
+    y.resize(x.size());
+    for (int i = 0; i < x.size(); i++) {
+        y[i] = filter.filter(x[i]);
+    }
+    return y;
+}
+
+void npy_save(std::string path, std::vector<double>& x);
+void npy_save(std::string path, std::vector<bool>& x);
+
+std::vector<double> fetch_y_axis(npy::npy_data<double>& acc);
+
+/** Returns the ssf_threshold as the filter output for debugging. */
+class DebugSsfStepDetectorThreshold : public SsfStepDetector {
+public:
+    DebugSsfStepDetectorThreshold(size_t len_refr);
+    double filter(double val);
+};
+
+#endif //PASADASUPERPROJECT_TEST_HELPERS_H

pasada-lib/CMakeLists.txt

@@ -2,6 +2,10 @@ project(Pasada_Lib)
 
 SET(PASADA_SRC
         library.cpp
+        iir_filter.cpp
+        ssf_filter.cpp
+        pd_signal.cpp
+        step_detector.cpp
 )
 
 if(PASADA_BUILD_TESTS)

pasada-lib/iir_filter.cpp

@@ -0,0 +1,68 @@
+//
+// Created by david on 02.03.2026.
+//
+
+#include "iir_filter.h"
+#include <iostream>
+
+#ifndef DEBUG_IIR
+#define DEBUG_IIR 0
+#endif
+
+#if (DEBUG_IIR == 1)
+#define DEBUG_PRINT(expr) do { expr; } while (0)
+#else
+#define DEBUG_PRINT(expr) while(0) { expr; }
+#endif
+
+Buf::Buf(size_t N): N(N), n(0) {
+    data.resize(N);
+    data.assign(N, 0.0);
+}
+void Buf::push(double val) {
+    data[n] = val;
+    n = (n+1) % N;
+}
+
+Filt::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 Filt::filter(double val) {
+    this->push(val);
+    return this->peek();
+}
+double Filt::peek() {
+    double sum = 0;
+    for (size_t i = offset; i < this->N; i++) {
+        //size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
+        size_t n = (this->N + this->n - i + shift - 1) % this->N;
+        DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl);
+        sum += this->data[n] * this->taps[i];
+    }
+    return sum;
+}
+void Filt::push(double val) {
+    Buf::push(val);
+}
+void Filt::prime(double val) {
+    data.assign(this->N, val);
+}
+size_t Filt::size() {
+    return this->N;
+}
+
+IirFilter::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 IirFilter::filter(double val) {
+    DEBUG_PRINT(std::cout << "x.filter(" << val << ")" << std::endl);
+    double xv = x.filter(val);
+    DEBUG_PRINT(std::cout << "xv=" << xv << std::endl);
+    DEBUG_PRINT(std::cout << "y.peek()" << std::endl);
+    double yv = y.peek();
+    DEBUG_PRINT(std::cout << "yv=" << yv << std::endl);
+    DEBUG_PRINT(std::cout << "---" << std::endl);
+    double yo = xv - yv;
+    y.push(yo);
+    return yo;
+}

pasada-lib/include/iir_filter.h

@@ -0,0 +1,49 @@
+//
+// Created by david on 02.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_IIR_FILTER_H
+#define PASADASUPERPROJECT_IIR_FILTER_H
+
+#include <utility>
+#include <vector>
+#include <stdexcept>
+
+/** Shift register implemented as a circular buffer. */
+class Buf {
+protected:
+    std::vector<double> data;
+    size_t N;
+    size_t n;
+public:
+    Buf(size_t N);
+    void push(double val);
+};
+
+/** Running filter base. */
+class Filt : public 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);
+    double filter(double val);
+    double peek();
+    void push(double val);
+    /** prime the filter by overwriting the entire buffer with 'val' */
+    void prime(double val);
+    size_t size();
+};
+
+/** Running IIR filter. */
+class IirFilter {
+protected:
+    Filt y;
+    Filt x;
+public:
+    IirFilter(std::vector<double> b, std::vector<double> a);
+    double filter(double val);
+};
+
+#endif //PASADASUPERPROJECT_IIR_FILTER_H

pasada-lib/include/pd_signal.h

@@ -0,0 +1,42 @@
+//
+// Created by david on 04.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_SIGNAL_H
+#define PASADASUPERPROJECT_SIGNAL_H
+
+#include <vector>
+#include <deque>
+
+namespace pd_signal {
+    /** `num` evenly spaced numbers over interval [start,stop] */
+    void linspace(std::vector<double>& data, double start, double stop, int num);
+    /** `num` evenly spaced numbers over interval [start,stop] with endpoint=true or [start,stop) with endpoint=false */
+    void linspace(std::vector<double>& data, double start, double stop, int num, bool endpoint);
+
+    /**
+     * Evaluate at points x the function given by the samples fp[xp[n]].
+     * Returned in y.
+     */
+    void interp(std::vector<double>& y, std::vector<double>& x, std::vector<double>& xp, std::vector<double>& fp);
+
+    /** resample to BEAT_LEN */
+    void resample(std::vector<double> &out, std::vector<double> &x, int beat_len);
+
+    /**
+     * normalized cross-correlation of the two signals of same length.
+     * normalization factor is <c>1 / sqrt(\sum_i x_i^2 * \sum_i y_i^2)</c>
+     */
+    double crossCorr(std::vector<double> &x, std::vector<double> &y);
+
+    /** clip 'val' to between 'a_min' and 'a_max'. */
+    double clip(double val, double a_min, double a_max);
+
+    /** two-dimensional mean of a collection of signals */
+    void mean(std::vector<double> &out, std::vector<std::vector<double> >& m);
+    /** two-dimensional mean of a collection of signals */
+    void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
+
+}
+
+#endif //PASADASUPERPROJECT_SIGNAL_H

pasada-lib/include/ssf_filter.h

@@ -0,0 +1,122 @@
+//
+// Created by david on 03.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_SSF_FILTER_H
+#define PASADASUPERPROJECT_SSF_FILTER_H
+
+#include "iir_filter.h"
+#include <deque>
+
+#define FPS 60
+#define MAX_BPM 300
+
+/**
+ * Sum-Slope Function filter.
+ *
+ * See Zong et al, 2003: An open-source algorithm to detect onset of arterial blood pressure pulses.
+ */
+class SsfFilter {
+protected:
+    size_t sw;
+    Filt f_delta_u;
+    Filt f_window;
+public:
+    SsfFilter(size_t upslope_width);
+    double filter(double val);
+};
+
+/**
+ * Provides dirac pulses upon steps, based on SSF input.
+ *
+ * Settling time is LEN_INIT (currently 3.0 sec),
+ * no steps are detected before.
+ */
+class SsfStepDetector {
+protected:
+    const size_t LEN_INIT;
+    const size_t LEN_TH_WIN;
+    size_t num_samples;
+    double ssf_threshold;
+    double ssf_threshold_nm1;
+    Filt f_ssf_threshold_smoothing;
+    size_t len_refr;
+    size_t n_refr;
+    bool is_refr;
+    double ssf_nm1;
+    Filt f_ssf_mean;
+public:
+    /**
+     * @param len_refr duration of refractory period, in samples
+     */
+    SsfStepDetector(size_t len_refr);
+    double filter(double val);
+    double peek_threshold();
+
+    static size_t initial_samples();
+};
+
+/**
+ * Running signal quality indicator.
+ */
+class RunningQuality {
+protected:
+    // TODO: make it a filter (output proper samples)
+
+    /** template beat is resampled to this #samples */
+    const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */;
+
+    /** threshold for accepting initial beats */
+    const double BEAT_CORR_THR_1 = 0.9;
+    /** threshold for accepting subsequent beats */
+    const double BEAT_CORR_THR_2 = 0.8;
+    /** absolute SSF threshold for accepting any beat */
+    const double SSF_THRESHOLD = 5.0;
+    /** number of recent beats to use for beat template. must be even (alternating feet have different patterns; make it symmetric) */
+    const int    NUM_BEATS = 4;
+
+    std::deque<std::vector<double> > beatTemplates;
+    std::vector<double> beatTemplate;
+    //std::vector<std::pair<int, int> > badBeatRanges;
+    double beatCorrThr2;
+    bool justLocked;
+    int idx;
+
+    /** for debugging only - disable SSF_THRESHOLD */
+    bool disableSsf;
+
+    void addTemplate(std::vector<double>& x);
+
+    void replaceTemplate(std::vector<double>& x);
+
+    virtual void dispatchLocked();
+    virtual void dispatchBeat(int idx, bool good, double posCorr);
+
+public:
+    RunningQuality();
+    explicit RunningQuality(bool disableSsf);
+    virtual ~RunningQuality();
+
+    // note: arg should be an iterator really, but can do later
+    /**
+     * @param beat individual beat accelero signal
+     * @return true if it is good beat
+     */
+    bool append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+};
+
+/**
+ * Signal quality indicator.
+ */
+class RunningQualityFilter {
+protected:
+    RunningQuality f_sqi;
+    std::vector<double> beat_buf;
+    std::vector<double> ssf_buf;
+    double sqi;
+public:
+    RunningQualityFilter(size_t upslope_width);
+    double filter(double y, double ssf, double step);
+};
+
+#endif //PASADASUPERPROJECT_SSF_FILTER_H

pasada-lib/include/step_detector.h

@@ -0,0 +1,52 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_STEP_DETECTOR_H
+#define PASADASUPERPROJECT_STEP_DETECTOR_H
+
+#include "iir_filter.h"
+#include "ssf_filter.h"
+#include <vector>
+
+class StepListener {
+public:
+    virtual ~StepListener() {}
+    virtual void playBeat() = 0;
+};
+
+/**
+ * Step detector from accelerometer signal.
+ *
+ * Settling time is 3.0 sec (defined in SsfStepDetector.LEN_INIT),
+ * no steps are detected before.
+ */
+class StepDetector {
+protected:
+    StepListener *listener;
+    IirFilter f_highpass;
+    Filt f_neg;
+    SsfFilter f_ssf;
+    SsfStepDetector f_ssd;
+    RunningQualityFilter f_sqi;
+
+    bool debug;
+    std::vector<double> buf_ssd;
+    std::vector<double> buf_sqi;
+    std::vector<double> buf_out;
+
+public:
+    StepDetector(StepListener *listener, bool debug = false);
+    void filter(std::vector<float> values);
+    std::vector<double> getBufSsd();
+    std::vector<double> getBufSqi();
+    std::vector<double> getBufOut();
+
+    /**
+     * Prime the filters using the given input signal.
+     * Used for debugging (non-realtime processing) to align the signal.
+     */
+    void primeFilters(std::vector<double> sig);
+};
+
+#endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/pd_signal.cpp

@@ -0,0 +1,142 @@
+//
+// Created by david on 04.03.2026.
+//
+
+#include "include/pd_signal.h"
+#include <stdexcept>
+#include <algorithm>
+#include <iostream>
+
+namespace pd_signal {
+
+static void throwIfNotAscending(std::vector<double>& xp) {
+    size_t N = xp.size();
+    for(int i = 0; i < N - 1; i++)
+        if((xp[i+1] - xp[i]) <= 0.0)
+            throw std::invalid_argument("xp is not strictly monotonically ascending");
+}
+
+static int binarySearch(std::vector<double>& data, double x) {
+    auto p = std::equal_range(data.begin(), data.end(), x);
+    if(p.first == data.end()) return -((int) data.size()) - 1; // out of range on upper end
+    int insertion_point = (int) std::distance(data.begin(), p.first);
+    if(p.first != p.second) return insertion_point; // element found
+    return -(insertion_point) - 1; // no element found directly
+}
+
+void linspace(std::vector<double>& data, double start, double stop, int num) {
+    linspace(data, start, stop, num, true);
+}
+
+// `num` evenly spaced numbers over interval [start,stop] with endpoint=true or [start,stop) with endpoint=false
+void linspace(std::vector<double>& data, double start, double stop, int num, bool endpoint) {
+    if(num < 0) throw std::invalid_argument("num must be >= 0");
+
+    int end = endpoint ? num : (num-1);
+    double step = (stop - start) / (double) end;
+
+    double d = start;
+    data.resize(num);
+
+    for(int i = 0; i < num; i++) {
+        data[i] = d;
+        d += step;
+    }
+}
+
+// Evaluate at points x the function given by the samples fp[xp[n]].
+void interp(std::vector<double>& y, std::vector<double>& x, std::vector<double>& xp, std::vector<double>& fp) {
+    if (xp.size() != fp.size()) throw std::invalid_argument("xp.size() != fp.size()");
+    size_t N = xp.size();
+    size_t M = x.size();
+    if (N < 2) throw std::invalid_argument("N < 2");
+    throwIfNotAscending(xp);
+
+    std::vector<double> slope, intercept;
+    y.resize(M);
+    slope.resize(N-1);
+    intercept.resize(N-1);
+
+    for(int i = 0; i < N-1; i++) {
+        double dxp = xp[i+1] - xp[i];
+        double dfp = fp[i+1] - fp[i];
+        slope[i] = dfp / dxp;
+        intercept[i] = fp[i] - slope[i] * xp[i];
+    }
+
+    for(int i = 0; i < M; i++) {
+        if(x[i] < xp[0]) {
+            y[i] = fp[0];
+        } else if(x[i] > xp[N-1]) {
+            y[i] = fp[N-1];
+        } else {
+            int idx = binarySearch(xp, x[i]);
+            if (idx <= -1) {
+                // not found precisely. interpolate
+                idx = -idx - 1; // convert back to array index (insertion point)
+                // idx==0 or idx==M cannot happen, handled above.
+                //assert(idx > 0 && idx < N);
+                idx = idx - 1; // one below insertion point (left point)
+                y[i] = slope[idx] * x[i] + intercept[idx];
+            } else {
+                // found precisely
+                y[i] = fp[idx];
+            }
+        }
+    }
+}
+
+// resample to BEAT_LEN
+void resample(std::vector<double> &out, std::vector<double> &x, int beat_len) {
+    std::vector<double> t;
+    std::vector<double> i;
+    linspace(t, 0, (double) (x.size()-1), beat_len, false);
+    linspace(i, 0, (int) (x.size()-1), (int) x.size(), false);
+    interp(out, t, i, x);
+}
+
+// normalized cross-correlation of the two signals of same length
+double crossCorr(std::vector<double> &x, std::vector<double> &y) {
+    if (x.size() != y.size()) throw std::invalid_argument("x.size() != y.size()");
+    double xs = 0.0, ys = 0.0, cs = 0.0;
+    for (size_t i = 0; i < x.size(); i++) {
+        xs += x[i] * x[i];
+        ys += y[i] * y[i];
+        cs += x[i] * y[i];
+    }
+    return cs / sqrt(xs * ys);
+}
+
+// clip 'val' to between 'a_min' and 'a_max'.
+double clip(double val, double a_min, double a_max) {
+    return std::min(std::max(val, a_min), a_max);
+}
+
+// two-dimensional mean of a collection of signals
+template<class T> void mean_tpl(std::vector<double> &out, T& m) {
+    if (m.empty()) {
+        out.resize(0);
+        return;
+    }
+    const size_t sz = m[0].size();
+    out.resize(sz);
+    out.assign(sz, 0.0);
+    const size_t N = m.size();
+    for (size_t i = 0; i < N; i++) {
+        for (size_t j = 0; j < sz; j++) {
+            out[j] += m[i][j];
+        }
+    }
+    for (size_t j = 0; j < sz; j++) {
+        out[j] /= static_cast<double>(N);
+    }
+}
+
+void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
+    mean_tpl(out, m);
+}
+void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
+    mean_tpl(out, m);
+}
+
+}

pasada-lib/ssf_filter.cpp

@@ -0,0 +1,202 @@
+//
+// Created by david on 03.03.2026.
+//
+
+#include "ssf_filter.h"
+#include "pd_signal.h"
+#include <limits>
+#include <cassert>
+#include <iomanip>
+#include <iostream>
+
+#ifndef DEBUG_SSF
+#define DEBUG_SSF 0
+#endif
+
+#if (DEBUG_SSF == 1)
+#define DEBUG_PRINT(expr) do { expr; } while (0)
+#else
+#define DEBUG_PRINT(expr) while(0) { expr; }
+#endif
+
+
+static std::vector<double> make_ones(size_t sw) {
+    std::vector<double> ones;
+    ones.resize(sw);
+    ones.assign(sw, 1.0);
+    return ones;
+}
+
+SsfFilter::SsfFilter(size_t upslope_width) :
+    sw(upslope_width),
+    // Filt(N, shift, offset, taps)
+    f_delta_u(2, 0, 0, std::vector<double> {1.0, -1.0}),
+    f_window(upslope_width, 0, 0, make_ones(upslope_width))
+{}
+double SsfFilter::filter(double val) {
+    double du = f_delta_u.filter(val);
+    double duc = std::max(0.0, du);
+    double ssf = f_window.filter(duc);
+    return ssf;
+}
+
+size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); }
+
+SsfStepDetector::SsfStepDetector(size_t len_refr) :
+    // note: also change above, in initial_samples()
+    LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold
+    LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
+    num_samples(0),
+    ssf_threshold(std::numeric_limits<double>::infinity()),
+    ssf_threshold_nm1(std::numeric_limits<double>::infinity()),
+    f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)),
+    len_refr(len_refr), n_refr(0), is_refr(false),
+    ssf_nm1(0.0),
+    f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN))
+{
+    assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
+}
+double SsfStepDetector::filter(double ssf) {
+    double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN);
+    double rv = 0.0;
+    if (num_samples >= LEN_INIT) {
+        // initial and subsequent threshold setting.
+        ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
+    }
+    // threshold crossing detection
+    // 'is_prev_lower' fixes a glitch where a falling threshold leads to undetected crossings
+    bool is_prev_lower = ssf_nm1 < ssf_threshold || ssf_nm1 < ssf_threshold_nm1;
+    bool is_cur_higher = ssf >= ssf_threshold;
+    bool is_txing = is_prev_lower && is_cur_higher;
+    // refractory period reset
+    if (num_samples - n_refr >= len_refr) is_refr = false;
+    // transition and not in refractory period? detected a step.
+    if (is_txing && !is_refr) {
+        rv = 1.0;
+        is_refr = true;
+        n_refr = num_samples;
+    }
+    if (num_samples == LEN_INIT) {
+        // initial threshold setting
+        ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
+        //DEBUG_PRINT(std::cerr << "before prime()" << std::endl);
+        f_ssf_threshold_smoothing.prime(ssf_threshold);
+    } else if (num_samples > LEN_TH_WIN) {
+        //DEBUG_PRINT(std::cerr << "adaptive threshold setting" << std::endl);
+        // adaptive threshold setting
+        // +2 is half the window size
+        // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number
+        if (num_samples == n_refr + 2) {
+            //DEBUG_PRINT(std::cerr << "setting adaptive threshold setting" << std::endl);
+            ssf_threshold_nm1 = ssf_threshold;
+            // the ssf peak comes 3 samples (half-window + 1 sample) after the crossing
+            ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6;
+        }
+    }
+    ssf_nm1 = ssf;
+    num_samples++;
+    return rv;
+}
+double SsfStepDetector::peek_threshold() {
+    return ssf_threshold;
+}
+
+
+void RunningQuality::addTemplate(std::vector<double>& x) {
+    beatTemplates.emplace_back(x);
+    while (beatTemplates.size() > NUM_BEATS) {
+        // sliding window on 'beat_templates', do not use all history
+        beatTemplates.pop_front();
+    }
+    pd_signal::mean(beatTemplate, beatTemplates);
+}
+
+void RunningQuality::replaceTemplate(std::vector<double>& x) {
+    beatTemplates.clear();
+    beatTemplates.emplace_back(x);
+    // essentially just a copy
+    pd_signal::mean(beatTemplate, beatTemplates);
+}
+
+void RunningQuality::dispatchLocked() { /* implement me, add Listener etc. */ }
+void RunningQuality::dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
+
+RunningQuality::RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
+RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
+RunningQuality::~RunningQuality() {}
+
+// note: arg should be an iterator really, but can do later
+bool RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
+    // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
+
+    std::vector<double> beat;
+    std::vector<double> ssf;
+    pd_signal::resample(beat, rawBeat, BEAT_LEN);
+    pd_signal::resample(ssf, rawSsf, BEAT_LEN);
+    //std::ranges::copy(rawBeat, std::back_inserter(beat));
+
+    // check ssf at sample 2 (mid-slope of 4 window of ssf)
+    // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding
+    double checkedSsf = ssf[(int) (2*((double)beat.size())/((double)rawBeat.size()))];
+
+    double corr = std::numeric_limits<double>::quiet_NaN();
+    double posCorr = std::numeric_limits<double>::quiet_NaN();
+    bool goodBeat = false;
+    if (beatTemplates.size() > 0) {
+        corr = pd_signal::crossCorr(ssf, beatTemplate);
+        posCorr = pd_signal::clip(corr, 0.0, 1.0);
+        double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1;
+        goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf);
+    }
+
+    if (beatTemplates.size() == 0) {
+        // cannot correlate the first beat, no template yet
+        DEBUG_PRINT(std::cerr << "(0) first beat -> addTemplate()" << std::endl);
+        addTemplate(ssf);
+        justLocked = false;
+    } else if (beatTemplates.size() <= 2) {
+        // restart if there is no clear correlation between beats
+        if (goodBeat) {
+            DEBUG_PRINT(std::cerr << "(2) good initial beat -> addTemplate()" << std::endl);
+            addTemplate(ssf);
+            if (beatTemplates.size() > 2)
+                justLocked = true;
+            //DEBUG_PRINT(std::cerr << "  (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl);
+        } else {
+            DEBUG_PRINT(std::cerr << "(2) bad initial beat idx=" << idx << " -> replaceTemplate() corr=" << std::fixed << std::setw(7) << std::setprecision(4) << corr << " checkedSsf=" << checkedSsf << std::endl);
+            replaceTemplate(ssf);
+            //badBeatRanges.clear();
+            justLocked = false;
+        }
+    } else {
+        // running mode: collect bad beats, but may be OK not to restart immediately
+
+        DEBUG_PRINT(std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl);
+        if (goodBeat) {
+            addTemplate(ssf);
+        } else {
+            // badBeatRanges.add(s, e)
+            // numNoisy++
+        }
+        // runningCorrs.add(posCorr)
+        if (justLocked) { dispatchLocked(); justLocked = false; }
+        dispatchBeat(idx, goodBeat, posCorr);
+    }
+    idx++;
+    if (!goodBeat) return 0.0;
+    return posCorr;
+}
+
+
+RunningQualityFilter::RunningQualityFilter(size_t upslope_width) : sqi(0.0) {}
+
+double RunningQualityFilter::filter(double y, double ssf, double step) {
+    if (step == 1.0) {
+        sqi = f_sqi.append(beat_buf, ssf_buf);
+        beat_buf.clear();
+        ssf_buf.clear();
+    }
+    beat_buf.push_back(y);
+    ssf_buf.push_back(ssf);
+    return sqi;
+}

pasada-lib/step_detector.cpp

@@ -0,0 +1,70 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#include "step_detector.h"
+
+// TODO: we are hardcoding filter coefficients for 60 Hz
+// TODO: this is tolerable for 50 Hz
+
+// TODO: check if we can do with floats instead of doubles
+// (check how much the [already bad] accuracy of filtering suffers)
+
+// TODO: in Java, check if delta timestamps effectively match FPS
+// TODO: FPS constant should be passed as argument to C++ (but we keep an FPS define to validate the coefficients)
+
+// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+static std::vector<double> hpf_taps_b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+static std::vector<double> hpf_taps_a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+static size_t upslope_width = 4;
+const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
+
+StepDetector::StepDetector(StepListener *listener, bool debug) :
+    listener(listener),
+    f_highpass(hpf_taps_b, hpf_taps_a),
+    f_neg(1, 0, 0, std::vector<double> {-1.0}),
+    f_ssf(upslope_width),
+    f_ssd(len_refr),
+    f_sqi(upslope_width),
+    debug(debug)
+{}
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+void StepDetector::filter(std::vector<float> values) {
+    // TODO: later on, we should use a vector projection towards gravity
+    auto s0 = (double) values[1]; // take y-axis value for now
+    auto s1 = f_highpass.filter(s0);
+    auto s2 = f_neg.filter(s1);
+    auto s3 = f_ssf.filter(s2);
+    auto s4 = f_ssd.filter(s3);
+    auto q5 = f_sqi.filter(s2, s3, s4);
+    if (debug) {
+        buf_ssd.push_back(s4);
+        buf_sqi.push_back(q5);
+        buf_out.push_back(s4 * (q5 > 0.0 ? 1.0 : 0.0));
+    }
+    // is step, step quality is OK, and we have a listener?
+    if(s4 > 0.0 && q5 > 0.0 && listener != nullptr) {
+        listener->playBeat();
+    }
+}
+
+std::vector<double> StepDetector::getBufSsd() { return buf_ssd; }
+std::vector<double> StepDetector::getBufSqi() { return buf_sqi; }
+std::vector<double> StepDetector::getBufOut() { return buf_out; }
+
+void StepDetector::primeFilters(std::vector<double> sig) {
+    const size_t N_INIT = SsfStepDetector::initial_samples();
+    // initialize: feed for priming the filters
+    for (size_t i = 0; i < N_INIT; i++) {
+        const auto a_i = static_cast<float>(sig[i]);
+        filter(std::vector<float> {0.0f, a_i, 0.0f});
+    }
+    // clear debug buffers
+    buf_ssd.clear();
+    buf_sqi.clear();
+    buf_out.clear();
+}