#include "doctest/doctest.h"
#include "fgc/AxisSpeed.h"

using namespace fgc;

namespace {
// The two real drivetrains. These numbers are the whole point of the module:
// the same VMAX means different things on them.
constexpr double kDelta = 983.33;   // tower 111 yaw, counts/deg
constexpr double kIota  = 3552.87;  // tower 121 yaw, counts/deg (derived from the homed travel)
}  // namespace

TEST_CASE("the clock constant matches what was measured on the rig") {
    // Three homing runs on tower 121 put it at ~0.73 counts/s per VMAX unit; the
    // datasheet's f_CLK/2^24 predicts 0.7153. If this drifts, every speed does.
    const double per_unit = vmaxToDegPerSec(1, 1.0);  // counts/s for counts_per_deg == 1
    CHECK(per_unit == doctest::Approx(0.7153).epsilon(0.001));
    CHECK(per_unit == doctest::Approx(0.73).epsilon(0.03));
}

TEST_CASE("the same VMAX is a different speed on each tower") {
    // This is the bug the module exists to remove: both towers carry 30000.
    CHECK(vmaxToDegPerSec(30000, kDelta) == doctest::Approx(21.8).epsilon(0.01));
    CHECK(vmaxToDegPerSec(30000, kIota) == doctest::Approx(6.04).epsilon(0.01));

    // And the host literal that was overriding the firmware on the Delta fleet.
    CHECK(vmaxToDegPerSec(50000, kDelta) == doctest::Approx(36.4).epsilon(0.01));
}

TEST_CASE("the fleet default resolves on both drivetrains") {
    auto d = planAxisSpeed(kDefaultScanSpeedDegPerSec, kDelta);
    REQUIRE(d.ok);
    CHECK(d.vmax == 10998);
    CHECK(d.actual_deg_s == doctest::Approx(8.0).epsilon(0.001));

    auto i = planAxisSpeed(kDefaultScanSpeedDegPerSec, kIota);
    REQUIRE(i.ok);
    CHECK(i.vmax == 39738);
    CHECK(i.actual_deg_s == doctest::Approx(8.0).epsilon(0.001));

    // Same physical speed, VMAX differing by the gear ratio - which is the whole
    // argument for declaring deg/s.
    CHECK(static_cast<double>(i.vmax) / d.vmax == doctest::Approx(kIota / kDelta).epsilon(0.001));
}

TEST_CASE("a round trip is stable to the integer quantisation") {
    for (double want : {2.0, 8.0, 21.8, 36.4}) {
        auto p = planAxisSpeed(want, kIota);
        REQUIRE(p.ok);
        CHECK(p.actual_deg_s == doctest::Approx(want).epsilon(0.001));
    }
}

TEST_CASE("direction sense belongs to Geometry, not to a speed") {
    // counts_per_deg may be negative to flip the axis; VMAX must not go negative.
    auto p = planAxisSpeed(8.0, -kDelta);
    REQUIRE(p.ok);
    CHECK(p.vmax > 0);
    CHECK(p.vmax == planAxisSpeed(8.0, kDelta).vmax);
}

TEST_CASE("an uncalibrated axis is refused, and the message says how to fix it") {
    auto p = planAxisSpeed(8.0, 0.0);
    CHECK_FALSE(p.ok);
    CHECK(p.error.find("counts_per_deg") != std::string::npos);
    CHECK(p.error.find("travel_deg") != std::string::npos);
}

TEST_CASE("AxisMap's uncalibrated default is refused, not quietly obeyed") {
    // The real bug this caught: an un-calibrated pitch sits at counts_per_deg
    // 1.0, which is > 0 and so passes a naive guard. 8 deg/s through it yields
    // VMAX 11 - an axis that looks configured and never finishes a sweep.
    auto p = planAxisSpeed(8.0, 1.0);
    CHECK_FALSE(p.ok);
    CHECK(p.error.find("not a calibrated drivetrain") != std::string::npos);

    // The floor is the encoder's own ungeared resolution, so a real axis - even
    // a direct-drive one - is never rejected.
    CHECK(planAxisSpeed(8.0, 11.2).ok);
}

TEST_CASE("a nonsensical speed is refused rather than silently substituted") {
    CHECK_FALSE(planAxisSpeed(0.0, kDelta).ok);
    CHECK_FALSE(planAxisSpeed(-5.0, kDelta).ok);
}

TEST_CASE("a speed below one VMAX unit fails instead of creeping") {
    // Substituting the slowest commandable speed would look like a working axis
    // that never finishes a sweep.
    auto p = planAxisSpeed(0.00001, kDelta);
    CHECK_FALSE(p.ok);
    CHECK(p.error.find("slowest commandable") != std::string::npos);
}

TEST_CASE("a speed beyond the register is clamped, and says so") {
    // 23-bit VMAX. On the IOTA's gear ratio this is reachable with a plausible
    // typo, so it must clamp rather than wrap.
    auto p = planAxisSpeed(5000.0, kIota);
    REQUIRE(p.ok);
    CHECK(p.clamped);
    CHECK(p.vmax == kTmcVmaxMax);
    // The reported speed is what the axis will really do, not what was asked.
    CHECK(p.actual_deg_s < 5000.0);
}

TEST_CASE("tower 121 cannot match tower 111's old speed, and the plan shows it") {
    // 36.4 deg/s on the IOTA needs VMAX ~181k - expressible, but 6x its current
    // 30000. Declaring deg/s is what makes that visible instead of mysterious.
    auto p = planAxisSpeed(36.4, kIota);
    REQUIRE(p.ok);
    CHECK(p.vmax > 180000);
    CHECK(p.vmax < 182000);
}