#include <doctest/doctest.h>

#include "fgc/TestRunner.h"
#include "fgc/HardwareChecks.h"

#include <algorithm>
#include "fgc/CompositeCameraSource.h"
#include "fgc/mock/MockCameraSource.h"
#include "fgc/mock/MockImuSource.h"
#include "fgc/mock/MockMotorController.h"

#include <chrono>
#include <thread>

using namespace fgc;

namespace {
// A tiny, fast profile so a full run completes in well under a second.
TestProfile fastProfile() {
    TestProfile p;
    p.name = "test";
    p.params["reps"] = 2;
    p.params["home_begin_timeout_ms"] = 80;
    p.params["home_timeout_ms"] = 2000;
    p.params["settle_timeout_ms"] = 2000;
    p.params["encoder_diag_reps"] = 1;
    p.params["hold_window_ms"] = 50;
    p.params["balance_steps"] = 3;
    p.params["imu_sample_window_ms"] = 80;
    p.params["imu_drift_window_ms"] = 80;
    p.text["backlash_step_counts"] = "500";
    return p;
}

Geometry unitGeometry() {
    Geometry g;
    g.yaw.counts_per_deg = 1000;  g.yaw.center_count = 0;  g.yaw.min_deg = -90; g.yaw.max_deg = 90;
    g.pitch.counts_per_deg = 1000; g.pitch.center_count = 0; g.pitch.min_deg = 0; g.pitch.max_deg = 60;
    return g;
}

std::optional<TestReport> runToCompletion(TestRunner& tr, int max_ms = 8000) {
    REQUIRE(tr.start());
    const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(max_ms);
    while (std::chrono::steady_clock::now() < deadline) {
        if (auto rep = tr.takeReport()) return rep;
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
    return std::nullopt;
}
}  // namespace

TEST_CASE("resolveTestSelection maps subsystem/test tokens") {
    uint32_t m = 0;
    std::string err;
    CHECK(resolveTestSelection("", "", m, err));
    CHECK(m == T_ALL);
    CHECK(resolveTestSelection("gimbal", "", m, err));
    CHECK(m == T_GIMBAL_ALL);
    CHECK(resolveTestSelection("gimbal", "homing", m, err));
    CHECK(m == T_GIMBAL_HOMING);
    CHECK(resolveTestSelection("host", "disk", m, err));
    CHECK(m == T_HOST_DISK);
    CHECK_FALSE(resolveTestSelection("bogus", "", m, err));
    CHECK_FALSE(resolveTestSelection("gimbal", "bogus", m, err));
}

TEST_CASE("the hardware subsystem resolves, and is part of a bare 'test'") {
    uint32_t m = 0;
    std::string err;
    CHECK(resolveTestSelection("hardware", "", m, err));
    CHECK(m == T_HW_ALL);
    CHECK(resolveTestSelection("hardware", "axes", m, err));
    CHECK(m == T_HW_AXES);
    CHECK(resolveTestSelection("hardware", "camera", m, err));
    CHECK(m == T_HW_CAMERA);
    CHECK_FALSE(resolveTestSelection("hardware", "bogus", m, err));

    // `test` with no arguments must include them, or the gate is only met by
    // someone who knew to ask for it.
    CHECK(resolveTestSelection("", "", m, err));
    CHECK((m & T_HW_ALL) == T_HW_ALL);

    // They must NOT need a homed gimbal: that guard aborts the whole run, and
    // `test hardware` is precisely what you run on a tower before homing it.
    CHECK((T_NEEDS_MOTOR & T_HW_ALL) == 0);
}

TEST_CASE("hardware leaves report 'not checked' rather than PASS when nothing is declared") {
    // The default context declares nothing, which is every un-migrated tower.
    MockMotorController motor;
    motor.start();
    TestRunner tr(motor, nullptr, unitGeometry(), T_HW_ALL, fastProfile(), ".");
    auto rep = runToCompletion(tr);
    REQUIRE(rep);

    for (const char* id : {"hw_axes", "hw_motor", "hw_imu", "hw_env"}) {
        const auto it = std::find_if(rep->sections.begin(), rep->sections.end(),
                                     [&](const TestSection& s) { return s.id == id; });
        REQUIRE(it != rep->sections.end());
        CHECK_FALSE(it->ran);   // reported, but not counted as a pass
    }
    // An undeclared rig must not fail the run either.
    CHECK(rep->all_pass);
}

TEST_CASE("TestRunner host-only run completes and produces sections") {
    MockMotorController motor;
    TestRunner tr(motor, nullptr, unitGeometry(), T_HOST_ALL, fastProfile(), ".");
    auto rep = runToCompletion(tr);
    REQUIRE(rep.has_value());
    CHECK_FALSE(tr.running());
    CHECK(rep->find("host_memory", "ram_avail_mb") != nullptr);
    CHECK(rep->find("host_disk", "free_gb") != nullptr);
}

TEST_CASE("TestRunner full run with mocks completes with all selected sections") {
    MockMotorController motor;
    MockImuSource imu;
    imu.start();
    TestRunner tr(motor, &imu, unitGeometry(), T_ALL, fastProfile(), ".");
    auto rep = runToCompletion(tr);
    REQUIRE(rep.has_value());

    auto has = [&](const char* id) {
        for (const auto& s : rep->sections) if (s.id == id) return true;
        return false;
    };
    CHECK(has("homing"));
    CHECK(has("encoder"));
    CHECK(has("friction"));
    CHECK(has("backlash"));
    CHECK(has("balance"));
    CHECK(has("imu_config"));
    CHECK(has("imu_health"));
    CHECK(has("imu_drift"));
    CHECK(has("host_thermal"));
}

TEST_CASE("TestRunner is cancellable") {
    MockMotorController motor;
    MockImuSource imu;
    imu.start();
    TestProfile p = fastProfile();
    p.params["imu_drift_window_ms"] = 5000;  // long enough to cancel mid-run
    TestRunner tr(motor, &imu, unitGeometry(), T_ALL, p, ".");
    REQUIRE(tr.start());
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    tr.cancel();
    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(6);
    while (tr.running() && std::chrono::steady_clock::now() < deadline)
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    CHECK_FALSE(tr.running());
}

// --- test camera ----------------------------------------------------------------

namespace {
const TestSection* section(const TestReport& r, const char* id) {
    auto it = std::find_if(r.sections.begin(), r.sections.end(),
                           [&](const TestSection& s) { return s.id == id; });
    return it == r.sections.end() ? nullptr : &*it;
}
std::optional<TestReport> runCamera(MockCameraSource& cam, uint32_t mask = T_CAM_ALL) {
    MockMotorController motor;
    motor.start();
    HardwareTestContext hw;
    hw.camera = &cam;
    hw.configured_cameras = {"rgb"};
    hw.frame_timeout_ms = {200};
    TestRunner tr(motor, nullptr, unitGeometry(), mask, fastProfile(), ".", hw);
    return runToCompletion(tr);
}
}  // namespace

TEST_CASE("the camera subsystem resolves, and is part of a bare 'test'") {
    uint32_t m = 0; std::string err;
    REQUIRE(resolveTestSelection("camera", "", m, err));
    CHECK(m == T_CAM_ALL);
    REQUIRE(resolveTestSelection("camera", "bright", m, err));
    CHECK(m == T_CAM_BRIGHT);
    CHECK_FALSE(resolveTestSelection("camera", "white", m, err));   // dropped: see testCamera
    CHECK_FALSE(resolveTestSelection("camera", "focus", m, err));
    CHECK((T_ALL & T_CAM_ALL) == T_CAM_ALL);
}

TEST_CASE("a healthy camera passes frames, forced dark and forced bright, and is put back") {
    MockCameraSource cam;
    auto rep = runCamera(cam);
    REQUIRE(rep);
    for (const char* id : {"cam_frames", "cam_dark", "cam_bright"}) {
        const TestSection* s = section(*rep, id);
        REQUIRE(s);
        CHECK(s->ran);
        CHECK(s->pass);
    }
    CHECK(rep->all_pass);
    // Put back: the mock's forced settings are gone, so it is at mid grey again.
    cam.start();
    Frame f;
    REQUIRE(cam.grabFrame(0, f, 100));
    const auto st = pixelStats(f.data);
    CHECK(st.pct_low < 1.0);
    CHECK(st.pct_high < 1.0);
}

TEST_CASE("with a thermal camera beside it, both deliver frames and only colour is forced") {
    std::vector<std::unique_ptr<ICameraSource>> v;
    v.push_back(std::make_unique<MockCameraSource>());
    v.push_back(std::make_unique<MockCameraSource>(1, 640, 480, PixelLayout::Mono16));
    CompositeCameraSource cam(std::move(v));
    MockMotorController motor;
    motor.start();
    HardwareTestContext hw;
    hw.camera = &cam;
    hw.configured_cameras = {"rgb", "tir"};
    hw.frame_timeout_ms = {200, 200};
    TestRunner tr(motor, nullptr, unitGeometry(), T_CAM_ALL, fastProfile(), ".", hw);
    auto rep = runToCompletion(tr);
    REQUIRE(rep);
    // 16-bit counts are two bytes a pixel, and that is a whole frame.
    CHECK(section(*rep, "cam_frames")->pass);
    // No exposure to force on a thermal camera: noted, not failed.
    const TestSection* dark = section(*rep, "cam_dark");
    REQUIRE(dark);
    CHECK(dark->pass);
    CHECK(std::any_of(dark->notes.begin(), dark->notes.end(), [](const std::string& n) {
        return n.find("tir: not applicable") != std::string::npos;
    }));
    CHECK(rep->all_pass);
}

TEST_CASE("a camera that delivers nothing fails the frames leaf") {
    MockCameraSource cam;
    cam.setFault(MockCameraSource::Fault::NoFrames);
    auto rep = runCamera(cam, T_CAM_FRAMES);
    REQUIRE(rep);
    const TestSection* s = section(*rep, "cam_frames");
    REQUIRE(s);
    CHECK(s->ran);
    CHECK_FALSE(s->pass);
}

TEST_CASE("a camera that ignores its settings fails both extremes - the scene does not rescue it") {
    MockCameraSource cam;
    cam.setFault(MockCameraSource::Fault::IgnoresSettings);
    auto rep = runCamera(cam, T_CAM_DARK | T_CAM_BRIGHT);
    REQUIRE(rep);
    CHECK_FALSE(section(*rep, "cam_dark")->pass);
    CHECK_FALSE(section(*rep, "cam_bright")->pass);
    CHECK_FALSE(rep->all_pass);
}

TEST_CASE("with no camera, the camera leaves are not checked rather than passed") {
    MockMotorController motor;
    motor.start();
    TestRunner tr(motor, nullptr, unitGeometry(), T_CAM_ALL, fastProfile(), ".");
    auto rep = runToCompletion(tr);
    REQUIRE(rep);
    for (const char* id : {"cam_frames", "cam_dark", "cam_bright"}) {
        const TestSection* s = section(*rep, id);
        REQUIRE(s);
        CHECK_FALSE(s->ran);
    }
}

TEST_CASE("bright on its own still compares against a dark frame it takes itself") {
    MockCameraSource cam;
    auto rep = runCamera(cam, T_CAM_BRIGHT);
    REQUIRE(rep);
    const TestSection* s = section(*rep, "cam_bright");
    REQUIRE(s);
    CHECK(s->pass);
    CHECK(section(*rep, "cam_dark") == nullptr);   // taken, but not reported
}