#include <doctest/doctest.h>

#include "fgc/GatedCameraSource.h"

#include <algorithm>
#include <cmath>
#include <vector>

using namespace fgc;

namespace {

// A fake camera whose frames are synthesised from whatever exposure/gain the gate
// asks for, so the whole feedback loop can be exercised without hardware.
class FakeCamera : public ICameraSource {
public:
    // Sensor model: mean luma is proportional to exposure x gain, saturating at 255.
    double sensitivity = 0.02;   // 5500 us at 0 dB -> ~110
    double sharpness_fill = 0;   // 0 = checkerboard detail; >0 = flat (blurred)
    int    fail_first_n = 0;     // acquisitions that fail outright
    bool   clip_always = false;

    // Per-attempt sharpness overrides, consumed in order; empty = always sharp.
    std::vector<double> scripted_blur;

    int    acquisitions = 0;
    std::vector<double> exposures_seen;
    bool   auto_disabled = false;
    bool   started = false;

    void open() override {}
    void close() override {}
    void start() override { started = true; }
    void stop() override { started = false; }
    bool trigger() override { return false; }  // never used when gating is on

    void setFrameCallback(FrameCallback cb) override { cb_ = std::move(cb); }
    int  cameraCount() const override { return 1; }

    bool setExposure(double us) override {
        exposure_ = us;
        exposures_seen.push_back(us);
        return true;
    }
    bool setGain(double db) override {
        gain_ = db;
        return true;
    }
    bool setAutoExposureGain(bool on) override {
        auto_disabled = !on;
        return true;
    }
    double currentGain() override { return gain_; }

    bool acquireFrame(Frame& out, int) override {
        if (acquisitions++ < fail_first_n) return false;

        const double lin  = exposure_ * std::pow(10.0, gain_ / 20.0);
        const double mean = std::min(250.0, lin * sensitivity);

        // Blur is modelled by flattening the image: a flat field has no
        // second-derivative energy, so its sharpness collapses.
        double blur = sharpness_fill;
        if (!scripted_blur.empty()) {
            blur = scripted_blur.front();
            scripted_blur.erase(scripted_blur.begin());
        }

        const uint32_t w = 64, h = 64;
        out = Frame{};
        out.width    = w;
        out.height   = h;
        out.channels = 1;
        out.timestamp_ms = 1000 + acquisitions;
        out.data.assign(static_cast<size_t>(w) * h, 0);
        for (uint32_t y = 0; y < h; ++y) {
            for (uint32_t x = 0; x < w; ++x) {
                double v = mean;
                if (blur <= 0.0) {
                    // Zero-mean detail on a period of 3. A period-2 checkerboard
                    // would alias against the stride-4 metric sampler (which only
                    // ever hits one phase) and skew the mean by the full amplitude.
                    const int k = static_cast<int>((x + y) % 3);
                    v = mean + (k == 0 ? -40.0 : (k == 1 ? 0.0 : 40.0));
                }
                if (clip_always && x < w / 2) v = 255;
                out.data[y * w + x] = static_cast<uint8_t>(std::clamp(v, 0.0, 255.0));
            }
        }
        return true;
    }

private:
    double        exposure_ = 1000.0;
    double        gain_     = 0.0;
    FrameCallback cb_;
};

GatedCameraSource::Params params(int max_attempts = 3) {
    GatedCameraSource::Params p;
    p.max_attempts       = max_attempts;
    p.min_attempts       = 1;
    p.acquire_timeout_ms = 10;
    p.settle_delay_ms    = 0;
    p.defaults           = {1000.0, 0.0};
    p.quality.sharpness_roi_px = 32;
    p.policy.target_mean       = 110.0;
    p.policy.mean_tolerance    = 12.0;
    p.policy.exposure_max_us   = 20000.0;
    p.policy.gain_max_db       = 12.0;
    return p;
}

// Build a gate over a fake camera, capturing delivered frames.
struct Rig {
    FakeCamera*                        cam;
    std::unique_ptr<GatedCameraSource> gate;
    std::vector<Frame>                 delivered;
    ExposureStore                      store{"", 1.0, 1800};

    explicit Rig(GatedCameraSource::Params p, bool with_store = false) {
        auto owned = std::make_unique<FakeCamera>();
        cam        = owned.get();
        gate       = std::make_unique<GatedCameraSource>(
            std::move(owned), with_store ? &store : nullptr, p,
            [] { return std::make_pair(90.0, 0.0); }, [] { return 1'700'000'000'000LL; },
            [](int) {});
        gate->setFrameCallback([this](const Frame& f) { delivered.push_back(f); });
    }
};

}  // namespace

TEST_CASE("a good first frame is kept without extra acquisitions") {
    Rig r(params());
    r.cam->sensitivity = 0.11;  // 1000 us default already lands on ~110

    CHECK(r.gate->trigger());
    CHECK(r.cam->acquisitions == 1);
    REQUIRE(r.delivered.size() == 1);
    CHECK_FALSE(r.delivered[0].degraded);
    CHECK(r.gate->lastReport().attempts == 1);
    CHECK(r.gate->lastReport().reason == "ok");
    CHECK_FALSE(r.gate->lastReport().degraded);
}

TEST_CASE("an underexposed frame is corrected and re-shot") {
    Rig r(params());
    r.cam->sensitivity = 0.02;  // 1000 us -> mean 20: far too dark

    CHECK(r.gate->trigger());
    CHECK(r.cam->acquisitions > 1);
    REQUIRE(r.delivered.size() == 1);
    CHECK_FALSE(r.delivered[0].degraded);
    // Exposure was actually raised on the camera between attempts.
    REQUIRE(r.cam->exposures_seen.size() >= 2);
    CHECK(r.cam->exposures_seen[1] > r.cam->exposures_seen[0]);
}

TEST_CASE("an exhausted budget still delivers the best attempt, flagged degraded") {
    auto p = params(/*max_attempts=*/2);
    Rig  r(p);
    r.cam->clip_always = true;  // half the frame is blown no matter what we do

    CHECK(r.gate->trigger());
    CHECK(r.cam->acquisitions == 2);
    REQUIRE(r.delivered.size() == 1);
    CHECK(r.delivered[0].degraded);  // never lose a waypoint, but mark it
    CHECK(r.gate->lastReport().degraded);
    CHECK(r.gate->lastReport().reason == "exhausted");
    CHECK(r.gate->lastReport().attempts == 2);
}

TEST_CASE("total acquisition failure delivers nothing and reports it") {
    auto p = params(/*max_attempts=*/2);
    Rig  r(p);
    r.cam->fail_first_n = 99;

    CHECK_FALSE(r.gate->trigger());
    CHECK(r.delivered.empty());
    CHECK_FALSE(r.gate->lastReport().captured);
}

TEST_CASE("a transient acquisition failure does not lose the waypoint") {
    Rig r(params());
    r.cam->sensitivity  = 0.11;
    r.cam->fail_first_n = 1;  // first attempt drops, second succeeds

    CHECK(r.gate->trigger());
    REQUIRE(r.delivered.size() == 1);
    CHECK_FALSE(r.delivered[0].degraded);
}

TEST_CASE("min_attempts always shoots extra and keeps the sharpest") {
    auto p = params(/*max_attempts=*/3);
    p.min_attempts = 2;
    Rig r(p);
    r.cam->sensitivity = 0.11;  // exposure is fine from the start
    // First frame blurred, second sharp: with min_attempts=2 the gate must take
    // both and keep the better one, which is the point of the setting.
    r.cam->scripted_blur = {1.0, 0.0};

    CHECK(r.gate->trigger());
    CHECK(r.cam->acquisitions == 2);
    REQUIRE(r.delivered.size() == 1);
    CHECK(r.gate->lastReport().metrics.sharpness > 0.0);
    CHECK_FALSE(r.delivered[0].degraded);
}

TEST_CASE("the gate takes exposure control away from the camera on start") {
    Rig r(params());
    r.gate->start();
    CHECK(r.cam->started);
    // The camera's own continuous auto would fight the gate, and its convergence
    // cost is exactly what the per-angle store exists to avoid.
    CHECK(r.cam->auto_disabled);
}

TEST_CASE("disabling the gate passes straight through to the inner camera") {
    auto p    = params();
    p.enabled = false;
    Rig r(p);

    CHECK_FALSE(r.gate->trigger());  // FakeCamera::trigger() is a no-op
    CHECK(r.cam->acquisitions == 0);
    CHECK(r.delivered.empty());
}

TEST_CASE("accepted settings are written back to the store for the next visit") {
    Rig r(params(), /*with_store=*/true);
    r.cam->sensitivity = 0.02;

    CHECK(r.gate->trigger());
    auto e = r.store.find(90.0, 0.0);
    REQUIRE(e.has_value());
    CHECK(e->exposure_us > 1000.0);   // the corrected value, not the cold default
    CHECK(e->sharpness > 0.0);
    CHECK(e->attempts >= 1);
}

TEST_CASE("a seeded angle converges in one attempt on the next visit") {
    Rig r(params(), /*with_store=*/true);
    r.cam->sensitivity = 0.02;

    CHECK(r.gate->trigger());
    const int first_sweep = r.cam->acquisitions;
    CHECK(first_sweep > 1);  // had to search for the right exposure

    r.cam->acquisitions = 0;
    CHECK(r.gate->trigger());
    // Second visit starts from the stored setting, so no search is needed. This is
    // the whole point of the per-angle memory.
    CHECK(r.cam->acquisitions == 1);
}