#include <doctest/doctest.h>

#include "fgc/TelemetryParser.h"

using namespace fgc;

TEST_CASE("parseTelemetryLine reads a two-axis ST line") {
    auto t = parseTelemetryLine("ST Y:A,982,969,80084000,0,8,8,Se P:H,500000,-4,80084000,0,8,8,SL");
    REQUIRE(t.has_value());

    CHECK(t->yaw.state == AxisState::Ready);
    CHECK(t->yaw.xactual == 982);
    CHECK(t->yaw.xenc == 969);
    CHECK(t->yaw.drv_status == 0x80084000u);
    CHECK(t->yaw.cs == 8);
    CHECK(t->yaw.standstill == true);
    CHECK(t->yaw.moving() == false);

    REQUIRE(t->pitch_present);
    CHECK(t->pitch.state == AxisState::Homing);
    CHECK(t->pitch.xactual == 500000);
    CHECK(t->pitch.xenc == -4);
    CHECK(t->pitch.standstill == true);
    CHECK(t->pitch.endstop_l == true);
    CHECK(t->pitch.moving() == true);  // Homing counts as moving
}

TEST_CASE("parseTelemetryLine reads a yaw-only ST line (no flags field)") {
    auto t = parseTelemetryLine("ST Y:R,10,12,00000000,0,0,0");
    REQUIRE(t.has_value());
    CHECK(t->yaw.state == AxisState::Reset);
    CHECK(t->yaw.xenc == 12);
    CHECK(t->yaw.standstill == false);
    CHECK_FALSE(t->pitch_present);
}

TEST_CASE("parseTelemetryLine rejects non-ST and malformed lines") {
    CHECK_FALSE(parseTelemetryLine("").has_value());
    CHECK_FALSE(parseTelemetryLine("OK").has_value());
    CHECK_FALSE(parseTelemetryLine("ERR bad axis").has_value());
    CHECK_FALSE(parseTelemetryLine("DG DONE").has_value());
    CHECK_FALSE(parseTelemetryLine("$;100;2;3;4;1;5;123.5;0;55;21;200;").has_value());  // old format
    CHECK_FALSE(parseTelemetryLine("ST Y:A,982").has_value());          // too few fields
    CHECK_FALSE(parseTelemetryLine("ST P:A,1,2,0,0,0,0").has_value());  // no yaw segment
}

TEST_CASE("parseTelemetryLine tolerates trailing CR") {
    auto t = parseTelemetryLine("ST Y:A,1,2,0,0,0,0,S\r");
    REQUIRE(t.has_value());
    CHECK(t->yaw.standstill == true);
}

TEST_CASE("parseAxisSegment reads the encoder-deviation flag 'd'") {
    // 'd' is the TMC's hardware encoder-deviation latch and the only stall signal
    // that can actually assert on this rig; 's' (StallGuard) is inert in
    // StealthChop. They are separate fields so enabling StallGuard later stays
    // meaningful.
    auto a = parseAxisSegment("A,982,969,80084000,0,8,8,Sd");
    REQUIRE(a.has_value());
    CHECK(a->deviation == true);
    CHECK(a->standstill == true);
    CHECK(a->stall == false);   // 'd' must not be mistaken for StallGuard

    // Flag order carries no meaning, and unrelated flags must not be disturbed.
    auto b = parseAxisSegment("A,982,969,80084000,0,8,8,dSL");
    REQUIRE(b.has_value());
    CHECK(b->deviation == true);
    CHECK(b->standstill == true);
    CHECK(b->endstop_l == true);

    // Absent unless present, including when the flags field is missing entirely.
    auto c = parseAxisSegment("A,982,969,80084000,0,8,8,S");
    REQUIRE(c.has_value());
    CHECK(c->deviation == false);
    auto d = parseAxisSegment("A,982,969,80084000,0,8,8");
    REQUIRE(d.has_value());
    CHECK(d->deviation == false);

    // Both can be set at once; they are independent signals.
    auto e = parseAxisSegment("A,982,969,80084000,0,8,8,sd");
    REQUIRE(e.has_value());
    CHECK(e->stall == true);
    CHECK(e->deviation == true);
}

TEST_CASE("the NEEDCFG state char is understood, not read as unknown") {
    // The firmware refuses to move until the host answers its NEED lines, and
    // says so with 'C'. A host that did not know the code would map it to
    // Unknown, which downstream reads as "no telemetry arrived" - blaming the
    // serial link for a configuration gap.
    auto t = fgc::parseTelemetryLine("ST Y:C,0,0,00000000,0,0,0,S");
    REQUIRE(t);
    CHECK(t->yaw.state == fgc::AxisState::NeedConfig);
    CHECK_FALSE(t->yaw.state == fgc::AxisState::Unknown);

    auto lower = fgc::parseTelemetryLine("ST Y:c,0,0,00000000,0,0,0,S");
    REQUIRE(lower);
    CHECK(lower->yaw.state == fgc::AxisState::NeedConfig);
}

TEST_CASE("an axis in NEEDCFG is not ready") {
    auto t = fgc::parseTelemetryLine("ST Y:C,0,0,00000000,0,0,0,S");
    REQUIRE(t);
    CHECK_FALSE(t->yaw.ready());
}