/**
 ******************************************************************************
 * @file    arm.h
 * @brief   6-DOF robotic arm kinematics control
 ******************************************************************************
 */

#ifndef ARM_H
#define ARM_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>

/* 机器人尺寸参数结构体 */
typedef struct {
    // 6个运动关节的DH参数
    float theta;
    float d;
    float a;
    float alpha;
    float theta_offset;
    float qmin;
    float qmax;
    float m;        // 连杆质量 (kg)
    float rc[3];    // 质心在 DH 连杆坐标系下的坐标 (m): {x, y, z}
                    // 由 URDF inertial/origin 经 Rz(-theta_offset) 旋转得到
} Arm6dof_DHParams_t;
typedef struct {
    // MIT控制参数（可选，如不设置则使用默认值）
    float kp;       // 位置刚度 (N·m/rad)，默认值见Joint类
    float kd;       // 阻尼系数 (N·m·s/rad)，默认值见Joint类
} Arm6dof_MotorMTICtrlParams_t;
typedef struct {
    Arm6dof_DHParams_t DH_params[6];
    Arm6dof_MotorMTICtrlParams_t motor_ctrl_params[6];  // 每个关节的MIT控制参数（可选）
    struct {
        float x, y, z;           // 工具中心偏移 (m)
        float roll, pitch, yaw;  // 工具姿态偏移 (rad)
    } tool_offset;
} Arm6dof_Params_t;
/* 末端位姿结构体 */
typedef struct {
    float x, y, z;           // 位置 (m)
    float roll, pitch, yaw;  // 姿态 (rad) - RPY欧拉角
} Arm6dof_Pose_t;

/* 关节角度结构体 */
typedef struct {
    float q[6];  // 6个关节角度 (rad)
} Arm6dof_JointAngles_t;

/**
 * @brief 机械臂控制类型枚举
 * - REMOTE_CARTESIAN: 遥控器/PC端控制 - 目标末端位姿（笛卡尔空间）
 * - CUSTOM_JOINT:     自定义控制器  - 目标六个关节角度
 */
typedef enum {
    ARM_CTRL_REMOTE_CARTESIAN = 0,  /* 遥控器控制 - 目标末端位姿（笛卡尔空间） */
    ARM_CTRL_CUSTOM_JOINT,          /* 自定义控制器 - 目标六个关节角度 */
} Arm_CtrlType_t;

/** 机械臂命令结构体 */
typedef struct {
    bool                  enable;               /* 使能开关 */
    bool                  set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
    Arm_CtrlType_t        ctrl_type;             /* 控制类型 */
    Arm6dof_Pose_t        target_pose;           /* 目标末端位姿（世界坐标系，供调试观察） */
    Arm6dof_JointAngles_t target_joints;         /* 目标六个关节角度（自定义控制器模式） */
} Arm_CMD_t;
/**
 * @brief 初始化机械臂运动学模型
 * @param dh_params 6个关节的DH参数数组
 * @note 必须先调用此函数再使用正/逆运动学
 */
void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]);

/**
 * @brief 正运动学：根据关节角度计算末端位姿
 * @param q 输入的6个关节角度 (rad)
 * @param pose 输出的末端位姿 (位置m, 姿态rad)
 * @retval 0: 成功, -1: 失败(未初始化)
 */
int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose);

/**
 * @brief 逆运动学：根据末端位姿计算关节角度
 * @param pose 输入的目标末端位姿 (位置m, 姿态rad)
 * @param q_init 初始关节角度猜测值 (用于数值解迭代)
 * @param q_result 输出的关节角度解 (rad)
 * @param tol 收敛误差容限，位置分量单位为(m)，建议0.001f(即1mm精度)
 * @param max_iter 最大迭代次数，默认10
 * @retval 0: 成功, -1: 失败(未初始化或无解)
 * @note 使用牛顿法数值求解，需要提供合理的初始猜测值
 */
int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init, 
                          Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter);

/**
 * @brief 获取指定关节的位姿 (用于可视化或调试)
 * @param q 输入的6个关节角度 (rad)
 * @param joint_num 关节编号 (1-6)
 * @param pose 输出的该关节位姿
 * @retval 0: 成功, -1: 失败
 */
int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose);
/**
 * @brief 设置机器人参数 (如果需要动态修改)
 * @param params 新的机器人尺寸参数
 */
void Arm6dof_SetParams(const Arm6dof_Params_t* params);

/**
 * @brief 计算重力补偿力矩（基于牛顿-欧拉逆动力学，速度和加速度为零）
 * @param q 当前6个关节角度 (rad)
 * @param gravity_torques 输出的6个关节重力补偿力矩 (N·m，DH参数单位为m时)
 * @retval 0: 成功, -1: 失败(未初始化)
 */
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]);

#ifdef __cplusplus
}
#endif

#endif // ARM_H