#include <vector>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <tinyxml2.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_bodyJointCnt[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};

const std::string g_bodyStartName[BODY_CNT] = {"LeftLink1", "RightLink1", "base_link"};

static float zeroFixedAngle = 0;

int g_linkStartIndex[BODY_CNT] = {0};

#define POLYHEDRONS_POINTS_CNT   (8)

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 double toRadians(float degrees) {
    return (static_cast<double>(degrees)) * (M_PI / 180.0);
}

enum
  {
    UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
  } JointTypeE;

int CollisionSimulator::InitJoint()
{
    const std::map<std::string, int> jointNameToIndexMap = {
        {"left_joint1", 0}, {"left_joint2", 1}, {"left_joint3", 2}, {"left_joint4", 3}, {"left_joint5", 4}, {"left_joint6", 5}, 
        {"right_joint1", 0}, {"right_joint2", 1}, {"right_joint3", 2}, {"right_joint4", 3}, {"right_joint5", 4}, {"right_joint6", 5},
        {"left_behind_hip_joint", 0}, {"left_behind_leg_joint", 1}, {"left_behind_wheel_joint", 2}, {"right_behind_hip_joint", 3},
        {"right_behind_leg_joint", 4}, {"right_behind_wheel_joint", 5}, {"body_joint", 6}, {"body_2_joint", 7}, {"head_joint", 8},
        {"left_front_roll_joint", 9}, {"left_front_hip_joint", 10}, {"left_front_knee_joint", 11}, 
        {"left_front_wheel_joint", 12}, {"right_front_roll_joint", 13}, {"right_front_hip_joint", 14}, {"right_front_knee_joint", 15}, {"right_front_wheel_joint", 16}
    };
    const std::map<std::string, int> jointNameToBodyType = {
        {"left_joint1", BODY_TYPE_LEFT_ARM}, {"left_joint2", BODY_TYPE_LEFT_ARM}, {"left_joint3", BODY_TYPE_LEFT_ARM}, {"left_joint4", BODY_TYPE_LEFT_ARM},
        {"left_joint5", BODY_TYPE_LEFT_ARM}, {"left_joint6", BODY_TYPE_LEFT_ARM}, 
        {"right_joint1", BODY_TYPE_RIGHT_ARM}, {"right_joint2", BODY_TYPE_RIGHT_ARM}, {"right_joint3", BODY_TYPE_RIGHT_ARM}, {"right_joint4", BODY_TYPE_RIGHT_ARM},
        {"right_joint5", BODY_TYPE_RIGHT_ARM}, {"right_joint6", BODY_TYPE_RIGHT_ARM},
        {"left_behind_hip_joint", BODY_TYPE_OTHERS}, {"left_behind_leg_joint", BODY_TYPE_OTHERS}, {"left_behind_wheel_joint", BODY_TYPE_OTHERS},
        {"right_behind_hip_joint", BODY_TYPE_OTHERS}, {"right_behind_leg_joint", BODY_TYPE_OTHERS}, {"right_behind_wheel_joint", BODY_TYPE_OTHERS},
        {"body_joint", BODY_TYPE_OTHERS}, {"body_2_joint", BODY_TYPE_OTHERS}, {"head_joint", BODY_TYPE_OTHERS}, {"left_front_roll_joint", BODY_TYPE_OTHERS},
        {"left_front_hip_joint", BODY_TYPE_OTHERS}, {"left_front_knee_joint", BODY_TYPE_OTHERS}, {"left_front_wheel_joint", BODY_TYPE_OTHERS},
        {"right_front_roll_joint", BODY_TYPE_OTHERS}, {"right_front_hip_joint", BODY_TYPE_OTHERS}, {"right_front_knee_joint", BODY_TYPE_OTHERS},
        {"right_front_wheel_joint", BODY_TYPE_OTHERS}
    };
    for (JointsInfo &joint : jointMap_) {
        if (joint.type != FIXED) {
            int jointAngleIndex = jointNameToIndexMap.at(joint.name);
            int bodyType = jointNameToBodyType.at(joint.name);
            for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
                joint.angle[i] = &joints_[i][bodyType][jointAngleIndex];
            }
        } else {
            for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
                joint.angle[i] = &zeroFixedAngle;
            }
        }
    }
    return 0;
}

int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_str) 
{
    if (maxObbCnt <= 0) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InitLinkCollisionDetectMatrix: invalid maxObbCnt=%d", maxObbCnt);
        return -1;
    }
    linkCollisionDetectMatrix_.assign(static_cast<size_t>(maxObbCnt) * static_cast<size_t>(maxObbCnt), 0);
    for (int i = 0; i < maxObbCnt; ++i) {
        for (int j = i + 1; j < maxObbCnt; ++j) {
            linkCollisionDetectMatrix_[static_cast<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(j)] = 1;
        }
    }
    tinyxml2::XMLDocument doc;
    tinyxml2::XMLError error = doc.Parse(srdf_str.c_str());
    if (error != tinyxml2::XML_SUCCESS) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to parse SRDF: %s, length: %ld", doc.ErrorStr(), srdf_str.length());
        return -1;
    }

    // 获取根节点<robot>
    tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
    if (!robot_root) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF has no <robot> root node");
        return -1;
    }

    int disable_count = 0;
    for (tinyxml2::XMLElement* dc_elem = robot_root->FirstChildElement("disable_collisions");
        dc_elem;
        dc_elem = dc_elem->NextSiblingElement("disable_collisions")) {
        const char* link1_c = dc_elem->Attribute("link1");
        const char* link2_c = dc_elem->Attribute("link2");
        if (!link1_c || !link2_c) {
            RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "disable_collisions missing link1/link2 attribute, skipping.");
            continue;
        }
        std::string link1 = link1_c;
        std::string link2 = link2_c;
        int link1Index = linkCollisionMap_.find(link1) != linkCollisionMap_.end() ? linkCollisionMap_[link1] : -1;
        int link2Index = linkCollisionMap_.find(link2) != linkCollisionMap_.end() ? linkCollisionMap_[link2] : -1;
        if (link1Index != -1 && link2Index != -1) {
            CollisionStructureInfo* colStruct1 = &collision_structures_[link1Index];
            CollisionStructureInfo* colStruct2 = &collision_structures_[link2Index];
            for (int obb1Idx : colStruct1->obbLinkIndexList) {
                for (int obb2Idx : colStruct2->obbLinkIndexList) {
                    if (obb1Idx >= 0 && obb1Idx < maxObbCnt && obb2Idx >= 0 && obb2Idx < maxObbCnt) {
                        linkCollisionDetectMatrix_[
                            static_cast<size_t>(obb1Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(obb2Idx)] = 0;
                        linkCollisionDetectMatrix_[
                            static_cast<size_t>(obb2Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(obb1Idx)] = 0;
                    } else {
                        RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
                            "disable_collisions references out-of-range OBB index (%d,%d) with maxObbCnt=%d",
                            obb1Idx, obb2Idx, maxObbCnt);
                    }
                }
            }
        }
        disable_count++;
    }
    // Build tuplesToCheck_ list
    tuplesToCheck_.clear();
    tuplesToCheck_.reserve(static_cast<size_t>(maxObbCnt) * (static_cast<size_t>(maxObbCnt) - 1) / 2);
    for (int i = 0; i < maxObbCnt; ++i) {
        for (int j = i + 1; j < maxObbCnt; ++j) {
            if (linkCollisionDetectMatrix_[static_cast<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(j)]) {
                AxisStats *stats = new AxisStats();
                tuplesToCheck_.emplace_back(i, j, stats);
            }
        }
    }
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), 
        "Initialized link collision detect matrix, disabled %d collision pairs, enabled pairs: %zu.",
        disable_count, tuplesToCheck_.size());
    return 0;
}

int CollisionSimulator::InitXmlStrFromFile(const std::string& robot_string)
{
    tinyxml2::XMLDocument doc;
    tinyxml2::XMLError error = doc.Parse(robot_string.c_str());
    if (error != tinyxml2::XML_SUCCESS) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to parse SRDF: %s, length: %ld", doc.ErrorStr(), robot_string.length());
        return -1;
    }

    tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
    if (!robot_root) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF has no <robot> root node");
        return -1;
    }
    if (!robot_root->Attribute("maxObbCnt")) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF <robot> missing required attribute maxObbCnt");
        return -1;
    }
    rootLinkIndex_ = robot_root->IntAttribute("rootLinkIndex_", 0);
    maxObbCnt = robot_root->IntAttribute("maxObbCnt", 0);
    if (maxObbCnt <= 0) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid maxObbCnt=%d parsed from SRDF", maxObbCnt);
        return -1;
    }
    for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
        addedOBBs_[i].resize(maxObbCnt);
    }
    int collision_structure_count = robot_root->IntAttribute("collision_structure_count", 0);
    int joint_count = robot_root->IntAttribute("joint_count", 0);
    if (collision_structure_count <= 0 || joint_count <= 0) {
        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid counts: collision_structure_count=%d, joint_count=%d", collision_structure_count, joint_count);
        return -1;
    }
    collision_structures_.resize(collision_structure_count);
    jointMap_.resize(joint_count);

    int obbCnt = 0;
    for (tinyxml2::XMLElement* link_elem = robot_root->FirstChildElement("collision_model");
        link_elem;
        link_elem = link_elem->NextSiblingElement("collision_model")) {
        CollisionStructureInfo col_struct;
        int linkIndex = col_struct.link_index = link_elem->IntAttribute("link_index", -1);
        const char* link_name_c = link_elem->Attribute("link_name");
        if (!link_name_c || linkIndex < 0 || linkIndex >= collision_structure_count) {
            RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "collision_model missing/invalid link_name or link_index=%d", linkIndex);
            return -1;
        }
        std::string linkName = col_struct.link_name = link_name_c;
        col_struct.geometry_type = (link_elem->IntAttribute("geometry_type", 0));
        col_struct.mesh_filename = link_elem->Attribute("mesh_filename") ? link_elem->Attribute("mesh_filename") : "";
        col_struct.joint_count = 0;
        col_struct.parent_link_index = link_elem->IntAttribute("parent_link_index", -1);
        
        for (tinyxml2::XMLElement* obb_elem = link_elem->FirstChildElement("obb");
            obb_elem;
            obb_elem = obb_elem->NextSiblingElement("obb")) {
            OBB obb;
            obb.center = Vector3d(
                obb_elem->DoubleAttribute("center_x", 0.0),
                obb_elem->DoubleAttribute("center_y", 0.0),
                obb_elem->DoubleAttribute("center_z", 0.0));
            obb.axis[0] = Vector3d(
                obb_elem->DoubleAttribute("axis_0_x", 1.0),
                obb_elem->DoubleAttribute("axis_0_y", 0.0),
                obb_elem->DoubleAttribute("axis_0_z", 0.0));
            obb.axis[1] = Vector3d(
                obb_elem->DoubleAttribute("axis_1_x", 0.0),
                obb_elem->DoubleAttribute("axis_1_y", 1.0),
                obb_elem->DoubleAttribute("axis_1_z", 0.0));
            obb.axis[2] = Vector3d(
                obb_elem->DoubleAttribute("axis_2_x", 0.0),
                obb_elem->DoubleAttribute("axis_2_y", 0.0),
                obb_elem->DoubleAttribute("axis_2_z", 1.0));
            obb.halfExtent[0] = obb_elem->DoubleAttribute("half_extent_x", 0.5);
            obb.halfExtent[1] = obb_elem->DoubleAttribute("half_extent_y", 0.5);
            obb.halfExtent[2] = obb_elem->DoubleAttribute("half_extent_z", 0.5);
            obb.maxRadius = obb_elem->DoubleAttribute("max_radius", 0.0);
            obb.selfMinProject[0] = obb_elem->DoubleAttribute("self_min_project_x", 0.0);
            obb.selfMinProject[1] = obb_elem->DoubleAttribute("self_min_project_y", 0.0);
            obb.selfMinProject[2] = obb_elem->DoubleAttribute("self_min_project_z", 0.0);
            obb.selfMaxProject[0] = obb_elem->DoubleAttribute("self_max_project_x", 0.0);
            obb.selfMaxProject[1] = obb_elem->DoubleAttribute("self_max_project_y", 0.0);
            obb.selfMaxProject[2] = obb_elem->DoubleAttribute("self_max_project_z", 0.0);
            gjk_interface::getSelfProject(obb);
            col_struct.obbModelList.push_back(obb);
            obbCnt++;
            int obbIndex = obb_elem->IntAttribute("index", -1);
            if (obbIndex < 0 || obbIndex >= maxObbCnt) {
                RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB index out of range: %d (max %d) for link %s", obbIndex, maxObbCnt, linkName.c_str());
                return -1;
            }
            col_struct.obbLinkIndexList.push_back(obbIndex);
            obbNameMap_[obbIndex] = linkName;

            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded OBB for link %s: center(%.2f, %.2f, %.2f), halfExtent(%.2f, %.2f, %.2f).",
                linkName.c_str(),
                obb.center.x(), obb.center.y(), obb.center.z(),
                obb.halfExtent[0], obb.halfExtent[1], obb.halfExtent[2]);
        }

        for (tinyxml2::XMLElement* joint_elem = link_elem->FirstChildElement("joint_index");
            joint_elem;
            joint_elem = joint_elem->NextSiblingElement("joint_index")) {
            int idx = joint_elem->IntAttribute("index", -1);
            if (idx < 0 || idx >= joint_count) {
                RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_index out of range: %d (joint_count %d) for link %s", idx, joint_count, linkName.c_str());
                return -1;
            }
            col_struct.jointIndexList.push_back(idx);
            col_struct.joint_count++;
        }

        collision_structures_[linkIndex] = col_struct;
        linkCollisionMap_[linkName] = linkIndex;
        for (int i = 0; i < BODY_CNT; i++) {
            if (g_bodyStartName[i] == linkName) {
                g_linkStartIndex[i] = linkIndex;
                break;
            }
        }
    }

    for (tinyxml2::XMLElement* joint_elem = robot_root->FirstChildElement("joint_model");
        joint_elem;
        joint_elem = joint_elem->NextSiblingElement("joint_model")) {
        JointsInfo joint_info;
        int joint_index = joint_elem->IntAttribute("joint_index", -1);
        const char* joint_name_c = joint_elem->Attribute("joint_name");
        if (!joint_name_c || joint_index < 0 || joint_index >= joint_count) {
            RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model missing/invalid joint_name or joint_index=%d", joint_index);
            return -1;
        }
        joint_info.name = joint_name_c;
        joint_info.type = (joint_elem->IntAttribute("type", 0));
        joint_info.parent_link = joint_elem->IntAttribute("parent_link", -1);
        joint_info.child_link = joint_elem->IntAttribute("child_link", -1);
        joint_info.limit_lower = joint_elem->DoubleAttribute("limit_lower", 0.0);
        joint_info.limit_upper = joint_elem->DoubleAttribute("limit_upper", 0.0);
        joint_info.limit_v = joint_elem->DoubleAttribute("limit_v", 0.0);
        joint_info.limit_e = joint_elem->DoubleAttribute("limit_e", 0.0);
        joint_info.nextLinkOffset = Vector3d(
            joint_elem->DoubleAttribute("nextLinkOffset_x", 0.0),
            joint_elem->DoubleAttribute("nextLinkOffset_y", 0.0),
            joint_elem->DoubleAttribute("nextLinkOffset_z", 0.0));
        joint_info.rotation = Quaterniond(
            joint_elem->DoubleAttribute("rotation_w", 1.0),
            joint_elem->DoubleAttribute("rotation_x", 0.0),
            joint_elem->DoubleAttribute("rotation_y", 0.0),
            joint_elem->DoubleAttribute("rotation_z", 0.0));
        joint_info.axis = Vector3d(
            joint_elem->DoubleAttribute("axis_x", 1.0),
            joint_elem->DoubleAttribute("axis_y", 0.0),
            joint_elem->DoubleAttribute("axis_z", 0.0));
        if (joint_info.child_link < 0 || joint_info.child_link >= collision_structure_count ||
            joint_info.parent_link < 0 || joint_info.parent_link >= collision_structure_count) {
            RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model link indices out of range for joint %s", joint_info.name.c_str());
            return -1;
        }
        jointMap_[joint_index] = joint_info;

        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded joint %s: axis(%.2f, %.2f, %.2f), offset(%.2f, %.2f, %.2f).",
            joint_info.name.c_str(), joint_info.axis.x(), joint_info.axis.y(), joint_info.axis.z(),
            joint_info.nextLinkOffset.x(), joint_info.nextLinkOffset.y(), joint_info.nextLinkOffset.z());
    }

    for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
        collision_structures_[rootLinkIndex_].rotation[i] = Quaterniond::Identity();
        collision_structures_[rootLinkIndex_].offset[i] = Vector3d::Zero();
    }

    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initialized collision structures from XML, total links: %ld, total OBBs: %d.", 
        collision_structures_.size(), obbCnt);
    return 0;
}

CollisionSimulator::CollisionSimulator(std::string robot_string, std::string srdf_string, float **jointsList, KinematicsManager **trajectory)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块...");

    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_TYPE_OTHERS; j++) {
            for (int k = 0; k < g_bodyJointCnt[j]; k++) {
                joints_[i][j][k] = jointsList[j][k];
            }
        }
        for (int j = 0; j < g_bodyJointCnt[BODY_TYPE_OTHERS]; j++) {
            joints_[i][BODY_TYPE_OTHERS][j] = 0;
        }
    }
    for (int i = 0; i < BODY_TYPE_OTHERS; ++i) {
        trajectory_[i] = trajectory[i];
    }
    simpleUpdateIndex_ = 0;
    InitXmlStrFromFile(robot_string);

    InitJoint();
    InitPolyhedronsList();
    InitLinkCollisionDetectMatrix(srdf_string);
}

CollisionSimulator::~CollisionSimulator()
{
    for (const auto& p : tuplesToCheck_) {
        delete std::get<2>(p);
    }
    tuplesToCheck_.clear();
}

int flashStateIndex = 0;
int CollisionSimulator::CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_)
{
    bool inCollision = false;
    OBB *obb1 = NULL;
    OBB *obb2 = NULL;
    for (const auto& p : tuplesToCheck_) {
        const int i = std::get<0>(p);
        const int j = std::get<1>(p);
        AxisStats *stats =  std::get<2>(p);
        inCollision = gjk_interface::checkOBBCollisionNew((*addedOBBsNext_)[i], (*addedOBBsNext_)[j], stats);
        if (inCollision) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision b", obbNameMap_[i].c_str(), obbNameMap_[j].c_str());
            obb1 = &(*addedOBBsNext_)[i];
            obb2 = &(*addedOBBsNext_)[j];
            break;
        }
        inCollision = gjk_interface::checkOBBCollisionNew((*addedOBBs)[i], (*addedOBBsNext_)[j], stats);
        if (inCollision) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision c", obbNameMap_[i].c_str(), obbNameMap_[j].c_str());
            obb1 = &(*addedOBBs)[i];
            obb2 = &(*addedOBBsNext_)[j];
            break;
        }
    }
    AxisStats *stats =  std::get<2>(tuplesToCheck_[flashStateIndex]);
    gjk_interface::BubbleSortAxisByHitCount(stats);
    if (inCollision) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb1: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
            obb1->center.x(), obb1->center.y(), obb1->center.z(),
            obb1->axis[0].x(), obb1->axis[0].y(), obb1->axis[0].z(),
            obb1->axis[1].x(), obb1->axis[1].y(), obb1->axis[1].z(),
            obb1->axis[2].x(), obb1->axis[2].y(), obb1->axis[2].z(),
            obb1->halfExtent[0], obb1->halfExtent[1], obb1->halfExtent[2]);
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb2: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
            obb2->center.x(), obb2->center.y(), obb2->center.z(),
            obb2->axis[0].x(), obb2->axis[0].y(), obb2->axis[0].z(),
            obb2->axis[1].x(), obb2->axis[1].y(), obb2->axis[1].z(),
            obb2->axis[2].x(), obb2->axis[2].y(), obb2->axis[2].z(),
            obb2->halfExtent[0], obb2->halfExtent[1], obb2->halfExtent[2]);
        return ARM_COLLISION;
    }
    return OK;
}

int CollisionSimulator::CheckCollisionInnerOld(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_)
{
    bool inCollision = false;
    OBB *obb1 = NULL;
    OBB *obb2 = NULL;
    for (const auto& p : tuplesToCheck_) {
        const int i = std::get<0>(p);
        const int j = std::get<1>(p);
        inCollision = gjk_interface::checkOBBCollision((*addedOBBsNext_)[i], (*addedOBBsNext_)[j]);
        if (inCollision) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision b", obbNameMap_[i].c_str(), obbNameMap_[j].c_str());
            obb1 = &(*addedOBBsNext_)[i];
            obb2 = &(*addedOBBsNext_)[j];
            break;
        }
        inCollision = gjk_interface::checkOBBCollision((*addedOBBs)[i], (*addedOBBsNext_)[j]);
        if (inCollision) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision c", obbNameMap_[i].c_str(), obbNameMap_[j].c_str());
            obb1 = &(*addedOBBs)[i];
            obb2 = &(*addedOBBsNext_)[j];
            break;
        }
    }
    if (inCollision) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb1: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
            obb1->center.x(), obb1->center.y(), obb1->center.z(),
            obb1->axis[0].x(), obb1->axis[0].y(), obb1->axis[0].z(),
            obb1->axis[1].x(), obb1->axis[1].y(), obb1->axis[1].z(),
            obb1->axis[2].x(), obb1->axis[2].y(), obb1->axis[2].z(),
            obb1->halfExtent[0], obb1->halfExtent[1], obb1->halfExtent[2]);
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb2: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
            obb2->center.x(), obb2->center.y(), obb2->center.z(),
            obb2->axis[0].x(), obb2->axis[0].y(), obb2->axis[0].z(),
            obb2->axis[1].x(), obb2->axis[1].y(), obb2->axis[1].z(),
            obb2->axis[2].x(), obb2->axis[2].y(), obb2->axis[2].z(),
            obb2->halfExtent[0], obb2->halfExtent[1], obb2->halfExtent[2]);
        return ARM_COLLISION;
    }
    return OK;
}

void CollisionSimulator::UpdatePolyhedrons(int bodyType, int simple)
{
    int linkIndex = g_linkStartIndex[bodyType];
    CollisionStructureInfo *colStruct = &collision_structures_[linkIndex];
    std::vector<CollisionStructureInfo *> child = {colStruct};
    int childIndex = 0;
    int childCnt = 1;
    while (childIndex < childCnt) {
        colStruct = child[childIndex];
        childIndex++;
        for (int i = 0; i < colStruct->joint_count; i++) {
            int jointIndex = colStruct->jointIndexList[i];
            JointsInfo *jointInfo = &jointMap_[jointIndex];
            Quaterniond rotation = colStruct->rotation[simple];
            Vector3d offset = colStruct->offset[simple];

            linkIndex = jointInfo->child_link;
            CollisionStructureInfo *nowChild = &collision_structures_[linkIndex];

            offset = offset + rotation * jointInfo->nextLinkOffset;
            if (jointInfo->type == REVOLUTE) {
                double angleRad = toRadians(jointInfo->angle[simple][0]);
                rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation;
            } else if (jointInfo->type == PRISMATIC) {
                double displacement = jointInfo->angle[simple][0];
                offset = offset + rotation * jointInfo->axis * displacement;
            }

            nowChild->rotation[simple] = rotation;
            nowChild->offset[simple] = offset;

            for (int i = 0; i < nowChild->obbModelList.size(); i++) {
                int obbIndex = nowChild->obbLinkIndexList[i];
                addedOBBs_[simple][obbIndex] = gjk_interface::updateOBBFromPoints(nowChild->obbModelList[i],
                    offset,
                    rotation);
            }
            
            if (nowChild->joint_count > 0) {
                childCnt++;
                child.push_back(nowChild);
            }
        }
    }
}

void CollisionSimulator::InitPolyhedronsList()
{
    for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
        UpdatePolyhedrons(BODY_TYPE_OTHERS, i);
    }
    for (int i = 0; i < maxObbCnt; i++) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InitPolyhedronsList obb %d: center(%.2f, %.2f, %.2f), axis1(%.2f, %.2f, %.2f), axis2(%.2f, %.2f, %.2f), axis3(%.2f, %.2f, %.2f), half_lengths(%.2f, %.2f, %.2f)",
            i,
            addedOBBs_[0][i].center.x(), addedOBBs_[0][i].center.y(), addedOBBs_[0][i].center.z(),
            addedOBBs_[0][i].axis[0].x(), addedOBBs_[0][i].axis[0].y(), addedOBBs_[0][i].axis[0].z(),
            addedOBBs_[0][i].axis[1].x(), addedOBBs_[0][i].axis[1].y(), addedOBBs_[0][i].axis[1].z(),
            addedOBBs_[0][i].axis[2].x(), addedOBBs_[0][i].axis[2].y(), addedOBBs_[0][i].axis[2].z(),
            addedOBBs_[0][i].halfExtent[0], addedOBBs_[0][i].halfExtent[1], addedOBBs_[0][i].halfExtent[2]);
    }
}

int checkCnt = 0;
int64_t prepareTime = 0;
int64_t checkTime = 0;
int64_t checkTime2 = 0;
int CollisionSimulator::CheckCollision(rclcpp::Node *node)
{
    int64_t currentTimeNs = node->get_clock()->now().nanoseconds();
    for (int bodyType = 0; bodyType < BODY_TYPE_OTHERS; bodyType++) {
        float *jointList = joints_[simpleUpdateIndex_][bodyType];
        trajectory_[bodyType]->getAngleForChecking(jointList);
    }
    if (CheckBodyAngleLimit(simpleUpdateIndex_, BODY_TYPE_OTHERS) != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckCollision joints limit error.");
        return ARM_AIM_CANNOT_REACH;
    }
    UpdatePolyhedrons(BODY_TYPE_OTHERS, simpleUpdateIndex_);
    int64_t updateNs = node->get_clock()->now().nanoseconds();
    prepareTime += updateNs - currentTimeNs;
    int lastSimpleIndex = (simpleUpdateIndex_ + COLLISION_CHECK_SIMPLE_CNT - 1) % COLLISION_CHECK_SIMPLE_CNT;
    std::vector<OBB> *addedOBBsNext_ = &(addedOBBs_[simpleUpdateIndex_]);
    std::vector<OBB> *addedOBBs = &(addedOBBs_[lastSimpleIndex]);
    int result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
    simpleUpdateIndex_ = (simpleUpdateIndex_ + 1) % COLLISION_CHECK_SIMPLE_CNT;

    int64_t time1 = node->get_clock()->now().nanoseconds();
    checkTime += time1 - updateNs;
    checkCnt++;
    if (checkCnt == 2000) {
        double checkAxisAve;
        gjk_interface::GetAxisLog(checkAxisAve);
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), 
            "Update time:%ld, check time1:%ld, check axis ave:%.4f.", prepareTime/checkCnt/1000,
                    checkTime/checkCnt/1000, checkAxisAve);
        checkTime = 0;
        prepareTime = 0;
        checkCnt = 0;
    }
    return result;
}

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::CheckBodyAngleLimit(int simple, int bodyType)
{
    CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[bodyType]];

    std::vector<CollisionStructureInfo *> child = {colStruct};
    int childIndex = 0;
    int childCnt = 1;
    while (childIndex < childCnt) {
        colStruct = child[childIndex];
        childIndex++;
        for (int i = 0; i < colStruct->joint_count; i++) {
            int jointIndex = colStruct->jointIndexList[i];
            JointsInfo *jointInfo = &jointMap_[jointIndex];
            float angles = toRadians(jointInfo->angle[simple][0]);
            if (angles < jointInfo->limit_lower || angles > jointInfo->limit_upper) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s angle limit error: %.2f not in [%.2f, %.2f]",
                    jointInfo->name.c_str(),
                    jointInfo->angle[simple][0],
                    jointInfo->limit_lower,
                    jointInfo->limit_upper);
                return ARM_AIM_CANNOT_REACH;
            }
            CollisionStructureInfo *nowChild = &collision_structures_[jointInfo->child_link];
            if (nowChild->joint_count > 0) {
                childCnt++;
                child.push_back(nowChild);
            }
        }
    }
    return OK;
}

int CollisionSimulator::CheckArmJointsAngleLimit(float *joints)
{
    CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[BODY_TYPE_LEFT_ARM]];

    std::vector<CollisionStructureInfo *> child = {colStruct};
    int childIndex = 0;
    int childCnt = 1;
    while (childIndex < childCnt) {
        colStruct = child[childIndex];
        for (int i = 0; i < colStruct->joint_count; i++) {
            int jointIndex = colStruct->jointIndexList[i];
            JointsInfo *jointInfo = &jointMap_[jointIndex];
            float angles = toRadians(joints[childIndex]);
            if (angles < jointInfo->limit_lower || angles > jointInfo->limit_upper) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s angle limit error: %.2f not in [%.2f, %.2f]",
                    jointInfo->name.c_str(),
                    joints[childIndex],
                    jointInfo->limit_lower,
                    jointInfo->limit_upper);
                return ARM_AIM_CANNOT_REACH;
            }
            CollisionStructureInfo *nowChild = &collision_structures_[jointInfo->child_link];
            if (nowChild->joint_count > 0) {
                childCnt++;
                child.push_back(nowChild);
            }
        }
        childIndex++;
    }
    return OK;
}

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;

    KinematicsManager *trajectory = trajectory_[bodyType];
    float *jointList20 = joints_[simple20][bodyType];
    float *jointList40 = joints_[simple40][bodyType];
    trajectory->getAngleForStarting(jointList20);
    trajectory->getAngleForChecking(jointList40);

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

    UpdatePolyhedrons(bodyType, simple20);
    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(bodyType, simple40);
    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;
}
