#ifndef COL_SIMULATOR_HPP
#define COL_SIMULATOR_HPP

#include <unordered_map>
#include <map>
#include <urdf_model/pose.h>
#include <std_msgs/msg/string.hpp>
#include "arm_define.h"
#include "arm_driver_define.hpp"
#include "gjk_interface.hpp"

#define COLLISION_CHECK_SIMPLE_CNT 2

typedef enum {
    BODY_TYPE_LEFT_ARM = 0,
    BODY_TYPE_RIGHT_ARM,
    BODY_TYPE_OTHERS,
    BODY_CNT
} BodyListE;

typedef enum {
    FRAME_CHANGE_TYPE_ARM_BASE_TO_ORI,
    FRAME_CHANGE_TYPE_ORI_TO_ARM_BASE,
    FRAME_CHANGE_TYPE_ARM_END_TO_ORI,
    FRAME_CHANGE_TYPE_ORI_TO_ARM_END,
    FRAME_CHANGE_TYPE_ERR
} FrameChangeTypeE;

typedef std::vector<Eigen::Vector3d> Polyhedron;

struct JointInfo {
    std::string name;               // 关节名称
    int type;               // 关节类型
    int parent_link;        // 父连杆
    int child_link;         // 子连杆
    Eigen::Quaterniond rotationAll;
    Eigen::Vector3d axis;           // 关节轴
    Eigen::Vector3d offset;         // 关节偏移
    Eigen::Vector3d leftBottom;     // 左下角点关于关节原点偏移
    Eigen::Vector3d nextLinkOffset; // 下一个连杆关于关节原点偏移
    Eigen::Quaterniond rotation;    // 关节旋转
    double limit_lower;
    double limit_upper;
    double limit_v;
    double limit_e;
    float *angle[COLLISION_CHECK_SIMPLE_CNT];   // 关节角度
};

struct CollisionStructure {
    std::string link_name;               // 所属链接名称
    std::string geometry_type;           // 几何类型
    std::string mesh_filename;           // mesh文件路径（如STL）
    std::vector<double> mesh_bounds;     // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
    Eigen::Quaterniond rotationAdd; // 不平行轴关节叠加旋转
    bool rotation_add_flag;
    std::vector<JointInfo> joint_list;
    int joint_count;
    int link_index;
    int parent_link_index;
};

class CollisionSimulator
{
public:
    CollisionSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float **jointsList, KinematicsManager **trajectory);
    CollisionSimulator(std::string urdf_string, float **jointsList, KinematicsManager **trajectory);
    ~CollisionSimulator();

    int CheckCollision();
    int UpdateCollisionForNewGoal(int bodyType);
    int AddPolyhedron(int id, const Polyhedron& poly);
    int RemovePolyhedron(int id);

private:
    void InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory);
    int CheckJointsAngleLimit(float *joints, int bodyType);
    int InitPolyhedrons(const urdf::Model &urdf_model);
    void InitArmSinglePolyhedron(int index, Polyhedron *poly, Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation);
    void UpdatePolyhedrons(int startLinkIndex, int endLinkIndex, int simple, float *jointList);
    int InitCollisionStructureList(const urdf::Model &urdf_model);
    int GetMeshLinkdimensions(const std::string& filename, CollisionStructure& col_struct);

    std::pair<Eigen::Vector3d, Eigen::Matrix3d> GetBodyTransform(int startIndex);

    void InitPolyhedronsList();
    void UpdateArmPolyhedrons();

    KinematicsManager *trajectory_[BODY_CNT];
    std::vector<Polyhedron> polyhedrons_[COLLISION_CHECK_SIMPLE_CNT];
    std::vector<OBB> addedOBBs_[COLLISION_CHECK_SIMPLE_CNT];

    std::vector<CollisionStructure> collision_structures_;
    std::vector<JointInfo> jointMap_;

    int simpleUpdateIndex_;
    float *joints_[COLLISION_CHECK_SIMPLE_CNT][BODY_CNT];

    std::unordered_map<int, Polyhedron> addedPolyhedrons_;
    std::map<std::string, int> linkCollisionMap_;
};

#endif // COL_SIMULATOR_HPP