#include <memory>
#include <vector>
#include <stack>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model/robot_model.h>
#include "col_simulator.hpp"

using namespace Eigen;
const int g_bodyLinkCnt[BODY_CNT] = {USED_ARM_DOF + 1, USED_ARM_DOF + 1, USED_OTHER_DOF + 1};
const int g_jointLinkCnt[BODY_CNT] = {USED_ARM_DOF, USED_ARM_DOF, USED_OTHER_DOF};
float g_leftArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
float g_rightArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
float g_otherJoints[COLLISION_CHECK_SIMPLE_CNT][USED_OTHER_DOF] = {0};

#define POLYHEDRONS_POINTS_CNT   (8)

enum OriginBoxIndex {
    LEFT_ARM_ORIGIN = 0,
    LEFT_ARM_UPDATE_ORIGIN,
    RIGHT_ARM_ORIGIN = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM],
    RIGHT_ARM_UPDATE_ORIGIN,
    OTHER_ORIGIN = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM] + g_bodyLinkCnt[BODY_TYPE_RIGHT_ARM],
    BASE = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM] + g_bodyLinkCnt[BODY_TYPE_RIGHT_ARM] + g_bodyLinkCnt[BODY_TYPE_OTHERS],
};

const int g_linkStartIndex[BODY_CNT + 1] = {
    LEFT_ARM_UPDATE_ORIGIN,
    RIGHT_ARM_UPDATE_ORIGIN,
    OTHER_ORIGIN,
    BASE
};

#define ROOT   (BASE - 1)

bool g_linkCollisionDetecMatrix[OTHER_ORIGIN][BASE] = {false};

Vector3d g_origin[BASE] = {Vector3d::Zero()};
Vector3d g_offset[BASE] = {Vector3d::Zero()};
Vector3d g_dir[BASE] = {Vector3d::Zero()};

static double toRadians(float degrees) {
    return (static_cast<double>(degrees)) * (M_PI / 180.0);
}

static int UpdateJointInfo(int i, int parent, Quaterniond &rotation, std::vector<JointInfo> &jointMap)
{
    int key = (parent << 8) | (i % 256);
    auto jointPair = jointMap.find(key);
    if (jointPair == jointMap.end()) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "lose joint %d -> %d", parent, i);
        return -1;
    }

    Vector3d position = rotation * jointPair->second.offset;
    g_origin[i] = position;

    rotation = rotation * jointPair->second.rotation;
    jointPair->second.rotationAll = rotation;
    Eigen::Vector3d axis = jointPair->second.axis;
    g_dir[i] = rotation * axis;
    return 0;
}

static int InitJoint(const urdf::Model &urdf_model, std::vector<JointInfo> &jointMap)
{
    for (const auto& joint_pair : urdf_model.joints_) {
        JointInfo jointInfo;
        // joint_pair是一个键值对: 键为关节名称，值为关节指针
        jointInfo.name = joint_pair.first;
        urdf::JointConstSharedPtr joint = joint_pair.second;

        jointInfo.type = joint->type;
        jointInfo.parent_link = -1;
        jointInfo.child_link = -1;
        auto link = g_linkMap.find(joint->parent_link_name);
        if (link != g_linkMap.end()) {
            jointInfo.parent_link = link->second;
        } else {
            continue;
        }
        link = g_linkMap.find(joint->child_link_name);
        if (link != g_linkMap.end()) {
            jointInfo.child_link = link->second;
        } else {
            continue;
        }
        jointInfo.offset = Vector3d(joint->parent_to_joint_origin_transform.position.x,
            joint->parent_to_joint_origin_transform.position.y,
            joint->parent_to_joint_origin_transform.position.z);
        jointInfo.rotation = Quaterniond(
            joint->parent_to_joint_origin_transform.rotation.w,
            joint->parent_to_joint_origin_transform.rotation.x,
            joint->parent_to_joint_origin_transform.rotation.y,
            joint->parent_to_joint_origin_transform.rotation.z);
        if (joint->limits != NULL) {
            jointInfo.limit_lower = joint->limits->lower;
            jointInfo.limit_upper = joint->limits->upper;
            jointInfo.limit_v = joint->limits->velocity;
            jointInfo.limit_e = joint->limits->effort;
        }
        jointInfo.angle = 0;
        jointInfo.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z);

        int key = ((jointInfo.parent_link % 256) << 8) | (jointInfo.child_link % 256);
        jointMap[key] = jointInfo;
    }
    Quaterniond rotation = Quaterniond::Identity();
    int result = UpdateJointInfo(LEFT, BASE, rotation, jointMap);
    if (result != 0) return -1;
    for (int i = LEFT + 1; i < RIGHT; ++i) {
        result = UpdateJointInfo(i, i - 1, rotation, jointMap);
        if (result != 0) return -1;
    }
    rotation = Quaterniond::Identity();
    result = UpdateJointInfo(RIGHT, BASE, rotation, jointMap);
    if (result != 0) return -1;
    for (int i = RIGHT + 1; i < BASE; ++i) {
        result = UpdateJointInfo(i, i - 1, rotation, jointMap);
        if (result != 0) return -1;
    }
    return 0;
}

int CollisionSimulator::GetMeshLinkdimensions(const std::string& filename, CollisionStructure& col_struct)
{
    // 这里可以添加读取网格文件并计算边界盒的代码
    // 假设已经计算出边界盒的最小和最大坐标
    col_struct.mesh_bounds = {min_x, max_x, min_y, max_y, min_z, max_z};
    return 0;
}

int CollisionSimulator::InitCollisionStructureList(const urdf::Model &urdf_model)
{
    int result = OK;
    int linkIndex = 0;
    for (const auto& link_pair : urdf_model.links_) {
        // link_pair是一个键值对: 键为链接名称，值为链接指针
        urdf::LinkConstSharedPtr link = link_pair.second;
        for (const auto& collision : link->collision_array) {
            CollisionStructure colStruct;
            colStruct.link_name = link->name;
            colStruct.geometry_type = collision->geometry->type;
            if (collision->geometry->type == urdf::Geometry::BOX) {
                urdf::Box* box = dynamic_cast<urdf::Box*>(collision->geometry.get());
                colStruct.mesh_bounds = {-box->dim.x / 2, box->dim.x / 2,
                                         -box->dim.y / 2, box->dim.y / 2,
                                         -box->dim.z / 2, box->dim.z / 2};
            } else if (collision->geometry->type == urdf::Geometry::MESH) {
                urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(collision->geometry.get());
                colStruct.mesh_filename = mesh->filename;
                result = GetMeshLinkdimensions(mesh->filename, colStruct);
                if (result != OK) {
                    RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to get mesh dimensions for %s", mesh->filename.c_str());
                    return result;
                }
            }
            colStruct.link_index = linkIndex;
            linkIndex++;
            collision_structures_.push_back(colStruct);
        }
    }
    return result;
}

int CollisionSimulator::InitPolyhedrons(const urdf::Model &urdf_model)
{
    InitCollisionStructureList(urdf_model);
    std::vector<CollisionStructure> collision_structures_temp;
    InitJoint(urdf_model, jointMap_);
    InitCollisionStructure(urdf_model, collision_structures_temp);
    return RemakeCollisionStructureList(collision_structures_temp);
}

void CollisionSimulator::InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory)
{
    for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
        joints_[i][BODY_TYPE_LEFT_ARM] = g_leftArmJoints[i];
        joints_[i][BODY_TYPE_RIGHT_ARM] = g_rightArmJoints[i];
        joints_[i][BODY_TYPE_OTHERS] = g_otherJoints[i];
        for (int j = 0; j < BODY_CNT; j++) {
            for (int k = 0; k < g_jointLinkCnt[j]; k++) {
                joints_[i][j][k] = jointsList[j][k];
            }
        }
    }

    InitPolyhedrons(urdf_model);
    InitPolyhedronsList();
    UpdateArmPolyhedrons();
}

CollisionSimulator::CollisionSimulator(std::string urdf_string, float **jointsList, KinematicsManager **trajectory)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块...");
    urdf::Model urdf_model;
    simpleUpdateIndex_ = 0;
    if (!urdf_model.initString(urdf_string)) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法解析URDF模型");
        return;
    }
    InitCollisionSimulator(urdf_model, jointsList, trajectory);
}

CollisionSimulator::CollisionSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float **jointsList, KinematicsManager **trajectory)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块...");
    std::string urdf_string = urdf_string_ptr->data;

    // 解析URDF模型
    urdf::Model urdf_model;
    simpleUpdateIndex_ = 0;
    if (!urdf_model.initString(urdf_string)) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法解析URDF模型");
        return;
    }

    InitCollisionSimulator(urdf_model, jointsList, trajectory);
}

void CollisionSimulator::InitArmSinglePolyhedron(int index, Polyhedron *poly,
    Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation)
{
    float length = collision_structures_[index].dimensions[0];  // 长度（沿u方向）
    float width = collision_structures_[index].dimensions[1];   // 宽度（沿v方向）
    float height = collision_structures_[index].dimensions[2];  // 高度（沿法向量方向）

    Vector3d lenVec = rotation * Vector3d(length, 0, 0);  // 长度方向
    Vector3d widVec = rotation * Vector3d(0, width, 0);  // 宽度方向
    Vector3d heiVec = rotation * Vector3d(0, 0, height); // 高度方向

    Vector3d bottomLeft = rotation * offset + origin;

    (*poly)[0] = bottomLeft;
    (*poly)[1] = bottomLeft + lenVec;
    (*poly)[2] = bottomLeft + widVec;
    (*poly)[3] = (*poly)[1] + widVec;
    (*poly)[4] = bottomLeft + heiVec;
    (*poly)[5] = (*poly)[1] + heiVec;
    (*poly)[6] = (*poly)[2] + heiVec;
    (*poly)[7] = (*poly)[3] + heiVec;
    if (index < BASE - 1) {
        origin = origin + rotation * g_origin[index + 1];
    }
}

static int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_)
{
    for (int i = g_linkStartIndex[LEFT_ARM_ORIGIN]; i < g_linkStartIndex[OTHER_ORIGIN]; ++i) {
        for (int j = i + 1; j < BASE; ++j) {
            if (!g_linkCollisionDetecMatrix[i][j]) {
                continue;
            }
            bool inCollision = gjk_interface::checkOBBCollision((*addedOBBsNext_)[i], (*addedOBBsNext_)[j]);
            if (inCollision) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect left %d and %d collision b", i, j);
                return ARM_COLLISION;
            }
            inCollision = gjk_interface::checkOBBCollision((*addedOBBs)[i], (*addedOBBsNext_)[j]);
            if (inCollision) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect left %d and %d collision c", i, j);
                return ARM_COLLISION;
            }
        }
    }
    return OK;
}

std::pair<Vector3d, Matrix3d> CollisionSimulator::GetBodyTransform(int startIndex)
{
    Vector3d offset_vec;
    Matrix3d rotation = Matrix3d::Identity();

    std::stack <int> indexStack;
    int bodyIndex = startIndex;

    while (bodyIndex != ROOT) {
        indexStack.push(bodyIndex);
        bodyIndex = jointMap_[bodyIndex].parent_link;
    }

    while (!indexStack.empty()) {
        int currentIndex = indexStack.top();
        indexStack.pop();
        if (jointMap_[currentIndex].type == urdf::Joint::FIXED) {
            offset_vec = offset_vec + rotation * g_origin[currentIndex];
        } else if (jointMap_[currentIndex].type == urdf::Joint::REVOLUTE) {
            double angleRad = toRadians(jointMap_[currentIndex].angle);
            rotation = AngleAxisd(angleRad, rotation * g_dir[currentIndex]) * rotation;
            offset_vec = offset_vec + rotation * g_origin[currentIndex];
        } else if (jointMap_[currentIndex].type == urdf::Joint::PRISMATIC) {
            double displacement = jointMap_[currentIndex].angle;
            offset_vec = offset_vec + rotation * (g_origin[currentIndex] + g_dir[currentIndex] * displacement);
        }
    }
    return { offset_vec, rotation };
}

void CollisionSimulator::UpdatePolyhedrons(int startLinkIndex, int endLinkIndex, int simple, float *jointList)
{
    std::vector<Polyhedron> *armPolyhedrons = &(polyhedrons_[simple]);
    auto [offset_vec, rotation] = GetBodyTransform(startLinkIndex);
    for (int i = startLinkIndex + 1; i < endLinkIndex; i++) {
        double angleRad = toRadians(jointList[i - startLinkIndex - 1]);
        rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
        InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i],
            offset_vec,
            g_offset[i],
            rotation);
        addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
            collision_structures_[i].dimensions[0],
            collision_structures_[i].dimensions[1],
            collision_structures_[i].dimensions[2]);
    }
}

void CollisionSimulator::InitPolyhedronsList()
{
    for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
        polyhedrons_[i].resize(BASE);
        addedOBBs_[i].resize(BASE);
        polyhedrons_[i][BASE].clear();
    }

    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "origin:");
    for (int i = 0; i < BASE; ++i) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i, g_origin[i].x(), g_origin[i].y(), g_origin[i].z());
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "offset:");
    for (int i = 0; i < BASE; ++i) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i, g_offset[i].x(), g_offset[i].y(), g_offset[i].z());
    }*/

    for (int simple = 0; simple < COLLISION_CHECK_SIMPLE_CNT; simple++) {
        for (int i = 0; i < BASE; i++) {
            for (int j = 0; j < POLYHEDRONS_POINTS_CNT; j++) {
                polyhedrons_[simple][i].push_back(Vector3d(0, 0, 0));
            }
        }
    }
    for (int simple = 0; simple < COLLISION_CHECK_SIMPLE_CNT; simple++) {
        std::vector<Polyhedron> *armPolyhedrons = &(polyhedrons_[simple]);
        Vector3d offset_temp = {0, 0 ,0};
        Vector3d origin_point_offset = {0, 0 ,0};
        if (collision_structures_[ROOT].geometry_type == "box") {
            origin_point_offset = Vector3d(-collision_structures_[ROOT].dimensions[0] / 2,
                -collision_structures_[ROOT].dimensions[1] / 2,
                -collision_structures_[ROOT].dimensions[2] / 2);
        } else if (collision_structures_[ROOT].geometry_type == "mesh") {
            origin_point_offset = Vector3d(collision_structures_[ROOT].mesh_bounds[0],
                collision_structures_[ROOT].mesh_bounds[2],
                collision_structures_[ROOT].mesh_bounds[4]);
        }
        Matrix3d rotation = Matrix3d::Identity();
        InitArmSinglePolyhedron(ROOT, &(*armPolyhedrons)[ROOT],
            offset_temp,
            origin_point_offset,
            rotation);
        addedOBBs_[simple][ROOT] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[ROOT],
            collision_structures_[ROOT].dimensions[0],
            collision_structures_[ROOT].dimensions[1],
            collision_structures_[ROOT].dimensions[2]);

        Vector3d offset_vec = g_origin[LEFT];
        for (int i = LEFT; i < RIGHT; i++) {
            InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i],
                offset_vec,
                g_offset[i],
                rotation);
            addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
                collision_structures_[i].dimensions[0],
                collision_structures_[i].dimensions[1],
                collision_structures_[i].dimensions[2]);
        }
        
        offset_vec = g_origin[RIGHT];
        for (int i = RIGHT; i < BASE; i++) {
            InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i],
                offset_vec,
                g_offset[i],
                rotation);
            addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
                collision_structures_[i].dimensions[0],
                collision_structures_[i].dimensions[1],
                collision_structures_[i].dimensions[2]);
        }
    }
    for (int i = LEFT_ARM_ORIGIN; i < BASE; ++i) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i);
        for (int j = 0; j < POLYHEDRONS_POINTS_CNT; ++j) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
                polyhedrons_[0][i][j].x(),
                polyhedrons_[0][i][j].y(),
                polyhedrons_[0][i][j].z());
        }
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------");
    }
}

void CollisionSimulator::UpdateArmPolyhedrons()
{
    std::vector<Polyhedron> *armPolyhedrons = &(polyhedrons_[simpleUpdateIndex_]);

    UpdatePolyhedrons(LEFT_ARM_UPDATE_ORIGIN, RIGHT_ARM_ORIGIN, simpleUpdateIndex_,
        joints_[simpleUpdateIndex_][BODY_TYPE_LEFT_ARM]);
    UpdatePolyhedrons(RIGHT_ARM_UPDATE_ORIGIN, OTHER_ORIGIN, simpleUpdateIndex_,
        joints_[simpleUpdateIndex_][BODY_TYPE_RIGHT_ARM]);

    /*for (int i = LEFT; i <= BASE; ++i) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i);
        for (int j = 0; j < POLYHEDRONS_POINTS_CNT; ++j) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
                polyhedrons_[0][i][j].x(),
                polyhedrons_[0][i][j].y(),
                polyhedrons_[0][i][j].z());
        }
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------");
    }
    for (int i = LEFT; i <= BASE; i++) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), "
            "half_length(%.4f, %.4f, %.4f)",
            addedOBBs_[simple][i].center.x(),
            addedOBBs_[simple][i].center.y(),
            addedOBBs_[simple][i].center.z(),
            addedOBBs_[simple][i].axis[0].x(),
            addedOBBs_[simple][i].axis[0].y(),
            addedOBBs_[simple][i].axis[0].z(),
            addedOBBs_[simple][i].axis[1].x(),
            addedOBBs_[simple][i].axis[1].y(),
            addedOBBs_[simple][i].axis[1].z(),
            addedOBBs_[simple][i].axis[2].x(),
            addedOBBs_[simple][i].axis[2].y(),
            addedOBBs_[simple][i].axis[2].z(),
            addedOBBs_[simple][i].halfExtent[0],
            addedOBBs_[simple][i].halfExtent[1],
            addedOBBs_[simple][i].halfExtent[2]);
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "left joints: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f",
        leftArmJoints_[simple][0],
        leftArmJoints_[simple][1],
        leftArmJoints_[simple][2],
        leftArmJoints_[simple][3],
        leftArmJoints_[simple][4],
        leftArmJoints_[simple][5]);*/
}

int CollisionSimulator::AddPolyhedron(int id, const Polyhedron& poly)
{
    if (addedPolyhedrons_.find(id) != addedPolyhedrons_.end()) {
        return -1; // 已存在相同ID的多面体
    }
    addedPolyhedrons_[id] = poly;
    return 0; // 成功添加
}

int CollisionSimulator::RemovePolyhedron(int id)
{
    auto it = addedPolyhedrons_.find(id);
    if (it != addedPolyhedrons_.end()) {
        addedPolyhedrons_.erase(it);
        return 0; // 成功移除
    }
    return -1; // 未找到对应ID的多面体
}

int CollisionSimulator::CheckJointsAngleLimit(float *joints, int bodyType)
{
    for (int i = 0; i < g_jointLinkCnt[bodyType]; ++i) {
        JointInfo *joint = &(jointMap_[g_linkStartIndex[bodyType] + i]);
        float angle = toRadians(joints[i]);
        if (angle < joint->limit_lower || angle > joint->limit_upper) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
                "CheckJointsAngleLimit: joint out of limit [%.4f, %.4f], current: %.4f(%.2f)",
                joint->limit_lower, joint->limit_upper, angle, joints[i]);
            return ARM_AIM_CANNOT_REACH;
        }
    }
    return 0;
}

int CollisionSimulator::UpdateCollisionForNewGoal(int bodyType)
{
    if (bodyType >= ARM_ID_ERR) return -1;
    int simple20 = (simpleUpdateIndex_ + 1) % COLLISION_CHECK_SIMPLE_CNT;
    int simple40 = (simple20 + 1) % COLLISION_CHECK_SIMPLE_CNT;

    int startLinkIndex = g_linkStartIndex[bodyType], endLinkIndex = startLinkIndex + g_jointLinkCnt[bodyType];
    KinematicsManager *trajectory = trajectory_[bodyType];
    float *jointList20 = joints_[simple20][bodyType];
    float *jointList40 = joints_[simple40][bodyType];
    trajectory->getAngleForStarting(jointList20);
    trajectory->getAngleForChecking(jointList40);

    if (CheckJointsAngleLimit(jointList20, bodyType) != 0 || CheckJointsAngleLimit(jointList40, bodyType) != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
        return ARM_AIM_CANNOT_REACH;
    }

    UpdatePolyhedrons(startLinkIndex, endLinkIndex, simple20, jointList20);
    std::vector<OBB> *addedOBBsNext_ = &(addedOBBs_[simple20]);
    std::vector<OBB> *addedOBBs = &(addedOBBs_[simpleUpdateIndex_]);
    int result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
    if (result != OK) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: bodyType %d collision", bodyType);
        return result;
    }

    UpdatePolyhedrons(startLinkIndex, endLinkIndex, simple40, jointList40);
    addedOBBsNext_ = &(addedOBBs_[simple40]);
    addedOBBs = &(addedOBBs_[simple20]);
    result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
    if (result != OK) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: bodyType %d collision", bodyType);
        return result;
    }
    return 0;
}


#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/collision_detection/collision_common.h>

int main(int argc, char**argv) {
    // 初始化 ROS 节点
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("moveit_collision_demo");

    // --------------------------
    // 步骤1：加载机器人模型，创建 PlanningScene
    // --------------------------
    // 加载机器人模型（需在 launch 文件中加载 URDF 和 SRDF）
    robot_model_loader::RobotModelLoader robot_model_loader(node, "robot_description");
    const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();

    // 创建 PlanningScene（管理碰撞环境）
    planning_scene::PlanningScene planning_scene(robot_model);

    // --------------------------
    // 步骤3：定义机器人姿态（待检测的状态）
    // --------------------------
    moveit::core::RobotState& robot_state = planning_scene.getCurrentStateNonConst();
    const moveit::core::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm");  // 机械臂组名称（需替换为你的机器人）

    // 设置关节角度（示例：Panda 机械臂的一个姿态）
    std::vector<double> joint_values = {0.0, 0.5, 0.0, -1.5, 0.0, 1.0, 0.0};
    robot_state.setJointGroupPositions(joint_model_group, joint_values);
    robot_state.update();  // 更新机器人状态

    // --------------------------
    // 步骤4：调用碰撞检测接口
    // --------------------------
    // 定义碰撞检测请求（设置需要的结果类型）
    collision_detection::CollisionRequest collision_request;
    collision_request.contacts = true;  // 是否返回接触信息（如碰撞点、法向量）
    collision_request.max_contacts = 10;  // 最大返回接触对数量
    collision_request.max_contacts_per_pair = 3;  // 每对碰撞体的最大接触点数量
    collision_request.verbose = false;  // 是否输出详细信息

    // 存储碰撞检测结果
    collision_detection::CollisionResult collision_result;

    // 执行碰撞检测（检测机器人当前状态是否碰撞）
    planning_scene.checkCollision(collision_request, collision_result);

    // --------------------------
    // 步骤5：处理碰撞结果
    // --------------------------
    if (collision_result.collision) {
        RCLCPP_WARN(node->get_logger(), "检测到碰撞！");

        // 输出碰撞对信息（机器人关节/链接与障碍物的碰撞）
        for (const auto& contact : collision_result.contacts) {
            RCLCPP_INFO(node->get_logger(), "碰撞对: %s 与 %s", 
                      contact.first.first.c_str(),  // 第一个碰撞体ID
                      contact.first.second.c_str()); // 第二个碰撞体ID
        }
    } else {
        RCLCPP_INFO(node->get_logger(), "未检测到碰撞。");
    }

    // 关闭节点
    rclcpp::shutdown();
    return 0;
}