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

#include <algorithm>

using namespace fgc;

namespace {

// Tower 121's yaw, fully declared: the case the handshake exists to serve.
AxisConfigSource iotaYaw() {
    AxisConfigSource s;
    s.have_current   = true;
    s.global_scaler  = 80;
    s.run_irun       = 31;
    s.homing_irun    = 7;
    s.center_count   = 500000;
    s.soft_margin    = 5000;
    s.have_travel    = true;
    s.steps_per_rev  = 1210778;
    s.have_deviation = true;
    s.enc_deviation  = 1500;
    s.normal_vmax    = 39738;  // 8 deg/s at 3552.87 counts/deg
    s.homing_vmax    = 30000;
    s.center_vmax    = 25000;
    s.have_wiring    = true;
    s.sw_mode        = 0;      // yaw: no physical stops, StallGuard homing
    s.has_encoder    = true;
    return s;
}

bool sent(const FirmwareConfigPlan& p, const std::string& text) {
    return std::find(p.sets.begin(), p.sets.end(), text) != p.sets.end();
}

}  // namespace

TEST_CASE("a NEED line parses into an axis and its keys") {
    auto r = parseNeedLine("NEED Y steps_per_rev global_scaler center margin");
    REQUIRE(r);
    CHECK(r->axis == 'Y');
    REQUIRE(r->keys.size() == 4);
    CHECK(r->keys[0] == "steps_per_rev");
    CHECK(r->keys[3] == "margin");

    auto p = parseNeedLine("NEED P sw_mode");
    REQUIRE(p);
    CHECK(p->axis == 'P');
}

TEST_CASE("an axis that needs nothing is well-formed, not a parse failure") {
    // How a firmware built for fewer axes than the rig has answers for the axis
    // it does not drive.
    auto r = parseNeedLine("NEED P");
    REQUIRE(r);
    CHECK(r->axis == 'P');
    CHECK(r->keys.empty());
    CHECK(planFirmwareConfig(*r, iotaYaw()).complete());
}

TEST_CASE("anything that is not a NEED line is ignored") {
    CHECK_FALSE(parseNeedLine("ST Y:R,0,0,00000000,0,0,0,S"));
    CHECK_FALSE(parseNeedLine("NEEDCFG"));
    CHECK_FALSE(parseNeedLine("NEED"));
    CHECK_FALSE(parseNeedLine("NEED X steps_per_rev"));  // not an axis
    CHECK_FALSE(parseNeedLine("NEED YY steps_per_rev"));
    CHECK_FALSE(parseNeedLine(""));
}

TEST_CASE("a fully declared axis is answered completely, in the order asked") {
    auto r = parseNeedLine(
        "NEED Y steps_per_rev global_scaler sw_mode enc_deviation normal_speed homing_speed "
        "center_speed center margin");
    REQUIRE(r);
    auto p = planFirmwareConfig(*r, iotaYaw());

    CHECK(p.complete());
    CHECK(p.unanswered.empty());
    CHECK(p.unknown.empty());
    REQUIRE(p.sets.size() == 9);
    CHECK(p.sets.front() == "SET Y steps_per_rev 1210778");
    CHECK(sent(p, "SET Y global_scaler 80"));
    CHECK(sent(p, "SET Y enc_deviation 1500"));
    CHECK(sent(p, "SET Y normal_speed 39738"));
    CHECK(p.sets.back() == "SET Y margin 5000");
}

TEST_CASE("center/margin are accepted under both their names") {
    auto legacy = parseNeedLine("NEED Y center margin");
    auto full   = parseNeedLine("NEED Y center_count soft_margin");
    REQUIRE(legacy);
    REQUIRE(full);
    CHECK(planFirmwareConfig(*legacy, iotaYaw()).sets[0] == "SET Y center 500000");
    CHECK(planFirmwareConfig(*full, iotaYaw()).sets[0] == "SET Y center_count 500000");
}

TEST_CASE("an undeclared current is unanswered, never sent as zero") {
    // A silently zeroed current is a motor that cannot hold, so the key must go
    // unanswered and leave the firmware in NEEDCFG.
    auto s = iotaYaw();
    s.have_current = false;
    auto r = parseNeedLine("NEED Y global_scaler run_irun homing_irun");
    REQUIRE(r);
    auto p = planFirmwareConfig(*r, s);

    CHECK_FALSE(p.complete());
    CHECK(p.sets.empty());
    CHECK(p.unanswered.size() == 3);
}

TEST_CASE("a speed that could not be converted is unanswered, not VMAX 0") {
    // VMAX 0 is a stopped axis, not a slow one. AxisSpeed refuses an
    // uncalibrated scale, and that refusal has to survive into the handshake.
    auto s = iotaYaw();
    s.normal_vmax = 0;
    auto r = parseNeedLine("NEED Y normal_speed homing_speed");
    REQUIRE(r);
    auto p = planFirmwareConfig(*r, s);

    CHECK_FALSE(p.complete());
    REQUIRE(p.unanswered.size() == 1);
    CHECK(p.unanswered[0] == "normal_speed");
    CHECK(sent(p, "SET Y homing_speed 30000"));
}

TEST_CASE("a key this host has never heard of is a version skew, not a gap") {
    // The two need different fixes - upgrade the host vs. fill in hardware.ini -
    // so they are reported separately.
    auto r = parseNeedLine("NEED Y steps_per_rev flux_capacitor");
    REQUIRE(r);
    auto p = planFirmwareConfig(*r, iotaYaw());

    CHECK_FALSE(p.complete());
    CHECK(p.unanswered.empty());
    REQUIRE(p.unknown.size() == 1);
    CHECK(p.unknown[0] == "flux_capacitor");
    // The keys it DID understand are still answered.
    CHECK(sent(p, "SET Y steps_per_rev 1210778"));
}

TEST_CASE("has_encoder is sent as 1/0, and follows the wiring declaration") {
    auto s = iotaYaw();
    s.has_encoder = false;
    auto r = parseNeedLine("NEED Y has_encoder");
    REQUIRE(r);
    CHECK(planFirmwareConfig(*r, s).sets[0] == "SET Y has_encoder 0");

    s.have_wiring = false;
    CHECK_FALSE(planFirmwareConfig(*r, s).complete());
}

TEST_CASE("the pitch axis is addressed as P") {
    auto r = parseNeedLine("NEED P center margin");
    REQUIRE(r);
    auto p = planFirmwareConfig(*r, iotaYaw());
    CHECK(p.sets[0] == "SET P center 500000");
    CHECK(p.sets[1] == "SET P margin 5000");
}

// --- End to end through the mock's NEEDCFG mode -----------------------------

#include "fgc/mock/MockMotorController.h"

TEST_CASE("the mock in NEEDCFG asks, and refuses to move until answered") {
    MockMotorController m(/*needcfg=*/true);
    m.start();

    auto lines = m.takeNeedLines();
    REQUIRE(lines.size() == 2);
    CHECK(lines[0].rfind("NEED Y ", 0) == 0);
    CHECK(lines[1].rfind("NEED P ", 0) == 0);
    // Taking them clears the queue: they are a request, not a state to poll.
    CHECK(m.takeNeedLines().empty());

    // Before the answer, motion commands are rejected.
    CHECK_FALSE(m.configured());
    m.sendCommand("HOME");
    CHECK_FALSE(m.telemetry().yaw.ready());

    // Answer the way the host would.
    AxisConfigSource src;
    src.have_current   = true;
    src.global_scaler  = 80;
    src.center_count   = 500000;
    src.soft_margin    = 5000;
    src.have_travel    = true;
    src.steps_per_rev  = 1210778;
    src.have_deviation = true;
    src.enc_deviation  = 1500;
    src.normal_vmax    = 39738;
    src.homing_vmax    = 30000;
    src.center_vmax    = 25000;
    src.have_wiring    = true;

    // Answer BOTH axes, as the host does - the axis unlocks only when every key
    // it asked for has been answered.
    for (const auto& l : lines) {
        auto n = parseNeedLine(l);
        REQUIRE(n);
        auto plan = planFirmwareConfig(*n, src);
        REQUIRE(plan.complete());
        for (const auto& cmd : plan.sets) m.sendCommand(cmd);
    }

    // The values landed, under the names the firmware asked for.
    const auto got = m.settings();
    CHECK(got.at("Y.steps_per_rev") == "1210778");
    CHECK(got.at("Y.global_scaler") == "80");
    CHECK(got.at("Y.normal_speed") == "39738");
    CHECK(got.at("Y.center") == "500000");
    CHECK(got.at("Y.margin") == "5000");

    // And the axis now moves.
    CHECK(m.configured());
    m.sendCommand("HOME");
    CHECK(m.telemetry().yaw.ready());
}

TEST_CASE("the default mock is untouched by the handshake") {
    // Every existing test constructs it this way; it must behave as before.
    MockMotorController m;
    m.start();
    CHECK(m.takeNeedLines().empty());
    m.sendCommand("HOME");
    CHECK(m.telemetry().yaw.ready());
}

TEST_CASE("a partial answer leaves the axis in NEEDCFG") {
    // The case that matters: hardware.ini declares most of what the firmware
    // wants, so most SETs land - and the axis still must not move.
    MockMotorController m(/*needcfg=*/true);
    m.start();
    auto lines = m.takeNeedLines();
    REQUIRE(lines.size() == 2);

    auto need = parseNeedLine(lines[0]);
    REQUIRE(need);
    AxisConfigSource partial;          // only the always-known anchors
    partial.center_count = 500000;
    partial.soft_margin  = 5000;

    auto plan = planFirmwareConfig(*need, partial);
    CHECK_FALSE(plan.complete());
    for (const auto& cmd : plan.sets) m.sendCommand(cmd);

    CHECK_FALSE(m.configured());
    m.sendCommand("HOME");
    CHECK_FALSE(m.telemetry().yaw.ready());
}

TEST_CASE("RESET is not a way out of NEEDCFG") {
    // Found by reading the firmware before flashing it: Motor::reset() set
    // ST_RESET unconditionally, so an axis that was never told its drivetrain
    // would come back accepting HOME and MOVE on the compile-time table -
    // reached by a command typed for unrelated reasons.
    MockMotorController m(/*needcfg=*/true);
    m.start();
    m.takeNeedLines();

    m.sendCommand("RESET");
    CHECK_FALSE(m.configured());
    m.sendCommand("HOME");
    CHECK_FALSE(m.telemetry().yaw.ready());
    CHECK(m.telemetry().yaw.state == AxisState::NeedConfig);
}

TEST_CASE("the mock leaves NEEDCFG per axis, as the firmware does") {
    // A yaw-only tower never answers for pitch - the host refuses to configure
    // an axis the tower does not declare. Yaw must still report configured, or
    // anything waiting on it (--test, above all) waits for nothing.
    MockMotorController m(/*needcfg=*/true);
    m.start();
    auto lines = m.takeNeedLines();
    REQUIRE(lines.size() == 2);

    AxisConfigSource src;
    src.have_current   = true;
    src.global_scaler  = 80;
    src.center_count   = 500000;
    src.soft_margin    = 5000;
    src.have_travel    = true;
    src.steps_per_rev  = 1210778;
    src.have_deviation = true;
    src.enc_deviation  = 1500;
    src.normal_vmax    = 39738;
    src.homing_vmax    = 30000;
    src.center_vmax    = 25000;
    src.have_wiring    = true;

    auto yaw = parseNeedLine(lines[0]);
    REQUIRE(yaw);
    for (const auto& cmd : planFirmwareConfig(*yaw, src).sets) m.sendCommand(cmd);

    CHECK(m.axisConfigured('Y'));
    CHECK_FALSE(m.axisConfigured('P'));
    CHECK_FALSE(m.configured());   // "every axis" keeps its meaning
    const auto t = m.telemetry();
    CHECK(t.yaw.state == AxisState::Reset);
    CHECK(t.pitch.state == AxisState::NeedConfig);

    m.sendCommand("DUMP");
    const std::string d = m.lastDump();
    CHECK(d.find("DUMP Y state=1") != std::string::npos);
    CHECK(d.find("DUMP P state=5") != std::string::npos);
}