#ifndef ROBOT_MODEL__ROBOT_MODEL_HPP_
#define ROBOT_MODEL__ROBOT_MODEL_HPP_

#include <vector>
#include <string>
#include <memory>

namespace robot_model
{

class UrdfParser;

// 关节结构体
struct Joint
{
    std::string name;          // 关节名称
    double angle;              // 关节角度（弧度）
    double min_angle;          // 最小角度限制
    double max_angle;          // 最大角度限制
    double effort_limit;       // 力/力矩限制

    Joint(const std::string & joint_name, double min, double max, double effort)
        : name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
};

// 连杆结构体
struct Link
{
    std::string name;          // 连杆名称
    double length;             // 连杆长度
    double mass;               // 连杆质量

    Link(const std::string & link_name, double len, double m)
        : name(link_name), length(len), mass(m) {}
};

// 机械臂结构体
struct Arm
{
    std::vector<std::shared_ptr<Joint>> joints;  // 关节
    std::vector<std::shared_ptr<Link>> links;    // 连杆
    std::string side;                            // 机械臂位置（如"left", "right"）
};

// 腿部结构体
struct Leg
{
    std::vector<std::shared_ptr<Joint>> joints;  // 关节
    std::vector<std::shared_ptr<Link>> links;    // 连杆
    std::string side;                            // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
};

class RobotModel
{
    // 声明UrdfParser为友元类，使其可以访问私有成员
    friend class UrdfParser;
    
public:
    /**
     * @brief 构造函数
     */
    RobotModel();

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

    /**
     * @brief 获取机械臂
     * @return 机械臂对象
     */
    const Arm & get_arm(size_t index) const;

    /**
     * @brief 获取腿部
     * @param index 腿的索引（0-3）
     * @return 腿部对象
     */
    const Leg & get_leg(size_t index) const;

    /**
     * @brief 设置关节角度
     * @param joint_name 关节名称
     * @param angle 目标角度（弧度）
     * @return 成功返回true，失败返回false
     */
    bool set_joint_angle(const std::string & joint_name, double angle);

    /**
     * @brief 获取关节角度
     * @param joint_name 关节名称
     * @return 关节角度（弧度），未找到返回0.0
     */
    double get_joint_angle(const std::string & joint_name) const;

    /**
     * @brief 打印机器人模型信息
     */
    void print_model_info() const;

private:
    /**
     * @brief 初始化机器人模型
     */
    void initialize_model();

    std::vector<Arm> arms_;            // 机械臂集合 （2个机械臂）
    std::vector<Leg> legs_;            // 腿部集合（4条腿）
};

}  // namespace robot_model

#endif  // ROBOT_MODEL__ROBOT_MODEL_HPP_
