//
// 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);
}