/**
 * @file arm_oop.hpp
 * @brief 机械臂类 - C++面向对象实现
 */

#pragma once

#include <cstring>
#include <cmath>
#include <stdint.h>
#include <array>
#include "joint.hpp"
#include "bsp/time.h"
#include "component/arm_kinematics/arm6dof.h"
#include "component/toolbox/robotics.h"
#define qLimitTolerance 0.75// 关节角度误差容限（rad），用于IK解的有效性验证
namespace arm {

// 控制模式
enum class ControlMode {
    IDLE = 0,
    JOINT_POSITION,
    CARTESIAN_POSITION,
    CARTESIAN_ANALYTICAL,  
    TRAJECTORY,
    TEACH,           // 示教模式（重力补偿 + 零刚度）
    GRAVITY_COMP,    // 重力补偿模式（带位置保持）
};

// 运动状态
enum class MotionState {
    STOPPED = 0,
    MOVING,
    REACHED,
    ERROR,
};
// ============================================================================
// RoboticArm 类 - 机械臂
// ============================================================================
class RoboticArm {
private:
    static constexpr size_t JOINT_NUM = 6;
    
    std::array<Joint*, JOINT_NUM> joints_;  // 关节数组（多态指针）

    // 末端状态
    struct {
        Arm6dof_Pose_t current;      // 当前末端位姿（FK计算）
        Arm6dof_Pose_t target;       // 目标末端位姿
    } end_effector_;
    
    // 控制状态
    struct {
        ControlMode mode;
        MotionState state;
        float position_tolerance;
        float velocity_tolerance;
        bool enable;
        bool gravity_comp_enable;  // 重力补偿使能
    } control_;
    
    // 逆运动学
    struct {
        Arm6dof_JointAngles_t angles;  // IK解算得到的关节角度
        bool valid;                    // IK解是否有效
     } inverseKinematics_;

    // 重力补偿
    struct {
        float torques[JOINT_NUM];  // 各关节重力补偿力矩
        float scales[JOINT_NUM];   // 每关节补偿系数（用于独立微调）
    } gravity_comp_;

    // 笛卡尔空间轨迹规划
    // 原理：将大步运动拆分为每帧一小步的线性插值，IK初始猜测始终是上一帧解，
    //       从而保证牛顿迭代永远在正确分支附近收敛，根本消除大跨度跳变问题
    struct {
        Arm6dof_Pose_t start;    // 轨迹起点（启动时的当前末端位姿）
        Arm6dof_Pose_t goal;     // 轨迹终点（目标末端位姿）
        float t;                 // 插值进度 [0.0, 1.0]，0=起点，1=终点
        bool  active;            // 是否正在执行轨迹
        float max_lin_vel;       // 末端线速度限制 (m/s)
        float max_ang_vel;       // 末端角速度限制 (rad/s)
        Arm6dof_JointAngles_t angles; // 轨迹模式下当前插值点的IK解算结果
        bool valid;                    // IK解是否有效
    } traj_;

    // 坐标系目标缓存
    // 用途：MoveCartesianTool / MoveCartesianHeading 每帧都接收坐标系下的绝对目标，
    //       若每帧都用当前 end_effector_.current 做变换，机器人运动时位姿变化会导致
    //       世界系目标每帧漂移，MoveCartesian 检测到 goal 变化就重置 traj_.t=0，
    //       t 永远无法到达 1。
    //       解决：仅当输入目标发生变化时才重新用当前位姿换算世界系目标并缓存；
    //             输入不变时复用缓存，轨迹可以连续推进直到 t=1。
    struct FrameCache_t {
        Arm6dof_Pose_t last_input;   // 上次收到的坐标系输入
        Arm6dof_Pose_t world_goal;   // 对应的世界系目标（换算结果）
        bool valid;                  // false=尚未初始化，首次调用强制换算
    };
    FrameCache_t tool_frame_cache_;
    FrameCache_t heading_frame_cache_;

    // 时间管理
    struct {
        float now;
        uint64_t last_wakeup;
        float dt;
    } timer_;

    static bool IsPoseChanged(const Arm6dof_Pose_t& lhs,
                              const Arm6dof_Pose_t& rhs,
                              float pos_eps,
                              float ang_eps) {
        return fabsf(lhs.x - rhs.x) > pos_eps ||
               fabsf(lhs.y - rhs.y) > pos_eps ||
               fabsf(lhs.z - rhs.z) > pos_eps ||
               fabsf(lhs.roll - rhs.roll) > ang_eps ||
               fabsf(lhs.pitch - rhs.pitch) > ang_eps ||
               fabsf(lhs.yaw - rhs.yaw) > ang_eps;
    }

    void RelaxAllJoints() {
        for (Joint* joint : joints_) {
            if (joint) {
                joint->Relax();
            }
        }
    }

    void SetJointTargetsToCurrent() {
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            if (joints_[i]) {
                joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
            }
        }
    }

    void FillCurrentJointAngles(Arm6dof_JointAngles_t& q) const {
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
        }
    }
    
public:
    // 构造函数
    RoboticArm() {
        joints_.fill(nullptr);
        
        control_.mode = ControlMode::IDLE;
        control_.state = MotionState::STOPPED;
        control_.position_tolerance = 0.02f;  // 0.02 rad
        control_.velocity_tolerance = 0.01f;
        control_.enable = false;
        control_.gravity_comp_enable = false;
        
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            gravity_comp_.torques[i] = 0.0f;
            gravity_comp_.scales[i] = 1.0f;
        }
        inverseKinematics_.valid = false;
        memset(&inverseKinematics_.angles, 0, sizeof(inverseKinematics_.angles));
        memset(&traj_,  0, sizeof(traj_));
        traj_.t           = 0.0f;
        traj_.active      = false;
        traj_.max_lin_vel = 0.15f;  // 150 mm/s，可通过 SetLinVelLimit() 调整
        traj_.max_ang_vel = 1.0f;   // ~57°/s，可通过 SetAngVelLimit() 调整

        memset(&tool_frame_cache_,    0, sizeof(tool_frame_cache_));
        memset(&heading_frame_cache_, 0, sizeof(heading_frame_cache_));
        tool_frame_cache_.valid    = false;
        heading_frame_cache_.valid = false;

        timer_.now = 0.0f;
        timer_.last_wakeup = 0;
        timer_.dt = 0.001f;
    }
    
    // 禁止拷贝
    RoboticArm(const RoboticArm&) = delete;
    RoboticArm& operator=(const RoboticArm&) = delete;
    
    // 添加关节（依赖注入）
    void AddJoint(size_t index, Joint* joint) {
        if (index < JOINT_NUM) {
            joints_[index] = joint;
        }
    }
    
    // 访问关节
    Joint* GetJoint(size_t index) {
        return (index < JOINT_NUM) ? joints_[index] : nullptr;
    }
    
    const Joint* GetJoint(size_t index) const {
        return (index < JOINT_NUM) ? joints_[index] : nullptr;
    }
    
    // 初始化所有关节
    int8_t Init() {
        for (Joint* joint : joints_) {  // 遍历关节数组
            if (joint) {
                joint->Register();
                joint->Enable();
            }
        }
        memset(&end_effector_.target, 0, sizeof(end_effector_.target));
        return 0;
    }

    // 更新所有关节状态
    int8_t Update() {
        // 更新每个关节
        for (Joint* joint : joints_) {  // 遍历关节数组
            if (joint) {
                joint->Enable();
                joint->Update();
            }
        }
        
        Arm6dof_JointAngles_t q;
        FillCurrentJointAngles(q);
        
        // 正运动学：计算当前末端位姿
        Arm6dof_ForwardKinematics(&q, &end_effector_.current);
        
        // 计算重力补偿力矩
        if (control_.gravity_comp_enable) {
            CalcGravityTorques(q);
        }
        
        return 0;
    }
    
    /**
     * @brief 计算当前姿态下各关节的重力补偿力矩
     * @param q 当前关节角度（由Update()统一读取后传入，避免重复读取）
     * @details 底层返回 raw_torques 后，再按每关节系数 scales[i] 独立缩放
     */
    int8_t CalcGravityTorques(const Arm6dof_JointAngles_t& q) {
        float raw_torques[JOINT_NUM];
        int ret = Arm6dof_GravityCompensation(&q, raw_torques);
        if (ret != 0) return ret;
        
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scales[i];
        }
        return 0;
    }

    /**
     * @brief 位置优先的粗解耦逆运动学（J1~J3主控，J4~J6保持）
     * @details
     * - 目标：实现“上下左右/前后”粗到位，先到目标附近，再手动微调腕部。
     * - 求解：仅对前三关节做 3x3 阻尼最小二乘迭代，误差用末端位置误差。
     * - 姿态：不强制跟踪输入姿态，J4~J6保持 q_seed（或当前角度）不变。
     */
    int8_t InverseKinematicsAnalytical(const Arm6dof_Pose_t* pose,
                                       Arm6dof_JointAngles_t* q_out,
                                       const Arm6dof_JointAngles_t* q_seed = nullptr) {
        if (!pose || !q_out) return -1;

        // 以 q_seed 为优先初值，保证轨迹连续；无 seed 时读取当前关节角。
        Arm6dof_JointAngles_t seed;
        if (q_seed) {
            seed = *q_seed;            
            
        } else {
            FillCurrentJointAngles(seed);
        }

        Arm6dof_JointAngles_t q = seed;
        auto clampToLimit = [&](size_t idx, float qv) -> float {
            if (!joints_[idx]) return qv;
            const auto& p = joints_[idx]->GetParams();
            if (qv < p.qmin) qv = p.qmin;
            if (qv > p.qmax) qv = p.qmax;
            return qv;
        };

        auto solve3x3 = [](const float A[3][3], const float b[3], float x[3]) -> bool {
            float aug[3][4] = {
                {A[0][0], A[0][1], A[0][2], b[0]},
                {A[1][0], A[1][1], A[1][2], b[1]},
                {A[2][0], A[2][1], A[2][2], b[2]}
            };

            for (int col = 0; col < 3; ++col) {
                int pivot = col;
                float max_abs = fabsf(aug[col][col]);
                for (int row = col + 1; row < 3; ++row) {
                    float v = fabsf(aug[row][col]);
                    if (v > max_abs) {
                        max_abs = v;
                        pivot = row;
                    }
                }
                if (max_abs < 1e-8f) {
                    return false;
                }
                if (pivot != col) {
                    for (int k = col; k < 4; ++k) {
                        float tmp = aug[col][k];
                        aug[col][k] = aug[pivot][k];
                        aug[pivot][k] = tmp;
                    }
                }

                float diag = aug[col][col];
                for (int k = col; k < 4; ++k) {
                    aug[col][k] /= diag;
                }
                for (int row = 0; row < 3; ++row) {
                    if (row == col) continue;
                    float f = aug[row][col];
                    for (int k = col; k < 4; ++k) {
                        aug[row][k] -= f * aug[col][k];
                    }
                }
            }

            x[0] = aug[0][3];
            x[1] = aug[1][3];
            x[2] = aug[2][3];
            return true;
        };

        constexpr float eps = 1e-3f;
        constexpr float lambda = 3e-3f;
        constexpr float max_joint_step = 0.12f;  // 单次迭代关节最大更新量(rad)
        constexpr int max_iter = 20;
        constexpr float pos_tol = 0.015f;        // 1.5cm 认为到达目标附近
        constexpr float fallback_tol = 0.05f;    // 5cm 内也视作可接受粗到位

        float best_err = 1e9f;
        Arm6dof_JointAngles_t best_q = q;

        for (int iter = 0; iter < max_iter; ++iter) {
            Arm6dof_Pose_t cur_pose;
            if (Arm6dof_ForwardKinematics(&q, &cur_pose) != 0) {
                break;
            }

            float ex = pose->x - cur_pose.x;
            float ey = pose->y - cur_pose.y;
            float ez = pose->z - cur_pose.z;
            float err_norm = sqrtf(ex * ex + ey * ey + ez * ez);

            if (err_norm < best_err) {
                best_err = err_norm;
                best_q = q;
            }
            if (err_norm <= pos_tol) {
                break;
            }

            // 仅对 J1~J3 构造位置雅可比，J4~J6保持不动。
            float J[3][3];
            for (int j = 0; j < 3; ++j) {
                Arm6dof_JointAngles_t q_pert = q;
                q_pert.q[j] += eps;
                Arm6dof_Pose_t pose_pert;
                if (Arm6dof_ForwardKinematics(&q_pert, &pose_pert) != 0) {
                    return -1;
                }
                J[0][j] = (pose_pert.x - cur_pose.x) / eps;
                J[1][j] = (pose_pert.y - cur_pose.y) / eps;
                J[2][j] = (pose_pert.z - cur_pose.z) / eps;
            }

            // 阻尼最小二乘：dq = (J^T J + λI)^-1 J^T e
            float H[3][3] = {{0}};
            float g[3] = {0};
            float e[3] = {ex, ey, ez};
            for (int r = 0; r < 3; ++r) {
                for (int c = 0; c < 3; ++c) {
                    H[r][c] = J[0][r] * J[0][c] + J[1][r] * J[1][c] + J[2][r] * J[2][c];
                }
                H[r][r] += lambda;
                g[r] = J[0][r] * e[0] + J[1][r] * e[1] + J[2][r] * e[2];
            }

            float dq[3] = {0};
            if (!solve3x3(H, g, dq)) {
                break;
            }

            for (int j = 0; j < 3; ++j) {
                if (dq[j] > max_joint_step) dq[j] = max_joint_step;
                if (dq[j] < -max_joint_step) dq[j] = -max_joint_step;
                q.q[j] = clampToLimit((size_t)j, q.q[j] + dq[j]);
            }
        }

        if (best_err > fallback_tol) {
            return -1;
        }

        // 维持腕部姿态关节，便于用户后续遥杆精细操作。
        best_q.q[3] = seed.q[3];
        best_q.q[4] = seed.q[4];
        best_q.q[5] = seed.q[5];

        // 最终写回前再次进行限位钳制。
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            best_q.q[i] = clampToLimit(i, best_q.q[i]);
        }

        *q_out = best_q;
        return 0;
    }
    
    /**
     * @brief 轨迹规划核心：基于固定的 start→goal 路径，由 traj_.t 驱动进度
     *
     * 设计原则：轨迹由 traj_.start / traj_.goal 固定，traj_.t 按速度限制逐帧递增。
     *   - IK目标点由 traj_.t 插值得到，与当前末端位姿无关，手动扰动不影响轨迹目标
     *   - IK初始猜测优先使用上一帧轨迹解（连续性好），首帧用当前关节角
     *
     * 每帧执行流程：
     *   1. 计算 start→goal 的总位移/角位移向量（固定）
     *   2. dt = min(v_max*dt/total, 1.0)  按速度限制计算本帧进度增量
     *   3. traj_.t += dt，clamp 到 [0, 1]
     *   4. interp = start + t * (goal - start)
     *   5. IK(interp, q_init=上一帧解 或 当前关节角) → 写入 traj_.angles
     */
    void StepTrajectory() {
        if (!traj_.active) return;
        constexpr float TWO_PI = 2.0f * (float)M_PI;

        // 计算 start→goal 的固定总位移向量（不随手动扰动变化）
        float tdx  = traj_.goal.x     - traj_.start.x;
        float tdy  = traj_.goal.y     - traj_.start.y;
        float tdz  = traj_.goal.z     - traj_.start.z;
        float total_dist = sqrtf(tdx*tdx + tdy*tdy + tdz*tdz);

        float tdr  = traj_.goal.roll  - traj_.start.roll;
        float tdp  = traj_.goal.pitch - traj_.start.pitch;
        float tdyw = traj_.goal.yaw   - traj_.start.yaw;
        // 姿态差 wrap 到 (-π, π]：走最短路径
        tdr  -= roundf(tdr  / TWO_PI) * TWO_PI;
        tdp  -= roundf(tdp  / TWO_PI) * TWO_PI;
        tdyw -= roundf(tdyw / TWO_PI) * TWO_PI;
        float total_ang = sqrtf(tdr*tdr + tdp*tdp + tdyw*tdyw);

        // 按速度限制计算本帧进度增量，位置/姿态约束取严格者
        float dt_p = (total_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / total_dist) : 1.0f;
        float dt_a = (total_ang  > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / total_ang)  : 1.0f;
        float dt   = dt_p < dt_a ? dt_p : dt_a;
        if (dt > 1.0f) dt = 1.0f;

        traj_.t += dt;
        if (traj_.t >= 1.0f) {
            traj_.t      = 1.0f;
            traj_.active = false;
        }

        // 线性插值：start + t * (goal - start)，轨迹固定，不受手动扰动影响
        Arm6dof_Pose_t interp;
        interp.x     = traj_.start.x     + traj_.t * tdx;
        interp.y     = traj_.start.y     + traj_.t * tdy;
        interp.z     = traj_.start.z     + traj_.t * tdz;
        interp.roll  = traj_.start.roll  + traj_.t * tdr;
        interp.pitch = traj_.start.pitch + traj_.t * tdp;
        interp.yaw   = traj_.start.yaw   + traj_.t * tdyw;

        // IK初始猜测：优先用上一帧轨迹IK解（连续性好，不受手动扰动影响）
        // traj_.valid=false 表示首帧或上帧IK失败，改用当前关节角
        Arm6dof_JointAngles_t q_init;
        if (traj_.valid) {
            q_init = traj_.angles;
        } else {
            FillCurrentJointAngles(q_init);
        }

        bool ik_success = false;
        if (control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
            ik_success = (InverseKinematicsAnalytical(&interp, &traj_.angles, &q_init) == 0);
        } else {
            ik_success = (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) == 0);
        }

        if (!ik_success) {
            traj_.valid = false;
            traj_.active = false;
            control_.state = MotionState::ERROR;
            return;
        }

        // 角度连续性修正：仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）
        // IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            if (!joints_[i]) continue;
            const auto& dh = joints_[i]->GetParams();
            if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue;  // 范围 ≤ 2π，无等效分支
            float diff = traj_.angles.q[i] - q_init.q[i];
            diff -= roundf(diff / TWO_PI) * TWO_PI;
            traj_.angles.q[i] = q_init.q[i] + diff;
        }

        // 限位检查
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            if (joints_[i]) {
                const auto& dh = joints_[i]->GetParams();
                if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) ||
                    traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) {
                    traj_.valid = false;
                    traj_.active = false;
                    control_.state = MotionState::ERROR;
                    return;
                }
            }
        }

        // 突变检查：以上一帧轨迹解（q_init）为参考，检查轨迹自身连续性
        static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f;  // rad ≈ 17°/帧
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            float delta = fabsf(traj_.angles.q[i] - q_init.q[i]);
            if (delta > TRAJ_MAX_JOINT_DELTA) {
                traj_.valid = false;
                traj_.active = false;
                control_.state = MotionState::ERROR;
                return;
            }
        }

        traj_.valid = true;
        end_effector_.target = interp;
    }

    int8_t GetPose(Arm6dof_Pose_t* pose) const {
        if (pose == nullptr) {
            return -1;
        }
        std::memcpy(pose, &end_effector_.current, sizeof(Arm6dof_Pose_t));
        return 0;
    }

    int8_t SetTargetPose(const Arm6dof_Pose_t pose) {
        end_effector_.target = pose;
        return 0;
    }

    int8_t GetTargetPose(Arm6dof_Pose_t* pose) const {
        if (pose == nullptr) {
            return -1;
        }
        std::memcpy(pose, &end_effector_.target, sizeof(Arm6dof_Pose_t));
        return 0;
    }

    // 控制循环
    int8_t Control() {
        // 更新时间
        uint64_t now_us = BSP_TIME_Get_us();
        timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f;
        timer_.last_wakeup = now_us;
        timer_.now = now_us / 1000000.0f;
        
        // 未使能则松弛
        if (!control_.enable) {
            RelaxAllJoints();
            control_.state = MotionState::STOPPED;
            return 0;
        }
        
        // 根据控制模式执行
        switch (control_.mode) {
            case ControlMode::IDLE:
                RelaxAllJoints();
                control_.state = MotionState::STOPPED;
                break;
                
            case ControlMode::JOINT_POSITION:
            case ControlMode::CARTESIAN_POSITION:
            case ControlMode::CARTESIAN_ANALYTICAL:
            case ControlMode::GRAVITY_COMP: {
                // 检查错误状态（不可达或超限）
                if (control_.state == MotionState::ERROR) {
                    // 进入错误状态，松弛所有关节（安全停止）
                    RelaxAllJoints();
                    control_.enable = false;  // 自动禁用
                    return -1;
                }
                
                // 将重力补偿力矩写入各关节前馈
                if (control_.gravity_comp_enable) {
                    for (size_t i = 0; i < JOINT_NUM; ++i) {
                        if (joints_[i]) {
                            joints_[i]->SetFeedforwardTorque(gravity_comp_.torques[i]);
                        }
                    }
                }
                
                if(control_.mode == ControlMode::JOINT_POSITION) {
   
                }

                // GRAVITY_COMP 模式：保持当前位置 + 重力补偿
                if (control_.mode == ControlMode::GRAVITY_COMP) {
                    SetJointTargetsToCurrent();
                }

                // CARTESIAN_POSITION 相关的轨迹模式：每帧推进轨迹一小步，再应用IK解到关节
                if (control_.mode == ControlMode::CARTESIAN_POSITION || control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
                    // 轨迹规划核心调用：插值 + IK，结果写入 traj_
                    StepTrajectory();

                    if (traj_.valid) {
                        for (size_t i = 0; i < JOINT_NUM; ++i) {
                            if (joints_[i]) {
                                joints_[i]->SetTargetAngle(traj_.angles.q[i]);
                            }
                        }
                    } else {
                        // IK解无效：松弛所有关节并进入错误状态
                        RelaxAllJoints();
                        control_.state = MotionState::ERROR;
                        return -1;
                    }
                }
                
                // 位置控制
                bool all_reached = true;
                for (Joint* joint : joints_) {  // 遍历关节数组
                    if (joint) {
                        joint->PositionControl(joint->GetTargetAngle(), timer_.dt);
                        if (!joint->IsReached(control_.position_tolerance)) {
                            all_reached = false;
                        }
                    }
                }
                control_.state = all_reached ? MotionState::REACHED : MotionState::MOVING;
                break;
            }
            
            case ControlMode::TEACH: {
                // 示教模式：仅施加重力补偿力矩，不施加位置刚度
                // 操作者可以自由拖动机械臂
                if (control_.gravity_comp_enable) {
                    for (size_t i = 0; i < JOINT_NUM; ++i) {
                        if (joints_[i]) {
                            joints_[i]->TorqueControl(gravity_comp_.torques[i]);
                        }
                    }
                } else {
                    // 未使能重力补偿时，示教模式下松弛
                    RelaxAllJoints();
                }
                control_.state = MotionState::MOVING;
                break;
            }
                
            default:
                control_.state = MotionState::ERROR;
                break;
        }
        
        return 0;
    }
    
    // ========================================================================
    // 高层API
    // ========================================================================
    
    void Enable(bool enable) {
        control_.enable = enable;
        if (!enable) {
            control_.mode = ControlMode::IDLE;
        }
    }
    
    void SetMode(ControlMode mode) {
        control_.mode = mode;
    }
    
    MotionState GetState() const {
        return control_.state;
    }
    
    /**
     * @brief 使能/禁用重力补偿
     * @param enable true=使能, false=禁用
     */
    void EnableGravityCompensation(bool enable) {
        control_.gravity_comp_enable = enable;
        if (!enable) {
            // 清除补偿力矩
            for (size_t i = 0; i < JOINT_NUM; ++i) {
                gravity_comp_.torques[i] = 0.0f;
                if (joints_[i]) joints_[i]->SetFeedforwardTorque(0.0f);
            }
        }
    }
    
    /**
     * @brief 设置全部关节统一补偿系数（兼容旧接口）
     */
    void SetGravityCompScale(float scale) {
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            gravity_comp_.scales[i] = scale;
        }
    }

    /**
     * @brief 设置单个关节补偿系数
     */
    void SetGravityCompScale(size_t index, float scale) {
        if (index < JOINT_NUM) {
            gravity_comp_.scales[index] = scale;
        }
    }

    /**
     * @brief 批量设置6个关节补偿系数
     */
    void SetGravityCompScales(const float scales[JOINT_NUM]) {
        if (!scales) return;
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            gravity_comp_.scales[i] = scales[i];
        }
    }

    /**
     * @brief 获取单个关节补偿系数
     */
    float GetGravityCompScale(size_t index) const {
        return (index < JOINT_NUM) ? gravity_comp_.scales[index] : 1.0f;
    }
    
    /**
     * @brief 获取指定关节的重力补偿力矩
     */
    float GetGravityTorque(size_t index) const {
        return (index < JOINT_NUM) ? gravity_comp_.torques[index] : 0.0f;
    }
    
    bool IsGravityCompEnabled() const {
        return control_.gravity_comp_enable;
    }
    
    // 关节空间控制（直接设置目标关节角度）
    int8_t MoveJoint(const float target_angles[JOINT_NUM]) {
        // 1. 检查限位
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            if (joints_[i]) {
                const auto& dh = joints_[i]->GetParams();
                if (target_angles[i] < dh.qmin || target_angles[i] > dh.qmax) {
                    return -1;  // 超限
                }
            }
        }
        
        // 2. 设置目标角度到各关节
        for (size_t i = 0; i < JOINT_NUM; ++i) {
            if (joints_[i]) {
                joints_[i]->SetTargetAngle(target_angles[i]);
            }
        }
        
        control_.state = MotionState::MOVING;
        return 0;
    }
    
    /**
     * @brief 设置笛卡尔空间目标位姿，自动启动轨迹规划
     * 目标发生变化时，从当前末端位姿重新规划轨迹。
     * 实际运动由 Control() 每帧调用 StepTrajectory() 推进。
     *
     * @param goal 目标末端位姿
     */
    int8_t MoveCartesian(const Arm6dof_Pose_t& goal) {
        // 检测目标是否发生有效变化（避免每帧重置轨迹）
        constexpr float POS_THRESH = 0.001f;   // 1mm
        constexpr float ANG_THRESH = 0.01f;    // ~0.57°
        bool goal_changed = IsPoseChanged(goal, traj_.goal, POS_THRESH, ANG_THRESH);
        if (goal_changed) {
            // 从当前末端位姿出发重新规划轨迹
            traj_.start  = end_effector_.current;
            traj_.goal   = goal;
            traj_.t      = 0.0f;
            traj_.active = true;
            traj_.valid = false;  // 首帧由 StepTrajectory 用当前关节角作IK初值
            control_.state = MotionState::MOVING;
        }
        return 0;
    }
    
    // 急停
    void Stop() {
        control_.state = MotionState::STOPPED;
    }

    /**
     * @brief 工具坐标系目标位姿控制
     *
     * 与 MoveCartesian 的区别：
     *   - MoveCartesian   ：输入世界系绝对位姿
     *   - MoveCartesianTool：输入工具坐标系下的目标位姿，内部变换到世界系
     *
     * 变换原理：
     *   位置： p_world = R_cur * [x_t, y_t, z_t]^T
     *   姿态： R_world = R_cur * R_target
     *
     * @param target_tool 工具坐标系下的目标位姿（绝对值）
     *        .x/.y/.z          ：工具系下的目标位置 (m)
     *        .roll/.pitch/.yaw ：工具系下的目标姿态 (rad)
     */
    void MoveCartesianTool(const Arm6dof_Pose_t& target_tool) {
        // 仅当用户输入目标发生变化时才重新换算世界系目标
        // 若输入不变（用户未推杆），复用缓存的世界系目标，保证轨迹可以连续推进至 t=1
        constexpr float POS_EPS = 0.001f;
        constexpr float ANG_EPS = 0.01f;
        const FrameCache_t& tc = tool_frame_cache_;
        bool input_changed = !tc.valid || IsPoseChanged(target_tool, tc.last_input, POS_EPS, ANG_EPS);

        if (input_changed) {
            // 用当前末端位姿做坐标系变换（仅在用户改变目标时触发一次）
            const Arm6dof_Pose_t& cur = end_effector_.current;
            float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
            Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));

            Arm6dof_Pose_t goal;
            float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z};
            Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr);
            goal.x = p_world[0][0];
            goal.y = p_world[1][0];
            goal.z = p_world[2][0];

            float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll};
            Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
            Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
            goal.yaw   = rpy_new[0][0];
            goal.pitch = rpy_new[1][0];
            goal.roll  = rpy_new[2][0];

            tool_frame_cache_.last_input = target_tool;
            tool_frame_cache_.world_goal = goal;
            tool_frame_cache_.valid      = true;
        }

        MoveCartesian(tool_frame_cache_.world_goal);
    }

    /**
     * @brief 航向参考系目标位姿控制
     *
     *   位置（航向系）：世界系绕Z轴旋转当前yaw角度得到，X/Y始终水平，Z始终世界竖直
     *   姿态（航向系）：roll/pitch/yaw 为末端在航向系下的绝对目标姿态，直观地对应末端各轴旋转
     *
     * 变换原理：
     *   位置 X/Y：p_world = Rz(yaw) * [x, y, 0]^T
     *   位置 Z  ：p_world.z = z  (世界竖直直接赋值)
     *   姿态    ：R_world = Rz(yaw) * R_target  （航向系下绝对姿态→世界系）
     *
     * @param target_heading
     *        .x/.y  ：航向系（水平）下的目标位置 (m)
     *        .z     ：世界竖直方向目标高度 (m)
     *        .roll/.pitch/.yaw ：航向系下的末端绝对姿态 (rad)
     *                          直观含义：yaw转动就是末端 yaw，pitch/roll 同理
     */
    void MoveCartesianHeading(const Arm6dof_Pose_t& target_heading) {
        // 仅当用户输入目标发生变化时才重新换算世界系目标（同 MoveCartesianTool 原理）
        constexpr float POS_EPS = 0.001f;
        constexpr float ANG_EPS = 0.01f;
        const FrameCache_t& hc = heading_frame_cache_;
        bool input_changed = !hc.valid || IsPoseChanged(target_heading, hc.last_input, POS_EPS, ANG_EPS);

        if (input_changed) {
            const Arm6dof_Pose_t& cur = end_effector_.current;
            float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
            Arm6dof_Pose_t goal;
            goal.x = cy * target_heading.x - sy * target_heading.y;
            goal.y = sy * target_heading.x + cy * target_heading.y;
            goal.z = target_heading.z;

            float rpy_yaw_arr[3]    = {cur.yaw, 0.0f, 0.0f};
            float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll};
            Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr))
                            * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
            Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
            goal.yaw   = rpy_new[0][0];
            goal.pitch = rpy_new[1][0];
            goal.roll  = rpy_new[2][0];

            heading_frame_cache_.last_input = target_heading;
            heading_frame_cache_.world_goal = goal;
            heading_frame_cache_.valid      = true;
        }

        MoveCartesian(heading_frame_cache_.world_goal);
    }
    
    // 获取末端位姿（FK结果，单位 m/rad）
    const Arm6dof_Pose_t& GetEndPose() const {
        return end_effector_.current;
    }

    // 获取最近一次 IK 解算结果
    const Arm6dof_JointAngles_t& GetIkAngles() const {
        return inverseKinematics_.angles;
    }

    // 获取最近一次 IK 是否有效
    bool IsIkValid() const {
        return inverseKinematics_.valid;
    }

    /** 设置末端线速度限制 (m/s)，默认 0.15 m/s */
    void SetLinVelLimit(float vel) { traj_.max_lin_vel = vel; }

    /** 设置末端角速度限制 (rad/s)，默认 1.0 rad/s */
    void SetAngVelLimit(float vel) { traj_.max_ang_vel = vel; }

    /** 获取当前轨迹进度 [0.0, 1.0]，1.0 表示已到达目标 */
    float GetTrajProgress() const { return traj_.t; }

    /** 轨迹是否正在执行（false 表示已到达目标或尚未启动） */
    bool IsTrajActive() const { return traj_.active; }

    /** 清除错误状态并同步目标位姿到当前位姿（错误恢复用） */
    void ResetError() {
        control_.state = MotionState::REACHED;
        control_.enable = true;
        inverseKinematics_.valid = true;
        traj_.valid = true;
        traj_.active = false;

        Arm6dof_JointAngles_t q;
        FillCurrentJointAngles(q);
        SetJointTargetsToCurrent();
        inverseKinematics_.angles = q;


        // 将轨迹目标同步为当前位姿，防止下次启动时从旧目标出发
        traj_.goal = end_effector_.current;
        end_effector_.target = end_effector_.current;
    }
};

} // namespace arm