#pragma once

#include "fgc/Config.h"
#include "fgc/IMotorController.h"
#include "fgc/Logger.h"

#include <map>
#include <mutex>
#include <set>
#include <sstream>
#include <string>
#include <vector>

namespace fgc {

// Simulated motor controller for development without hardware. Models the new
// firmware's encoder-count protocol: HOME makes both axes READY; MOVE sets a
// per-axis target; each telemetry() read steps the encoder toward its target
// and reports standstill once reached - so the capture scheduler's
// move -> settle -> trigger loop runs exactly as it would against real hardware.
class MockMotorController : public IMotorController {
public:
    // `needcfg`: emit NEED lines on start and refuse to move until they are
    // answered, the way the real firmware will (hardware-config-plan.md
    // section 6). Off by default so every existing test sees the old mock.
    //
    // This is what lets the HOST half of the handshake be built and verified
    // before the firmware half exists - and it is not throwaway: it stays as the
    // way the handshake is exercised on a dev box with no rig.
    explicit MockMotorController(bool needcfg = false) : needcfg_(needcfg) {}

    // The pins a real image would have been built with, from hardware.ini
    // [Pins]. Echoed in DUMP in NEEDCFG mode only, which is the mock's stand-in
    // for current firmware; the plain mode is a firmware predating the echo.
    void setBuiltPins(const HardwareConfig::Pins& p) {
        std::lock_guard<std::mutex> lock(mutex_);
        pins_ = p;
    }

    void start() override {
        LOG_INFO << "[mock] motor controller started";
        if (!needcfg_) return;
        std::lock_guard<std::mutex> lock(mutex_);
        static const char* kKeys[] = {"steps_per_rev", "global_scaler", "run_irun",
                                      "homing_irun",   "sw_mode",       "enc_deviation",
                                      "normal_speed",  "homing_speed",  "center_speed",
                                      "center",        "margin"};
        need_lines_.clear();
        unanswered_.clear();
        for (const char* axis : {"Y", "P"}) {
            std::string line = std::string("NEED ") + axis;
            for (const char* k : kKeys) {
                line += std::string(" ") + k;
                // Track the same set the line asks for, so the axis unlocks only
                // when EVERY key has been answered - a partial answer must leave
                // it in NEEDCFG, which is the behaviour worth simulating.
                unanswered_.insert(std::string(axis) + "." + k);
            }
            need_lines_.push_back(line);
        }
        configured_ = false;
        LOG_INFO << "[mock] NEEDCFG: asking the host for config, refusing to move until answered";
    }
    void stop() override { LOG_INFO << "[mock] motor controller stopped"; }

    void sendCommand(const std::string& cmd) override {
        LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
        std::lock_guard<std::mutex> lock(mutex_);
        std::istringstream in(cmd);
        std::string verb;
        in >> verb;
        for (auto& c : verb) c = static_cast<char>(std::toupper(static_cast<unsigned char>(c)));

        if (verb == "SET") {
            // Record what the host answered, so a test can assert the handshake
            // actually put the right values on the wire.
            std::string axis, key, value;
            in >> axis >> key >> value;
            settings_[axis + "." + key] = value;
            unanswered_.erase(axis + "." + key);
            if (unanswered_.empty()) configured_ = true;
            return;
        }
        if (needcfg_ && !configured_ &&
            (verb == "HOME" || verb == "MOVE" || verb == "ENABLE")) {
            LOG_WARN << "[mock] ERR need config (" << verb << " rejected)";
            return;
        }
        if (verb == "HOME") {
            homed_ = true;
            deviation_ = false;   // homing clears the latch, as on the real driver
        } else if (verb == "STALL") {
            // Test hook: fake the hardware encoder-deviation latch so the STALL
            // badge is reachable without jamming a real axis. Latches like the
            // real one - only HOME/RESET clears it.
            deviation_ = true;
        } else if (verb == "RESET") {
            deviation_ = false;
            // A RESET is not a way out of NEEDCFG (firmware motor.cpp says why).
            if (needcfg_ && !configured_) homed_ = false;
        } else if (verb == "MOVE") {
            std::string a;
            in >> a;
            auto comma = a.find(',');
            if (comma != std::string::npos) {  // MOVE <yaw>,<pitch>
                tryParse(a.substr(0, comma), yaw_target_);
                tryParse(a.substr(comma + 1), pitch_target_);
            } else {  // MOVE Y|P <pos>
                long pos = 0;
                if (in >> pos) {
                    if (a == "Y" || a == "y") yaw_target_ = pos;
                    else if (a == "P" || a == "p") pitch_target_ = pos;
                }
            }
        } else if (verb == "STOP") {
            yaw_target_   = yaw_.xenc;
            pitch_target_ = pitch_.xenc;
        } else if (verb == "DUMP") {
            // Mirror the real firmware DUMP wire format (see firmware/src/motor.cpp
            // and main.cpp) so the host-side DumpParser is exercised without
            // hardware. Register values are illustrative, not simulated; the
            // encoder positions track the mock's live state.
            // 5 = NEEDCFG, appended to the firmware enum so the older codes keep
            // their meanings. See motor.h.
            std::ostringstream d;
            d << "DUMP BEGIN build=mock uptime=0 mcusr=0x01 free_ram=4096";
            if (needcfg_ && pins_.spi_sck >= 0)
                d << " spi=" << pins_.spi_sck << "," << pins_.spi_mosi << "," << pins_.spi_miso;
            d << "\n";
            auto axis = [&](char L, const AxisTelemetry& a) {
                // In NEEDCFG mode, echo back what the host actually SET, so the
                // whole round trip - NEED, SET, DUMP, `test hardware motor` -
                // can be exercised with no firmware. Outside it, stay on the old
                // fixed line: that is a firmware predating the handshake, and
                // the host must keep telling the two apart.
                std::string owned = " speed=200000 center=500000 soft_margin=5000";
                if (needcfg_) {
                    const std::string ax(1, L);
                    auto val = [&](const char* k, const char* dflt) {
                        auto it = settings_.find(ax + "." + k);
                        return it == settings_.end() ? std::string(dflt) : it->second;
                    };
                    owned = " speed=" + val("normal_speed", "200000") +
                            " center=" + val("center", "500000") +
                            " soft_margin=" + val("margin", "5000") +
                            " steps_per_rev=" + val("steps_per_rev", "0") +
                            " enc_dev=" + val("enc_deviation", "0") +
                            " enc_const=" + val("enc_const", "794432") +
                            " homing_speed=" + val("homing_speed", "0") +
                            " center_speed=" + val("center_speed", "0") +
                            " sw_mode=" + val("sw_mode", "0") +
                            " gs=" + val("global_scaler", "0") +
                            " irun=" + val("run_irun", "0") +
                            " irun_home=" + val("homing_irun", "0");
                }
                const int st = (needcfg_ && !axisConfigured(L)) ? 5 : (homed_ ? 3 : 1);
                d << "DUMP " << L << " state=" << st
                  << " hsub=0 enabled=" << (homed_ ? 1 : 0)
                  << " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
                  << owned
                  << " hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1";
                const long cs = L == 'Y' ? pins_.yaw_cs : pins_.pitch_cs;
                const long en = L == 'Y' ? pins_.yaw_en : pins_.pitch_en;
                if (needcfg_ && cs >= 0) d << " cs=" << cs << " en=" << en;
                d << "\n";
                d << "DUMP " << L << " TMC GCONF=0x00000004 GSTAT=0x00000000"
                  << " IOIN=0x30000008 TSTEP=0x000fffff RAMPMODE=0x00000000"
                  << " XACTUAL=0x" << std::hex << (a.xactual & 0xffffffff)
                  << " VACTUAL=0x00000000 XTARGET=0x" << (a.xenc & 0xffffffff) << std::dec
                  << " SW_MODE=0x00000000 RAMP_STAT=0x00000300 X_ENC=0x" << std::hex
                  << (a.xenc & 0xffffffff) << std::dec
                  << " ENC_STATUS=0x00000000 CHOPCONF=0x10410150"
                  << " DRV_STATUS=0x80084000 PWM_SCALE=0x00000008 PWM_AUTO=0x00000000\n";
            };
            axis('Y', yaw_);
            axis('P', pitch_);
            d << "DUMP END\n";
            dump_ = d.str();
            LOG_INFO << "firmware dump:\n" << dump_;
        } else if (verb == "DIAG") {
            // Canned DG stream (real firmware format) so the host DiagParser /
            // logfile path are demoable without hardware. Logged live like the
            // real capture would be.
            std::ostringstream d;
            auto axis = [&](char L) {
                d << "DG BEGIN " << L << "\n";
                const int speeds[3] = {12500, 25000, 50000};
                for (int s : speeds)
                    for (bool fwd : {true, false})
                        d << "DG " << L << " S" << s << (fwd ? " FWD" : " REV")
                          << " ERR_PEAK 120 ERR_RMS 30 ERR_STILL 40 CS_MIN 10 CS_MAX 20"
                          << " SG_MIN 300 PWM_AVG 96 FLAGS 0x00000000 PASS\n";
                d << "DG " << L << " RESULT PASS\n";
            };
            axis('Y');
            axis('P');
            d << "DG DONE\n";
            diag_ = d.str();
            ++diag_seq_;
            std::istringstream ds(diag_);
            std::string ln;
            while (std::getline(ds, ln)) LOG_INFO << ln;  // stream to LOG pane
        }
        // ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
    }

    MotorTelemetry telemetry() override {
        std::lock_guard<std::mutex> lock(mutex_);
        step('Y', yaw_, yaw_target_);
        step('P', pitch_, pitch_target_);
        MotorTelemetry t;
        t.yaw           = yaw_;
        t.pitch         = pitch_;
        t.pitch_present = true;
        return t;
    }

    std::string lastDump() override {
        std::lock_guard<std::mutex> lock(mutex_);
        return dump_;
    }

    std::string lastDiag() override {
        std::lock_guard<std::mutex> lock(mutex_);
        return diag_;
    }
    unsigned diagSeq() const override { return diag_seq_; }

    std::vector<std::string> takeNeedLines() override {
        std::lock_guard<std::mutex> lock(mutex_);
        std::vector<std::string> out;
        out.swap(need_lines_);
        return out;
    }

    // Test/inspection hooks: what the host SET, and whether the axis is unlocked.
    std::map<std::string, std::string> settings() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return settings_;
    }
    bool configured() const { return configured_; }
    // One axis has every key it asked for. The real firmware leaves NEEDCFG per
    // axis, so a yaw-only tower - which rightly never answers for pitch - still
    // gets a configured yaw.
    bool axisConfigured(char L) const {
        const std::string pre = std::string(1, L) + ".";
        for (const auto& k : unanswered_)
            if (k.compare(0, pre.size(), pre) == 0) return false;
        return true;
    }

    bool connected() const override { return true; }

private:
    bool                     needcfg_ = false;
    HardwareConfig::Pins     pins_;
    bool                     configured_ = false;
    std::vector<std::string> need_lines_;
    std::map<std::string, std::string> settings_;
    std::set<std::string>    unanswered_;

    static constexpr long kStep = 80000;  // counts advanced per telemetry read

    void step(char L, AxisTelemetry& a, long target) {
        long d = target - a.xenc;
        if (d >= -kStep && d <= kStep) {
            a.xenc       = target;
            a.standstill = true;
        } else {
            a.xenc += (d > 0 ? kStep : -kStep);
            a.standstill = false;
        }
        a.xactual = a.xenc;
        // An axis waiting on its config says so, rather than looking like a
        // healthy un-homed one - that distinction is the whole point of the state.
        a.state   = (needcfg_ && !axisConfigured(L)) ? AxisState::NeedConfig
                                              : (homed_ ? AxisState::Ready : AxisState::Reset);
        a.deviation = deviation_;
    }

    static void tryParse(const std::string& s, long& out) {
        try {
            out = std::stol(s);
        } catch (const std::exception&) {}
    }

    mutable std::mutex mutex_;
    AxisTelemetry yaw_;
    AxisTelemetry pitch_;
    long          yaw_target_   = 0;
    long          pitch_target_ = 0;
    bool          homed_        = false;
    bool          deviation_    = false;  // faked encoder-deviation latch (see STALL above)
    std::string   dump_;
    std::string   diag_;
    unsigned      diag_seq_ = 0;
};

}  // namespace fgc