//
// Created by david on 04.03.2026.
//
#include <gtest/gtest.h>
//#include <utility>
#include <deque>
#include "pd_signal.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, 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);
}

/**
 * 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) {
        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 replaceTemplate(std::vector<double>& x) {
        beatTemplates.clear();
        beatTemplates.emplace_back(x);
        // essentially just a copy
        pd_signal::mean(beatTemplate, beatTemplates);
    }

    virtual void dispatchLocked() { /* implement me, add Listener etc. */ }
    virtual void dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }

public:
    RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
    explicit RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
    virtual ~RunningQuality() {}

    // note: arg should be an iterator really, but can do later
    /**
     * @param beat individual beat accelero signal
     */
    void 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;
        resample(beat, rawBeat, BEAT_LEN);
        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
            std::cerr << "(0) first beat -> addTemplate()" << std::endl;
            addTemplate(beat);
            justLocked = false;
        } else if (beatTemplates.size() <= 2) {
            // restart if there is no clear correlation between beats
            if (goodBeat) {
                std::cerr << "(2) good initial beat -> addTemplate()" << std::endl;
                addTemplate(beat);
                if (beatTemplates.size() > 2)
                    justLocked = true; // TODO why not set? wrong compiler optimization? (is it unaware of member change?)
                //std::cerr << "  (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl;
            } else {
                std::cerr << "(2) bad initial beat -> replaceTemplate()" << std::endl;
                replaceTemplate(beat);
                //badBeatRanges.clear();
                justLocked = false;
            }
        } else {
            // running mode: collect bad beats, but may be OK not to restart immediately

            std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl;
            if (goodBeat) {
                addTemplate(beat);
            } else {
                // badBeatRanges.add(s, e)
                // numNoisy++
            }
            // runningCorrs.add(posCorr)
            if (justLocked) { dispatchLocked(); justLocked = false; }
            dispatchBeat(idx, goodBeat, posCorr);
        }
        idx++;
    }
};

class DebugRunningQuality : public RunningQuality {
protected:
    virtual void dispatchLocked() { locked = true; }
    virtual void dispatchBeat(int idx, bool good, double posCorr) { corrs.push_back(posCorr); }

    bool locked;
    std::vector<double> corrs;

public:
    DebugRunningQuality(): locked(false) {}
    explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
    virtual ~DebugRunningQuality() {}
    bool isLocked() { return locked; }
    std::vector<double> getCorrs() { return corrs; }
    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);
}