#include <doctest/doctest.h>

#include "fgc/OperatingMode.h"

#include <string>
#include <vector>

using namespace fgc;

namespace {

std::vector<std::string> tok(const std::string& line) {
    std::vector<std::string> out;
    std::string              w;
    for (char c : line) {
        if (c == ' ') { if (!w.empty()) out.push_back(w); w.clear(); }
        else w += c;
    }
    if (!w.empty()) out.push_back(w);
    return out;
}

}  // namespace

// --- pan positions --------------------------------------------------------------

TEST_CASE("the user's example: 90 +/- 15 at 6 degrees is six pictures, 75 to 105") {
    const auto h = panHeadings({90.0, 30.0, 0.0}, 6.0);
    REQUIRE(h.size() == 6);
    const double want[] = {75, 81, 87, 93, 99, 105};
    for (size_t i = 0; i < h.size(); ++i) CHECK(h[i] == doctest::Approx(want[i]));
}

TEST_CASE("a span that does not divide evenly gets one more picture, never a gap") {
    const auto h = panHeadings({90.0, 32.0, 0.0}, 6.0);
    REQUIRE(h.size() == 7);
    CHECK(h.front() == doctest::Approx(74.0));
    CHECK(h.back() == doctest::Approx(106.0));
    for (size_t i = 1; i < h.size(); ++i) CHECK(h[i] - h[i - 1] <= 6.0 + 1e-9);
}

TEST_CASE("a span narrower than one picture is the one heading") {
    CHECK(panHeadings({90.0, 5.9, 0.0}, 6.0) == std::vector<double>{90.0});
    CHECK(panHeadings({90.0, 0.0, 0.0}, 6.0) == std::vector<double>{90.0});
}

TEST_CASE("exactly one picture wide is its two edges") {
    const auto h = panHeadings({90.0, 6.0, 0.0}, 6.0);
    REQUIRE(h.size() == 2);
    CHECK(h[0] == doctest::Approx(87.0));
    CHECK(h[1] == doctest::Approx(93.0));
}

TEST_CASE("a pan across north wraps its bearings") {
    const auto h = panHeadings({5.0, 30.0, 0.0}, 6.0);
    REQUIRE(h.size() == 6);
    CHECK(h.front() == doctest::Approx(350.0));
    CHECK(h[2] == doctest::Approx(2.0));
    CHECK(h.back() == doctest::Approx(20.0));
}

// --- the command ----------------------------------------------------------------

TEST_CASE("mode pan: pitch is optional and defaults to level") {
    auto r = parseModeCommand(tok("mode pan 90 30"));
    REQUIRE(r.valid);
    CHECK(r.mode == Mode::Pan);
    CHECK(r.pan.heading == 90.0);
    CHECK(r.pan.span == 30.0);
    CHECK(r.pan.pitch == 0.0);
    r = parseModeCommand(tok("mode pan 450 30 10"));
    REQUIRE(r.valid);
    CHECK(r.pan.heading == doctest::Approx(90.0));   // bearings wrap
    CHECK(r.pan.pitch == 10.0);
}

TEST_CASE("mode auto, mode manual, and a bare mode that only asks") {
    CHECK(parseModeCommand(tok("mode auto")).mode == Mode::Auto);
    CHECK(parseModeCommand(tok("mode MANUAL")).mode == Mode::Manual);
    const auto q = parseModeCommand(tok("mode"));
    CHECK(q.valid);
    CHECK(q.query);
}

TEST_CASE("a bad mode command is refused with the reason, never half-applied") {
    for (const char* bad : {"mode pan", "mode pan 90", "mode pan x 30", "mode pan 90 30x",
                            "mode pan 90 -1", "mode pan 90 361", "mode pan 90 30 up",
                            "mode pan 90 30 10 4", "mode sweep", "mode auto now"}) {
        CAPTURE(bad);
        const auto r = parseModeCommand(tok(bad));
        CHECK_FALSE(r.valid);
        CHECK_FALSE(r.error.empty());
    }
}

// --- what each mode accepts -------------------------------------------------------

TEST_CASE("auto and pan accept only what cannot move the gimbal or change a setting") {
    for (Mode m : {Mode::Auto, Mode::Pan}) {
        CAPTURE(modeName(m));
        for (const char* ok : {"help", "help imu", "gimbal status", "gimbal dump", "imu status",
                               "imu config", "debug", "trace serial on", "mode",
                               "mode manual", "mode pan 90 30", "cancel", "exit", "GIMBAL DUMP"}) {
            CAPTURE(ok);
            CHECK(allowedIn(m, ok));
        }
        for (const char* no : {"gimbal nudge yaw 5", "gimbal move 10,0", "gimbal raw \"HOME\"",
                               "gimbal home", "gimbal stop", "gimbal heading offset 2",
                               "gimbal speed y 1000", "set fps 1", "set camera profile night",
                               "imu zero", "imu profile general", "test", "test hardware",
                               "camera reset", "snapshot", "calib save", "gimbal"}) {
            CAPTURE(no);
            CHECK_FALSE(allowedIn(m, no));
        }
    }
}

TEST_CASE("manual accepts everything") {
    for (const char* any : {"gimbal nudge yaw 5", "gimbal raw \"HOME\"", "set fps 1", "test",
                            "camera reset", "exit", "anything at all"})
        CHECK(allowedIn(Mode::Manual, any));
}