#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);
}
