#include <vector>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>

#include "rm_arm_simulator.hpp"

using namespace Eigen;
#define MAX_ARM_POLYHEDRONS_CNT 8
#define ARM_POLYHEDRONS_POINT   8
#define ARM_POLYHEDRONS_CNT     15

static const bool LinkCollisionDetecMatrix[(USED_ARM_DOF + 1) * 2][ARM_POLYHEDRONS_CNT] = {
    //  BASE    L1      L2      L3      L4      L5      L6      BASE    R1      R2      R3      R4      R5      R6      BASE
    {   false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false}, // BASE
    {   false,  false,  false,  false,  false,  true,   true,   false,  true,   true,   true,   true,   true,   true,   false}, // L1
    {   false,  false,  false,  false,  false,  true,   true,   false,  true,   true,   true,   true,   true,   true,   false}, // L2
    {   false,  false,  false,  false,  false,  false,  false,  false,  true,   true,   true,   true,   true,   true,   true}, // L3
    {   false,  false,  false,  false,  false,  false,  false,  false,  true,   true,   true,   true,   true,   true,   true}, // L4
    {   false,  true,   true,   false,  false,  false,  false,  false,  true,   true,   true,   true,   true,   true,   true}, // L5
    {   false,  true,   true,   false,  false,  false,  false,  false,  true,   true,   true,   true,   true,   true,   true}, // L6
    {   false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false,  false}, // BASE
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , false , false , false , false,  true,   true,   false}, // R1
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , false , false , false , false , true ,  true,   false}, // R2
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , false , false , false , false , false , false,  true}, // R3
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , false , false , false , false , false , false,  true}, // R4
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , true ,  true ,  false , false , false , false,  true}, // R5
    {   false,  true ,  true ,  true ,  true ,  true ,  true ,  false , true ,  true ,  false , false , false , false,  true}  // R6
};

static std::map<std::string, int> g_linkMap = {
    {"base_link_leftarm", 0},
    {"LeftLink1", 1},
    {"LeftLink2", 2},
    {"LeftLink3", 3},
    {"LeftLink4", 4},
    {"LeftLink5", 5},
    {"LeftLink6", 6},
    {"base_link_rightarm", 7},
    {"RightLink1", 8},
    {"RightLink2", 9},
    {"RightLink3", 10},
    {"RightLink4", 11},
    {"RightLink5", 12},
    {"RightLink6", 13},
    {"body_2_link", 14},
    {"left_behind_hip_link", 15}
};

enum OriginBoxIndex {
    BASE = 14,
    LEFT = 0,
    RIGHT = 7
};

using namespace Eigen;

std::vector<Vector3d> g_origin = {
    Vector3d(0, 0, 0),               // BASE
    Vector3d(0, 0, 0),            // L1
    Vector3d(0, 0, 0),           // L2
    Vector3d(0, 0, 0),           // L3
    Vector3d(0, 0, 0),           // L4
    Vector3d(0, 0, 0),           // L5
    Vector3d(0, 0, 0),           // L6
    Vector3d(0, 0, 0),               // BASE
    Vector3d(0, 0, 0),            // R1
    Vector3d(0, 0, 0),           // R2
    Vector3d(0, 0, 0),           // R3
    Vector3d(0, 0, 0),           // R4
    Vector3d(0, 0, 0),           // R5
    Vector3d(0, 0, 0)            // R6
};

std::vector<Vector3d> g_offset = {
    Vector3d(0, 0, 0),               // BASE
    Vector3d(0, 0, 0),               // L1
    Vector3d(0, 0, 0),               // L2
    Vector3d(0, 0, 0),               // L3
    Vector3d(0, 0, 0),           // L4
    Vector3d(0, 0, 0),            // L5
    Vector3d(0, 0, 0),           // L6
    Vector3d(0, 0, 0),               // BASE
    Vector3d(0, 0, 0),               // R1
    Vector3d(0, 0, 0),               // R2
    Vector3d(0, 0, 0),               // R3
    Vector3d(0, 0, 0),           // R4
    Vector3d(0, 0, 0),            // R5
    Vector3d(0, 0, 0)            // R6
};

std::vector<Vector3d> g_dir = {
    Vector3d(0, 0, 0),              // BASE
    Vector3d::UnitX() * -1,              // L1
    Vector3d::UnitZ(),              // L2
    Vector3d::UnitZ(),              // L3
    Vector3d::UnitX() * -1,              // L4
    Vector3d::UnitZ(),              // L5
    Vector3d::UnitX() * -1,              // L6
    Vector3d(0, 0, 0),              // BASE
    Vector3d::UnitX(),              // R1
    Vector3d::UnitZ() * -1,              // R2
    Vector3d::UnitZ() * -1,              // R3
    Vector3d::UnitX(),              // R4
    Vector3d::UnitZ() * -1,              // R5
    Vector3d::UnitX(),              // R6
};

static int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_);

static void InitArmSinglePolyhedron(ArmSimulator *simulator, int index, Polyhedron *poly,
    Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation)
{
    float length = simulator->collision_structures_[index].dimensions[0];  // 长度（沿u方向）
    float width = simulator->collision_structures_[index].dimensions[1];   // 宽度（沿v方向）
    float height = simulator->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 void InitArmPolyhedronsList(ArmSimulator *simulator)
{
    for (int i = 0; i < ARM_COLLISION_CHECK_SIMPLE_CNT; i++) {
        simulator->armPolyhedrons_[i].resize(ARM_POLYHEDRONS_CNT);
        simulator->addedOBBs_[i].resize(ARM_POLYHEDRONS_CNT);
        simulator->armPolyhedrons_[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 < ARM_COLLISION_CHECK_SIMPLE_CNT; simple++) {
        for (int i = 0; i < ARM_POLYHEDRONS_CNT; i++) {
            for (int j = 0; j < ARM_POLYHEDRONS_POINT; j++) {
                simulator->armPolyhedrons_[simple][i].push_back(Vector3d(0, 0, 0));
            }
        }
    }
    for (int simple = 0; simple < ARM_COLLISION_CHECK_SIMPLE_CNT; simple++) {
        std::vector<Polyhedron> *armPolyhedrons = &(simulator->armPolyhedrons_[simple]);
        Vector3d offset_temp = {0, 0 ,0};
        Vector3d origin_point_offset = {0, 0 ,0};
        if (simulator->collision_structures_[BASE].geometry_type == "box") {
            origin_point_offset = Vector3d(-simulator->collision_structures_[BASE].dimensions[0] / 2,
                -simulator->collision_structures_[BASE].dimensions[1] / 2,
                -simulator->collision_structures_[BASE].dimensions[2] / 2);
        } else if (simulator->collision_structures_[BASE].geometry_type == "mesh") {
            origin_point_offset = Vector3d(simulator->collision_structures_[BASE].mesh_bounds[0],
                simulator->collision_structures_[BASE].mesh_bounds[2],
                simulator->collision_structures_[BASE].mesh_bounds[4]);
        }
        Matrix3d rotation = Matrix3d::Identity();
        InitArmSinglePolyhedron(simulator, BASE, &(*armPolyhedrons)[BASE],
            offset_temp,
            origin_point_offset,
            rotation);
        simulator->addedOBBs_[simple][BASE] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[BASE],
            simulator->collision_structures_[BASE].dimensions[0],
            simulator->collision_structures_[BASE].dimensions[1],
            simulator->collision_structures_[BASE].dimensions[2]);

        Vector3d offset_vec = g_origin[LEFT];
        for (int i = LEFT; i < RIGHT; i++) {
            InitArmSinglePolyhedron(simulator, i, &(*armPolyhedrons)[i],
                offset_vec,
                g_offset[i],
                rotation);
            simulator->addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
                simulator->collision_structures_[i].dimensions[0],
                simulator->collision_structures_[i].dimensions[1],
                simulator->collision_structures_[i].dimensions[2]);
        }
        
        offset_vec = g_origin[RIGHT];
        for (int i = RIGHT; i < BASE; i++) {
            InitArmSinglePolyhedron(simulator, i, &(*armPolyhedrons)[i],
                offset_vec,
                g_offset[i],
                rotation);
            simulator->addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
                simulator->collision_structures_[i].dimensions[0],
                simulator->collision_structures_[i].dimensions[1],
                simulator->collision_structures_[i].dimensions[2]);
        }
    }
    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 < ARM_POLYHEDRONS_POINT; ++j) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
                simulator->armPolyhedrons_[0][i][j].x(),
                simulator->armPolyhedrons_[0][i][j].y(),
                simulator->armPolyhedrons_[0][i][j].z());
        }
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------");
    }
}

static int RemakeCollisionStructureList(ArmSimulator *simulator,
    std::vector<CollisionStructure> &collision_structures_temp)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "共提取到 %zu 个碰撞结构", collision_structures_temp.size());
    simulator->collision_structures_.resize(g_linkMap.size());
    for (const auto& col : collision_structures_temp) {
        int index = g_linkMap[col.link_name];
        simulator->collision_structures_[index] = col;
    }
    for (const auto& col : simulator->collision_structures_) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "链接: %s, 类型: %s, ",
                col.link_name.c_str(),
                col.geometry_type.c_str());
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "尺寸: %.2f x %.2f x %.2f",
                col.dimensions[0],
                col.dimensions[1],
                col.dimensions[2]);
    }
    return 0;
}

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

static int UpdateJointAngles(ArmSimulator *simulator)
{
    if (simulator == NULL) return -1;
    int simple = simulator->simpleUpdateIndex_;
    simulator->leftTrajectory_->getAngleForChecking
        (simulator->leftArmJoints_[simple]);
    simulator->rightTrajectory_->getAngleForChecking
        (simulator->rightArmJoints_[simple]);
    if (simulator->CheckArmJointsAngleLimit(simulator->leftArmJoints_[simple]) != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
        return ARM_AIM_CANNOT_REACH;
    }
    if (simulator->CheckArmJointsAngleLimit(simulator->rightArmJoints_[simple]) != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
        return ARM_AIM_CANNOT_REACH;
    }
    return 0;
}

static void UpdateArmPolyhedrons(ArmSimulator *simulator)
{
    if (simulator == NULL) return;
    int simple = simulator->simpleUpdateIndex_;
    std::vector<Polyhedron> *armPolyhedrons = &(simulator->armPolyhedrons_[simple]);

    Vector3d offset_vec = g_origin[LEFT] + g_origin[LEFT + 1];
    Vector3d normal = Vector3d(-1, 0, 0);
    // Matrix3d rotation = AngleAxisf(-M_PI / 2, Vector3d::UnitX()) * Matrix3d::Identity();
    Matrix3d rotation = Matrix3d::Identity();
    Vector3d new_normal;
    for (int i = LEFT + 1; i < RIGHT; i++) {
        double angleRad = toRadians(simulator->leftArmJoints_[simple][i - LEFT - 1]);
        rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
        new_normal = rotation * normal;
        /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d normal: (%.4f, %.4f, %.4f) rotation: "
            "(%.4f, %.4f, %.4f)"
            "(%.4f, %.4f, %.4f)"
            "(%.4f, %.4f, %.4f)",
            i, new_normal.x(), new_normal.y(), new_normal.z(),
            rotation(0, 0), rotation(0, 1), rotation(0, 2),
            rotation(1, 0), rotation(1, 1), rotation(1, 2),
            rotation(2, 0), rotation(2, 1), rotation(2, 2));*/
        InitArmSinglePolyhedron(simulator, i, &(*armPolyhedrons)[i],
            offset_vec,
            g_offset[i],
            rotation);
        simulator->addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
            simulator->collision_structures_[i].dimensions[0],
            simulator->collision_structures_[i].dimensions[1],
            simulator->collision_structures_[i].dimensions[2]);
    }
    offset_vec = g_origin[RIGHT] + g_origin[RIGHT + 1];
    normal = Vector3d(1, 0, 0);
    rotation = Matrix3d::Identity();
    for (int i = RIGHT + 1; i < BASE; i++) {
        double angleRad = toRadians(simulator->rightArmJoints_[simple][i - RIGHT - 1]);
        rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
        new_normal = rotation * normal;
        InitArmSinglePolyhedron(simulator, i, &(*armPolyhedrons)[i],
            offset_vec,
            g_offset[i],
            rotation);
        simulator->addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i],
            simulator->collision_structures_[i].dimensions[0],
            simulator->collision_structures_[i].dimensions[1],
            simulator->collision_structures_[i].dimensions[2]);
    }

    /*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 < ARM_POLYHEDRONS_POINT; ++j) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
                simulator->armPolyhedrons_[0][i][j].x(),
                simulator->armPolyhedrons_[0][i][j].y(),
                simulator->armPolyhedrons_[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)",
            simulator->addedOBBs_[simple][i].center.x(),
            simulator->addedOBBs_[simple][i].center.y(),
            simulator->addedOBBs_[simple][i].center.z(),
            simulator->addedOBBs_[simple][i].axis[0].x(),
            simulator->addedOBBs_[simple][i].axis[0].y(),
            simulator->addedOBBs_[simple][i].axis[0].z(),
            simulator->addedOBBs_[simple][i].axis[1].x(),
            simulator->addedOBBs_[simple][i].axis[1].y(),
            simulator->addedOBBs_[simple][i].axis[1].z(),
            simulator->addedOBBs_[simple][i].axis[2].x(),
            simulator->addedOBBs_[simple][i].axis[2].y(),
            simulator->addedOBBs_[simple][i].axis[2].z(),
            simulator->addedOBBs_[simple][i].halfExtent[0],
            simulator->addedOBBs_[simple][i].halfExtent[1],
            simulator->addedOBBs_[simple][i].halfExtent[2]);
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "left joints: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f",
        simulator->leftArmJoints_[simple][0],
        simulator->leftArmJoints_[simple][1],
        simulator->leftArmJoints_[simple][2],
        simulator->leftArmJoints_[simple][3],
        simulator->leftArmJoints_[simple][4],
        simulator->leftArmJoints_[simple][5]);*/
}

double truncate_to_4_decimal(double num) {
    double int_part; // 存储整数部分
    double frac_part = std::modf(num, &int_part); // 分离小数部分

    // 截断小数部分到前 4 位
    double truncated_frac = std::trunc(frac_part * 10000.0) * 0.0001;

    // 合并整数部分和截断后的小数部分
    return int_part + truncated_frac;
}

static void ArmPoseChangeToOriFrame(float *pose, const JointInfo &joint)
{
    Vector3d offset;        // 关节偏移
    Quaterniond rotation; // 关节旋转
    offset.x() = pose[0];
    offset.y() = pose[1];
    offset.z() = pose[2];
    rotation.x() = pose[3];
    rotation.y() = pose[4];
    rotation.z() = pose[5];
    rotation.w() = pose[6];
    rotation = joint.rotationAll * rotation;
    offset = joint.rotationAll * offset + joint.offset;
    pose[0] = truncate_to_4_decimal(offset.x());
    pose[1] = truncate_to_4_decimal(offset.y());
    pose[2] = truncate_to_4_decimal(offset.z());
    pose[3] = truncate_to_4_decimal(rotation.x());
    pose[4] = truncate_to_4_decimal(rotation.y());
    pose[5] = truncate_to_4_decimal(rotation.z());
    pose[6] = truncate_to_4_decimal(rotation.w());
}

static void OriFrameChangeToArmPose(float *pose, const JointInfo &joint)
{
    Vector3d offset;
    Quaterniond rotation;
    offset.x() = pose[0];
    offset.y() = pose[1];
    offset.z() = pose[2];
    rotation.x() = pose[3];
    rotation.y() = pose[4];
    rotation.z() = pose[5];
    rotation.w() = pose[6];
    Quaterniond inv_rotation = joint.rotationAll.inverse();
    Vector3d inv_position = {offset.x() - joint.offset.x(),
        offset.y() - joint.offset.y(),
        offset.z() - joint.offset.z()};
    offset = inv_rotation * inv_position;
    rotation = inv_rotation * rotation;
    pose[0] = truncate_to_4_decimal(offset.x());
    pose[1] = truncate_to_4_decimal(offset.y());
    pose[2] = truncate_to_4_decimal(offset.z());
    pose[3] = truncate_to_4_decimal(rotation.x());
    pose[4] = truncate_to_4_decimal(rotation.y());
    pose[5] = truncate_to_4_decimal(rotation.z());
    pose[6] = truncate_to_4_decimal(rotation.w());
}

static void InitPointsFromBoundsAndRotation(JointInfo &joint, CollisionStructure& col_struct)
{
    std::vector<Vector3d> now = {
        Vector3d(col_struct.mesh_bounds[0], col_struct.mesh_bounds[2], col_struct.mesh_bounds[4]),
        Vector3d(col_struct.mesh_bounds[0], col_struct.mesh_bounds[2], col_struct.mesh_bounds[5]),
        Vector3d(col_struct.mesh_bounds[0], col_struct.mesh_bounds[3], col_struct.mesh_bounds[4]),
        Vector3d(col_struct.mesh_bounds[0], col_struct.mesh_bounds[3], col_struct.mesh_bounds[5]),
        Vector3d(col_struct.mesh_bounds[1], col_struct.mesh_bounds[2], col_struct.mesh_bounds[4]),
        Vector3d(col_struct.mesh_bounds[1], col_struct.mesh_bounds[2], col_struct.mesh_bounds[5]),
        Vector3d(col_struct.mesh_bounds[1], col_struct.mesh_bounds[3], col_struct.mesh_bounds[4]),
        Vector3d(col_struct.mesh_bounds[1], col_struct.mesh_bounds[3], col_struct.mesh_bounds[5])
    };
    int leftBottom = -1, xIndex = 0, yIndex = 0, zIndex = 0;
    double xMax = 0, yMax = 0, zMax = 0;
    for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
        // printf ("ori %.4f, %.4f, %.4f", now[i].x, now[i].y, now[i].z);
    }
    for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
        now[i] = joint.rotationAll * now[i];
        // printf ("aft %.4f, %.4f, %.4f", now[i].x(), now[i].y(), now[i].z());
        if (leftBottom != -1) {
            if (now[i].x() - now[leftBottom].x()<= 1e-2 &&
                now[i].y() - now[leftBottom].y()<= 1e-2 &&
                now[i].z() - now[leftBottom].z()<= 1e-2) {
                leftBottom = i;
            }
            if (xMax <= now[i].x()) {
                xMax = now[i].x();
                xIndex = i;
            }
            if (yMax <= now[i].y()) {
                yMax = now[i].y();
                yIndex = i;
            }
            if (zMax <= now[i].z()) {
                zMax = now[i].z();
                zIndex = i;
            }
        } else {
            leftBottom = 0;
        }
    }
    Vector3d leftBottomPoint = now[0];
    now[0] = now[leftBottom];
    now[leftBottom] = leftBottomPoint;
    g_offset[joint.child_link] = now[0];
    for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
        col_struct.origin_points.push_back(now[i]);
    }
    col_struct.mesh_bounds = {now[0].x(), xMax, now[0].y(), yMax, now[0].z(), zMax};
    col_struct.dimensions[0] = xMax - now[0].x();
    col_struct.dimensions[1] = yMax - now[0].y();
    col_struct.dimensions[2] = zMax - now[0].z();
}

static bool GetMeshDimensions(const std::string& filename, CollisionStructure& col_struct)
{
    // 加载网格文件
    shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
    if (!mesh) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法加载网格文件: %s", filename.c_str());
        return false;
    }
    // 计算边界盒
    double min_x = mesh->vertices[0], max_x = mesh->vertices[0];
    double min_y = mesh->vertices[1], max_y = mesh->vertices[1];
    double min_z = mesh->vertices[2], max_z = mesh->vertices[2];
    int min_index_x = 0, min_index_y = 0, min_index_z = 0;
    int max_index_x = 0, max_index_y = 0, max_index_z = 0;

    for (unsigned int i = 1; i < mesh->vertex_count; ++i) {
        if (min_x > mesh->vertices[3*i]) {
            min_x = mesh->vertices[3*i];
            min_index_x = 3*i;
        }
        if (min_y > mesh->vertices[3*i + 1]) {
            min_y = mesh->vertices[3*i + 1];
            min_index_y = 3*i;
        }
        if (min_z > mesh->vertices[3*i + 2]) {
            min_z = mesh->vertices[3*i + 2];
            min_index_z = 3*i;
        }
        if (max_x < mesh->vertices[3*i]) {
            max_x = mesh->vertices[3*i];
            max_index_x = 3*i;
        }
        if (max_y < mesh->vertices[3*i + 1]) {
            max_y = mesh->vertices[3*i + 1];
            max_index_y = 3*i;
        }
        if (max_z < mesh->vertices[3*i + 2]) {
            max_z = mesh->vertices[3*i + 2];
            max_index_z = 3*i;
        }
    }

    // 存储原始边界盒
    col_struct.mesh_bounds = {min_x, max_x, min_y, max_y, min_z, max_z};
    
    // 计算原始尺寸
    double size_x = max_x - min_x;
    double size_y = max_y - min_y;
    double size_z = max_z - min_z;
    col_struct.dimensions = {size_x, size_y, size_z};
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "网格文件: %s", filename.c_str());
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "顶点数量: %u, 三角形数量: %u 大小:(%.4f, %.4f, %.4f)", mesh->vertex_count, mesh->triangle_count,
        col_struct.dimensions[0], col_struct.dimensions[1], col_struct.dimensions[2]);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "边界盒: x(%.4f, %.4f), y(%.4f, %.4f), z(%.4f, %.4f)",
        min_x, max_x, min_y, max_y, min_z, max_z);
    // 释放网格资源
    delete mesh;
    return true;
}

// 加载网格并计算边界盒
static bool loadMeshDimensions(const std::string& filename, JointInfo &joint, CollisionStructure& col_struct)
{
    // 加载网格文件
    shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
    if (!mesh) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法加载网格文件: %s", filename.c_str());
        return false;
    }
    // 计算边界盒
    double min_x = mesh->vertices[0], max_x = mesh->vertices[0];
    double min_y = mesh->vertices[1], max_y = mesh->vertices[1];
    double min_z = mesh->vertices[2], max_z = mesh->vertices[2];
    int min_index_x = 0, min_index_y = 0, min_index_z = 0;
    int max_index_x = 0, max_index_y = 0, max_index_z = 0;

    for (unsigned int i = 1; i < mesh->vertex_count; ++i) {
        if (min_x > mesh->vertices[3*i]) {
            min_x = mesh->vertices[3*i];
            min_index_x = 3*i;
        }
        if (min_y > mesh->vertices[3*i + 1]) {
            min_y = mesh->vertices[3*i + 1];
            min_index_y = 3*i;
        }
        if (min_z > mesh->vertices[3*i + 2]) {
            min_z = mesh->vertices[3*i + 2];
            min_index_z = 3*i;
        }
        if (max_x < mesh->vertices[3*i]) {
            max_x = mesh->vertices[3*i];
            max_index_x = 3*i;
        }
        if (max_y < mesh->vertices[3*i + 1]) {
            max_y = mesh->vertices[3*i + 1];
            max_index_y = 3*i;
        }
        if (max_z < mesh->vertices[3*i + 2]) {
            max_z = mesh->vertices[3*i + 2];
            max_index_z = 3*i;
        }
    }

    // 存储原始边界盒
    col_struct.mesh_bounds = {min_x, max_x, min_y, max_y, min_z, max_z};
    
    // 计算原始尺寸
    double size_x = max_x - min_x;
    double size_y = max_y - min_y;
    double size_z = max_z - min_z;
    col_struct.dimensions = {size_x, size_y, size_z};
    InitPointsFromBoundsAndRotation(joint, col_struct);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "网格文件: %s", filename.c_str());
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "顶点数量: %u, 三角形数量: %u 大小:(%.4f, %.4f, %.4f)", mesh->vertex_count, mesh->triangle_count,
        col_struct.dimensions[0], col_struct.dimensions[1], col_struct.dimensions[2]);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "边界盒: x(%.4f, %.4f), y(%.4f, %.4f), z(%.4f, %.4f)",
        min_x, max_x, min_y, max_y, min_z, max_z);
    
    // 释放网格资源
    delete mesh;
    return true;
}

static int InitCollisionStructure(urdf::Model &urdf_model, ArmSimulator *simulator,
    std::vector<CollisionStructure> &collision_structures_temp)
{
    for (const auto& link_pair : urdf_model.links_) {
        const std::string& link_name = link_pair.first;
        const urdf::LinkConstSharedPtr& link = link_pair.second;

        // 检查链接是否有碰撞元素
        if (link->collision) {
            CollisionStructure col_struct;
            col_struct.link_name = link_name;
            
            // 获取原点位置和姿态
            if (link->collision != NULL) {
                col_struct.origin_xyz = {
                    link->collision->origin.position.x,
                    link->collision->origin.position.y,
                    link->collision->origin.position.z
                };
                col_struct.origin_rpy = {
                    link->collision->origin.rotation.x,
                    link->collision->origin.rotation.y,
                    link->collision->origin.rotation.z
                };
            }
            bool valid = true;
            // 获取几何形状和尺寸
            if (link->collision->geometry) {
                switch (link->collision->geometry->type) {
                    case urdf::Geometry::BOX: {
                        col_struct.geometry_type = "box";
                        auto box = static_cast<urdf::Box*>(link->collision->geometry.get());
                        col_struct.dimensions = {box->dim.x, box->dim.y, box->dim.z};
                        break;
                    }
                    case urdf::Geometry::CYLINDER: {
                        col_struct.geometry_type = "cylinder";
                        auto cylinder = static_cast<urdf::Cylinder*>(link->collision->geometry.get());
                        col_struct.dimensions = {cylinder->radius, cylinder->length};
                        break;
                    }
                    case urdf::Geometry::SPHERE: {
                        col_struct.geometry_type = "sphere";
                        auto sphere = static_cast<urdf::Sphere*>(link->collision->geometry.get());
                        col_struct.dimensions = {sphere->radius};
                        break;
                    }
                    case urdf::Geometry::MESH: {
                        col_struct.geometry_type = "mesh";
                        auto mesh = static_cast<urdf::Mesh*>(link->collision->geometry.get());
                        // 存储STL文件路径（关键信息）
                        col_struct.mesh_filename = mesh->filename;  // 需要在结构体中添加这个成员
                        auto index = g_linkMap.find(link_name);
                        if (index == g_linkMap.end()) {
                            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "no link");
                            valid = false;
                            break;
                        }
                        int key = 0;
                        if (index->second == LEFT || index->second == RIGHT) {
                            key = (BASE << 8) | index->second;
                        } else {
                            key = ((index->second - 1) << 8) | (index->second);
                        }
                        auto joint = simulator->jointMap_.find(key);
                        if (joint == simulator->jointMap_.end()) {
                            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "no joint");
                            GetMeshDimensions(mesh->filename, col_struct);
                            break;
                        }
                        loadMeshDimensions(mesh->filename, joint->second, col_struct);
                        break;
                    }
                    default:
                        col_struct.geometry_type = "unknown";
                        break;
                }
            }
            if (valid) {
                collision_structures_temp.push_back(col_struct);
            }
        }
    }
    return 0;
}

static int UpdateJointInfo(int i, int parent, Quaterniond &rotation, std::map<int, 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;
    Vector3d axis = jointPair->second.axis;
    g_dir[i] = rotation * axis;
    return 0;
}

static int InitJoint(urdf::Model &urdf_model, std::map<int, 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;
}

static int InitArmPolyhedrons(ArmSimulator *simulator, urdf::Model &urdf_model)
{
    std::vector<CollisionStructure> collision_structures_temp;
    InitJoint(urdf_model, simulator->jointMap_);
    InitCollisionStructure(urdf_model, simulator, collision_structures_temp);
    return RemakeCollisionStructureList(simulator, collision_structures_temp);
}

ArmSimulator::ArmSimulator(std::string urdf_string, float *left_now_joints, float *right_now_joints,
    KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory)
    : leftTrajectory_(leftTrajectory), rightTrajectory_(rightTrajectory)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ArmSimulator: 初始化机械臂碰撞检测模块...");
    urdf::Model urdf_model;
    simpleUpdateIndex_ = 0;
    if (!urdf_model.initString(urdf_string)) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法解析URDF模型");
        return;
    }
    for (int i = 0; i < ARM_COLLISION_CHECK_SIMPLE_CNT; i++) {
        for (int j = 0; j < USED_ARM_DOF; j++) {
            leftArmJoints_[i][j] = left_now_joints[j];
            rightArmJoints_[i][j] = right_now_joints[j];
        }
    }

    InitArmPolyhedrons(this, urdf_model);
    InitArmPolyhedronsList(this);
    UpdateArmPolyhedrons(this);
}

ArmSimulator::ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float *left_now_joints, float *right_now_joints,
    KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory)
    : leftTrajectory_(leftTrajectory), rightTrajectory_(rightTrajectory)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ArmSimulator: 初始化机械臂碰撞检测模块...");
    std::string urdf_string = urdf_string_ptr->data;
    for (int i = 0; i < ARM_COLLISION_CHECK_SIMPLE_CNT; i++) {
        for (int j = 0; j < USED_ARM_DOF; j++) {
            leftArmJoints_[i][j] = left_now_joints[j];
            rightArmJoints_[i][j] = right_now_joints[j];
        }
    }

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

    InitArmPolyhedrons(this, urdf_model);
    InitArmPolyhedronsList(this);
    UpdateArmPolyhedrons(this);
}

ArmSimulator::~ArmSimulator()
{
}

static int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_)
{
    int result = SAFE;
    for (int i = LEFT; i < RIGHT; ++i) {
        for (int j = i + 1; j < ARM_POLYHEDRONS_CNT; ++j) {
            if (!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);
                result |= LEFT_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);
                result |= LEFT_ARM_COLLISION;
            }
        }
    }
    for (int i = RIGHT; i < BASE; ++i) {
        for (int j = i + 1; j < ARM_POLYHEDRONS_CNT; ++j) {
            if (!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 right %d and %d collision b", i, j);
                result |= RIGHT_ARM_COLLISION;
            }
            inCollision = gjk_interface::checkOBBCollision((*addedOBBs)[i], (*addedOBBsNext_)[j]);
            if (inCollision) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect right %d and %d collision c", i, j);
                result |= RIGHT_ARM_COLLISION;
            }
        }
    }
    return result;
}

int ArmSimulator::UpdateCollisionForNewGoal(int arm)
{
    if (arm >= ARM_ID_ERR) return -1;
    int simple20 = (simpleUpdateIndex_ + 1) % ARM_COLLISION_CHECK_SIMPLE_CNT;
    int simple40 = (simple20 + 1) % ARM_COLLISION_CHECK_SIMPLE_CNT;
    std::vector<Polyhedron> *armPolyhedrons20 = &(armPolyhedrons_[simple20]);
    std::vector<Polyhedron> *armPolyhedrons40 = &(armPolyhedrons_[simple40]);
    if (armPolyhedrons20->size() != ARM_POLYHEDRONS_CNT || armPolyhedrons40->size() != ARM_POLYHEDRONS_CNT) return -1;
    int startLinkIndex, endLinkIndex;
    float *jointList20;
    float *jointList40;

    if (arm == LEFT_ARM) {
        leftTrajectory_->getAngleForStarting(leftArmJoints_[simple20]);
        leftTrajectory_->getAngleForChecking(leftArmJoints_[simple40]);
        startLinkIndex = LEFT;
        endLinkIndex = RIGHT;
        jointList20 = leftArmJoints_[simple20];
        jointList40 = leftArmJoints_[simple40];
    } else if (arm == RIGHT_ARM) {
        rightTrajectory_->getAngleForStarting(rightArmJoints_[simple20]);
        rightTrajectory_->getAngleForChecking(rightArmJoints_[simple40]);
        startLinkIndex = RIGHT;
        endLinkIndex = BASE;
        jointList20 = rightArmJoints_[simple20];
        jointList40 = rightArmJoints_[simple40];
    }

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

    Vector3d offset_vec = g_origin[startLinkIndex] + g_origin[startLinkIndex + 1];
    Matrix3d rotation = Matrix3d::Identity();
    for (int i = startLinkIndex + 1; i < endLinkIndex; i++) {
        double angleRad = toRadians(jointList20[i - startLinkIndex - 1]);
        rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
        InitArmSinglePolyhedron(this, i, &(*armPolyhedrons20)[i],
            offset_vec,
            g_offset[i],
            rotation);
        addedOBBs_[simple20][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons20)[i],
            collision_structures_[i].dimensions[0],
            collision_structures_[i].dimensions[1],
            collision_structures_[i].dimensions[2]);
    }
    std::vector<OBB> *addedOBBsNext_ = &(addedOBBs_[simple20]);
    std::vector<OBB> *addedOBBs = &(addedOBBs_[simpleUpdateIndex_]);
    int result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
    if (result != SAFE) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: arm %d collision", arm);
        return result;
    }

    offset_vec = g_origin[startLinkIndex] + g_origin[startLinkIndex + 1];
    rotation = Matrix3d::Identity();
    for (int i = startLinkIndex + 1; i < endLinkIndex; i++) {
        double angleRad = toRadians(jointList40[i - startLinkIndex - 1]);
        rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
        InitArmSinglePolyhedron(this, i, &(*armPolyhedrons40)[i],
            offset_vec,
            g_offset[i],
            rotation);
        addedOBBs_[simple40][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons40)[i],
            collision_structures_[i].dimensions[0],
            collision_structures_[i].dimensions[1],
            collision_structures_[i].dimensions[2]);
    }
    addedOBBsNext_ = &(addedOBBs_[simple40]);
    addedOBBs = &(addedOBBs_[simple20]);
    result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
    if (result != SAFE) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: arm %d collision", arm);
        return result;
    }
    return 0;
}

int ArmSimulator::CheckCollision()
{
    int result = UpdateJointAngles(this);
    if (result != 0) {
        return result;
    }
    UpdateArmPolyhedrons(this);
    int nowIndex = (simpleUpdateIndex_ + ARM_COLLISION_CHECK_SIMPLE_CNT - 1) % ARM_COLLISION_CHECK_SIMPLE_CNT;
    std::vector<OBB> *addedOBBsNext_ = &(addedOBBs_[simpleUpdateIndex_]);
    std::vector<OBB> *addedOBBs = &(addedOBBs_[nowIndex]);
    result = CheckCollisionInner(addedOBBs, addedOBBsNext_);

    if (result != SAFE) {
        if (result & LEFT_ARM_COLLISION) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckCollision: left next joint: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, now: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f",
                leftArmJoints_[simpleUpdateIndex_][0],
                leftArmJoints_[simpleUpdateIndex_][1],
                leftArmJoints_[simpleUpdateIndex_][2],
                leftArmJoints_[simpleUpdateIndex_][3],
                leftArmJoints_[simpleUpdateIndex_][4],
                leftArmJoints_[simpleUpdateIndex_][5],
                leftArmJoints_[nowIndex][0],
                leftArmJoints_[nowIndex][1],
                leftArmJoints_[nowIndex][2],
                leftArmJoints_[nowIndex][3],
                leftArmJoints_[nowIndex][4],
                leftArmJoints_[nowIndex][5]);
        }
        if (result & RIGHT_ARM_COLLISION) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckCollision: right next joint: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, now: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f",
                rightArmJoints_[simpleUpdateIndex_][0],
                rightArmJoints_[simpleUpdateIndex_][1],
                rightArmJoints_[simpleUpdateIndex_][2],
                rightArmJoints_[simpleUpdateIndex_][3],
                rightArmJoints_[simpleUpdateIndex_][4],
                rightArmJoints_[simpleUpdateIndex_][5],
                rightArmJoints_[nowIndex][0],
                rightArmJoints_[nowIndex][1],
                rightArmJoints_[nowIndex][2],
                rightArmJoints_[nowIndex][3],
                rightArmJoints_[nowIndex][4],
                rightArmJoints_[nowIndex][5]);
        }

        result = ARM_COLLISION;
    } else {
        simpleUpdateIndex_ = (simpleUpdateIndex_ + 1) % ARM_COLLISION_CHECK_SIMPLE_CNT;
    }

    return result;
}

int ArmSimulator::ChangeOriFrameToArmBasePose(float *pose, int arm)
{
    if (!pose || arm >= ARM_ID_ERR) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: invalid pose");
        return -1;
    }
    int key = 0;
    if (arm == LEFT_ARM) {
        key = (BASE << 8) | (LEFT);
    } else {
        key = (BASE << 8) | (RIGHT);
    }
    auto joint = jointMap_.find(key);
    if (joint == jointMap_.end()) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: no joint");
        return -1;
    }
    OriFrameChangeToArmPose(pose,joint->second);
    return 0;
}

int ArmSimulator::GetFrameChange(float *pose, int arm, int64_t frameTime, JointInfo *jointInfo)
{
    if (!pose || arm >= ARM_ID_ERR) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: invalid pose");
        return -1;
    }
    int key = 0, parent = 0;
    int result = 0;
    float jointList[USED_ARM_DOF] = {0};
    if (arm == LEFT_ARM) {
        parent = LEFT;
        result = leftTrajectory_->getAngleFromTime(jointList, frameTime);
    } else {
        parent = RIGHT;
        result = rightTrajectory_->getAngleFromTime(jointList, frameTime);
    }
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: get joint fail");
        return -1;
    }
    Quaterniond start = Quaterniond::Identity();
    Quaterniond rotationAll = Quaterniond::Identity();
    Vector3d offset(0, 0, 0);        // 关节偏移
    for (int i = 0; i < USED_ARM_DOF; ++i) {
        key = (parent << 8) | (parent + 1);
        auto joint = jointMap_.find(key);
        if (joint == jointMap_.end()) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: no joint");
            return -1;
        }

        double angleRad = toRadians(jointList[i]);
        rotationAll = joint->second.rotation * rotationAll;
        Quaterniond axis(AngleAxisd(angleRad, start * rotationAll * joint->second.axis));
        start = axis * start;
        offset = offset + start * rotationAll * joint->second.offset;
        // RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "offset: (%.4f, %.4f, %.4f)", offset.x(), offset.y(), offset.z());
        parent = parent + 1;
    }
    jointInfo->rotationAll = start;
    jointInfo->offset = offset;
    return 0;
}

int ArmSimulator::ChangeArmBaseFrameToArmEndPose(float *pose, int arm, int64_t frameTime)
{
    JointInfo jointInfo;
    int result = GetFrameChange(pose, arm, frameTime, &jointInfo);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangeArmBaseFrameToArmEndPose: get frame change fail");
        return -1;
    }
    OriFrameChangeToArmPose(pose, jointInfo);
    return 0;
}

int ArmSimulator::ChangeArmEndPoseToArmBaseFrame(float *pose, int arm, int64_t frameTime)
{
    JointInfo jointInfo;
    int result = GetFrameChange(pose, arm, frameTime, &jointInfo);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangeArmEndPoseToArmBaseFrame: get frame change fail");
        return -1;
    }

    ArmPoseChangeToOriFrame(pose, jointInfo);
    return 0;
}

int ArmSimulator::ChangeArmBasePoseToOriFrame(float *pose, int arm)
{
    if (!pose || arm >= ARM_ID_ERR) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToOriFrame: invalid pose");
        return -1;
    }
    int key = 0;
    if (arm == LEFT_ARM) {
        key = (BASE << 8) | (LEFT);
    } else {
        key = (BASE << 8) | (RIGHT);
    }
    auto joint = jointMap_.find(key);
    if (joint == jointMap_.end()) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToOriFrame: no joint");
        return -1;
    }
    ArmPoseChangeToOriFrame(pose, joint->second);
    return 0;
}

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

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

int ArmSimulator::CheckArmJointsAngleLimit(float *joints)
{
    for (int i = 0; i < USED_ARM_DOF; ++i) {
        auto joint = jointMap_.find(((i) << 8) | (i + 1));
        if (joint != jointMap_.end()) {
            float angle = toRadians(joints[i]);
            if (angle < joint->second.limit_lower || angle > joint->second.limit_upper) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
                    "CheckArmJointsAngleLimit: joint out of limit [%.4f, %.4f], current: %.4f(%.2f)",
                    joint->second.limit_lower, joint->second.limit_upper, angle, joints[i]);
                return ARM_AIM_CANNOT_REACH;
            }
        } else {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckArmJointsAngleLimit: no joint");
            return ARM_AIM_CANNOT_REACH;
        }
    }
    return 0;
}
