#include <doctest/doctest.h>

#include "fgc/Calibration.h"

#include <cmath>

using namespace fgc;

TEST_CASE("linearFit recovers slope/intercept exactly for collinear points") {
    // counts = 983.33*deg + 500000  (a yaw-like calibration)
    std::vector<std::pair<double, double>> xy;
    for (int deg = -45; deg <= 45; deg += 10)
        xy.emplace_back(deg, 983.33 * deg + 500000.0);

    LinearFit f = linearFit(xy);
    REQUIRE(f.ok);
    CHECK(f.slope == doctest::Approx(983.33));
    CHECK(f.intercept == doctest::Approx(500000.0));
    CHECK(f.r2 == doctest::Approx(1.0));
}

TEST_CASE("linearFit is robust to noise (slope close, r2 high)") {
    std::vector<std::pair<double, double>> xy = {
        {-40, -39500}, {-20, -20100}, {0, 300}, {20, 19800}, {40, 40200}};
    LinearFit f = linearFit(xy);
    REQUIRE(f.ok);
    CHECK(f.slope == doctest::Approx(1000.0).epsilon(0.05));
    CHECK(f.r2 > 0.99);
}

TEST_CASE("linearFit rejects degenerate input") {
    CHECK_FALSE(linearFit({}).ok);
    CHECK_FALSE(linearFit({{1.0, 2.0}}).ok);          // single point
    CHECK_FALSE(linearFit({{5.0, 1.0}, {5.0, 9.0}}).ok);  // no x spread
}

TEST_CASE("circularMeanDeg handles the +/-180 wrap") {
    CHECK(circularMeanDeg({10, 20, 30}) == doctest::Approx(20.0));
    // Mean of 170 and -170 is 180 (not 0) — must not average naively to 0.
    CHECK(std::abs(circularMeanDeg({170, -170})) == doctest::Approx(180.0));
    CHECK(circularMeanDeg({}) == doctest::Approx(0.0));
}

TEST_CASE("unwrapNear shifts an angle to within +/-180 of the previous sample") {
    CHECK(unwrapNear(10.0, 20.0)   == doctest::Approx(20.0));    // already near
    CHECK(unwrapNear(350.0, 355.0) == doctest::Approx(355.0));
    CHECK(unwrapNear(350.0, 5.0)   == doctest::Approx(365.0));   // crossed 360 going up
    CHECK(unwrapNear(10.0, 355.0)  == doctest::Approx(-5.0));    // crossed 0 going down
    CHECK(unwrapNear(720.0, 10.0)  == doctest::Approx(730.0));   // multiple turns up
    CHECK(unwrapNear(-360.0, 10.0) == doctest::Approx(-350.0));  // multiple turns down
}

TEST_CASE("unwrapping a 0..360 sweep across the wrap yields a clean linear fit") {
    // A yaw sweep whose true (unwrapped) heading is 300..420 deg, but the IMU
    // reports it wrapped into 0..360. With per-sample unwrapNear the fit recovers
    // the straight line; without it the 360->0 jump would wreck R^2 (the bug from
    // the field calibration log).
    std::vector<std::pair<double, double>> pts;  // (deg, counts), counts = 100*heading
    double prev = 0.0;
    bool   have = false;
    for (int i = 0; i <= 12; ++i) {
        const double heading   = 300.0 + i * 10.0;          // 300..420 (true)
        const double reported  = std::fmod(heading, 360.0);  // 0..360 (wrapped)
        double d = have ? unwrapNear(prev, reported) : reported;
        prev = d;
        have = true;
        pts.emplace_back(d, heading * 100.0);
    }
    LinearFit f = linearFit(pts);
    CHECK(f.ok);
    CHECK(f.slope == doctest::Approx(100.0));
    CHECK(f.r2 > 0.9999);
}