#ifndef ROBOT_MODEL__ROBOT_KINEMATICS_HPP_
#define ROBOT_MODEL__ROBOT_KINEMATICS_HPP_

#include "robot_model.hpp"
#include <Eigen/Dense>
#include <vector>
#include <memory>

namespace robot_model
{

class RobotKinematics
{
public:
    /**
     * @brief 构造函数
     * @param robot_model 机器人模型指针
     */
    explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);

    /**
     * @brief 析构函数
     */
    ~RobotKinematics() = default;

    // 手臂运动学

    /**
     * @brief 计算手臂正运动学
     * @param joint_angles 关节角度数组
     * @return 末端执行器位置 (x, y, z)
     */
    Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);

    /**
     * @brief 计算手臂逆运动学
     * @param target_pos 目标位置 (x, y, z)
     * @param[out] joint_angles 计算得到的关节角度
     * @return 成功返回true，失败返回false
     */
    bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);

    // 腿部运动学

    /**
     * @brief 计算腿部正运动学
     * @param leg_index 腿的索引 (0-3)
     * @param joint_angles 关节角度数组
     * @return 足部位置 (x, y, z)
     */
    Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);

    /**
     * @brief 计算腿部逆运动学
     * @param leg_index 腿的索引 (0-3)
     * @param target_pos 目标位置 (x, y, z)
     * @param[out] joint_angles 计算得到的关节角度
     * @return 成功返回true，失败返回false
     */
    bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);

private:
    /**
     * @brief 创建旋转矩阵 (绕Z轴)
     * @param angle 旋转角度(弧度)
     * @return 旋转矩阵
     */
    Eigen::Matrix3d rotate_z(double angle);

    /**
     * @brief 创建旋转矩阵 (绕Y轴)
     * @param angle 旋转角度(弧度)
     * @return 旋转矩阵
     */
    Eigen::Matrix3d rotate_y(double angle);

    /**
     * @brief 创建旋转矩阵 (绕X轴)
     * @param angle 旋转角度(弧度)
     * @return 旋转矩阵
     */
    Eigen::Matrix3d rotate_x(double angle);

    std::shared_ptr<RobotModel> robot_model_;  // 机器人模型指针
};

}  // namespace robot_model

#endif  // ROBOT_MODEL__ROBOT_KINEMATICS_HPP_
