#ifndef COL_SIMULATOR_HPP
#define COL_SIMULATOR_HPP

#include <unordered_map>
#include <map>
#include <rclcpp/rclcpp.hpp>
#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 JointsInfo {
    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;
    float *angle[COLLISION_CHECK_SIMPLE_CNT];   // 关节角度
};

struct CollisionStructureInfo {
    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> obbLinkIndexList; // 网格边界盒所属链接索引
    std::vector<int> jointIndexList; // 网格边界盒所属关节索引

    Eigen::Quaterniond rotation[COLLISION_CHECK_SIMPLE_CNT];      // 链接旋转
    Eigen::Vector3d offset[COLLISION_CHECK_SIMPLE_CNT];          // 链接偏移

    int joint_count;
    int link_index;
    int parent_link_index;
};

class CollisionSimulator
{
public:
    CollisionSimulator(std::string robot_string, std::string srdf_string, float **jointsList, KinematicsManager **trajectory);
    ~CollisionSimulator();

    int CheckCollision(rclcpp::Node *node);
    int UpdateCollisionForNewGoal(int bodyType);
    int AddPolyhedron(int id, const Polyhedron& poly);
    int RemovePolyhedron(int id);

    int CheckArmJointsAngleLimit(float *joints);
private:
    int CheckBodyAngleLimit(int simple, int bodyType);
    int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_);
    int CheckCollisionInnerOld(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_);
    void UpdatePolyhedrons(int bodyType, int simple);
    int InitLinkCollisionDetectMatrix(const std::string& srdf_str);
    void InitPolyhedronsList();

    int InitXmlStrFromFile(const std::string& robot_string);
    int InitJoint();

    KinematicsManager *trajectory_[BODY_CNT];
    std::vector<Polyhedron> polyhedrons_[COLLISION_CHECK_SIMPLE_CNT];
    std::vector<OBB> addedOBBs_[COLLISION_CHECK_SIMPLE_CNT];
    std::map<int, std::string> obbNameMap_;

    std::vector<CollisionStructureInfo> collision_structures_;
    std::vector<JointsInfo> jointMap_;

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

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

    // Collision pair enable matrix (flattened upper-triangle including i<j)
    std::vector<uint8_t> linkCollisionDetectMatrix_;
    // Precomputed list of enabled OBB pairs to check
    std::vector<std::tuple<int, int, AxisStats*>> tuplesToCheck_;
};

#endif // COL_SIMULATOR_HPP