#include <doctest/doctest.h>

#include "fgc/ScanGrid.h"

using namespace fgc;

TEST_CASE("parseScanCsv reads waypoints and skips comments/blanks") {
    auto pts = parseScanCsv("# header\n\n-90,10\n0, 20 \n90,30\n");
    REQUIRE(pts.size() == 3);
    CHECK(pts[0].yaw_deg == doctest::Approx(-90.0));
    CHECK(pts[1].pitch_deg == doctest::Approx(20.0));
    CHECK(pts[2].yaw_deg == doctest::Approx(90.0));
}

TEST_CASE("parseScanCsv throws on a malformed line") {
    CHECK_THROWS(parseScanCsv("0,0\nnonsense\n"));
    CHECK_THROWS(parseScanCsv("1,abc\n"));
}

TEST_CASE("generateScanGrid produces yaw_intervals x pitch_levels points") {
    ScanConfig c;
    c.yaw_intervals = 3;
    c.yaw_min_deg = -90;
    c.yaw_max_deg = 90;
    c.pitch_levels = "10,30";
    auto pts = generateScanGrid(c);
    REQUIRE(pts.size() == 6);
    // pitch-major, yaw inner (min -> max)
    CHECK(pts[0].yaw_deg == doctest::Approx(-90.0));
    CHECK(pts[0].pitch_deg == doctest::Approx(10.0));
    CHECK(pts[1].yaw_deg == doctest::Approx(0.0));
    CHECK(pts[2].yaw_deg == doctest::Approx(90.0));
    CHECK(pts[3].pitch_deg == doctest::Approx(30.0));
}

TEST_CASE("ScanGrid cursor always runs front to back") {
    ScanGrid g({{0, 0}, {1, 0}, {2, 0}});
    CHECK(g.current().yaw_deg == doctest::Approx(0.0));
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(2.0));
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(0.0));  // wrapped, not reversed
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(2.0));
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(0.0));  // and again
}

TEST_CASE("ScanGrid index wraps in step with the waypoints") {
    ScanGrid g({{0, 0}, {1, 0}, {2, 0}, {3, 0}});
    for (std::size_t pass = 0; pass < 3; ++pass) {
        for (std::size_t i = 0; i < g.size(); ++i) {
            CHECK(g.index() == i);
            CHECK(g.current().yaw_deg == doctest::Approx(static_cast<double>(i)));
            g.advance();
        }
    }
    CHECK(g.index() == 0);
}

TEST_CASE("ScanGrid reset returns to the first waypoint") {
    ScanGrid g({{0, 0}, {1, 0}, {2, 0}});
    g.advance();
    g.advance();
    CHECK(g.index() == 2);
    g.reset();
    CHECK(g.index() == 0);
    CHECK(g.current().yaw_deg == doctest::Approx(0.0));
    // Still forward after a reset - there is no direction state to get stuck in.
    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));
}

TEST_CASE("ScanGrid single point and empty are safe") {
    ScanGrid one({{5, 5}});
    one.advance();
    CHECK(one.current().yaw_deg == doctest::Approx(5.0));
    one.advance();
    CHECK(one.index() == 0);

    ScanGrid none;
    CHECK(none.empty());
    none.advance();  // must not divide by zero or index an empty vector
    CHECK(none.empty());
}