#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, centre at count 0 pointing at 0 deg.
// Reachable yaw travel -90..90, pitch 0..60.
// Fields: counts_per_deg, center_count, deg_at_center, min_deg, max_deg.
Geometry testGeometry() {
    Geometry g;
    g.yaw   = {10.0, 0, 0.0, -90.0, 90.0};
    g.pitch = {10.0, 0, 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.setSettleWait(0, 0);  // these predate the settle wait; timing has its own test
    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();          // first tick records standstill
    clock = 1710;
    sch.tick();          // next one keeps the frame
    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.setSettleWait(0, 0);  // these predate the settle wait; timing has its own test
    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();
    clock = 1710;
    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.setSettleWait(0, 0);  // these predate the settle wait; timing has its own test
    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 refuses an unreachable target heading instead of clamping") {
    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.setSettleWait(0, 0);  // these predate the settle wait; timing has its own test
    sch.setCaptureActive(true);

    chan.next.control_code_available = true;
    chan.next.control_code = 1;
    chan.next.heading_available = true;
    // 200 deg is 160 deg the short way round, past the 90 deg limit: in the arc
    // this gimbal physically cannot reach.
    chan.next.target_heading = "200";

    clock = 1600;
    sch.tick();
    // No move at all. Clamping to the 90 deg limit would have imaged a direction
    // 110 deg away from the one requested and labelled it "200".
    CHECK(motor.cmds.empty());
    CHECK_FALSE(sch.moving());
}

TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conversion") {
    // A `gimbal heading fit` 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.setSettleWait(0, 0);  // these predate the settle wait; timing has its own test
    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();          // records standstill
    clock = 1710;
    sch.tick();          // keeps the frame

    // 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, 0.0, -90.0, 90.0};
    g2.pitch = {10.0, 0, 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);
}

TEST_CASE("CaptureScheduler holds still for settle_wait_ms before keeping a frame") {
    long long   clock = 0;
    FakeMotor   motor;
    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.setSettleWait(1000, 2000);
    sch.setCaptureActive(true);
    chan.next.control_code_available = true;
    chan.next.control_code = 0;

    clock = 1600;
    sch.tick();
    REQUIRE(motor.cmds.size() == 1);

    // Standstill starts the wait; the camera is not touched during it.
    settleAt(motor, 0, 0);
    clock = 2000;
    sch.tick();
    CHECK(cam.triggers == 0);
    clock = 2900;  // 900 ms after settling: still inside the wait
    sch.tick();
    CHECK(cam.triggers == 0);

    // Past the wait, the newest frame is kept and the sweep advances.
    clock = 3100;
    sch.tick();
    CHECK(cam.triggers == 1);
    CHECK_FALSE(sch.moving());
}

TEST_CASE("CaptureScheduler abandons a waypoint after frame_timeout_ms") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    cam.ok = false;  // camera never has a fresh frame
    FakeChannel chan;
    ScanGrid    grid({{0.0, 0.0}, {90.0, 0.0}});

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

    clock = 1600;
    sch.tick();
    settleAt(motor, 0, 0);
    clock = 1700;
    sch.tick();

    // Retries through the timeout window without giving up.
    clock = 3500;  // 1800 ms after settling: past the wait, inside the timeout
    sch.tick();
    CHECK(sch.moving());
    CHECK(grid.index() == 0);

    // Past wait + timeout, the waypoint is skipped rather than blocking the sweep.
    clock = 4800;
    sch.tick();
    CHECK_FALSE(sch.moving());
    CHECK(grid.index() == 1);
}

// --- manual snapshot -------------------------------------------------------
//
// The snapshot exists so an operator aiming by hand gets exactly what a scan
// waypoint produces. These pin the two properties that make that true: it goes
// through the same settle gate, and it never overlaps a scan.

TEST_CASE("requestSnapshot captures one frame once the gimbal has settled") {
    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.setSettleWait(1000, 2000);
    // Capture inactive: this is manual mode.

    settleAt(motor, 0, 0);
    sch.requestSnapshot();
    CHECK(sch.snapshotPending());

    clock = 10;
    sch.tick();                       // first standstill seen; starts the settle clock
    CHECK(cam.triggers == 0);

    clock = 500;
    sch.tick();                       // still inside the settle wait
    CHECK(cam.triggers == 0);

    clock = 1100;
    sch.tick();                       // settle wait elapsed
    CHECK(cam.triggers == 1);
    CHECK_FALSE(sch.snapshotPending());

    // One request, one image: further ticks must not keep firing.
    clock = 5000;
    sch.tick();
    CHECK(cam.triggers == 1);
}

TEST_CASE("a snapshot waits out a slew rather than shooting mid-move") {
    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.setSettleWait(1000, 60000);   // long frame budget: the point here is the settle

    settleAt(motor, 0, 0);
    motor.tel.yaw.standstill = false;  // a jog is in flight
    sch.requestSnapshot();

    for (clock = 10; clock < 3000; clock += 500) sch.tick();
    CHECK(cam.triggers == 0);          // never fires while moving

    motor.tel.yaw.standstill = true;
    clock = 3100;
    sch.tick();                        // standstill seen, clock starts here
    CHECK(cam.triggers == 0);
    clock = 4200;
    sch.tick();
    CHECK(cam.triggers == 1);
}

TEST_CASE("a scan owns the camera: snapshots are refused and never queue up") {
    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.setSettleWait(0, 0);
    settleAt(motor, 0, 0);

    sch.requestSnapshot();
    sch.setCaptureActive(true);
    // Arming is dropped, not deferred: firing it whenever the scan happens to end
    // would attribute an image to whatever the gimbal was pointing at then.
    CHECK_FALSE(sch.snapshotPending());

    sch.requestSnapshot();             // asked for again mid-scan
    clock = 1600;
    sch.tick();                        // the scan's own move, not a snapshot
    sch.setCaptureActive(false);
    clock = 3000;
    sch.tick();
    CHECK_FALSE(sch.snapshotPending());
}

TEST_CASE("the snapshot frame budget is measured from settling, not from the keypress") {
    // Pressing Space mid-slew must not spend the camera's budget on the slew.
    // Before this was fixed, a long jog could exhaust the timeout before the
    // gimbal had even stopped, and the snapshot was abandoned having never asked
    // the camera for anything.
    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.setSettleWait(1000, 2000);

    settleAt(motor, 0, 0);
    motor.tel.yaw.standstill = false;   // a long slew is in flight
    sch.requestSnapshot();

    // Slew for far longer than the whole settle+timeout budget.
    for (clock = 10; clock < 30000; clock += 500) sch.tick();
    CHECK(sch.snapshotPending());       // still armed, not timed out
    CHECK(cam.triggers == 0);

    motor.tel.yaw.standstill = true;
    clock = 30500;
    sch.tick();                         // standstill seen; the budget starts HERE
    clock = 31600;                      // settle wait elapsed
    sch.tick();
    CHECK(cam.triggers == 1);
}

TEST_CASE("a snapshot that times out can simply be asked for again") {
    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.setSettleWait(1000, 2000);
    settleAt(motor, 0, 0);

    cam.ok = false;                     // camera has no frame to give
    sch.requestSnapshot();
    for (clock = 10; clock < 6000; clock += 250) sch.tick();
    CHECK_FALSE(sch.snapshotPending());  // gave up rather than staying armed forever
    const int attempts = cam.triggers;
    CHECK(attempts > 0);                 // it did keep retrying within the budget

    // It must not keep trying after giving up.
    for (clock = 6000; clock < 12000; clock += 250) sch.tick();
    CHECK(cam.triggers == attempts);

    // And the next press starts a fresh budget and succeeds.
    cam.ok = true;
    sch.requestSnapshot();
    clock = 12500;
    sch.tick();
    clock = 13600;
    sch.tick();
    CHECK(cam.triggers == attempts + 1);
    CHECK_FALSE(sch.snapshotPending());
}