/**
 * @file deploy_config.h
 * @brief invicta_deploy 自己的配置（TOML）：config/invicta_deploy.toml
 *
 * 只包含本节点需要的参数（关节表与标定、腿的运动学、状态机时序与增益、策略与步态、
 * 本节点的话题/队列/控制周期）。本节点不读别的节点的配置文件。
 */
#pragma once

#include <toml++/toml.hpp>

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <map>
#include <string>
#include <vector>

namespace invicta {

/// 单个关节：限位（模型帧）。硬件标定 dir/offset 在 invicta_hardware，见该节点 README
struct JointSpec {
    std::string name;
    bool is_wheel = false;
    int leg = -1;
    double lower = 0., upper = 0.;  ///< 位置限位 [rad]
    double vel_limit = 0.;          ///< 速度限位 [rad/s]
};

/// 一条腿：摆臂 + 轮
struct LegSpec {
    std::string name;
    std::string swing_joint, wheel_joint;
    double splay_sign = 1.;  ///< 模型姿态约定：趴下时摆臂倒向（前腿 +1 向前、后腿 −1 向后）
    int swing_index = -1, wheel_index = -1;
};

class DeployConfig {
public:
    // ---- 机器人 ----
    std::string robot_name = "invicta";
    std::string robot_version = "INVICTA";
    std::vector<std::string> joint_names;
    std::vector<JointSpec> joints;  ///< 顺序同 joint_names
    std::vector<LegSpec> legs;
    int dof = 0;

    // ---- 腿的运动学（2 自由度：摆臂 + 轮）----
    double swing_link_len = 0.5;  ///< L：髋轴 → 轮轴 [m]
    double wheel_radius = 0.06;
    double stand_height = 0.48, pre_height = 0.20, lie_height = 0.0;

    // ---- 时序与增益 ----
    double stand_duration_s = 1.5, pre_hold_s = 0.0, liedown_duration_s = 2.0, damping_hold_s = 3.0;
    double stand_kp = 200., stand_kd = 4., lie_kp = 300., lie_kd = 4.5;
    double damping_kd_leg = 4., damping_kd_wheel = 1.;
    bool wheel_kp_zero = true;

    // ---- 站立后自动进 RL ----
    bool rl_auto_enter = true;
    double rl_enter_delay = 0.5;
    bool rl_require_pose = true;
    double rl_joint_tolerance = 0.15, rl_velocity_tolerance = 1.0;
    bool rl_require_imu = true;

    // ---- 策略 ----
    std::string policy_path;  ///< 相对本包目录、或绝对路径
    int policy_decimation = 4;
    double omega_scale = 0.25, dof_vel_scale = 0.05;
    bool zero_obs_wheels = true;
    std::vector<std::string> policy_joint_order;
    std::vector<double> action_scale, default_joint_pos, policy_kp, policy_kd;
    int action_dim = 0, obs_dim = 0;

    // ---- 步态 → 模型 ----
    uint32_t default_gait = 0x1001;
    std::map<uint32_t, std::string> gait_policies;

    // ---- 本节点话题与节奏 ----
    std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
    std::string topic_joint_commands = "joint_commands";
    std::string topic_motion_command = "motion_command", topic_motion_state = "motion_state";
    int control_period_ms = 5;
    double state_report_hz = 20.0;
    int queue_motion_command = 1, queue_joint_commands = 1, queue_state = 10;

    bool Load(const std::string& path, std::string* error = nullptr);

    // ---- 查询与运动学 ----
    int IndexOf(const std::string& name) const {
        auto it = std::find(joint_names.begin(), joint_names.end(), name);
        return it == joint_names.end() ? -1 : int(it - joint_names.begin());
    }
    const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
    bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
    std::vector<int> WheelIndices() const {
        std::vector<int> v;
        for (int i = 0; i < dof; ++i)
            if (joints[size_t(i)].is_wheel) v.push_back(i);
        return v;
    }
    /// 模型约定的摆臂角：θ = splay_sign · acos(h / L)
    double SwingAngleForHeight(double h, int leg) const {
        const double L = swing_link_len;
        double c = (L > 1e-9) ? h / L : 1.0;
        c = std::max(-1.0, std::min(1.0, c));
        return legs[size_t(leg)].splay_sign * std::acos(c);
    }
    double HeightForSwingAngle(double theta) const { return swing_link_len * std::cos(theta); }

    std::string Summary() const;
};

inline DeployConfig& Config() {
    static DeployConfig cfg;
    return cfg;
}

// ------------------------------------------------------------------ TOML 读取小工具
namespace detail {

inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }

template <typename T>
inline void Get(const toml::table* sect, const char* key, T& out) {
    if (!sect) return;
    if (auto v = (*sect)[key].value<T>()) out = *v;
}
inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
    if (!sect) return;
    if (auto v = (*sect)[key].value<std::string>()) out = *v;
}
inline void GetBool(const toml::table* sect, const char* key, bool& out) {
    if (!sect) return;
    if (auto v = (*sect)[key].value<bool>()) out = *v;
}
inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
    if (!sect) return;
    if (const auto* arr = (*sect)[key].as_array()) {
        out.clear();
        for (const auto& e : *arr)
            if (auto v = e.value<double>()) out.push_back(*v);
    }
}
inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
    if (!sect) return;
    if (const auto* arr = (*sect)[key].as_array()) {
        out.clear();
        for (const auto& e : *arr)
            if (auto v = e.value<std::string>()) out.push_back(*v);
    }
}

}  // namespace detail

inline bool DeployConfig::Load(const std::string& path, std::string* error) {
    const auto fail = [&](const std::string& msg) {
        if (error) *error = msg;
        return false;
    };

    toml::table tbl;
    try {
        tbl = toml::parse_file(path);
    } catch (const toml::parse_error& e) {
        return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
    }

    const toml::table* robot = detail::Section(tbl, "robot");
    if (!robot) return fail("missing [robot] section");
    detail::GetStr(robot, "name", robot_name);
    detail::GetStr(robot, "version", robot_version);
    detail::GetStrArr(robot, "joint_names", joint_names);
    if (joint_names.empty()) return fail("[robot].joint_names is empty");
    dof = int(joint_names.size());
    if (dof % 2 != 0) return fail("joint_names count must be even (one swing + one wheel per leg)");

    std::vector<double> lower, upper, vel_limit;
    const toml::table* limits = detail::Section(tbl, "joint_limits");
    detail::GetDoubleArr(limits, "lower", lower);
    detail::GetDoubleArr(limits, "upper", upper);
    detail::GetDoubleArr(limits, "vel_limit", vel_limit);
    if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
        return fail("[joint_limits] lower/upper/vel_limit length must equal joint count");
    }
    joints.assign(size_t(dof), JointSpec{});
    for (int i = 0; i < dof; ++i) {
        joints[size_t(i)].name = joint_names[size_t(i)];
        joints[size_t(i)].lower = lower[size_t(i)];
        joints[size_t(i)].upper = upper[size_t(i)];
        joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
    }

    legs.clear();
    if (const auto* leg_arr = tbl["legs"].as_array()) {
        for (const auto& elem : *leg_arr) {
            const toml::table* lt = elem.as_table();
            if (!lt) continue;
            LegSpec leg;
            detail::GetStr(lt, "name", leg.name);
            detail::GetStr(lt, "swing_joint", leg.swing_joint);
            detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
            detail::Get(lt, "splay_sign", leg.splay_sign);
            leg.swing_index = IndexOf(leg.swing_joint);
            leg.wheel_index = IndexOf(leg.wheel_joint);
            if (leg.swing_index < 0 || leg.wheel_index < 0) {
                return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
            }
            joints[size_t(leg.swing_index)].leg = int(legs.size());
            joints[size_t(leg.wheel_index)].leg = int(legs.size());
            joints[size_t(leg.wheel_index)].is_wheel = true;
            legs.push_back(leg);
        }
    }
    if (legs.empty()) return fail("missing [[legs]] section");
    if (int(legs.size()) * 2 != dof) return fail("leg count does not match joint count (2 joints per leg)");

    const toml::table* gc = detail::Section(tbl, "leg");
    detail::Get(gc, "swing_link_len", swing_link_len);
    detail::Get(gc, "wheel_radius", wheel_radius);
    detail::Get(gc, "stand_height", stand_height);
    detail::Get(gc, "pre_height", pre_height);
    detail::Get(gc, "lie_height", lie_height);
    const toml::table* timing = gc ? detail::Section(*gc, "timing") : nullptr;
    detail::Get(timing, "stand_duration_s", stand_duration_s);
    detail::Get(timing, "pre_hold_s", pre_hold_s);
    detail::Get(timing, "liedown_duration_s", liedown_duration_s);
    detail::Get(timing, "damping_hold_s", damping_hold_s);
    const toml::table* gains = gc ? detail::Section(*gc, "gains") : nullptr;
    detail::Get(gains, "stand_kp", stand_kp);
    detail::Get(gains, "stand_kd", stand_kd);
    detail::Get(gains, "lie_kp", lie_kp);
    detail::Get(gains, "lie_kd", lie_kd);
    detail::Get(gains, "damping_kd_leg", damping_kd_leg);
    detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
    detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);

    if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len must be > 0");
    for (const auto& pair : {std::make_pair("stand_height", stand_height),
                             std::make_pair("pre_height", pre_height),
                             std::make_pair("lie_height", lie_height)}) {
        if (pair.second > swing_link_len) {
            return fail(std::string("[leg].") + pair.first + " greater than swing_link_len, physically unreachable");
        }
    }

    const toml::table* rl = detail::Section(tbl, "rl");
    detail::GetBool(rl, "auto_enter", rl_auto_enter);
    detail::Get(rl, "enter_delay_s", rl_enter_delay);
    detail::GetBool(rl, "require_pose", rl_require_pose);
    detail::Get(rl, "joint_tolerance", rl_joint_tolerance);
    detail::Get(rl, "velocity_tolerance", rl_velocity_tolerance);
    detail::GetBool(rl, "require_imu", rl_require_imu);

    if (const toml::table* pol = detail::Section(tbl, "policy")) {
        detail::GetStr(pol, "path", policy_path);
        detail::Get(pol, "decimation", policy_decimation);
        detail::Get(pol, "omega_scale", omega_scale);
        detail::Get(pol, "dof_vel_scale", dof_vel_scale);
        detail::GetBool(pol, "zero_obs_wheels", zero_obs_wheels);
        detail::GetStrArr(pol, "joint_order", policy_joint_order);
        detail::GetDoubleArr(pol, "action_scale", action_scale);
        detail::GetDoubleArr(pol, "default_joint_pos", default_joint_pos);
        detail::GetDoubleArr(pol, "kp", policy_kp);
        detail::GetDoubleArr(pol, "kd", policy_kd);
        if (policy_joint_order.empty()) policy_joint_order = joint_names;
        if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order length must equal joint count");
        for (const auto& n : policy_joint_order) {
            if (IndexOf(n) < 0) return fail("[policy].joint_order entry " + n + " not in joint_names");
        }
        action_dim = dof;
        for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
            if (int(arr->size()) != action_dim) {
                return fail("[policy] action_scale/default_joint_pos/kp/kd length must equal joint count");
            }
        }
        obs_dim = 9 + 3 * action_dim;  // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
    }

    const toml::table* gait = detail::Section(tbl, "gait");
    detail::Get(gait, "default", default_gait);
    if (gait) {
        if (const toml::table* pol = detail::Section(*gait, "policies")) {
            for (const auto& [k, v] : *pol) {
                try {
                    gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
                } catch (const std::exception&) {
                    return fail("[gait.policies] key must convert to an integer gait value");
                }
            }
        }
    }

    const toml::table* topics = detail::Section(tbl, "topics");
    detail::GetStr(topics, "joint_states", topic_joint_states);
    detail::GetStr(topics, "imu", topic_imu);
    detail::GetStr(topics, "joint_commands", topic_joint_commands);
    detail::GetStr(topics, "motion_command", topic_motion_command);
    detail::GetStr(topics, "motion_state", topic_motion_state);

    const toml::table* control = detail::Section(tbl, "control");
    detail::Get(control, "period_ms", control_period_ms);
    detail::Get(control, "state_report_hz", state_report_hz);

    const toml::table* queues = detail::Section(tbl, "queues");
    detail::Get(queues, "motion_command", queue_motion_command);
    detail::Get(queues, "joint_commands", queue_joint_commands);
    detail::Get(queues, "state", queue_state);

    if (error) error->clear();
    return true;
}

inline std::string DeployConfig::Summary() const {
    char buf[512];
    std::string s;
    snprintf(buf, sizeof(buf), "[config] %s dof=%d legs=%zu L=%.3f m  stand=%.3f pre=%.3f lie=%.3f\n",
             robot_name.c_str(), dof, legs.size(), swing_link_len, stand_height, pre_height, lie_height);
    s += buf;
    for (size_t i = 0; i < legs.size(); ++i) {
        const double sw = SwingAngleForHeight(stand_height, int(i));
        const double li = SwingAngleForHeight(lie_height, int(i));
        snprintf(buf, sizeof(buf), "[config]   %s splay=%+.0f stand swing=%+.1f deg lie down=%+.1f deg\n",
                 legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
        s += buf;
    }
    snprintf(buf, sizeof(buf), "[config] policy=%s obs=%d action=%d decimation=%d gait(default)=0x%04X\n",
             policy_path.c_str(), obs_dim, action_dim, policy_decimation, default_gait);
    s += buf;
    return s;
}

}  // namespace invicta