#include <doctest/doctest.h>

#include "fgc/CalibrationRoutine.h"
#include "fgc/Geometry.h"
#include "fgc/IImuSource.h"
#include "fgc/IMotorController.h"

#include <chrono>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

using namespace fgc;

namespace {

// A two-axis firmware DUMP with soft limits (subset of the real format; enough
// for parseDump to expose lim_neg/lim_pos per axis).
const char* kDump =
    "DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n"
    "DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 "
    "speed=50000 eeprom_restored=0 has_encoder=1\n"
    "DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
    "DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 "
    "speed=150000 eeprom_restored=0 has_encoder=1\n"
    "DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
    "DUMP END\n";

// Counts-per-degree the fake IMU reports, so a clean sweep recovers this slope.
constexpr double kFakeCpd = 10000.0;

// Fake motor: records every command in order, and "executes" HOME/MOVE instantly
// (axis jumps to target, standstill + READY) so waitSettle/waitHomed return at
// once. IMU angle is derived from the live encoder position (see FakeImu).
struct FakeImu;
struct FakeMotor : IMotorController {
    mutable std::mutex       mtx;
    MotorTelemetry           tel;
    std::vector<std::string> events;   // unified, ordered log (motor + imu)
    bool                     pitch_present = true;
    bool                     have_dump = true;

    void start() override {}
    void stop() override {}

    void record(const std::string& s) {
        std::lock_guard<std::mutex> lk(mtx);
        events.push_back(s);
    }

    void sendCommand(const std::string& c) override {
        record(c);
        std::lock_guard<std::mutex> lk(mtx);
        std::istringstream iss(c);
        std::string verb;
        iss >> verb;
        if (verb == "HOME") {
            tel.yaw.state = AxisState::Ready;   tel.yaw.standstill = true;
            tel.pitch.state = AxisState::Ready; tel.pitch.standstill = true;
            tel.pitch_present = pitch_present;
        } else if (verb == "MOVE") {
            std::string axis;
            long target = 0;
            iss >> axis >> target;
            AxisTelemetry& a = (axis == "Y") ? tel.yaw : tel.pitch;
            a.xenc = target;
            a.standstill = true;
            a.state = AxisState::Ready;
        }
    }

    MotorTelemetry telemetry() override {
        std::lock_guard<std::mutex> lk(mtx);
        return tel;
    }
    std::string lastDump() override { return have_dump ? kDump : ""; }
    std::string lastDiag() override { return ""; }
    unsigned    diagSeq() const override { return 0; }
    bool        connected() const override { return true; }
};

// Fake IMU: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw
// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation /
// headingReset into the shared motor event log so ordering can be asserted.
struct FakeImu : IImuSource {
    FakeMotor& motor;
    long       yaw_zero = 0;
    int        scenario_type = 39;     // starts on a mag-using profile
    bool       has_profiles = true;    // expose an available list (incl. a no-mag one)
    explicit FakeImu(FakeMotor& m) : motor(m) {}

    void start() override {}
    void stop() override {}
    bool connected() const override { return true; }

    std::optional<ImuSample> sample() override {
        MotorTelemetry t = motor.telemetry();
        ImuSample s;
        s.valid = true;
        s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
        double y = (t.yaw.xenc - yaw_zero) / kFakeCpd;
        s.yaw_deg = static_cast<float>(y);
        return s;
    }

    std::optional<ImuDeviceConfig> config() const override {
        ImuDeviceConfig c;
        c.valid = true;
        c.has_scenario = true;
        c.scenario_type = static_cast<uint8_t>(scenario_type);
        if (has_profiles)
            c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
                                    {53, 11, "VRU_general"}};
        return c;
    }

    void noRotation(int seconds) override {
        motor.record("noRotation:" + std::to_string(seconds));
    }
    void headingReset() override {
        motor.record("headingReset");
        std::lock_guard<std::mutex> lk(motor.mtx);
        yaw_zero = motor.tel.yaw.xenc;  // current pose becomes yaw 0
    }
    bool setFilterProfile(int type) override {
        motor.record("setProfile:" + std::to_string(type));
        scenario_type = type;
        return true;
    }
};

Geometry uncalibrated() {
    Geometry g;
    g.yaw   = {1.0, 0, -100000.0, 100000.0};
    g.pitch = {1.0, 0, -100000.0, 100000.0};
    return g;
}

// Fast timings so a full run completes in well under a second.
CalibParams fastParams() {
    CalibParams p;
    p.positions         = 4;
    p.dwell_ms          = 4;
    p.sample_ms         = 1;
    p.settle_timeout_ms = 200;
    p.home_timeout_ms   = 500;
    p.norotation_s      = 0;
    p.reset_settle_ms   = 2;
    return p;
}

// Run to completion (or fail the test on timeout).
void runToCompletion(CalibrationRoutine& cal) {
    REQUIRE(cal.start());
    auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
    while (cal.running()) {
        if (std::chrono::steady_clock::now() > deadline) { FAIL("calibration did not finish"); }
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

int indexOf(const std::vector<std::string>& v, const std::string& needle) {
    for (int i = 0; i < static_cast<int>(v.size()); ++i)
        if (v[i].find(needle) != std::string::npos) return i;
    return -1;
}
int lastIndexOf(const std::vector<std::string>& v, const std::string& needle) {
    int found = -1;
    for (int i = 0; i < static_cast<int>(v.size()); ++i)
        if (v[i].find(needle) != std::string::npos) found = i;
    return found;
}

}  // namespace

TEST_CASE("calibration homes first when the gimbal is not READY") {
    FakeMotor motor;            // starts in Boot (not ready)
    motor.tel.pitch_present = true;
    FakeImu imu(motor);

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    const auto& ev = motor.events;
    REQUIRE_FALSE(ev.empty());
    CHECK(ev.front() == "HOME");                       // homing is the first action
    CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move
}

TEST_CASE("calibration does NOT home when already READY") {
    FakeMotor motor;
    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
    motor.tel.pitch_present = true;
    FakeImu imu(motor);

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    CHECK(indexOf(motor.events, "HOME") == -1);  // no homing issued
}

TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") {
    FakeMotor motor;
    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
    motor.tel.pitch_present = true;
    FakeImu imu(motor);

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    const auto& ev = motor.events;
    // Yaw is parked at its first position before the pitch sweep begins.
    const int first_yaw_move = indexOf(ev, "MOVE Y");
    const int first_pitch_move = indexOf(ev, "MOVE P");
    REQUIRE(first_yaw_move >= 0);
    REQUIRE(first_pitch_move >= 0);
    CHECK(first_yaw_move < first_pitch_move);

    // No-mag profile switch, drift correction, then heading reset happen AFTER the
    // pitch sweep and BEFORE the yaw sweep reaches its far end.
    const int prof  = indexOf(ev, "setProfile:53");  // VRU_general from the fake list
    const int norot = indexOf(ev, "noRotation");
    const int reset = indexOf(ev, "headingReset");
    REQUIRE(prof >= 0);
    REQUIRE(norot >= 0);
    REQUIRE(reset >= 0);
    CHECK(first_pitch_move < prof);      // pitch calibrated before the profile switch
    CHECK(prof < norot);                 // switch to no-mag, then correct drift
    CHECK(norot < reset);                // correct drift, then reset
    CHECK(reset < lastIndexOf(ev, "MOVE Y"));  // yaw sweep continues after the reset

    // Pitch is commanded to 0 deg before the heading reset.
    CHECK(indexOf(ev, "MOVE P 0") >= 0);
    CHECK(indexOf(ev, "MOVE P 0") < reset);
}

TEST_CASE("calibration skips the profile switch when no no-mag profile exists") {
    FakeMotor motor;
    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
    motor.tel.pitch_present = true;
    FakeImu imu(motor);
    imu.has_profiles = false;  // device reports no available profiles

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    const auto& ev = motor.events;
    CHECK(indexOf(ev, "setProfile") == -1);     // nothing to switch to
    CHECK(indexOf(ev, "headingReset") >= 0);    // still resets + sweeps yaw
    CHECK(cal.report().valid);
}

TEST_CASE("calibration recovers the fake IMU slope for both axes") {
    FakeMotor motor;
    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
    motor.tel.pitch_present = true;
    FakeImu imu(motor);

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    CalibReport rep = cal.report();
    REQUIRE(rep.valid);
    CHECK(rep.all_ok);
    REQUIRE(rep.axes.size() == 2);
    for (const auto& ax : rep.axes) {
        CHECK(ax.ok);
        CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
        CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001));
    }

    // The new geometry is published for the session to apply.
    auto geo = cal.takeResult();
    REQUIRE(geo.has_value());
    CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
    CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
}

TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") {
    FakeMotor motor;
    motor.pitch_present = false;
    motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
    motor.tel.pitch_present = false;
    FakeImu imu(motor);

    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
    runToCompletion(cal);

    const auto& ev = motor.events;
    CHECK(indexOf(ev, "MOVE P") == -1);        // no pitch moves
    CHECK(indexOf(ev, "headingReset") >= 0);   // still resets + sweeps yaw
    CHECK(indexOf(ev, "MOVE Y") >= 0);

    CalibReport rep = cal.report();
    REQUIRE(rep.valid);
    REQUIRE(rep.axes.size() == 1);
    CHECK(rep.axes[0].axis == 'Y');
    CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
}