#ifndef CREATE_ROBOT_DESCRIBE_HPP
#define CREATE_ROBOT_DESCRIBE_HPP

#include <string>
#include <vector>
#include <memory>
#include <map>
#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
#include "gjk_interface.hpp"

struct JointsModel {
    std::string name;               // 关节名称
    int type;               // 关节类型
    int parent_link;        // 父连杆
    int child_link;         // 子连杆
    Eigen::Vector3d axis;           // 关节轴
    
    Eigen::Vector3d nextLinkOffset; // 下一个连杆关于关节原点偏移
    Eigen::Quaterniond rotation;    // 关节旋转
    double limit_lower;
    double limit_upper;
    double limit_v;
    double limit_e;
};

struct CollisionModel {
    std::string link_name;               // 所属链接名称
    int geometry_type;           // 几何类型
    std::string mesh_filename;           // mesh文件路径（如STL）
    std::vector<OBB> obbModelList;  // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
    std::vector<int> jointIndexList; // 网格边界盒所属链接索引
    std::vector<int> obbObbIndexList; // 网格边界盒所属链接索引
    int joint_count;
    int link_index;
    int parent_link_index;

    Eigen::Quaterniond rotation;      // 链接旋转
    Eigen::Vector3d offset;          // 链接偏移
};

class RobotDescription
{
public:
    RobotDescription(const std::string &urdf, const std::string &srdf);
    ~RobotDescription();
    // 其他成员函数和变量

    std::vector<CollisionModel> collision_structures_;
    std::vector<JointsModel> jointMap_;
    std::map<std::string, int> linkCollisionMap_;

    int rootLinkIndex_;
    int maxObbCnt;

    bool *g_linkCollisionDetectMatrix;
private:
    int InitJointModel();
    int InitJoint(const urdf::Model &urdf_model);
    int GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct);
    int InitCollisionStructureList(const urdf::Model &urdf_model);
    void UpdatePolyhedrons();
    int InitLinkCollisionDetectMatrix(const std::string& srdf_str);
};

#endif // CREATE_ROBOT_DESCRIBE_HPP