/**
 * @file joint.hpp
 * @brief 关节类 - 封装单个关节的所有功能
 */

#pragma once

#include <stdint.h>
#include <cmath>
#include "motor_base.hpp"
#include "component/arm_kinematics/arm6dof.h"

namespace arm {

struct JointControlParams {
    float qmin;
    float qmax;
    float kp;
    float kd;
};

// ============================================================================
// Joint 类 - 关节对象
// ============================================================================
class Joint {
private:
    uint8_t id_;                    // 关节编号
    IMotor* motor_;                 // 电机指针（多态）
    JointControlParams params_;     // 关节控制参数
    float q_offset_;                // 零位偏移
    const Joint* coupled_joint_;    // 耦合源关节（J3 与 J2 耦合）
    
    // 状态
    struct {
        float current_angle;
        float current_velocity;
        float current_torque;
        float target_angle;
        float feedforward_torque;  // 前馈力矩（如重力补偿）
        bool online;
    } state_;
    
public:
    // 构造函数
        Joint(uint8_t id, IMotor* motor, const JointControlParams& params,
          float q_offset, float freq)
                : id_(id), motor_(motor), params_(params), q_offset_(q_offset),
          coupled_joint_(nullptr) {
                
        state_.current_angle = 0.0f;
        state_.current_velocity = 0.0f;
        state_.current_torque = 0.0f;
        state_.target_angle = 0.0f;
        state_.feedforward_torque = 0.0f;
        state_.online = false;
    }

        // 兼容构造：允许直接传入机械臂DH参数，但仅提取关节层需要的控制字段
        Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params,
                    float q_offset, float freq)
                : Joint(id,
                                motor,
                                JointControlParams{dh_params.qmin, dh_params.qmax, 0.0f, 0.0f},
                                q_offset,
                                freq) {}
    
    // 禁止拷贝（电机资源唯一）
    Joint(const Joint&) = delete;
    Joint& operator=(const Joint&) = delete;
    
    // Getters
    uint8_t GetId() const { return id_; }
    float GetCurrentAngle() const { return state_.current_angle; }
    float GetCurrentVelocity() const { return state_.current_velocity; }
    float GetTargetAngle() const { return state_.target_angle; }
    bool IsOnline() const { return state_.online; }
    const JointControlParams& GetParams() const { return params_; }
    float GetOffset() const { return q_offset_; }
    
    // Setters
    void SetOffset(float offset) { q_offset_ = offset; }

    /**
     * @brief 设置机械耦合源关节
     * @param joint 耦合源关节指针（读取角度时减去其角度，写入时加回）
     * @note  用于关节间机械耦合补偿，如 J3 与 J2 耦合：
     *        实际J3角度 = 电机J3原始角度 - J2当前角度
     */
    void SetCoupledJoint(const Joint* joint) { coupled_joint_ = joint; }
    
    void SetTargetAngle(float target) { 
        state_.target_angle = target; 
    }
    
    void SetFeedforwardTorque(float torque) {
        state_.feedforward_torque = torque;
    }
    
    float GetFeedforwardTorque() const { return state_.feedforward_torque; }
    
    int8_t Register() {
        return motor_->Register();
    }
    
    int8_t Enable() {
        return motor_->Enable();
    }
    
    int8_t Update() {
        int8_t ret = motor_->Update();
        
        float raw = motor_->GetAngle() - q_offset_;

        // 多圈关节（运动范围 > 2π，如 J1/J4/J6）：直接透传多圈绝对值，不截断
        // 单圈关节（运动范围 ≤ 2π，如 J2/J3/J5）：归一化到 [-π, π]
        //   目的：消除 DM 电机 0~2π 单圈编码在过零点时的跳变
        //   零点不变：raw=0 时不触发任何折叠
        //   注意：j3 范围 [0,3]、j5 范围 [-1.75,1.75] 均在 [-π,π] 内，
        //         归一化后其实际运动段不会碰到 ±π 边界，计算安全
        const bool multi_turn = (params_.qmax - params_.qmin) > 2.0f * M_PI;
        if (!multi_turn) {
            if (raw >  M_PI) raw -= 2.0f * M_PI;
            if (raw < -M_PI) raw += 2.0f * M_PI;
        }

        // 机械耦合补偿：减去耦合源关节的当前角度以得到真实关节角度
        // 例：J3 与 J2 耦合时，真实J3 = 电机J3原始角 - J2当前角
        if (coupled_joint_) {
            raw -= coupled_joint_->GetCurrentAngle();
        }

        state_.current_angle    = raw;
        state_.current_velocity = motor_->GetVelocity();
        state_.current_torque   = motor_->GetTorque();
        state_.online           = motor_->IsOnline();
        
        return ret;
    }
    
    int8_t Relax() {
        return motor_->Relax();
    }
    
    // 位置控制
    int8_t PositionControl(float target_angle, float dt) {
        // 检查限位
        if (target_angle < params_.qmin || target_angle > params_.qmax) {
            return -1;
        }
        
        state_.target_angle = target_angle;
      
        // 总前馈力矩 = 重力补偿等前馈力矩
        float total_feedforward = state_.feedforward_torque;
        
        // MIT控制参数：优先使用DH参数中配置的kp/kd，否则使用默认值
        float kp = params_.kp;
        float kd = params_.kd;
        
        // 如果未配置（=0），使用默认值（按电机类型）
        if (kp <= 0.0f) kp = (id_ < 3) ? 10.0f : 50.0f;  // LZ vs DM默认值
        if (kd <= 0.0f) kd = (id_ < 3) ? 0.5f : 3.0f;
        
        // 机械耦合补偿：发送给电机的目标需加回耦合源关节的角度
        float motor_target = target_angle;
        if (coupled_joint_) {
            motor_target += coupled_joint_->GetCurrentAngle();
        }

        return motor_->PositionControl(
            motor_target, 
            0.0f,                      // 速度
            kp, kd,                    // 刚度和阻尼
            total_feedforward          // 前馈力矩（重力补偿）
        );
    }
    
    // 纯力矩控制
    // @param torque: 输出力矩（N·m）
    // @return: 0=成功，-1=失败
    int8_t TorqueControl(float torque) {
        state_.feedforward_torque = torque;  // 存储当前前馈力矩
        return motor_->PositionControl(
            state_.current_angle,  // 保持当前位置
            0.0f,                              // 速度
            0.0f, 0.0f,                        // kp=0, kd=0（无刚度无阻尼）
            torque                             // 纯力矩
        );
    }
    
    // 检查是否到达目标
    bool IsReached(float tolerance) const {
        return std::fabs(state_.target_angle - state_.current_angle) < tolerance;
    }
};

} // namespace arm