//
// Created by david on 10.05.2026.
//

#include <gtest/gtest.h>

#include "step_detector.h"
#include "npy.hpp"
#include "test_helpers.h"

/**
 * These test a sweep over running speed (6.0 - 18.0 km/h with - normally - 20 sec steps).
 * - 'acc_1' is with phone oscillations in a loose pocket
 * - 'acc_2' is with phone fixed in front, in a side orientation (TODO: getting y axis does not work here)
 * Both are 4-dim vectors with (ts, x, y, z) entries.
 */
TEST(HelloTest, Zong_SSF_Test5_a1) {
    npy::npy_data acc = npy::read_npy<double>("test5/acc_1.npy");

    std::vector<double> signal = fetch_y_axis(acc, 2); // (ts, x, y, z) entries -> fetch 'y'

    double fps = 60.0;

    // 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);
    //auto y = 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
    SsfFilter f_ssf(fps);
    auto ssf = apply_filter(f_ssf, y_neg);

    //std::cerr << "before stage 3" << std::endl;

    // Stage 3: threshold detection
    DebugSsfStepDetectorThreshold f_ssd_thr(fps);
    auto ssf_threshold = apply_filter(f_ssd_thr, ssf);

    //std::cerr << "before writing results 1 and doing step detection" << std::endl;

    npy_save("test5/ssf_a1_y.npy", y);
    npy_save("test5/ssf_a1_ssf.npy", ssf);
    npy_save("test5/ssf_a1_ssf_threshold.npy", ssf_threshold);

    SsfStepDetector f_ssd(fps);
    auto steps = apply_filter(f_ssd, ssf);

    //std::cerr << "before writing results 2" << std::endl;

    npy_save("test5/ssf_a1_steps.npy", steps);
}