/**
 * @file remote_node.h
 * @brief 遥控/设备协议节点：UDP + JSON 报文 ↔ ROS 话题
 *
 * 职责边界（见 proto.md）：
 *   - 报文层：Header 校验、JSON 解析、错误码（0xE0xx）、MsgId 配对
 *   - 语义层：只做「语法/枚举/类型」校验，以及协议明确规定的状态前提（如步态仅 RL 下有效）；
 *     运动状态是否真的切过去，由 invicta_deploy 决定，并通过 /motion_state 回报真实状态
 *     （协议里 ErrorCode=0 的含义是「命令已接收并触发，不代表业务成功」）
 *   - 状态上报：1002/6 基础 2 Hz、1002/4 运控 10 Hz、1002/5 设备 2 Hz、1002/3 异常 2 Hz。
 *     默认对**所有在线客户端**推送（老版 App 不发 1002 查询，只靠推送）；`telemetry/require_subscription`
 *     设为 true 则退回协议原文的「先查询再推送」。
 */
#pragma once

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>

#include <algorithm>
#include <atomic>
#include <chrono>
#include <cmath>
#include <ctime>
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include "heartbeat.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/JointDiagnostics.h"
#include "invicta_msgs/MotionCommand.h"
#include "invicta_msgs/MotionState.h"
#include "invicta_msgs/error_code.h"
#include "remote_config.h"
#include "json.hpp"
#include "protocol_defs.h"
#include "udp_server.h"

namespace invicta_remote {

using namespace invicta_msgs;  // MotionParam / GaitParam / UsageMode 及名字函数
using json = nlohmann::json;

class RemoteNode {
public:
    explicit RemoteNode(const ros::NodeHandle& pnh) : pnh_(pnh), cfg_(invicta_remote::Config()) {
        // 参数来自本节点自己的 config/invicta_remote.toml
        udp_port_ = cfg_.udp_port;
        motion_command_topic_ = cfg_.topic_motion_command;
        motion_state_topic_ = cfg_.topic_motion_state;
        joint_states_topic_ = cfg_.topic_joint_states;
        imu_topic_ = cfg_.topic_imu;
        joint_diagnostics_topic_ = cfg_.topic_joint_diagnostics;
        heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s;
        unsupported_as_success_ = cfg_.unsupported_as_success;

        telemetry_enabled_ = cfg_.telemetry_enabled;
        require_subscription_ = cfg_.telemetry_require_subscription;
        basic_hz_ = cfg_.telemetry_basic_hz;
        motion_hz_ = cfg_.telemetry_motion_hz;
        device_hz_ = cfg_.telemetry_device_hz;
        error_hz_ = cfg_.telemetry_error_hz;

        heartbeat_ = std::make_unique<Heartbeat>(heartbeat_timeout_s_);
        motion_command_pub_ = nh_.advertise<invicta_msgs::MotionCommand>(motion_command_topic_, 1);
        motion_state_sub_ = nh_.subscribe(motion_state_topic_, 10, &RemoteNode::MotionStateCallback, this);
        joint_state_sub_ = nh_.subscribe(joint_states_topic_, 10, &RemoteNode::JointStateCallback, this);
        imu_sub_ = nh_.subscribe(imu_topic_, 10, &RemoteNode::ImuCallback, this);
        diagnostics_sub_ =
            nh_.subscribe(joint_diagnostics_topic_, 10, &RemoteNode::JointDiagnosticsCallback, this);

        RegisterHandlers();
    }

    ~RemoteNode() { StopTelemetry(); }

    void Start() {
        udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_);
        StartTelemetry();

        ROS_INFO_STREAM("[invicta_remote] protocol node ready: " << motion_command_topic_ << " <- " << motion_state_topic_);
        ROS_INFO("[invicta_remote] implemented: heartbeat / axis command(2,21) / motion state(2,22) / gait(2,23) / "
                 "usage mode(1101,5 normal only) / key(1101,12)");
        ROS_INFO("[invicta_remote] telemetry: 1002/6 %.0f Hz, 1002/4 %.0f Hz, 1002/5 %.0f Hz, 1002/3 %.0f Hz"
                 "(%s)", basic_hz_, motion_hz_, device_hz_, error_hz_,
                 require_subscription_ ? "query subscription required" : "push autonomously");
        ROS_INFO("[invicta_remote] not implemented: light, sleep, charge, navigation, velocity command (reply 0xE00A)");
    }

private:
    struct Result {
        int code = kSuccess;
        std::string message;
        static Result Ok() { return Result{}; }
        static Result Err(int c, const std::string& m = "") {
            Result r;
            r.code = c;
            r.message = m.empty() ? ErrorMessage(c) : m;
            return r;
        }
    };

    using Handler = std::function<Result(json& items, const Frame& frame)>;

    // ---------------------------------------------------------------- 帧入口
    std::string OnFrame(const Frame& frame) {
        heartbeat_->Touch(frame.ip, frame.port);

        if (!frame.format_ok) {
            return BuildResponse(0, 0, Result::Err(kErrUnsupportedDataFormat));
        }

        json doc;
        try {
            doc = json::parse(frame.body);
        } catch (const std::exception& e) {
            std::cout << "[GAMEPAD] JSON parse failed: " << e.what() << std::endl;
            return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
        }

        if (!doc.is_object() || !doc.contains("PatrolDevice") || !doc["PatrolDevice"].is_object()) {
            return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
        }
        json& root = doc["PatrolDevice"];
        if (!root.contains("Type") || !root.contains("Command") || !root["Type"].is_number_integer() ||
            !root["Command"].is_number_integer()) {
            return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
        }

        const int type = root["Type"].get<int>();
        const int command = root["Command"].get<int>();
        json items = root.contains("Items") && root["Items"].is_object() ? root["Items"] : json::object();

        LogReceived(type, command, items);

        auto it = handlers_.find({type, command});
        if (it == handlers_.end()) {
            return BuildResponse(type, command, Result::Err(kErrUnsupportedProtocolType));
        }

        // 状态上报类查询：直接回一帧当前数据（同时记录订阅，见 HandleStatusQuery）
        if (type == kTypeStatus) {
            return StatusQueryResponse(command, items, frame);
        }

        Result res = it->second(items, frame);
        if (res.code != kSuccess) {
            std::cout << "[GAMEPAD] Type=" << type << " Command=" << command << " -> 0x" << std::hex << res.code
                      << std::dec << " " << res.message << std::endl;
        }
        return BuildResponse(type, command, res);
    }

    std::string BuildResponse(int type, int command, const Result& res) const {
        json doc;
        doc["PatrolDevice"]["Items"]["ErrorCode"] = res.code;
        doc["PatrolDevice"]["Items"]["ErrorMessage"] = res.message;
        doc["PatrolDevice"]["Type"] = type;
        doc["PatrolDevice"]["Command"] = command;
        doc["PatrolDevice"]["Time"] = CurrentDateTime();
        return doc.dump();
    }

    static std::string CurrentDateTime() {
        char buf[32];
        std::time_t t = std::time(nullptr);
        std::tm tm_buf{};
        localtime_r(&t, &tm_buf);
        std::strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", &tm_buf);
        return std::string(buf);
    }

    void LogReceived(int type, int command, const json& items) {
        const auto now = std::chrono::steady_clock::now();
        auto key = std::make_pair(type, command);
        auto it = last_rx_log_.find(key);
        if (it != last_rx_log_.end() && now - it->second < std::chrono::seconds(1)) return;
        last_rx_log_[key] = now;
        std::cout << "[GAMEPAD] recv Type=" << type << " Command=" << command << " items=" << items.dump()
                  << std::endl;
    }

    // ---------------------------------------------------------------- 指令发布
    void PublishMotionCommand() {
        invicta_msgs::MotionCommand msg;
        msg.header.stamp = ros::Time::now();
        msg.target_state = latest_.target_state;
        msg.gait = latest_.gait;
        msg.usage_mode = latest_.usage_mode;
        msg.x = latest_.x;
        msg.y = latest_.y;
        msg.yaw = latest_.yaw;
        motion_command_pub_.publish(msg);
    }

    // ---------------------------------------------------------------- 状态缓存
    void MotionStateCallback(const invicta_msgs::MotionState::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        actual_motion_state_ = msg->motion_state;
        actual_gait_ = msg->gait;
        actual_usage_mode_ = msg->usage_mode;
        actual_charge_ = msg->charge;
        actual_hes_ = msg->hes;
        actual_sleep_ = msg->sleep;
        actual_error_code_ = msg->error_code;
        actual_version_ = msg->version;
        have_state_ = true;
    }

    void JointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        joint_names_ = msg->name;
        joint_pos_.assign(msg->position.begin(), msg->position.end());
        joint_vel_.assign(msg->velocity.begin(), msg->velocity.end());
        // 本机 8 关节（每腿 swing + wheel），不是 M20 的 16；上报时由 FillM20Joints 折叠到 16 槽
        have_joints_ = !joint_pos_.empty();
    }

    void JointDiagnosticsCallback(const invicta_msgs::JointDiagnostics::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        diag_names_ = msg->name;
        diag_temperature_.assign(msg->temperature.begin(), msg->temperature.end());
        diag_status_.assign(msg->status.begin(), msg->status.end());
    }

    void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
        const double w = msg->orientation.w, x = msg->orientation.x, y = msg->orientation.y,
                     z = msg->orientation.z;
        const double n = std::sqrt(w * w + x * x + y * y + z * z);
        if (n < 1e-6) return;
        const double qw = w / n, qx = x / n, qy = y / n, qz = z / n;
        std::lock_guard<std::mutex> lock(state_mutex_);
        rpy_[0] = std::atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
        rpy_[1] = std::asin(std::max(-1.0, std::min(1.0, 2.0 * (qw * qy - qz * qx))));
        rpy_[2] = std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
        omega_z_ = msg->angular_velocity.z;
        have_imu_ = true;
    }

    // ---------------------------------------------------------------- M20 16 关节布局
    // 协议 §3.2 固定 16 槽：每腿 [HipX, HipY, Knee, Wheel]，腿序 前左/前右/后左/后右。
    // 本机每腿只有 swing + wheel：HipY ← 摆臂角，Wheel ← 轮转速。
    // HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，恒为 0——这不是缺数据，而是当前机构
    // 「无侧摆、腿笔直」的正确取值，客户端按 16 槽解析时姿态不会异常。
    // 保留 16 槽而不是裁成 8 槽，是为了让同一套 App 不必区分机型。
    static constexpr int kM20Joints = 16;

    static const char* M20JointName(int slot) {
        static const char* kNames[kM20Joints] = {
            "LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
            "RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
            "LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
            "RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
        return kNames[slot];
    }

    /// 关节名 → 腿序号（0 前左 fl、1 前右 fr、2 后左 hl、3 后右 hr）；不认识返回 -1
    static int LegIndexFromJointName(const std::string& n) {
        if (n.compare(0, 2, "fl") == 0) return 0;
        if (n.compare(0, 2, "fr") == 0) return 1;
        if (n.compare(0, 2, "hl") == 0) return 2;
        if (n.compare(0, 2, "hr") == 0) return 3;
        return -1;
    }

    /**
     * @brief 一组关节名 → M20 的 16 槽
     *
     * @param names       关节名（/joint_states 或 /joint_diagnostics 的 name）
     * @param value_count 该消息实际带的数据条数（无 name 时按它做下标回退）
     * @param slot_index  输出：槽 → names 下标，-1 表示本机没有这个自由度
     *
     * HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，永远 -1；swing → HipY，wheel → Wheel。
     * 优先按 name 解析（fl_swing / fl_wheel …），无 name 时退回「每腿两个、先 swing 后 wheel」。
     */
    static void BuildSlotToIndex(const std::vector<std::string>& names, size_t value_count,
                                 int slot_index[kM20Joints]) {
        for (int i = 0; i < kM20Joints; ++i) slot_index[i] = -1;
        const bool by_name = !names.empty() && names.size() == value_count;
        const size_t n = by_name ? names.size() : std::min(value_count, size_t(kM20Joints / 2));
        for (size_t k = 0; k < n; ++k) {
            int leg = -1;
            bool is_wheel = false;
            if (by_name) {
                leg = LegIndexFromJointName(names[k]);
                if (leg < 0) continue;
                const bool wheel = names[k].find("wheel") != std::string::npos;
                const bool swing = names[k].find("swing") != std::string::npos;
                if (!wheel && !swing) continue;
                is_wheel = wheel;
            } else {
                leg = int(k / 2);
                is_wheel = (k % 2 == 1);
            }
            slot_index[leg * 4 + (is_wheel ? 3 : 1)] = int(k);
        }
    }

    /// 16 槽取值：协议里 Joint 数组与具名字段的混合量（角度，轮为 rad/s）
    void FillM20Joints(double value[kM20Joints]) const {
        for (int i = 0; i < kM20Joints; ++i) value[i] = 0.0;
        int slot_index[kM20Joints];
        BuildSlotToIndex(joint_names_, std::min(joint_pos_.size(), joint_vel_.size()), slot_index);
        for (int slot = 0; slot < kM20Joints; ++slot) {
            const int k = slot_index[slot];
            if (k < 0) continue;
            value[slot] = (slot % 4 == 3) ? joint_vel_[size_t(k)] : joint_pos_[size_t(k)];
        }
    }

    // ---------------------------------------------------------------- 状态上报
    json BasicDoc(int type, int command) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        json doc;
        json& items = doc["PatrolDevice"]["Items"];
        auto& bs = items["BasicStatus"];
        bs["MotionState"] = int(actual_motion_state_);
        bs["Gait"] = int(actual_gait_ == 0 ? kGaitBasic : actual_gait_);
        bs["Charge"] = int(actual_charge_);
        bs["HES"] = int(actual_hes_);
        bs["ControlUsageMode"] = int(actual_usage_mode_);
        bs["Direction"] = 0;
        bs["OOA"] = 0;
        bs["PowerManagement"] = 0;
        bs["Sleep"] = int(actual_sleep_);
        bs["Version"] = actual_version_;
        doc["PatrolDevice"]["Type"] = type;
        doc["PatrolDevice"]["Command"] = command;
        doc["PatrolDevice"]["Time"] = CurrentDateTime();
        return doc;
    }

    json MotionDoc(int type, int command) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        json doc;
        json& items = doc["PatrolDevice"]["Items"];
        auto& ms = items["MotionStatus"];
        ms["Roll"] = rpy_[0];
        ms["Pitch"] = rpy_[1];
        ms["Yaw"] = rpy_[2];
        ms["OmegaZ"] = omega_z_;
        ms["LinearX"] = 0.0;    // 待硬件/里程计
        ms["LinearY"] = 0.0;
        ms["Height"] = 0.0;     // 待运动学估计
        ms["Payload"] = 0.0;
        ms["RemainMile"] = 0.0; // 待电池/里程

        auto& motor = items["MotorStatus"];
        double value[kM20Joints];
        FillM20Joints(value);
        if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) {
            ROS_WARN_THROTTLE(30.0, "[invicta_remote] %s not received yet, 1002/4 joints reported as 0",
                              joint_states_topic_.c_str());
        }
        motor["Joint"] = std::vector<double>(value, value + kM20Joints);
        for (int i = 0; i < kM20Joints; ++i) motor[M20JointName(i)] = value[i];
        doc["PatrolDevice"]["Type"] = type;
        doc["PatrolDevice"]["Command"] = command;
        doc["PatrolDevice"]["Time"] = CurrentDateTime();
        return doc;
    }

    json DeviceDoc(int type, int command) {
        std::lock_guard<std::mutex> lock(state_mutex_);
        json doc;
        json& items = doc["PatrolDevice"]["Items"];
        // 电池 / CPU / GPS 还没有数据源，用 0 占位；电机温度来自 /joint_diagnostics
        json battery = {{"Voltage", 0.0}, {"BatteryLevel", 0.0}, {"battery_temperature", 0.0},
                        {"charge", false}, {"serial", ""}};
        items["BatteryList"] = json::array({battery, battery});
        items["BatteryStatus"] = {{"VoltageLeft", 0.0}, {"VoltageRight", 0.0},
                                  {"BatteryLevelLeft", 0.0}, {"BatteryLevelRight", 0.0},
                                  {"battery_temperatureLeft", 0.0}, {"battery_temperatureRight", 0.0},
                                  {"chargeLeft", false}, {"chargeRight", false}};
        // 温度按 M20 布局展开：本机 8 个电机落在对应槽（swing → HipY、wheel → Wheel），
        // 其余槽没有对应电机填 0；驱动器温度本协议不返回，恒 0。
        int slot_index[kM20Joints];
        BuildSlotToIndex(diag_names_, diag_temperature_.size(), slot_index);
        json motor_temp = json::array();
        json driver_temp = json::array();
        for (int slot = 0; slot < kM20Joints; ++slot) {
            const int k = slot_index[slot];
            motor_temp.push_back((k >= 0 && size_t(k) < diag_temperature_.size()) ? diag_temperature_[size_t(k)] : 0.0);
            driver_temp.push_back(0.0);
        }
        items["DeviceTemperature"] = {{"Motor", motor_temp}, {"Driver", driver_temp}};
        auto& temps = items["DeviceTemperature"];
        for (int slot = 0; slot < kM20Joints; ++slot) {
            const std::string base = M20JointName(slot);
            temps[base + "Motor"] = motor_temp[slot];
            temps[base + "Driver"] = 0.0;
        }
        items["Led"] = {{"Fill", {{"Front", 0}, {"Back", 0}}}};
        items["GPS"] = {{"Latitude", 0.0}, {"Longitude", 0.0}, {"Speed", 0.0}, {"Course", 0.0},
                        {"FixQuality", 0}, {"NumSatellites", 0}, {"Altitude", 0.0},
                        {"HDOP", 0.0}, {"VDOP", 0.0}, {"PDOP", 0.0}, {"VisibleSatellites", 0}};
        items["DevEnable"] = {{"FanSpeed", 100}, {"LoadPower", 1}, {"LedHost", 1}, {"LedExt", 1}, {"FP", 1},
                              {"Lidar", {{"Front", 1}, {"Back", 1}}}, {"GPS", 1},
                              {"Video", {{"Front", 1}, {"Back", 1}}}};
        items["CPU"] = {{"AOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}},
                        {"NOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}},
                        {"GOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}}};
        doc["PatrolDevice"]["Type"] = type;
        doc["PatrolDevice"]["Command"] = command;
        doc["PatrolDevice"]["Time"] = CurrentDateTime();
        return doc;
    }

    json ErrorDoc(int type, int command) {
        json doc;
        json list = json::array();
        uint32_t mask = 0;
        {
            std::lock_guard<std::mutex> lock(state_mutex_);
            mask = actual_error_code_;
        }
        // 内部错误位 → 协议 errorCode（proto.md §3.4）；没有对应协议码的位（如心跳超时）只在上报里省略
        for (const auto& item : RobotErrorList(mask)) {
            json e;
            e["errorCode"] = item.error_code;
            e["component"] = item.component;
            list.push_back(e);
        }
        if (list.empty()) list = json::array();
        doc["PatrolDevice"]["Items"]["ErrorList"] = list;
        doc["PatrolDevice"]["Type"] = type;
        doc["PatrolDevice"]["Command"] = command;
        doc["PatrolDevice"]["Time"] = CurrentDateTime();
        return doc;
    }

    /// 查询响应：把对应上报的当前值作为响应返回（协议 §3 的「查询触发」）
    std::string StatusQueryResponse(int command, const json&, const Frame& frame) {
        heartbeat_->SetSubscribed(frame.ip, frame.port, command);
        if (!subscribed_logged_.count({frame.ip, command})) {
            subscribed_logged_.insert({frame.ip, command});
            std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command
                      << std::endl;
        }
        switch (command) {
            case kCmdStatusBasic: return BasicDoc(kTypeStatus, command).dump();
            case kCmdStatusMotion: return MotionDoc(kTypeStatus, command).dump();
            case kCmdStatusDevice: return DeviceDoc(kTypeStatus, command).dump();
            case kCmdStatusError: return ErrorDoc(kTypeStatus, command).dump();
            default: return BuildResponse(kTypeStatus, command, Result::Err(kErrUnsupportedProtocolType));
        }
    }

    void StartTelemetry() {
        if (!telemetry_enabled_) return;
        telemetry_running_ = true;
        telemetry_thread_ = std::thread(&RemoteNode::TelemetryLoop, this);
    }

    void StopTelemetry() {
        telemetry_running_ = false;
        if (telemetry_thread_.joinable()) telemetry_thread_.join();
    }

    void TelemetryLoop() {
        using clock = std::chrono::steady_clock;
        const auto t0 = clock::now();
        double next_basic = 0, next_motion = 0, next_device = 0, next_error = 0;
        auto now = [&]() { return std::chrono::duration<double>(clock::now() - t0).count(); };

        while (ros::ok() && telemetry_running_) {
            const double t = now();
            if (t >= next_basic) {
                next_basic = t + (basic_hz_ > 0 ? 1.0 / basic_hz_ : 0.5);
                PushToClients(BasicDoc(kTypeStatus, kCmdStatusBasic).dump(), kCmdStatusBasic);
            }
            if (t >= next_motion) {
                next_motion = t + (motion_hz_ > 0 ? 1.0 / motion_hz_ : 0.1);
                PushToClients(MotionDoc(kTypeStatus, kCmdStatusMotion).dump(), kCmdStatusMotion);
            }
            if (t >= next_device) {
                next_device = t + (device_hz_ > 0 ? 1.0 / device_hz_ : 0.5);
                PushToClients(DeviceDoc(kTypeStatus, kCmdStatusDevice).dump(), kCmdStatusDevice);
            }
            if (t >= next_error) {
                next_error = t + (error_hz_ > 0 ? 1.0 / error_hz_ : 0.5);
                PushToClients(ErrorDoc(kTypeStatus, kCmdStatusError).dump(), kCmdStatusError);
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
        }
    }

    void PushToClients(const std::string& body, int status_command) {
        if (!udp_server_ || body.empty()) return;
        for (const auto& c : heartbeat_->GetClients()) {
            if (require_subscription_ && !IsSubscribed(c, status_command)) continue;
            udp_server_->SendTo(c.ip, c.port, body, next_push_id_++);
        }
    }

    static bool IsSubscribed(const ClientInfo& c, int status_command) {
        switch (status_command) {
            case kCmdStatusBasic: return c.subscribed_basic;
            case kCmdStatusMotion: return c.subscribed_motion;
            case kCmdStatusDevice: return c.subscribed_device;
            case kCmdStatusError: return c.subscribed_error;
            default: return false;
        }
    }

    // ---------------------------------------------------------------- 各命令处理
    Result HandleHeartbeat(json&, const Frame& frame) {
        const auto now = std::chrono::steady_clock::now();
        if (now - last_heartbeat_log_ > std::chrono::seconds(2)) {
            last_heartbeat_log_ = now;
            std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)"
                      << std::endl;
        }
        return Result::Ok();
    }

    /// 2/21 轴指令（常规模式），比例 [-1,1]，协议建议 ≥20 Hz
    Result HandleAxis(json& items, const Frame& frame) {
        if (!items.contains("X") || !items.contains("Y") || !items.contains("Yaw")) {
            return Result::Err(kErrMissingFields, "Missing necessary fields: X/Y/Yaw");
        }
        if (!items["X"].is_number() || !items["Y"].is_number() || !items["Yaw"].is_number()) {
            return Result::Err(kErrMismatchedDataType, "Mismatched data type: X/Y/Yaw must be float");
        }

        const std::string client = frame.ip + ":" + std::to_string(frame.port);
        const auto now = std::chrono::steady_clock::now();
        if (!axis_client_.empty() && axis_client_ != client &&
            now - axis_client_time_ < std::chrono::seconds(2)) {
            return Result::Err(kErrMismatchedClient, "Mismatched request client: " + axis_client_ + " busy");
        }
        axis_client_ = client;
        axis_client_time_ = now;

        latest_.x = items["X"].get<float>();
        latest_.y = items["Y"].get<float>();
        latest_.yaw = items["Yaw"].get<float>();
        PublishMotionCommand();
        return Result::Ok();
    }

    /// 2/22 运动状态转换
    Result HandleMotionState(json& items, const Frame&) {
        if (!items.contains("MotionParam"))
            return Result::Err(kErrMissingFields, "Missing necessary fields: MotionParam");
        if (!items["MotionParam"].is_number_integer()) {
            return Result::Err(kErrMismatchedDataType, "Mismatched data type: MotionParam must be int");
        }
        const int param = items["MotionParam"].get<int>();
        if (!IsKnownMotionParam(param)) {
            return Result::Err(kErrUnsupportedFunction, "Unsupported MotionParam: " + std::to_string(param));
        }
        latest_.target_state = uint8_t(param);
        PublishMotionCommand();
        return Result::Ok();
    }

    /// 2/23 步态切换：不同步态对应不同 RL 模型，协议规定仅 RL 控制状态下有效
    Result HandleGait(json& items, const Frame&) {
        if (!items.contains("GaitParam")) return Result::Err(kErrMissingFields, "Missing necessary fields: GaitParam");
        if (!items["GaitParam"].is_number_integer()) {
            return Result::Err(kErrMismatchedDataType, "Mismatched data type: GaitParam must be int");
        }
        const int gait = items["GaitParam"].get<int>();
        if (!IsKnownGait(uint32_t(gait))) {
            return Result::Err(kErrUnsupportedFunction, "Unsupported GaitParam: " + std::to_string(gait));
        }
        {
            std::lock_guard<std::mutex> lock(state_mutex_);
            if (have_state_ && actual_motion_state_ != kMotionRlControl) {
                return Result::Err(kErrNotAllowedOperation,
                                   std::string("Not allowed operation: gait switch only valid in RL control state (current ") +
                                       MotionParamName(actual_motion_state_) + ")");
            }
        }
        latest_.gait = uint32_t(gait);
        PublishMotionCommand();
        return Result::Ok();
    }

    /// 1101/5 使用模式：0 常规 / 1 导航 / 2 辅助
    Result HandleUsageMode(json& items, const Frame&) {
        if (!items.contains("Mode")) return Result::Err(kErrMissingFields, "Missing necessary fields: Mode");
        if (!items["Mode"].is_number_integer()) {
            return Result::Err(kErrMismatchedDataType, "Mismatched data type: Mode must be int");
        }
        const int mode = items["Mode"].get<int>();
        if (mode == kUsageNormal) {
            latest_.usage_mode = uint8_t(mode);
            PublishMotionCommand();
            return Result::Ok();
        }
        return Unsupported("usage mode " + std::to_string(mode) + " (only normal mode supported)");
    }

    /// 1101/12 按键事件：新版 App（≥ V1.5.11）用按键，这里映射到同一套运动状态
    Result HandleKey(json& items, const Frame&) {
        static const std::map<std::string, uint8_t> key_to_motion = {{"G20_KEY_L1", kMotionStanding},
                                                                     {"G20_KEY_L2", kMotionRlControl},
                                                                     {"G20_KEY_R1", kMotionLieDown},
                                                                     {"G20_KEY_R2", kMotionJointDamping}};
        if (items.contains("KeyList") && items["KeyList"].is_array()) {
            for (const auto& key : items["KeyList"]) {
                if (!key.is_string()) continue;
                const std::string name = key.get<std::string>();
                auto it = key_to_motion.find(name);
                if (it != key_to_motion.end()) {
                    latest_.target_state = it->second;
                    PublishMotionCommand();
                } else {
                    std::cout << "[GAMEPAD] unknown key " << name << std::endl;
                }
            }
        }
        return Result::Ok();
    }

    /// 未实现的功能：统一返回 0xE00A（可用参数改成回 0）
    Result Unsupported(const std::string& what) {
        if (unsupported_as_success_) {
            std::cout << "[GAMEPAD] " << what << " not implemented, reply 0 per unsupported_as_success=true" << std::endl;
            return Result::Ok();
        }
        std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl;
        return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what);
    }

    // ---------------------------------------------------------------- 注册
    void RegisterHandlers() {
        handlers_[{kTypeHeartbeat, kCmdHeartbeat}] = [this](json& i, const Frame& f) { return HandleHeartbeat(i, f); };

        handlers_[{kTypeMotion, kCmdAxisControl}] = [this](json& i, const Frame& f) { return HandleAxis(i, f); };
        handlers_[{kTypeMotion, kCmdMotionState}] = [this](json& i, const Frame& f) { return HandleMotionState(i, f); };
        handlers_[{kTypeMotion, kCmdGait}] = [this](json& i, const Frame& f) { return HandleGait(i, f); };
        handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); };
        handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) {
            return Unsupported("velocity command 2/25 (navigation mode)");
        };

        handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) {
            return Unsupported("light 1101/2 (no light hardware on this platform)");
        };
        handlers_[{kTypeDevice, kCmdUsageMode}] = [this](json& i, const Frame& f) { return HandleUsageMode(i, f); };
        handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("sleep set 1101/6"); };
        handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) {
            return Unsupported("sleep query 1101/7");
        };
        handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); };

        // 状态上报：存在性用于入口判断，实际响应在 StatusQueryResponse 里生成
        for (int cmd : {kCmdStatusBasic, kCmdStatusMotion, kCmdStatusDevice, kCmdStatusError}) {
            handlers_[{kTypeStatus, cmd}] = [](json&, const Frame&) { return Result::Ok(); };
        }
    }

private:
    struct handlers_key_hash {
        size_t operator()(const std::pair<int, int>& p) const {
            return std::hash<long long>()((static_cast<long long>(p.first) << 32) ^ static_cast<unsigned>(p.second));
        }
    };
    struct ip_cmd_hash {
        size_t operator()(const std::pair<std::string, int>& p) const {
            return std::hash<std::string>()(p.first) * 131u + static_cast<unsigned>(p.second);
        }
    };

    const invicta_remote::RemoteConfig& cfg_;
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    ros::Publisher motion_command_pub_;
    ros::Subscriber motion_state_sub_;
    ros::Subscriber joint_state_sub_;
    ros::Subscriber imu_sub_;
    ros::Subscriber diagnostics_sub_;

    std::unique_ptr<Heartbeat> heartbeat_;
    std::unique_ptr<UdpServer> udp_server_;
    std::unordered_map<std::pair<int, int>, Handler, handlers_key_hash> handlers_;
    std::unordered_map<std::pair<int, int>, std::chrono::steady_clock::time_point, handlers_key_hash> last_rx_log_;
    std::unordered_set<std::pair<std::string, int>, ip_cmd_hash> subscribed_logged_;

    // 参数
    int udp_port_ = kUdpDefaultPort;
    std::string motion_command_topic_ = "motion_command";
    std::string motion_state_topic_ = "motion_state";
    std::string joint_states_topic_ = "joint_states";
    std::string imu_topic_ = "imu/data";
    std::string joint_diagnostics_topic_ = "joint_diagnostics";
    double heartbeat_timeout_s_ = 3.0;
    bool unsupported_as_success_ = false;

    bool telemetry_enabled_ = true;
    bool require_subscription_ = false;
    double basic_hz_ = 2.0, motion_hz_ = 10.0, device_hz_ = 2.0, error_hz_ = 2.0;
    std::atomic<bool> telemetry_running_{false};
    std::thread telemetry_thread_;
    std::atomic<int> next_push_id_{0};

    // 最近一次「指令意图」，每条指令帧都会整体重发（deploy 侧对重复请求做去重）
    struct Latest {
        uint8_t target_state = kMotionIdle;
        uint32_t gait = 0;
        uint8_t usage_mode = kUsageNormal;
        float x = 0.f, y = 0.f, yaw = 0.f;
    } latest_;

    std::string axis_client_;
    std::chrono::steady_clock::time_point axis_client_time_{};
    std::chrono::steady_clock::time_point last_heartbeat_log_{};

    // 机器人实际状态（订阅 /motion_state、/joint_states、/imu/data）
    std::mutex state_mutex_;
    uint8_t actual_motion_state_ = kMotionIdle;
    uint32_t actual_gait_ = kGaitBasic;
    uint8_t actual_usage_mode_ = kUsageNormal;
    uint8_t actual_charge_ = 0, actual_hes_ = 0, actual_sleep_ = 0;
    /// 机器人内部错误位掩码（来自 /motion_state）；展开成协议 ErrorList 见 invicta_msgs/error_code.h
    uint32_t actual_error_code_ = 0;
    std::string actual_version_ = "INVICTA";
    bool have_state_ = false;
    std::vector<std::string> joint_names_;
    std::vector<double> joint_pos_, joint_vel_;
    bool have_joints_ = false;
    /// 启动后 2 s 内不报「还没收到关节状态」：节点刚起来时上游本来就还没发
    const std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
    std::vector<std::string> diag_names_;
    std::vector<double> diag_temperature_;
    std::vector<uint8_t> diag_status_;
    double rpy_[3] = {0., 0., 0.};
    double omega_z_ = 0.;
    bool have_imu_ = false;
};

}  // namespace invicta_remote