#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;
    void           start() override {}
    void           stop() override {}
    void           sendCommand(const std::string& c) override { cmds.push_back(c); }
    MotorTelemetry telemetry() override { return tel; }
    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;
    }
};

}  // namespace

TEST_CASE("CaptureScheduler drives the auto-sweep capture cycle") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;

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

    chan.next.control_code_available = true;
    chan.next.control_code = 0;
    motor.tel.is_moving = 1;

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

    // After the interval, while moving: stop command issued, timer reset.
    clock = 1600;
    sch.tick();
    REQUIRE(motor.cmds.size() == 1);
    CHECK(motor.cmds[0] == "p");

    // >100ms later, still moving: camera fires.
    clock = 1750;
    sch.tick();
    CHECK(cam.triggers == 1);
}

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

    chan.next.control_code_available = true;
    chan.next.control_code = 1;
    chan.next.heading_available = true;
    chan.next.target_heading = "180";

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

TEST_CASE("CaptureScheduler stays idle when capture inactive") {
    long long   clock = 0;
    FakeMotor   motor;
    FakeCamera  cam;
    FakeChannel chan;
    CaptureScheduler sch(motor, cam, chan, 1.0, [&] { return clock; });
    motor.tel.is_moving = 1;  // moving, but capture not active

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