#include <doctest/doctest.h>

#include "fgc/PitchSetup.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 center=500000 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 center=500000 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: reported pitch is the live pitch encoder / kFakeCpd, so a correct
// routine recovers kFakeCpd exactly. Yaw is not modelled - pitch setup never
// looks at it.
struct FakeImu : IImuSource {
    FakeMotor& motor;
    bool       is_connected = true;
    explicit FakeImu(FakeMotor& m) : motor(m) {}

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

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

Geometry uncalibrated() {
    Geometry g;
    // center_count 0 so the fake's "counts = deg * kFakeCpd" holds exactly and
    // deg_at_center comes out as 0.
    g.yaw   = {1.0, 0, 0.0, -100000.0, 100000.0};
    g.pitch = {1.0, 0, 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;
    return p;
}

// Run to completion (or fail the test on timeout).
void runToCompletion(PitchSetupRoutine& 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("pitch setup 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("pitch setup homes first when the gimbal is not READY") {
    FakeMotor motor;            // starts in Boot (not ready)
    motor.tel.pitch_present = true;
    FakeImu imu(motor);

    PitchSetupRoutine 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("pitch setup 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);

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

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

TEST_CASE("pitch setup never moves yaw") {
    // Yaw is tied to north by `gimbal heading setup` against landmarks, not by
    // this routine. Touching it here would silently undo that calibration.
    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);

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

    CHECK(indexOf(motor.events, "MOVE Y") == -1);
    CHECK(indexOf(motor.events, "MOVE P") >= 0);

    // And the published geometry leaves the yaw map exactly as it found it.
    auto geo = cal.takeResult();
    REQUIRE(geo.has_value());
    CHECK(geo->yaw.counts_per_deg == doctest::Approx(uncalibrated().yaw.counts_per_deg));
    CHECK(geo->yaw.deg_at_center == doctest::Approx(uncalibrated().yaw.deg_at_center));
}

TEST_CASE("pitch setup recovers the fake IMU slope and reports one axis") {
    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);

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

    CalibReport rep = cal.report();
    REQUIRE(rep.valid);
    CHECK(rep.all_ok);
    REQUIRE(rep.axes.size() == 1);
    CHECK(rep.axes[0].axis == 'P');
    CHECK(rep.axes[0].ok);
    CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
    CHECK(rep.axes[0].r2 == doctest::Approx(1.0).epsilon(0.001));

    auto geo = cal.takeResult();
    REQUIRE(geo.has_value());
    CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
}

TEST_CASE("pitch setup parks level when it is done") {
    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);

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

    // With center_count 0 and the fake's slope, 0 deg is count 0 - and the last
    // pitch move must be to it, not to wherever the sweep happened to end.
    CHECK(motor.events.back() == "MOVE P 0");
}

TEST_CASE("pitch setup aborts without an IMU rather than guessing") {
    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.is_connected = false;

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

    CHECK(motor.events.empty());            // nothing moved
    CHECK_FALSE(cal.report().valid);
    CHECK_FALSE(cal.takeResult().has_value());
}

TEST_CASE("pitch setup aborts on a yaw-only gimbal") {
    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);

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

    CHECK(indexOf(motor.events, "MOVE P") == -1);
    CHECK_FALSE(cal.report().valid);
}