#include <doctest/doctest.h>

#include "fgc/CaptureScheduler.h"

#include <string>
#include <vector>

using namespace fgc;

namespace {

struct FakeMotor : IMotorController {
    MotorTelemetry           tel;
    std::vector<std::string> cmds;
    FakeMotor() { tel.pitch_present = true; }  // 2-axis rig by default; 1-axis tests clear this

    void           start() override {}
    void           stop() override {}
    void           sendCommand(const std::string& c) override { cmds.push_back(c); }
    MotorTelemetry telemetry() override { return tel; }
    std::string    lastDump() override { return ""; }
    std::string    lastDiag() override { return ""; }
    unsigned       diagSeq() const override { return 0; }
    bool           connected() const override { return true; }
};

struct FakeCamera : ICameraSource {
    int  triggers = 0;
    bool ok = true;
    void open() override {}
    void close() override {}
    void start() override {}
    void stop() override {}
    bool trigger() override { ++triggers; return ok; }
    void setFrameCallback(FrameCallback) override {}
    int  cameraCount() const override { return 1; }
};

struct FakeChannel : IControlChannel {
    ControlCommand next;
    int            last_status = -1;
    bool connect() override { return true; }
    void disconnect() override {}
    bool connected() const override { return true; }
    void publishStatus(int c) override { last_status = c; }
    void publishCamEvent(const CamEvent&) override {}
    ControlCommand poll() override {
        ControlCommand c = next;
        next = {};
        return c;
    }
};

// Simple linear map: 10 counts/deg, zero at 0. Yaw clamps -90..90, pitch 0..60.
Geometry testGeometry() {
    Geometry g;
    g.yaw   = {10.0, 0, -90.0, 90.0};
    g.pitch = {10.0, 0, 0.0, 60.0};
    return g;
}

// Mark both axes settled at the scheduler's current target.
void settleAt(FakeMotor& m, long yaw, long pitch) {
    m.tel.yaw.xenc = yaw;     m.tel.yaw.standstill = true;     m.tel.yaw.state = AxisState::Ready;
    m.tel.pitch.xenc = pitch; m.tel.pitch.standstill = true;   m.tel.pitch.state = AxisState::Ready;
    m.tel.pitch_present = true;
}

}  // namespace

TEST_CASE("CaptureScheduler sweeps the scan grid with MOVE + settle + trigger") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}, {90.0, 0.0}});  // -> yaw counts 0, then 900

    CaptureScheduler sch(motor, cam, chan, 1.0 /*img/s -> 1000ms*/, testGeometry(), grid,
                         [&] { return clock; });
    sch.setCaptureActive(true);

    chan.next.control_code_available = true;
    chan.next.control_code = 0;

    // Before the interval elapses: nothing moves, but status is echoed.
    clock = 500;
    sch.tick();
    CHECK(motor.cmds.empty());
    CHECK(chan.last_status == 0);

    // After the interval: MOVE to the first grid point (0,0) is issued.
    clock = 1600;
    sch.tick();
    REQUIRE(motor.cmds.size() == 1);
    CHECK(motor.cmds[0] == "MOVE 0,0");
    CHECK(sch.moving());
    CHECK(cam.triggers == 0);  // not yet settled

    // Report standstill at the target -> camera fires, cursor advances.
    settleAt(motor, 0, 0);
    clock = 1700;
    sch.tick();
    CHECK(cam.triggers == 1);
    CHECK_FALSE(sch.moving());

    // Next interval: MOVE to the second grid point (90 deg -> 900 counts).
    clock = 2800;
    sch.tick();
    CHECK(motor.cmds.back() == "MOVE 900,0");
}

TEST_CASE("CaptureScheduler scans a 1-axis (yaw-only) rig with yaw-only MOVE/settle") {
    long long   clock = 0;
    FakeMotor   motor;
    motor.tel.pitch_present = false;  // yaw-only gimbal: no P: segment in telemetry
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}, {90.0, 0.0}});

    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
    sch.setCaptureActive(true);
    chan.next.control_code_available = true;
    chan.next.control_code = 0;

    // The MOVE must address yaw alone, not the two-axis form a 1-axis firmware rejects.
    clock = 1600;
    sch.tick();
    REQUIRE(motor.cmds.size() == 1);
    CHECK(motor.cmds[0] == "MOVE Y 0");

    // Settle on yaw only (pitch never reports standstill on this rig) -> capture fires.
    motor.tel.yaw.xenc = 0;
    motor.tel.yaw.standstill = true;
    motor.tel.yaw.state = AxisState::Ready;
    clock = 1700;
    sch.tick();
    CHECK(cam.triggers == 1);

    clock = 2800;
    sch.tick();
    CHECK(motor.cmds.back() == "MOVE Y 900");
}

TEST_CASE("CaptureScheduler ControlCode 1 drives yaw to the target heading") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}});
    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
    sch.setCaptureActive(true);

    chan.next.control_code_available = true;
    chan.next.control_code = 1;
    chan.next.heading_available = true;
    chan.next.target_heading = "45";  // 45 deg * 10 = 450 counts

    clock = 1600;
    sch.tick();
    REQUIRE(!motor.cmds.empty());
    CHECK(motor.cmds.back() == "MOVE 450,0");
    CHECK(sch.controlCode() == 1);
}

TEST_CASE("CaptureScheduler clamps the target heading to the soft range") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}});
    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
    sch.setCaptureActive(true);

    chan.next.control_code_available = true;
    chan.next.control_code = 1;
    chan.next.heading_available = true;
    chan.next.target_heading = "200";  // clamped to yaw max 90 deg -> 900 counts

    clock = 1600;
    sch.tick();
    CHECK(motor.cmds.back() == "MOVE 900,0");
}

TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conversion") {
    // A `gimbal calib` updates the live geometry; the scheduler must adopt it so
    // the scan/directed targets use the calibrated counts/deg (not the stale copy).
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}});
    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
    sch.setCaptureActive(true);

    chan.next.control_code_available = true;
    chan.next.control_code = 1;
    chan.next.heading_available = true;
    chan.next.target_heading = "45";  // 45 deg

    clock = 1600;
    sch.tick();
    CHECK(motor.cmds.back() == "MOVE 450,0");  // 45 * 10 counts/deg

    // Settle + trigger so the scheduler is ready to move again.
    settleAt(motor, 450, 0);
    clock = 1700;
    sch.tick();

    // Recalibrate to 20 counts/deg; the same persisted heading must now resolve
    // to twice the counts — no new heading sent.
    Geometry g2;
    g2.yaw   = {20.0, 0, -90.0, 90.0};
    g2.pitch = {10.0, 0, 0.0, 60.0};
    sch.setGeometry(g2);

    clock = 2800;
    sch.tick();
    CHECK(motor.cmds.back() == "MOVE 900,0");  // 45 * 20 counts/deg
}

TEST_CASE("CaptureScheduler stays idle when capture inactive") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}});
    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
    // capture not active

    clock = 5000;
    sch.tick();
    CHECK(motor.cmds.empty());
    CHECK(cam.triggers == 0);
}