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

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

}  // namespace fgc