libpasada/pasada-lib/step_detector.cpp

//
// Created by david on 15.03.2026.
//

#include "step_detector.h"

#include "pd_signal.h"

StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
    listener(listener),
    f_grav(fps),
    f_ssf(fps),
    f_ssd(fps),
    f_sqi(fps),
    debug(debug)
{}

static int gravity_num_taps(double fps) {
    return 5.0 * fps;
}

// 5 secs buffer, prime y with direction of gravity (for tests & faster init)
GravityFilter::GravityFilter(double fps) :
    N(gravity_num_taps(fps)),
    gauss_taps(pd_signal::gauss(N, N/2, N/4)),
    gx(N, 0, 0, gauss_taps),
    gy(N, 0, 0, gauss_taps),
    gz(N, 0, 0, gauss_taps)
{
    gy.prime(-9.81);
}

double GravityFilter::filter(std::vector<double> values) {
    gx.push(values[0]);
    gy.push(values[1]);
    gz.push(values[2]);
    double x = gx.peek(), y = gy.peek(), z = gz.peek();
    double g = sqrt(x * x + y * y + z * z);
    // e = mean(a)
    double ex = x / g, ey = y / g, ez = z / g;
    // e \in a
    double vx = values[0] * ex;
    double vy = values[1] * ey;
    double vz = values[2] * ez;
    return vx + vy + vz;
}

void StepDetector::filter(double ts, std::vector<float> values) {
    // resample to smooth over Android sensor FPS variations
    res_x.push(ts, values[0]);
    res_y.push(ts, values[1]);
    res_z.push(ts, values[2]);
    while (res_x.peek()) {
        double x = res_x.get(), y = res_y.get(), z = res_z.get();
        std::vector<double> samp { x, y, z };
        // gravity filtering
        double a = f_grav.filter(samp);
        // pass on accel sample
        filter_a(a);
    }
}

void StepDetector::filter_a(double s2) {
    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; }

double StepDetector::primeFilters(double fps, std::vector<double> sig) {
    const size_t N_INIT = SsfStepDetector::initial_samples(fps);
    // initialize: feed for priming the filters
    double ts = 0;
    for (size_t i = 0; i < N_INIT; i++) {
        const auto a_i = static_cast<float>(sig[i]);
        filter(ts, std::vector<float> {0.0f, a_i, 0.0f});
        ts += 1.0 / fps;
    }
    // clear debug buffers
    buf_ssd.clear();
    buf_sqi.clear();
    buf_out.clear();
    return ts;
}