#pragma once

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

#include <mutex>
#include <sstream>
#include <string>

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:
    void start() override { LOG_INFO << "[mock] motor controller started"; }
    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 == "HOME") {
            homed_ = true;
        } 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.
            const int st = homed_ ? 3 : 1;  // READY : RESET
            std::ostringstream d;
            d << "DUMP BEGIN build=mock uptime=0 mcusr=0x01 free_ram=4096\n";
            auto axis = [&](char L, const AxisTelemetry& a) {
                d << "DUMP " << L << " state=" << st
                  << " hsub=0 enabled=" << (homed_ ? 1 : 0)
                  << " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
                  << " speed=200000 eeprom_restored=1 has_encoder=1\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(yaw_, yaw_target_);
        step(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_; }

    bool connected() const override { return true; }

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

    void step(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;
        a.state   = homed_ ? AxisState::Ready : AxisState::Reset;
    }

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

    std::mutex    mutex_;
    AxisTelemetry yaw_;
    AxisTelemetry pitch_;
    long          yaw_target_   = 0;
    long          pitch_target_ = 0;
    bool          homed_        = false;
    std::string   dump_;
    std::string   diag_;
    unsigned      diag_seq_ = 0;
};

}  // namespace fgc
