feat: Resampler: Normalizes incoming Android sensor sampling rate

pasada-lib/pd_resamp.cpp

@@ -0,0 +1,103 @@
+//
+// Created by david on 17.05.2026.
+//
+
+#include "pd_resamp.h"
+#include "pd_signal.h"
+
+Resampler::Resampler(): N(INITIAL_SAMPLES+1), n(0), m(0), initialized(false), read_valid(false), fs(0.0) {
+    times.resize(N);
+    data.resize(N);
+    times.assign(N, 0.0);
+    data.assign(N, 0.0);
+}
+
+void Resampler::push(double ts, double val) {
+    // i: previous write position
+    auto i = static_cast<size_t>((static_cast<int>(n) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    auto im = static_cast<size_t>((static_cast<int>(m) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    if (ts < times[i]) throw std::invalid_argument("we expect ts to be time-ascending");
+    // j: current write position
+    auto j = n;
+    times[n] = ts;
+    data[n] = val;
+    n = (n+1) % N;
+    // note: we do not currently handle overrun (assume caller keeps contract)
+    if (n == INITIAL_SAMPLES && !initialized) {
+        compute_fs();
+        read_valid = true;
+        return; // returns INITIAL_SAMPLES all at once
+        // ??? need to compute 'dt' etc. for last sample! -> fall through
+    }
+    // once initialized, we skip ahead as much as possible - avoid buffering too much
+    double dt = ts - times[im];
+    double dtr = dt * 1e-9 * fs;
+    if (dtr < 0) { throw std::invalid_argument("dt is negative"); }
+    // case 1: 'ts' is less than next sample
+    if (dtr < 0.99) {
+        // drop samples (cannot be bothered with interpolation):
+        // practice shows that Android initially provides a high sampling rate, which subsequently drops later on,
+        // so we simply skip the implementation here.
+        read_valid = false;
+        return;
+    }
+    // case 2: 'ts' is exactly next sample
+    if (0.99 < dtr && dtr < 1.01) {
+        m = j; // skip directly to actual sample
+        read_valid = true;
+        return;
+    }
+    // case 3: 'ts' skips samples
+    if (dtr >= 1.01) {
+        auto ts_nm1 = times[i];
+        // x = ts[n-1] + np.linspace(1.0, dtr, int(np.round(dtr)), endpoint=True) / fs * 1e9
+        // xp = [ts[n-1], ts[n]]
+        // fp = [a[n-1], a[n]]
+        // y = np.interp(x, xp, fp)
+        std::vector<double> y;
+        std::vector<double> x;
+        std::vector<double> xp { times[i], ts };
+        std::vector<double> fp { data[i], val };
+        pd_signal::linspace(x, 1.0, dtr, static_cast<int>(round(dtr)), false);
+        for (auto& e : x) e = ts_nm1 + e / fs * 1e9;
+        pd_signal::interp(y, x, xp, fp);
+        // write to data[_ : n]
+        int s = static_cast<int>(x.size());
+        auto p0 = static_cast<size_t>((static_cast<int>(n) - s + static_cast<int>(N)) % static_cast<int>(N));
+        for (int p = 0; p < s; p++) {
+            data[(p0 + p) % N] = y[p];
+        }
+        m = p0; // provides round(dtr) samples output, interpolated
+        read_valid = true;
+        return;
+    }
+}
+
+bool Resampler::peek() {
+    if (!initialized) { return false; }
+    if (!read_valid) { return false; }
+    return n != m;
+}
+
+double Resampler::get() {
+    if (!initialized) { throw std::runtime_error("not initialized"); }
+    if (n == m) { throw std::runtime_error("empty buffer"); }
+    double val = data[m];
+    m = (m+1) % N;
+    return val;
+}
+
+double Resampler::get_fs() const {
+    return fs;
+}
+
+void Resampler::compute_fs() {
+    // compute 'fs' according to first INITIAL_SAMPLES
+    // we ignore 'n' as this is only ever called once per Resampler lifetime, and assume 'times' has been filled from 0 on
+    std::vector<double> delta_times;
+    pd_signal::diff(delta_times, times);
+    delta_times.resize(INITIAL_SAMPLES-1); // trim off trailing buffer slot (which is not filled)
+    double mean_dt = pd_signal::mean(delta_times);
+    fs = 1e9 / mean_dt;
+    initialized = true;
+}