#include <doctest/doctest.h>

#include "fgc/Geometry.h"
#include "fgc/HeadingFit.h"

#include <cmath>

using namespace fgc;

namespace {

// A rig whose yaw runs at -529.8 counts/deg (the direction sense is flipped) with
// the homed centre at count 500000. Build the sighting a landmark at `bearing`
// would produce on a gimbal already calibrated to `cpd` / `at_center`.
long countsFor(double bearing, double cpd, double at_center, long center = 500000) {
    return center + static_cast<long>(std::lround((bearing - at_center) * cpd));
}

}  // namespace

TEST_CASE("two sightings recover the map exactly") {
    const double cpd = -529.8, at_center = 137.4;
    std::vector<HeadingSighting> s{
        {countsFor(90.0, cpd, at_center), 90.0},
        {countsFor(200.0, cpd, at_center), 200.0},
    };

    auto r = fitHeading(s, 500000);
    REQUIRE(r.ok);
    CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(1e-4));
    CHECK(r.deg_at_center == doctest::Approx(at_center).epsilon(1e-4));
    CHECK(r.n == 2);
    CHECK(r.r2 == doctest::Approx(1.0));
}

TEST_CASE("sighting order does not matter") {
    const double cpd = 983.33, at_center = 40.0;
    std::vector<HeadingSighting> forward{
        {countsFor(10.0, cpd, at_center), 10.0},
        {countsFor(70.0, cpd, at_center), 70.0},
        {countsFor(130.0, cpd, at_center), 130.0},
    };
    std::vector<HeadingSighting> jumbled{forward[2], forward[0], forward[1]};

    auto a = fitHeading(forward, 500000);
    auto b = fitHeading(jumbled, 500000);
    REQUIRE(a.ok);
    REQUIRE(b.ok);
    CHECK(a.counts_per_deg == doctest::Approx(b.counts_per_deg));
    CHECK(a.deg_at_center == doctest::Approx(b.deg_at_center));
}

TEST_CASE("a set straddling north fits as one line, not two") {
    // Centre points at 350 deg; landmarks at 320 and 30 sit either side of north.
    // Without unwrapping, 30 would look like it is 290 deg *below* 320 and the
    // fitted scale would come out with the wrong sign and magnitude.
    const double cpd = 100.0, at_center = 350.0;
    std::vector<HeadingSighting> s{
        {countsFor(320.0, cpd, at_center), 320.0},
        {countsFor(390.0, cpd, at_center), 30.0},  // 390 == 30 deg true
    };

    auto r = fitHeading(s, 500000);
    REQUIRE(r.ok);
    CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(1e-6));
    CHECK(r.deg_at_center == doctest::Approx(350.0).epsilon(1e-6));
    CHECK(r.span_deg == doctest::Approx(70.0));
}

TEST_CASE("more sightings are least-squares fitted and report a usable R^2") {
    const double cpd = 983.33, at_center = 40.0;
    std::vector<HeadingSighting> s;
    // A couple of counts of aiming slop per sighting, alternating.
    const long slop[] = {40, -35, 30, -45, 25};
    for (int i = 0; i < 5; ++i) {
        const double bearing = 10.0 + 40.0 * i;
        s.push_back({countsFor(bearing, cpd, at_center) + slop[i], bearing});
    }

    auto r = fitHeading(s, 500000);
    REQUIRE(r.ok);
    CHECK(r.n == 5);
    CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(0.01));
    CHECK(r.deg_at_center == doctest::Approx(at_center).epsilon(0.01));
    CHECK(r.r2 > 0.999);   // slop is tiny against a 160 deg baseline
}

TEST_CASE("a single sighting cannot determine a scale") {
    std::vector<HeadingSighting> s{{500000, 137.0}};
    auto r = fitHeading(s, 500000);
    CHECK_FALSE(r.ok);
    CHECK(r.error.find("two sightings") != std::string::npos);
}

TEST_CASE("a short baseline is refused rather than fitted") {
    // Two points ALWAYS fit exactly, so R^2 would be 1 and the result would look
    // perfect. Only the baseline reveals that a count of aiming error would swing
    // counts_per_deg wildly.
    const double cpd = 983.33, at_center = 40.0;
    std::vector<HeadingSighting> s{
        {countsFor(40.0, cpd, at_center), 40.0},
        {countsFor(43.0, cpd, at_center), 43.0},
    };
    auto r = fitHeading(s, 500000);
    CHECK_FALSE(r.ok);
    CHECK(r.error.find("apart") != std::string::npos);
    CHECK(r.span_deg == doctest::Approx(3.0));
}

TEST_CASE("distinct bearings at the same encoder position are refused") {
    // The gimbal did not move between sightings, so one bearing must be wrong.
    std::vector<HeadingSighting> s{{500000, 40.0}, {500000, 120.0}};
    auto r = fitHeading(s, 500000);
    CHECK_FALSE(r.ok);
}

TEST_CASE("identical sightings are refused") {
    std::vector<HeadingSighting> s{{500000, 40.0}, {500000, 40.0}};
    auto r = fitHeading(s, 500000);
    CHECK_FALSE(r.ok);
}

TEST_CASE("the fitted map round-trips through AxisMap") {
    const double cpd = -529.8, at_center = 137.4;
    std::vector<HeadingSighting> s{
        {countsFor(60.0, cpd, at_center), 60.0},
        {countsFor(210.0, cpd, at_center), 210.0},
    };
    auto r = fitHeading(s, 500000);
    REQUIRE(r.ok);

    AxisMap m;
    m.counts_per_deg = r.counts_per_deg;
    m.center_count   = 500000;
    m.deg_at_center  = r.deg_at_center;
    m.min_deg        = r.deg_at_center - 160.0;
    m.max_deg        = r.deg_at_center + 160.0;

    // The map now reads back the very bearings that were sighted.
    for (const auto& sight : s)
        CHECK(m.headingDeg(sight.counts) == doctest::Approx(sight.heading_deg).epsilon(1e-4));
}

TEST_CASE("parseHeadingSightings accepts the wizard's token form") {
    std::vector<HeadingSighting> out;
    std::string                  err;

    REQUIRE(parseHeadingSightings({"500123:137.4", "480000:-12"}, out, err));
    REQUIRE(out.size() == 2);
    CHECK(out[0].counts == 500123);
    CHECK(out[0].heading_deg == doctest::Approx(137.4));
    CHECK(out[1].counts == 480000);
    CHECK(out[1].heading_deg == doctest::Approx(-12.0));

    CHECK_FALSE(parseHeadingSightings({"500123"}, out, err));          // no colon
    CHECK_FALSE(parseHeadingSightings({"500123:"}, out, err));         // no bearing
    CHECK_FALSE(parseHeadingSightings({"abc:1"}, out, err));           // counts not a number
    CHECK_FALSE(parseHeadingSightings({"1:2:3"}, out, err));           // trailing junk
    CHECK_FALSE(parseHeadingSightings({"500123x:12"}, out, err));      // trailing junk in counts
    CHECK_FALSE(err.empty());
}