#include "fgc/TestRunner.h"

#include "fgc/DiagParser.h"
#include "fgc/DumpParser.h"
#include "fgc/HostMetrics.h"
#include "fgc/IImuSource.h"
#include "fgc/IMotorController.h"
#include "fgc/Logger.h"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <numeric>
#include <thread>

#include <unistd.h>  // gethostname

namespace fgc {

using namespace std::chrono_literals;
using clock_t_ = std::chrono::steady_clock;

namespace {

long long nowEpochMs() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::system_clock::now().time_since_epoch())
        .count();
}

std::string hostName() {
    char buf[256] = {0};
    if (gethostname(buf, sizeof(buf) - 1) == 0) return buf;
    return "unknown";
}

// ---- small stats over a sample vector ----
struct Stats {
    double mean = 0, sd = 0, min = 0, max = 0, spread = 0;
    int    n = 0;
};
Stats stats(const std::vector<double>& v) {
    Stats s;
    s.n = static_cast<int>(v.size());
    if (v.empty()) return s;
    s.min = s.max = v.front();
    double sum = 0;
    for (double x : v) { sum += x; s.min = std::min(s.min, x); s.max = std::max(s.max, x); }
    s.mean = sum / v.size();
    double acc = 0;
    for (double x : v) acc += (x - s.mean) * (x - s.mean);
    s.sd = v.size() > 1 ? std::sqrt(acc / (v.size() - 1)) : 0.0;
    s.spread = s.max - s.min;
    return s;
}

// ---- metric builders ----
TestMetric& add(TestSection& s, const std::string& name, double value,
                const std::string& unit, bool lower_is_better = true) {
    s.metrics.push_back(TestMetric{});
    TestMetric& m = s.metrics.back();
    m.name = name;
    m.value = value;
    m.unit = unit;
    m.lower_is_better = lower_is_better;
    return m;
}
void thresh(TestMetric& m, double t) {
    m.has_threshold = true;
    m.threshold = t;
    m.pass = m.lower_is_better ? (m.value <= t) : (m.value >= t);
}
void rollup(TestSection& s) {
    s.ran = true;
    s.pass = true;
    for (const auto& m : s.metrics)
        if (!m.pass) s.pass = false;
}

}  // namespace

bool resolveTestSelection(const std::string& subsystem, const std::string& test,
                          uint32_t& mask, std::string& err) {
    auto leaf = [&](const char* s, const char* t, uint32_t bit,
                    uint32_t& m) -> bool {
        if (subsystem == s && (test.empty() || test == t)) { m |= bit; return true; }
        return false;
    };
    if (subsystem.empty()) { mask = T_ALL; return true; }

    uint32_t m = 0;
    bool sub_ok = false;
    if (subsystem == "gimbal") {
        sub_ok = true;
        if (test.empty()) m = T_GIMBAL_ALL;
        else if (test == "homing")   m = T_GIMBAL_HOMING;
        else if (test == "encoder")  m = T_GIMBAL_ENCODER;
        else if (test == "friction") m = T_GIMBAL_FRICTION;
        else if (test == "backlash") m = T_GIMBAL_BACKLASH;
        else if (test == "balance")  m = T_GIMBAL_BALANCE;
        else { err = "unknown gimbal test: " + test; return false; }
    } else if (subsystem == "imu") {
        sub_ok = true;
        if (test.empty()) m = T_IMU_ALL;
        else if (test == "config") m = T_IMU_CONFIG;
        else if (test == "health") m = T_IMU_HEALTH;
        else if (test == "drift")  m = T_IMU_DRIFT;
        else { err = "unknown imu test: " + test; return false; }
    } else if (subsystem == "host") {
        sub_ok = true;
        if (test.empty()) m = T_HOST_ALL;
        else if (test == "thermal") m = T_HOST_THERMAL;
        else if (test == "disk")    m = T_HOST_DISK;
        else if (test == "memory")  m = T_HOST_MEMORY;
        else if (test == "load")    m = T_HOST_LOAD;
        else { err = "unknown host test: " + test; return false; }
    }
    (void)leaf;
    if (!sub_ok) { err = "unknown test subsystem: " + subsystem; return false; }
    mask = m;
    return true;
}

TestRunner::TestRunner(IMotorController& motor, IImuSource* imu, Geometry geo,
                       uint32_t selection, TestProfile profile, std::string disk_path)
    : motor_(motor), imu_(imu), geo_(geo), selection_(selection),
      profile_(std::move(profile)), disk_path_(std::move(disk_path)) {}

TestRunner::~TestRunner() {
    cancel_ = true;
    if (thread_.joinable()) thread_.join();
}

bool TestRunner::start() {
    if (running_.exchange(true)) {
        LOG_WARN << "test already running";
        return false;
    }
    cancel_ = false;
    {
        std::lock_guard<std::mutex> lk(result_mutex_);
        report_.reset();
    }
    thread_ = std::thread([this] { run(); });
    return true;
}

void TestRunner::cancel() { cancel_ = true; }

std::optional<TestReport> TestRunner::takeReport() {
    std::lock_guard<std::mutex> lk(result_mutex_);
    if (!report_) return std::nullopt;
    auto out = std::move(report_);
    report_.reset();
    return out;
}

TestProgress TestRunner::progress() const {
    std::lock_guard<std::mutex> lk(progress_mutex_);
    return progress_;
}

void TestRunner::setProgress(const std::string& section, int step, int total,
                             const char* phase) {
    std::lock_guard<std::mutex> lk(progress_mutex_);
    progress_.running = true;
    progress_.section = section;
    progress_.step = step;
    progress_.total = total;
    progress_.phase = phase;
}

void TestRunner::run() {
    struct Done {
        TestRunner* self;
        ~Done() {
            std::lock_guard<std::mutex> lk(self->progress_mutex_);
            self->progress_ = TestProgress{};
            self->running_ = false;
        }
    } done{this};

    TestReport r;
    r.ts_ms = nowEpochMs();
    r.profile = profile_.name.empty() ? "default" : profile_.name;
    r.host = hostName();
    r.fw = "";  // populated from a DUMP build string if available

    // A homing run also surfaces the firmware build id for the report header.
    if (!cancelled() && (selection_ & T_GIMBAL_HOMING)) testHoming(r);
    if (!cancelled() && (selection_ & T_GIMBAL_ENCODER)) testEncoder(r);
    if (!cancelled() && (selection_ & T_GIMBAL_FRICTION)) testFriction(r);
    if (!cancelled() && (selection_ & T_GIMBAL_BACKLASH)) testBacklash(r);
    if (!cancelled() && (selection_ & T_GIMBAL_BALANCE)) testBalance(r);
    if (!cancelled() && (selection_ & T_IMU_CONFIG)) testImuConfig(r);
    if (!cancelled() && (selection_ & T_IMU_HEALTH)) testImuHealth(r);
    if (!cancelled() && (selection_ & T_IMU_DRIFT)) testImuDrift(r);
    if (!cancelled() && (selection_ & T_HOST_ALL)) testHost(r);

    r.all_pass = true;
    for (const auto& s : r.sections)
        if (s.ran && !s.pass) r.all_pass = false;

    {
        std::lock_guard<std::mutex> lk(result_mutex_);
        report_ = std::move(r);
    }
    if (cancelled()) LOG_WARN << "test cancelled";
}

// ---------------------------------------------------------------------------
// Motor helpers
// ---------------------------------------------------------------------------
namespace {

// Poll telemetry until `pred` is true or `timeout` elapses. Returns elapsed ms,
// or -1 on timeout/cancel. Calls `tick` each poll for sampling.
template <typename Pred, typename Tick>
double pollUntil(IMotorController& motor, const std::atomic<bool>& cancel,
                 Pred pred, Tick tick, std::chrono::milliseconds timeout,
                 std::chrono::milliseconds period = 20ms) {
    const auto t0 = clock_t_::now();
    const auto deadline = t0 + timeout;
    while (clock_t_::now() < deadline) {
        if (cancel.load()) return -1;
        MotorTelemetry t = motor.telemetry();
        tick(t);
        if (pred(t))
            return std::chrono::duration<double, std::milli>(clock_t_::now() - t0).count();
        std::this_thread::sleep_for(period);
    }
    return -1;
}

bool axisReady(const MotorTelemetry& t) {
    return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
}

}  // namespace

void TestRunner::testHoming(TestReport& r) {
    TestSection s;
    s.id = "homing";
    s.title = "homing reproducibility";
    const auto t_start = clock_t_::now();
    const int reps = std::max(2, profile_.geti("reps", 5));
    const auto home_to = std::chrono::milliseconds(profile_.geti("home_timeout_ms", 65000));

    motor_.sendCommand("ENABLE Y");
    motor_.sendCommand("ENABLE P");

    std::vector<double> yaw_neg, yaw_pos, pit_neg, pit_pos, durations;
    int failures = 0;
    bool pitch_present = false;

    for (int i = 0; i < reps && !cancelled(); ++i) {
        setProgress("homing", i + 1, reps, "homing");
        motor_.sendCommand("HOME");
        // wait for homing to begin (axis leaves READY), tolerating EEPROM fast-path
        pollUntil(motor_, cancel_, [](const MotorTelemetry& t) { return !axisReady(t); },
                  [](const MotorTelemetry&) {},
                  std::chrono::milliseconds(profile_.geti("home_begin_timeout_ms", 3000)));
        double dur = pollUntil(
            motor_, cancel_,
            [](const MotorTelemetry& t) {
                if (t.yaw.state == AxisState::Error ||
                    (t.pitch_present && t.pitch.state == AxisState::Error))
                    return true;
                return axisReady(t);
            },
            [](const MotorTelemetry&) {}, home_to);
        MotorTelemetry t = motor_.telemetry();
        if (dur < 0 || t.yaw.state == AxisState::Error ||
            (t.pitch_present && t.pitch.state == AxisState::Error)) {
            ++failures;
            continue;
        }
        durations.push_back(dur);

        // Read the homed endstop limits from a fresh DUMP.
        motor_.sendCommand("DUMP");
        std::this_thread::sleep_for(150ms);
        DumpData d = parseDump(motor_.lastDump());
        if (r.fw.empty() && !d.build.empty()) r.fw = d.build;
        for (const auto& ax : d.axes) {
            if (ax.axis == 'Y') { yaw_neg.push_back(ax.lim_neg); yaw_pos.push_back(ax.lim_pos); }
            else if (ax.axis == 'P') {
                pitch_present = true;
                pit_neg.push_back(ax.lim_neg);
                pit_pos.push_back(ax.lim_pos);
            }
        }
    }

    const double spread_max = profile_.get("home_spread_max_counts", 80);
    if (!yaw_neg.empty()) {
        thresh(add(s, "yaw_lim_neg_spread", stats(yaw_neg).spread, "counts"), spread_max);
        thresh(add(s, "yaw_lim_pos_spread", stats(yaw_pos).spread, "counts"), spread_max);
    }
    if (pitch_present) {
        thresh(add(s, "pitch_lim_neg_spread", stats(pit_neg).spread, "counts"), spread_max);
        thresh(add(s, "pitch_lim_pos_spread", stats(pit_pos).spread, "counts"), spread_max);
    }
    if (!durations.empty()) {
        Stats ds = stats(durations);
        add(s, "home_ms_mean", ds.mean, "ms");
        add(s, "home_ms_max", ds.max, "ms");
        auto& tm = add(s, "home_ms_spread", ds.spread, "ms");
        if (profile_.get("home_ms_max", 0) > 0)
            thresh(tm, profile_.get("home_ms_max", 0) - profile_.get("home_ms_min", 0));
    }
    thresh(add(s, "homing_failures", failures, "count"), 0);
    if (yaw_neg.empty() && failures == 0) s.notes.push_back("no DUMP limits parsed (mock or no encoder)");

    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t_start).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

void TestRunner::testEncoder(TestReport& r) {
    TestSection s;
    s.id = "encoder";
    s.title = "encoder tracking + repeatability";
    const auto t_start = clock_t_::now();

    // (a) firmware DIAG tracking quality, repeated for spread.
    const int reps = std::max(1, profile_.geti("encoder_diag_reps", 2));
    std::vector<double> y_peak, y_still, p_peak, p_still;
    bool any_diag = false;
    for (int i = 0; i < reps && !cancelled(); ++i) {
        setProgress("encoder", i + 1, reps, "diag");
        unsigned seq0 = motor_.diagSeq();
        motor_.sendCommand("DIAG");
        // wait for a fresh diag completion
        const auto deadline = clock_t_::now() + 60s;
        while (clock_t_::now() < deadline && !cancelled() && motor_.diagSeq() == seq0)
            std::this_thread::sleep_for(50ms);
        if (motor_.diagSeq() == seq0) { s.notes.push_back("DIAG did not complete"); continue; }
        any_diag = true;
        DiagResult dr = parseDiag(motor_.lastDiag());
        for (const auto& ax : dr.axes) {
            long peak = -1, still = -1;
            for (const auto& tst : ax.tests) {
                if (tst.err_peak < 0) continue;
                peak = std::max(peak, tst.err_peak);
                still = std::max(still, tst.err_still);
            }
            if (peak < 0) continue;
            if (ax.axis == 'Y') { y_peak.push_back(peak); y_still.push_back(still); }
            else if (ax.axis == 'P') { p_peak.push_back(peak); p_still.push_back(still); }
        }
    }
    const double err_peak_max = profile_.get("encoder_err_peak_max_counts", 0);
    auto emit = [&](const char* ax, std::vector<double>& peak, std::vector<double>& still) {
        if (peak.empty()) return;
        auto& mp = add(s, std::string(ax) + "_err_peak", stats(peak).max, "counts");
        if (err_peak_max > 0) thresh(mp, err_peak_max);
        add(s, std::string(ax) + "_err_peak_spread", stats(peak).spread, "counts");
        add(s, std::string(ax) + "_err_still", stats(still).max, "counts");
    };
    emit("yaw", y_peak, y_still);
    emit("pitch", p_peak, p_still);
    if (!any_diag) s.notes.push_back("no DIAG data captured");

    // (c) hold integrity: hold-corrector delta across a short window.
    motor_.sendCommand("DUMP");
    std::this_thread::sleep_for(150ms);
    DumpData d0 = parseDump(motor_.lastDump());
    std::this_thread::sleep_for(std::chrono::milliseconds(profile_.geti("hold_window_ms", 2000)));
    if (cancelled()) { s.duration_ms = 0; rollup(s); r.sections.push_back(std::move(s)); return; }
    motor_.sendCommand("DUMP");
    std::this_thread::sleep_for(150ms);
    DumpData d1 = parseDump(motor_.lastDump());
    auto holdOf = [](const DumpData& d, char ax) -> long {
        for (const auto& a : d.axes) if (a.axis == ax) return a.hold_corrections;
        return 0;
    };
    if (d0.valid && d1.valid) {
        thresh(add(s, "yaw_hold_corrections", holdOf(d1, 'Y') - holdOf(d0, 'Y'), "count"),
               profile_.get("hold_corrections_max", 0));
    }

    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t_start).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

// Sample motor load (cs/pwm peak, sg min) and time a move to a target. Returns
// elapsed ms (-1 on timeout). `axis` is 'Y' or 'P'.
namespace {
struct LoadResult {
    double ms = -1;
    double cs_peak = 0, pwm_peak = 0, sg_min = 1e9;
};
LoadResult moveAndSampleLoad(IMotorController& motor, const std::atomic<bool>& cancel,
                             char axis, long target, std::chrono::milliseconds timeout) {
    LoadResult lr;
    motor.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
    std::this_thread::sleep_for(40ms);  // let it leave standstill
    lr.ms = pollUntil(
        motor, cancel,
        [axis](const MotorTelemetry& t) {
            const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw;
            return a.standstill;
        },
        [&](const MotorTelemetry& t) {
            const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw;
            lr.cs_peak = std::max(lr.cs_peak, (double)a.cs);
            lr.pwm_peak = std::max(lr.pwm_peak, (double)a.pwm);
            lr.sg_min = std::min(lr.sg_min, (double)a.sg);
        },
        timeout);
    return lr;
}
}  // namespace

void TestRunner::testFriction(TestReport& r) {
    TestSection s;
    s.id = "friction";
    s.title = "full-range traverse time + load";
    const auto t_start = clock_t_::now();
    const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000));
    const int reps = std::max(1, profile_.geti("friction_reps", profile_.geti("reps", 5)));
    s.notes.push_back("load via ST sampling (firmware LP not implemented)");

    auto runAxis = [&](char axis, double min_deg, double max_deg) {
        long lo = (axis == 'P') ? geo_.pitch.toCounts(min_deg) : geo_.yaw.toCounts(min_deg);
        long hi = (axis == 'P') ? geo_.pitch.toCounts(max_deg) : geo_.yaw.toCounts(max_deg);
        std::vector<double> t_fwd, t_rev, l_fwd, l_rev;
        for (int i = 0; i < reps && !cancelled(); ++i) {
            setProgress("friction", i + 1, reps, axis == 'P' ? "pitch" : "yaw");
            LoadResult f = moveAndSampleLoad(motor_, cancel_, axis, hi, move_to);
            LoadResult b = moveAndSampleLoad(motor_, cancel_, axis, lo, move_to);
            if (f.ms > 0) { t_fwd.push_back(f.ms); l_fwd.push_back(f.cs_peak); }
            if (b.ms > 0) { t_rev.push_back(b.ms); l_rev.push_back(b.cs_peak); }
        }
        const std::string a = (axis == 'P') ? "pitch" : "yaw";
        if (!t_fwd.empty()) {
            add(s, a + "_traverse_ms_fwd", stats(t_fwd).mean, "ms");
            add(s, a + "_traverse_ms_rev", stats(t_rev).mean, "ms");
            add(s, a + "_load_peak_fwd", stats(l_fwd).max, "cs");
            add(s, a + "_load_peak_rev", stats(l_rev).max, "cs");
            double asym = std::fabs(stats(t_fwd).mean - stats(t_rev).mean);
            add(s, a + "_traverse_asym_ms", asym, "ms");
        }
    };
    runAxis('Y', geo_.yaw.min_deg, geo_.yaw.max_deg);
    {
        MotorTelemetry t = motor_.telemetry();
        if (t.pitch_present) runAxis('P', geo_.pitch.min_deg, geo_.pitch.max_deg);
    }

    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t_start).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

void TestRunner::testBacklash(TestReport& r) {
    TestSection s;
    s.id = "backlash";
    s.title = "reversal dead-band";
    const auto t_start = clock_t_::now();
    const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000));
    std::vector<long> steps = profile_.getLongList("backlash_step_counts");
    if (steps.empty()) steps = {500};

    auto followErr = [&](char axis) -> long {
        MotorTelemetry t = motor_.telemetry();
        const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw;
        return std::labs(a.xactual - a.xenc);
    };
    auto stepMove = [&](char axis, long delta) {
        MotorTelemetry t = motor_.telemetry();
        const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw;
        long target = a.xenc + delta;
        moveAndSampleLoad(motor_, cancel_, axis, target, move_to);
    };

    auto runAxis = [&](char axis) {
        const std::string a = (axis == 'P') ? "pitch" : "yaw";
        for (long step : steps) {
            if (cancelled()) return;
            setProgress("backlash", 1, (int)steps.size(), a.c_str());
            // advance a few steps forward, then reverse and read the dead-band
            for (int k = 0; k < 3; ++k) stepMove(axis, +step);
            long mid_err = followErr(axis);
            stepMove(axis, -step);
            long reversal_err = followErr(axis);
            auto& m = add(s, a + "_deadband_s" + std::to_string(step), reversal_err - mid_err, "counts");
            if (profile_.get("backlash_deadband_max_counts", 0) > 0)
                thresh(m, profile_.get("backlash_deadband_max_counts", 0));
        }
    };
    runAxis('Y');
    {
        MotorTelemetry t = motor_.telemetry();
        if (t.pitch_present) runAxis('P');
    }

    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t_start).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

void TestRunner::testBalance(TestReport& r) {
    TestSection s;
    s.id = "balance";
    s.title = "pitch up/down current symmetry";
    const auto t_start = clock_t_::now();
    MotorTelemetry tp = motor_.telemetry();
    if (!tp.pitch_present) {
        s.notes.push_back("no pitch axis present; skipped");
        s.ran = false;
        r.sections.push_back(std::move(s));
        return;
    }
    const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000));
    const int N = std::max(2, profile_.geti("balance_steps", 10));
    const double lo = geo_.pitch.min_deg, hi = geo_.pitch.max_deg;

    std::vector<double> up(N, 0), down(N, 0);
    // upward
    for (int i = 0; i < N && !cancelled(); ++i) {
        setProgress("balance", i + 1, N, "up");
        double deg = lo + (hi - lo) * (i + 1) / N;
        LoadResult lr = moveAndSampleLoad(motor_, cancel_, 'P', geo_.pitch.toCounts(deg), move_to);
        up[i] = lr.cs_peak;
    }
    // downward (reverse order)
    for (int i = N - 1; i >= 0 && !cancelled(); --i) {
        setProgress("balance", N - i, N, "down");
        double deg = lo + (hi - lo) * i / N;
        LoadResult lr = moveAndSampleLoad(motor_, cancel_, 'P', geo_.pitch.toCounts(deg), move_to);
        down[i] = lr.cs_peak;
    }
    double max_asym = 0, sum_asym = 0;
    for (int i = 0; i < N; ++i) {
        double d = std::fabs(up[i] - down[i]);
        max_asym = std::max(max_asym, d);
        sum_asym += d;
        add(s, "step" + std::to_string(i) + "_up", up[i], "cs");
        add(s, "step" + std::to_string(i) + "_down", down[i], "cs");
    }
    add(s, "asym_mean", N ? sum_asym / N : 0, "cs");
    thresh(add(s, "asym_max", max_asym, "cs"), profile_.get("balance_asym_max", 4));

    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t_start).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

// ---------------------------------------------------------------------------
// IMU modules
// ---------------------------------------------------------------------------
void TestRunner::testImuConfig(TestReport& r) {
    TestSection s;
    s.id = "imu_config";
    s.title = "IMU configuration";
    if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; }
    setProgress("imu_config", 1, 1, "reading");
    imu_->refreshConfig();
    auto cfg = imu_->config();
    if (!cfg || !cfg->valid) {
        s.notes.push_back("device config unavailable");
        thresh(add(s, "config_readable", 0, ""), 1);  // lower_is_better default => fails (0<=1 true)
        s.metrics.back().lower_is_better = false;
        s.metrics.back().pass = false;
        s.ran = true; s.pass = false;
        r.sections.push_back(std::move(s));
        return;
    }
    auto& c = *cfg;
    s.notes.push_back("device: " + c.product_code + " fw " + c.firmware);
    // Expected sample rate.
    double exp_hz = profile_.get("imu_expected_hz", 100);
    auto& mh = add(s, "sample_rate_hz", c.sample_rate_hz, "Hz", false);
    mh.has_threshold = true; mh.threshold = exp_hz * 0.95;
    mh.pass = std::fabs(c.sample_rate_hz - exp_hz) <= exp_hz * 0.05;
    // Output content present.
    auto& mo = add(s, "orientation_output", c.out_orientation ? 1 : 0, "", false);
    mo.has_threshold = true; mo.threshold = 1; mo.pass = c.out_orientation;
    // Active XKF profile matches expectation, if configured.
    std::string want = profile_.gets("imu_expected_profile");
    if (!want.empty()) {
        auto& mp = add(s, "xkf_profile_match", c.scenario_label == want ? 1 : 0, "", false);
        mp.has_threshold = true; mp.threshold = 1; mp.pass = (c.scenario_label == want);
        if (!mp.pass) s.notes.push_back("XKF profile is '" + c.scenario_label + "', expected '" + want + "'");
    }
    rollup(s);
    r.sections.push_back(std::move(s));
}

namespace {
// Collect IMU samples for `window`. Returns euler vectors + temp + dropped count.
struct ImuStats {
    std::vector<double> roll, pitch, yaw, temp;
    int dropped = 0, n = 0;
    double accel_norm_mean = 0;
};
ImuStats sampleImu(IImuSource& imu, const std::atomic<bool>& cancel,
                   std::chrono::milliseconds window) {
    ImuStats st;
    const auto deadline = clock_t_::now() + window;
    int last_counter = -1;
    double accel_sum = 0;
    int accel_n = 0;
    while (clock_t_::now() < deadline && !cancel.load()) {
        auto smp = imu.sample();
        if (smp && smp->valid) {
            st.roll.push_back(smp->roll_deg);
            st.pitch.push_back(smp->pitch_deg);
            st.yaw.push_back(smp->yaw_deg);
            st.temp.push_back(smp->temp_c);
            double an = std::sqrt(smp->acc[0] * smp->acc[0] + smp->acc[1] * smp->acc[1] +
                                  smp->acc[2] * smp->acc[2]);
            accel_sum += an;
            ++accel_n;
            int cnt = smp->sample_counter;
            if (last_counter >= 0) {
                int gap = (cnt - last_counter) & 0xFFFF;
                if (gap > 1) st.dropped += gap - 1;
            }
            last_counter = cnt;
            ++st.n;
        }
        std::this_thread::sleep_for(5ms);
    }
    st.accel_norm_mean = accel_n ? accel_sum / accel_n : 0;
    return st;
}
}  // namespace

void TestRunner::testImuHealth(TestReport& r) {
    TestSection s;
    s.id = "imu_health";
    s.title = "IMU health (static)";
    if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; }
    const auto window = std::chrono::milliseconds(profile_.geti("imu_sample_window_ms", 3000));
    setProgress("imu_health", 1, 1, "sampling");
    ImuStats st = sampleImu(*imu_, cancel_, window);
    if (st.n == 0) {
        s.notes.push_back("no IMU samples");
        s.ran = true; s.pass = false;
        r.sections.push_back(std::move(s));
        return;
    }
    double secs = window.count() / 1000.0;
    add(s, "sample_rate_hz", secs > 0 ? st.n / secs : 0, "Hz", false);
    thresh(add(s, "dropped_samples", st.dropped, "count"), profile_.get("imu_dropped_max", 0));
    thresh(add(s, "yaw_noise", stats(st.yaw).sd, "deg"), profile_.get("imu_yaw_noise_max_deg", 0.5));
    add(s, "pitch_noise", stats(st.pitch).sd, "deg");
    add(s, "roll_noise", stats(st.roll).sd, "deg");
    thresh(add(s, "accel_norm_err", std::fabs(st.accel_norm_mean - 9.81), "m/s2"),
           profile_.get("imu_accel_norm_err_max", 0.5));
    auto& mt = add(s, "temperature", st.temp.empty() ? 0 : stats(st.temp).mean, "C");
    if (profile_.get("imu_temp_max_c", 0) > 0) thresh(mt, profile_.get("imu_temp_max_c", 0));
    rollup(s);
    r.sections.push_back(std::move(s));
}

void TestRunner::testImuDrift(TestReport& r) {
    TestSection s;
    s.id = "imu_drift";
    s.title = "IMU yaw drift (static)";
    if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; }
    const auto window = std::chrono::milliseconds(profile_.geti("imu_drift_window_ms", 60000));
    setProgress("imu_drift", 1, 1, "drift window");

    // Sample yaw vs time; least-squares slope (deg/min).
    std::vector<double> ts, yaw;
    const auto t0 = clock_t_::now();
    const auto deadline = t0 + window;
    double peak_min = 1e9, peak_max = -1e9;
    while (clock_t_::now() < deadline && !cancelled()) {
        auto smp = imu_->sample();
        if (smp && smp->valid) {
            double tmin = std::chrono::duration<double>(clock_t_::now() - t0).count() / 60.0;
            ts.push_back(tmin);
            yaw.push_back(smp->yaw_deg);
            peak_min = std::min(peak_min, (double)smp->yaw_deg);
            peak_max = std::max(peak_max, (double)smp->yaw_deg);
        }
        std::this_thread::sleep_for(20ms);
    }
    if (ts.size() < 3) {
        s.notes.push_back("insufficient IMU samples for drift fit");
        s.ran = true; s.pass = false;
        r.sections.push_back(std::move(s));
        return;
    }
    // slope = cov(t,yaw)/var(t)
    double mt = std::accumulate(ts.begin(), ts.end(), 0.0) / ts.size();
    double my = std::accumulate(yaw.begin(), yaw.end(), 0.0) / yaw.size();
    double cov = 0, var = 0;
    for (size_t i = 0; i < ts.size(); ++i) {
        cov += (ts[i] - mt) * (yaw[i] - my);
        var += (ts[i] - mt) * (ts[i] - mt);
    }
    double slope = var > 1e-12 ? cov / var : 0.0;  // deg per minute
    thresh(add(s, "yaw_drift_deg_min", std::fabs(slope), "deg/min"),
           profile_.get("imu_yaw_drift_max_deg_min", 1.0));
    add(s, "yaw_peak_excursion", peak_max - peak_min, "deg");
    s.duration_ms = std::chrono::duration<double, std::milli>(clock_t_::now() - t0).count();
    rollup(s);
    r.sections.push_back(std::move(s));
}

// ---------------------------------------------------------------------------
// Host module
// ---------------------------------------------------------------------------
void TestRunner::testHost(TestReport& r) {
    using namespace hostmetrics;
    if (selection_ & T_HOST_THERMAL) {
        TestSection s; s.id = "host_thermal"; s.title = "host CPU temperature";
        ThermalInfo t = readThermal();
        if (t.ok) {
            auto& m = add(s, "cpu_temp", t.max_temp_c, "C");
            if (profile_.get("cpu_temp_max_c", 0) > 0) thresh(m, profile_.get("cpu_temp_max_c", 85));
            add(s, "throttled", t.throttled ? 1 : 0, "");
            s.notes.push_back("hottest zone: " + t.hottest_zone);
        } else s.notes.push_back("no thermal zones readable");
        rollup(s); r.sections.push_back(std::move(s));
    }
    if (selection_ & T_HOST_DISK) {
        TestSection s; s.id = "host_disk"; s.title = "image partition free space";
        DiskInfo d = readDisk(disk_path_.empty() ? "." : disk_path_);
        if (d.ok) {
            auto& mf = add(s, "free_gb", d.free_gb, "GB", false);
            if (profile_.get("disk_free_min_gb", 0) > 0) thresh(mf, profile_.get("disk_free_min_gb", 5));
            add(s, "used_pct", d.used_pct, "%");
        } else s.notes.push_back("statvfs failed for " + d.path);
        rollup(s); r.sections.push_back(std::move(s));
    }
    if (selection_ & T_HOST_MEMORY) {
        TestSection s; s.id = "host_memory"; s.title = "host memory";
        MemInfo m = readMeminfo();
        if (m.ok) {
            auto& ma = add(s, "ram_avail_mb", m.avail_mb, "MB", false);
            if (profile_.get("ram_avail_min_mb", 0) > 0) thresh(ma, profile_.get("ram_avail_min_mb", 256));
            auto& ms = add(s, "swap_used_mb", m.swap_used_mb, "MB");
            if (profile_.get("swap_used_max_mb", 0) > 0) thresh(ms, profile_.get("swap_used_max_mb", 0));
        } else s.notes.push_back("/proc/meminfo unreadable");
        rollup(s); r.sections.push_back(std::move(s));
    }
    if (selection_ & T_HOST_LOAD) {
        TestSection s; s.id = "host_load"; s.title = "host load average";
        LoadInfo l = readLoad();
        if (l.ok) {
            add(s, "load1", l.load1, "");
            add(s, "load5", l.load5, "");
            add(s, "cpus", l.cpus, "");
            if (l.cpus > 0) thresh(add(s, "load1_per_cpu", l.load1 / l.cpus, ""),
                                   profile_.get("load_per_cpu_max", 1.5));
        } else s.notes.push_back("load average unavailable");
        rollup(s); r.sections.push_back(std::move(s));
    }
}

}  // namespace fgc