#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_;
        }
        // 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_;
    }

    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_;
};

}  // namespace fgc