#include <fstream>
#include <string>
#include <stdexcept>  // 用于抛出异常
#include <filesystem>
#include <iostream>
#include <moveit/robot_model/robot_model.h>
#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <create_robot_describe.hpp>

#define UNUSED(x) (void)(x)

typedef CGAL::Exact_predicates_inexact_constructions_kernel     CgalK;
typedef CgalK::Point_3                                          CgalPoint;
namespace fs = std::filesystem;
using namespace tinyxml2;
using namespace Eigen;

int RobotDescription::InitJointModel()
{
    int linkIndex = rootLinkIndex_;
    CollisionModel *colStruct = &collision_structures_[linkIndex];
    std::vector<int> jointList;
    int index = 0;
    int maxJointCount = 0;
    Quaterniond rotationAll = Quaterniond::Identity();
    while (colStruct->joint_count > 0 || index < maxJointCount) {
        for (int i = 0; i < colStruct->joint_count; i++) {
            jointList.push_back(colStruct->jointIndexList[i]);
            maxJointCount++;
            int jointIndex = colStruct->jointIndexList[i];
            JointsModel *jointInfo = &jointMap_[jointIndex];
            printf("链接 %s 包含关节 %s, 原offset: (%.4f, %.4f, %.4f)\n", colStruct->link_name.c_str(), jointInfo->name.c_str(),
                   jointInfo->nextLinkOffset.x(),
                   jointInfo->nextLinkOffset.y(),
                   jointInfo->nextLinkOffset.z());
            jointInfo->rotation = rotationAll * jointInfo->rotation;
            jointInfo->axis = jointInfo->rotation * jointInfo->axis;
            jointInfo->nextLinkOffset = rotationAll * jointInfo->nextLinkOffset;
            printf("更新后关节 %s 旋转四元数: (%.4f, %.4f, %.4f, %.4f), 轴向: (%.4f, %.4f, %.4f), 偏移: (%.4f, %.4f, %.4f)\n",
                   jointInfo->name.c_str(),
                   jointInfo->rotation.w(),
                   jointInfo->rotation.x(),
                   jointInfo->rotation.y(),
                   jointInfo->rotation.z(),
                   jointInfo->axis.x(),
                   jointInfo->axis.y(),
                   jointInfo->axis.z(),
                   jointInfo->nextLinkOffset.x(),
                   jointInfo->nextLinkOffset.y(),
                   jointInfo->nextLinkOffset.z());
        }
        int jointIndex = jointList[index];
        JointsModel *jointInfo = &jointMap_[jointIndex];
        rotationAll = jointInfo->rotation;
        index++;
        linkIndex = jointInfo->child_link;
        colStruct = &collision_structures_[linkIndex];
        for (size_t i = 0; i < colStruct->obbModelList.size(); i++) {
            colStruct->obbModelList[i] = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i],
                Vector3d::Zero(),
                rotationAll);
        }
    }
    return 0;
}

int RobotDescription::InitJoint(const urdf::Model &urdf_model)
{
    int joint_index = 0;
    for (const auto& joint_pair : urdf_model.joints_) {
        urdf::JointConstSharedPtr joint = joint_pair.second;
        JointsModel joint_info;
        joint_info.name = joint->name;
        joint_info.type = joint->type;
        joint_info.parent_link = -1;
        joint_info.child_link = -1;
        if (joint->parent_link_name != "") {
            if (linkCollisionMap_.find(joint->parent_link_name) != linkCollisionMap_.end()) {
                joint_info.parent_link = linkCollisionMap_[joint->parent_link_name];
            } else {
                printf("Joint %s 的父链接 %s 未找到！", joint->name.c_str(), joint->parent_link_name.c_str());
                return -1;
            }
        }
        if (joint->child_link_name != "") {
            if (linkCollisionMap_.find(joint->child_link_name) != linkCollisionMap_.end()) {
                joint_info.child_link = linkCollisionMap_[joint->child_link_name];
            } else {
                printf("Joint %s 的子链接 %s 未找到！", joint->name.c_str(), joint->child_link_name.c_str());
                return -1;
            }
        }
        joint_info.nextLinkOffset = 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);
        joint_info.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->type != urdf::Joint::FIXED) {
            joint_info.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z);
            if (joint->limits != NULL) {
                joint_info.limit_lower = joint->limits->lower;
                joint_info.limit_upper = joint->limits->upper;
                joint_info.limit_v = joint->limits->velocity;
                joint_info.limit_e = joint->limits->effort;
            }
        } else {
            joint_info.axis = Vector3d::Zero();
            joint_info.limit_lower = 0.0;
            joint_info.limit_upper = 0.0;
            joint_info.limit_v = 0.0;
            joint_info.limit_e = 0.0;
        }
        jointMap_.push_back(joint_info);
        CollisionModel *parentColStruct = &collision_structures_[joint_info.parent_link];
        parentColStruct->jointIndexList.push_back(joint_index);
        parentColStruct->joint_count++;
        CollisionModel *childColStruct = &collision_structures_[joint_info.child_link];
        childColStruct->parent_link_index = joint_info.parent_link;

        joint_index++;
    }
    InitJointModel();
    return 0;
}

static OBB GetMeshesBounds(const std::string& filename)
{
    OBB obb;
    shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
    if (!mesh) {
        printf( "无法加载网格文件: %s", filename.c_str());
        return obb;
    }
    // 计算边界盒
    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];

    for (unsigned int i = 1; i < mesh->vertex_count; ++i) {
        if (min_x > mesh->vertices[3*i]) {
            min_x = mesh->vertices[3*i];
        }
        if (min_y > mesh->vertices[3*i + 1]) {
            min_y = mesh->vertices[3*i + 1];
        }
        if (min_z > mesh->vertices[3*i + 2]) {
            min_z = mesh->vertices[3*i + 2];
        }
        if (max_x < mesh->vertices[3*i]) {
            max_x = mesh->vertices[3*i];
        }
        if (max_y < mesh->vertices[3*i + 1]) {
            max_y = mesh->vertices[3*i + 1];
        }
        if (max_z < mesh->vertices[3*i + 2]) {
            max_z = mesh->vertices[3*i + 2];
        }
    }
    
    std::vector<Vector3d> points = {
        Vector3d(min_x, min_y, min_z),
        Vector3d(max_x, min_y, min_z),
        Vector3d(min_x, max_y, min_z),
        Vector3d(max_x, max_y, min_z),
        Vector3d(min_x, min_y, max_z),
        Vector3d(max_x, min_y, max_z),
        Vector3d(min_x, max_y, max_z),
        Vector3d(max_x, max_y, max_z)
    };
    obb = gjk_interface::createOBBFromPoints(points);

    // 释放网格资源
    delete mesh;
    return obb;
}

int RobotDescription::GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct)
{
    shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
    if (!mesh) {
        printf("无法加载网格文件: %s\n", filename.c_str());
        return false;
    }

    std::vector<Eigen::Vector3d> eigen_points;

    std::vector<CgalPoint> points;

    for (unsigned int i = 0; i < mesh->vertex_count; ++i) {
        points.emplace_back(mesh->vertices[3*i], mesh->vertices[3*i + 1], mesh->vertices[3*i + 2]);
    }
    std::array<CgalPoint, 8> obb_points;
    CGAL::oriented_bounding_box(points, obb_points, CGAL::parameters::use_convex_hull(true));
    printf("计算%s得到的 OBB 顶点:\n", col_struct.link_name.c_str());
    for (const auto& pt : obb_points) {
        eigen_points.push_back(Eigen::Vector3d(pt.x(), pt.y(), pt.z()));
        printf("(%.4f, %.4f, %.4f)\n", pt.x(), pt.y(), pt.z());
    }

    /*UNUSED(filename);
    static std::map<std::string, std::array<Vector3d, 8>> pointMap = {
        {"LeftLink1",
            {Vector3d(0.0411, 0.0456, -0.1191), Vector3d(-0.0409, 0.0458, -0.1191), Vector3d(-0.0411, -0.0444, -0.1013), Vector3d(0.0409, -0.0446, -0.1013), Vector3d(0.0410, -0.0137, 0.0545), Vector3d(0.0412, 0.0765, 0.0366), Vector3d(-0.0408, 0.0767, 0.0366), Vector3d(-0.0410, -0.0135, 0.0545)}},
        {"LeftLink2",
            {Vector3d(0.2879, 0.0332, -0.0733), Vector3d(-0.0475, 0.0417, -0.0150), Vector3d(-0.0496, -0.0393, -0.0149), Vector3d(0.2858, -0.0478, -0.0732), Vector3d(0.3004, -0.0481, 0.0110), Vector3d(0.3025, 0.0329, 0.0109), Vector3d(-0.0329, 0.0414, 0.0691), Vector3d(-0.0349, -0.0396, 0.0692)}},
        {"LeftLink3",
            {Vector3d(0.0464, 0.0342, 0.0241), Vector3d(-0.0012, 0.0428, -0.0180), Vector3d(-0.0025, -0.1243, -0.0505), Vector3d(0.0451, -0.1329, -0.0084), Vector3d(0.0019, -0.1416, 0.0386), Vector3d(0.0032, 0.0254, 0.0710), Vector3d(-0.0443, 0.0340, 0.0289), Vector3d(-0.0456, -0.1331, -0.0036)}},
        {"LeftLink4",
            {Vector3d(0.0286, 0.0314, -0.0811), Vector3d(-0.0294, 0.0307, -0.0811), Vector3d(-0.0285, -0.0395, -0.0933), Vector3d(0.0295, -0.0388, -0.0933), Vector3d(0.0298, -0.0592, 0.0251), Vector3d(0.0289, 0.0110, 0.0372), Vector3d(-0.0291, 0.0103, 0.0372), Vector3d(-0.0282, -0.0600, 0.0251)}},
        {"LeftLink5",
            {Vector3d(0.0290, 0.0328, -0.0184), Vector3d(-0.0282, 0.0330, -0.0190), Vector3d(-0.0284, -0.1144, -0.0341), Vector3d(0.0288, -0.1145, -0.0335), Vector3d(0.0282, -0.1208, 0.0281), Vector3d(0.0284, 0.0265, 0.0432), Vector3d(-0.0288, 0.0266, 0.0427), Vector3d(-0.0290, -0.1207, 0.0275)}},
        {"LeftLink6",
            {Vector3d(0.0492, 0.0104, -0.0290), Vector3d(-0.0023, 0.0469, -0.0290), Vector3d(-0.0404, -0.0069, -0.0290), Vector3d(0.0111, -0.0434, -0.0290), Vector3d(0.0111, -0.0434, 0.0000), Vector3d(0.0492, 0.0104, 0.0000), Vector3d(-0.0023, 0.0469, 0.0000), Vector3d(-0.0404, -0.0069, 0.0000)}},
        {"RightLink1",
            {Vector3d(0.0411, 0.0456, -0.1191), Vector3d(-0.0409, 0.0458, -0.1191), Vector3d(-0.0411, -0.0444, -0.1013), Vector3d(0.0409, -0.0446, -0.1013), Vector3d(0.0410, -0.0137, 0.0545), Vector3d(0.0412, 0.0765, 0.0366), Vector3d(-0.0408, 0.0767, 0.0366), Vector3d(-0.0410, -0.0135, 0.0545)}},
        {"RightLink2",
            {Vector3d(0.3025, 0.0329, 0.0109), Vector3d(0.2879, 0.0332, -0.0733), Vector3d(0.2858, -0.0478, -0.0732), Vector3d(0.3004, -0.0481, 0.0110), Vector3d(-0.0349, -0.0396, 0.0692), Vector3d(-0.0329, 0.0414, 0.0691), Vector3d(-0.0475, 0.0417, -0.0150), Vector3d(-0.0496, -0.0393, -0.0149)}},
        {"RightLink3",
            {Vector3d(0.0464, 0.0342, 0.0241), Vector3d(-0.0012, 0.0428, -0.0180), Vector3d(-0.0025, -0.1243, -0.0505), Vector3d(0.0451, -0.1329, -0.0084), Vector3d(0.0019, -0.1416, 0.0386), Vector3d(0.0032, 0.0254, 0.0710), Vector3d(-0.0443, 0.0340, 0.0289), Vector3d(-0.0456, -0.1331, -0.0036)}},
        {"RightLink4",
            {Vector3d(0.0295, -0.0388, -0.0933), Vector3d(-0.0285, -0.0395, -0.0933), Vector3d(-0.0282, -0.0600, 0.0251), Vector3d(0.0298, -0.0592, 0.0251), Vector3d(0.0289, 0.0110, 0.0372), Vector3d(0.0286, 0.0314, -0.0811), Vector3d(-0.0294, 0.0307, -0.0811), Vector3d(-0.0291, 0.0103, 0.0372)}},
        {"RightLink5",
            {Vector3d(0.0288, -0.1145, -0.0335), Vector3d(-0.0284, -0.1144, -0.0341), Vector3d(-0.0290, -0.1207, 0.0275), Vector3d(0.0282, -0.1208, 0.0281), Vector3d(0.0284, 0.0265, 0.0432), Vector3d(0.0290, 0.0328, -0.0184), Vector3d(-0.0282, 0.0330, -0.0190), Vector3d(-0.0288, 0.0266, 0.0427)}},
        {"RightLink6",
            {Vector3d(0.0492, 0.0104, -0.0290), Vector3d(-0.0023, 0.0469, -0.0290), Vector3d(-0.0404, -0.0069, -0.0290), Vector3d(0.0111, -0.0434, -0.0290), Vector3d(0.0111, -0.0434, 0.0000), Vector3d(0.0492, 0.0104, 0.0000), Vector3d(-0.0023, 0.0469, 0.0000), Vector3d(-0.0404, -0.0069, 0.0000)}},
        {"base_link",
            {Vector3d(0.2085, 0.1895, -0.0856), Vector3d(-0.1905, 0.1895, -0.0880), Vector3d(-0.1905, -0.1895, -0.0880), Vector3d(0.2085, -0.1895, -0.0856), Vector3d(0.2071, -0.1895, 0.1679), Vector3d(0.2071, 0.1895, 0.1679), Vector3d(-0.1920, 0.1895, 0.1656), Vector3d(-0.1920, -0.1895, 0.1656)}},
        {"base_link_leftarm",
            {Vector3d(0.0661, -0.0353, 0.1385), Vector3d(0.0661, -0.0353, -0.0000), Vector3d(-0.0339, -0.0699, -0.0000), Vector3d(-0.0339, -0.0699, 0.1385), Vector3d(-0.0698, 0.0339, 0.1385), Vector3d(0.0302, 0.0685, 0.1385), Vector3d(0.0302, 0.0685, -0.0000), Vector3d(-0.0698, 0.0339, -0.0000)}},
        {"base_link_rightarm",
            {Vector3d(0.0661, -0.0353, -0.0000), Vector3d(0.0302, 0.0685, -0.0000), Vector3d(-0.0698, 0.0339, -0.0000), Vector3d(-0.0339, -0.0699, -0.0000), Vector3d(-0.0339, -0.0699, 0.1385), Vector3d(0.0661, -0.0353, 0.1385), Vector3d(0.0302, 0.0685, 0.1385), Vector3d(-0.0698, 0.0339, 0.1385)}},
        {"body_2_link",
            {Vector3d(0.1595, 0.0756, -0.0750), Vector3d(-0.1415, 0.0735, -0.0750), Vector3d(-0.1390, -0.2855, -0.0750), Vector3d(0.1620, -0.2834, -0.0750), Vector3d(0.1620, -0.2834, 0.5416), Vector3d(0.1595, 0.0756, 0.5416), Vector3d(-0.1415, 0.0735, 0.5416), Vector3d(-0.1390, -0.2855, 0.5416)}},
        {"body_link",
            {Vector3d(0.0593, 0.1034, -0.0010), Vector3d(-0.0842, 0.1024, -0.0010), Vector3d(-0.0828, -0.1036, -0.0010), Vector3d(0.0607, -0.1026, -0.0010), Vector3d(0.0607, -0.1026, 0.1600), Vector3d(0.0593, 0.1034, 0.1600), Vector3d(-0.0842, 0.1024, 0.1600), Vector3d(-0.0828, -0.1036, 0.1600)}},
        {"head_link",
            {Vector3d(0.0672, 0.1175, -0.0000), Vector3d(-0.0658, 0.1165, 0.0000), Vector3d(-0.0642, -0.1175, 0.0000), Vector3d(0.0689, -0.1165, -0.0000), Vector3d(0.0689, -0.1165, 0.2031), Vector3d(0.0672, 0.1175, 0.2031), Vector3d(-0.0658, 0.1165, 0.2031), Vector3d(-0.0642, -0.1175, 0.2031)}},
        {"left_behind_hip_link",
            {Vector3d(0.0626, 0.0721, 0.0288), Vector3d(-0.1058, 0.0721, -0.4340), Vector3d(-0.1058, 0.0000, -0.4340), Vector3d(0.0626, 0.0000, 0.0288), Vector3d(-0.0295, 0.0000, 0.0623), Vector3d(-0.0295, 0.0721, 0.0623), Vector3d(-0.1979, 0.0721, -0.4005), Vector3d(-0.1979, 0.0000, -0.4005)}},
        {"left_behind_leg_link",
            {Vector3d(0.0648, -0.0402, -0.0206), Vector3d(0.0648, 0.0235, -0.0206), Vector3d(-0.0927, 0.0235, -0.4532), Vector3d(-0.0927, -0.0402, -0.4532), Vector3d(-0.2204, -0.0402, -0.4068), Vector3d(-0.0629, -0.0402, 0.0259), Vector3d(-0.0629, 0.0235, 0.0259), Vector3d(-0.2204, 0.0235, -0.4068)}},
        {"left_behind_wheel_link",
            {Vector3d(0.0852, 0.0370, -0.0945), Vector3d(-0.0944, 0.0370, -0.0853), Vector3d(-0.0944, -0.0125, -0.0853), Vector3d(0.0852, -0.0125, -0.0945), Vector3d(0.0944, -0.0125, 0.0853), Vector3d(0.0944, 0.0370, 0.0853), Vector3d(-0.0852, 0.0370, 0.0945), Vector3d(-0.0852, -0.0125, 0.0945)}},
        {"left_front_hip_link",
            {Vector3d(0.1987, 0.1315, -0.3903), Vector3d(0.1020, 0.1315, -0.4259), Vector3d(0.1020, 0.0190, -0.4259), Vector3d(0.1987, 0.0190, -0.3903), Vector3d(0.0305, 0.0190, 0.0661), Vector3d(0.0305, 0.1315, 0.0661), Vector3d(-0.0661, 0.1315, 0.0305), Vector3d(-0.0661, 0.0190, 0.0305)}},
        {"left_front_knee_link",
            {Vector3d(0.2234, 0.0230, -0.3181), Vector3d(0.1136, 0.0230, -0.3665), Vector3d(0.1136, -0.0620, -0.3665), Vector3d(0.2234, -0.0620, -0.3181), Vector3d(0.0518, -0.0620, 0.0708), Vector3d(0.0518, 0.0230, 0.0708), Vector3d(-0.0580, 0.0230, 0.0224), Vector3d(-0.0580, -0.0620, 0.0224)}},
        {"left_front_roll_link",
            {Vector3d(0.0960, 0.0565, 0.0000), Vector3d(-0.0060, 0.0565, -0.1020), Vector3d(-0.0060, -0.0285, -0.1020), Vector3d(0.0960, -0.0285, 0.0000), Vector3d(-0.0060, -0.0285, 0.1020), Vector3d(-0.0060, 0.0565, 0.1020), Vector3d(-0.1080, 0.0565, -0.0000), Vector3d(-0.1080, -0.0285, -0.0000)}},
        {"left_front_wheel_link",
            {Vector3d(0.1223, 0.0000, -0.0336), Vector3d(-0.0336, 0.0000, -0.1223), Vector3d(-0.0336, -0.0570, -0.1223), Vector3d(0.1223, -0.0570, -0.0336), Vector3d(0.0336, -0.0570, 0.1223), Vector3d(0.0336, 0.0000, 0.1223), Vector3d(-0.1223, 0.0000, 0.0336), Vector3d(-0.1223, -0.0570, 0.0336)}},
        {"right_behind_hip_link",
            {Vector3d(-0.1058, -0.0000, -0.4340), Vector3d(-0.1979, -0.0000, -0.4005), Vector3d(-0.1979, -0.0721, -0.4005), Vector3d(-0.1058, -0.0721, -0.4340), Vector3d(0.0626, -0.0721, 0.0288), Vector3d(0.0626, -0.0000, 0.0288), Vector3d(-0.0295, -0.0000, 0.0623), Vector3d(-0.0295, -0.0721, 0.0623)}},
        {"right_behind_leg_link",
            {Vector3d(0.0648, 0.0402, -0.0206), Vector3d(-0.0927, 0.0402, -0.4532), Vector3d(-0.0927, -0.0235, -0.4532), Vector3d(0.0648, -0.0235, -0.0206), Vector3d(-0.0629, -0.0235, 0.0259), Vector3d(-0.0629, 0.0402, 0.0259), Vector3d(-0.2204, 0.0402, -0.4068), Vector3d(-0.2204, -0.0235, -0.4068)}},
        {"right_behind_wheel_link",
            {Vector3d(0.1259, 0.0125, -0.0183), Vector3d(-0.0182, 0.0125, -0.1259), Vector3d(-0.0182, -0.0370, -0.1259), Vector3d(0.1259, -0.0370, -0.0183), Vector3d(0.0182, -0.0370, 0.1259), Vector3d(0.0182, 0.0125, 0.1259), Vector3d(-0.1259, 0.0125, 0.0183), Vector3d(-0.1259, -0.0370, 0.0183)}},
        {"right_front_hip_link",
            {Vector3d(0.1956, -0.0190, -0.3918), Vector3d(0.0987, -0.0190, -0.4267), Vector3d(0.0987, -0.1320, -0.4267), Vector3d(0.1956, -0.1320, -0.3918), Vector3d(0.0310, -0.1320, 0.0659), Vector3d(0.0310, -0.0190, 0.0659), Vector3d(-0.0659, -0.0190, 0.0310), Vector3d(-0.0659, -0.1320, 0.0310)}},
        {"right_front_knee_link",
            {Vector3d(0.2234, 0.0620, -0.3181), Vector3d(0.1136, 0.0620, -0.3665), Vector3d(0.1136, -0.0230, -0.3665), Vector3d(0.2234, -0.0230, -0.3181), Vector3d(0.0518, -0.0230, 0.0708), Vector3d(0.0518, 0.0620, 0.0708), Vector3d(-0.0580, 0.0620, 0.0224), Vector3d(-0.0580, -0.0230, 0.0224)}},
        {"right_front_roll_link",
            {Vector3d(0.0960, 0.0285, 0.0000), Vector3d(-0.0060, 0.0285, -0.1020), Vector3d(-0.0060, -0.0565, -0.1020), Vector3d(0.0960, -0.0565, 0.0000), Vector3d(-0.0060, -0.0565, 0.1020), Vector3d(-0.0060, 0.0285, 0.1020), Vector3d(-0.1080, 0.0285, -0.0000), Vector3d(-0.1080, -0.0565, -0.0000)}},
        {"right_front_wheel_link",
            {Vector3d(0.1262, 0.0528, -0.0125), Vector3d(-0.0125, 0.0528, -0.1262), Vector3d(-0.0125, -0.0042, -0.1262), Vector3d(0.1262, -0.0042, -0.0125), Vector3d(0.0125, -0.0042, 0.1262), Vector3d(0.0125, 0.0528, 0.1262), Vector3d(-0.1262, 0.0528, 0.0125), Vector3d(-0.1262, -0.0042, 0.0125)}},

    };
    std::array<Vector3d, 8> eigen_points_arr = pointMap.at(col_struct.link_name);
    std::vector<Eigen::Vector3d> eigen_points(eigen_points_arr.begin(), eigen_points_arr.end());*/

    OBB boundsObb = GetMeshesBounds(filename);
    OBB obb = gjk_interface::createOBBFromPoints(eigen_points);
    if (boundsObb.halfExtent[0] > 0) {
        double v1 = boundsObb.halfExtent[0] * boundsObb.halfExtent[1] * boundsObb.halfExtent[2];
        double v2 = obb.halfExtent[0] * obb.halfExtent[1] * obb.halfExtent[2];
        printf("网格文件: %s 计算得到的 边界 体积: %.6f, 计算 OBB 体积: %.6f\n", filename.c_str(), v1 * 8, v2 * 8);
    }
    col_struct.obbModelList.push_back(obb);
    return 0;
}

int RobotDescription::InitCollisionStructureList(const urdf::Model &urdf_model)
{
    int result = 0;
    int linkIndex = 0;
    maxObbCnt = 0;
    for (const auto& link_pair : urdf_model.links_) {
        // link_pair是一个键值对: 键为链接名称，值为链接指针
        urdf::LinkConstSharedPtr link = link_pair.second;
        for (const auto& collision : link->collision_array) {
            CollisionModel colStruct;
            colStruct.link_name = link->name;
            colStruct.geometry_type = collision->geometry->type;
            linkCollisionMap_[link->name] = linkIndex;
            int obbCnt = 1;
            if (collision->geometry->type == urdf::Geometry::BOX) {
                urdf::Box* box = dynamic_cast<urdf::Box*>(collision->geometry.get());
                std::vector<Eigen::Vector3d> obb_points = {
                    Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2),
                    Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2),
                    Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2),
                    Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2),
                    Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2),
                    Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2),
                    Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, box->dim.z / 2),
                    Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, box->dim.z / 2)
                };
                OBB obb = gjk_interface::createOBBFromPoints(obb_points);
                colStruct.obbModelList.push_back(obb);
            } 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 != 0) {
                    printf("Failed to get mesh dimensions for %s\n", mesh->filename.c_str());
                    return result;
                }
            }
            for (int i = 0; i < obbCnt; i++) {
                colStruct.obbObbIndexList.push_back(maxObbCnt);
                maxObbCnt++;
            }
            colStruct.link_index = linkIndex;
            colStruct.joint_count = 0;
            if (link->name == "base_link") {
                rootLinkIndex_ = linkIndex;
                colStruct.parent_link_index = -1;
                colStruct.rotation = Quaterniond::Identity();
                colStruct.offset = Vector3d::Zero();
                printf("设置根链接: %s, 索引: %d 四元数: (%.4f, %.4f, %.4f, %.4f)\n", colStruct.link_name.c_str(), linkIndex,
                       colStruct.rotation.x(), colStruct.rotation.y(), colStruct.rotation.z(), colStruct.rotation.w());
            }
            collision_structures_.push_back(colStruct);
            linkIndex++;
        }
    }
    return result;
}

void RobotDescription::UpdatePolyhedrons()
{
    int linkIndex = rootLinkIndex_;
    CollisionModel *colStruct = &collision_structures_[linkIndex];
    std::vector<int> jointList;
    int index = 0;
    int maxJointCount = 0;
    while (colStruct->joint_count > 0 || index < maxJointCount) {
        for (int i = 0; i < colStruct->joint_count; i++) {
            jointList.push_back(colStruct->jointIndexList[i]);
            maxJointCount++;
        }
        int jointIndex = jointList[index];
        JointsModel *jointInfo = &jointMap_[jointIndex];
        Quaterniond rotation = collision_structures_[jointInfo->parent_link].rotation;
        Vector3d offset = collision_structures_[jointInfo->parent_link].offset;

        index++;
        linkIndex = jointInfo->child_link;
        colStruct = &collision_structures_[linkIndex];

        offset = offset + rotation * jointInfo->nextLinkOffset;
        colStruct->rotation = rotation;
        colStruct->offset = offset;

        for (size_t i = 0; i < colStruct->obbModelList.size(); i++) {
            OBB obb = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i],
                offset,
                rotation);
            printf("更新链接 %s 的 OBB:\n", colStruct->link_name.c_str());
            printf("中心点: (%.4f, %.4f, %.4f)\n", obb.center(0), obb.center(1), obb.center(2));
            /*printf("轴向:\n");
            for (int j = 0; j < 3; j++) {
                printf("  (%.4f, %.4f, %.4f)\n", obb.axis[j].x(), obb.axis[j].y(), obb.axis[j].z());
            }
            printf("半尺寸: (%.4f, %.4f, %.4f)\n", obb.halfExtent[0], obb.halfExtent[1], obb.halfExtent[2]);*/
        }
    }
}

int RobotDescription::InitLinkCollisionDetectMatrix(const std::string& srdf_str) 
{
    g_linkCollisionDetectMatrix = new bool[maxObbCnt * maxObbCnt];
    for (int i = 0; i < maxObbCnt; ++i) {
        for (int j = i + 1; j < maxObbCnt; ++j) {
            g_linkCollisionDetectMatrix[i * maxObbCnt + j] = true;
        }
    }
    tinyxml2::XMLDocument doc;
    tinyxml2::XMLError error = doc.Parse(srdf_str.c_str());
    if (error != tinyxml2::XML_SUCCESS) {
        printf("Failed to parse SRDF: %s, length: %ld\n", doc.ErrorStr(), srdf_str.length());
        return -1;
    }

    // 获取根节点<robot>
    tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
    if (!robot_root) {
        printf("SRDF has no <robot> root node\n");
        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")) {
        std::string link1 = dc_elem->Attribute("link1");
        std::string link2 = dc_elem->Attribute("link2");
        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) {
            CollisionModel* colStruct1 = &collision_structures_[link1Index];
            CollisionModel* colStruct2 = &collision_structures_[link2Index];
            for (int obb1Idx : colStruct1->obbObbIndexList) {
                for (int obb2Idx : colStruct2->obbObbIndexList) {
                    g_linkCollisionDetectMatrix[obb1Idx * maxObbCnt + obb2Idx] = false;
                    g_linkCollisionDetectMatrix[obb2Idx * maxObbCnt + obb1Idx] = false;
                }
            }
        }
        disable_count++;
    }
    printf("Initialized link collision detect matrix, disabled %d collision pairs. total %d\n", disable_count * 2, maxObbCnt * maxObbCnt);
    /*for (int i = 0; i < maxObbCnt; ++i) {
        for (int j = 0; j < maxObbCnt; ++j) {
            if (g_linkCollisionDetectMatrix[i * maxObbCnt + j]) {
                printf("true ");
            } else {
                printf("false ");
            }
        }
        printf("\n");
    }*/
    return 0;
}

RobotDescription::RobotDescription(const std::string &urdf, const std::string &srdf)
{
    urdf::Model urdf_model;
    if (!urdf_model.initString(urdf)) {
        printf("无法解析URDF模型\n");
        return;
    }
    InitCollisionStructureList(urdf_model);
    InitJoint(urdf_model);
    UpdatePolyhedrons();
    InitLinkCollisionDetectMatrix(srdf);
}

RobotDescription::~RobotDescription()
{
    // 清理资源（如果有的话）
    delete g_linkCollisionDetectMatrix;
}

/**
 * @brief 以 std::string 形式读取文件全部内容
 * @param file_path 文件绝对路径或相对路径
 * @return 文件内容的 std::string
 * @throws 若文件不存在、无法打开或读取失败，抛出 std::runtime_error
 */
std::string readFileToString(const std::string& file_path) {
    // 1. 打开文件（二进制模式避免换行符转换，确保内容完整）
    std::ifstream file(file_path, std::ios::in | std::ios::binary);
    if (!file.is_open()) {
        throw std::runtime_error("无法打开文件：" + file_path + "（可能路径错误或权限不足）\n");
    }

    // 2. 读取文件内容到 string
    std::string content;
    try {
        // 定位到文件末尾，获取文件大小
        file.seekg(0, std::ios::end);
        // 预留内存（避免多次扩容，提升效率）
        content.reserve(static_cast<size_t>(file.tellg()));
        // 回到文件开头
        file.seekg(0, std::ios::beg);

        // 用迭代器读取全部内容（高效且简洁）
        content.assign(std::istreambuf_iterator<char>(file), std::istreambuf_iterator<char>());
    } catch (const std::exception& e) {
        file.close();  // 异常时确保文件关闭
        throw std::runtime_error("读取文件失败：" + file_path + "（错误信息：" + e.what() + "）\n");
    }

    // 3. 关闭文件（ifstream 析构时会自动关闭，但显式关闭更严谨）
    file.close();

    // 4. 可选：检查内容是否为空（根据需求决定是否抛出异常）
    if (content.empty()) {
        std::cerr << "[警告] 文件内容为空：" << file_path << std::endl;
        // 若需要严格检查，可抛出异常：
        // throw std::runtime_error("文件内容为空：" + file_path);
    }

    return content;
}

void searchRecursive(const fs::path& dir, const std::string& target, std::string& result, std::string& result_dir) {
    for (const auto& entry : fs::directory_iterator(dir)) {
        if (entry.is_directory()) {
            searchRecursive(entry.path(), target, result, result_dir);  // 递归子目录
            if (!result.empty()) return;  // 找到后提前返回
        } else if (entry.is_regular_file() && entry.path().filename() == target) {
            result = entry.path().string();
            result_dir = entry.path().parent_path().string();
            return;
        }
    }
}

std::string findFileInCurrentDir(const std::string& target_filename, std::string& result_dir) {
    // 获取当前工作目录
    std::string current_dir;
    try {
        current_dir = fs::current_path().string();
    } catch (const fs::filesystem_error& e) {
        throw std::runtime_error("无法获取当前目录：" + std::string(e.what()));
    }

    // 遍历当前目录下的所有文件（不递归子目录）
    try {
        std::string found_path;
        searchRecursive(current_dir, target_filename, found_path, result_dir);
        if (!found_path.empty()) {
            return found_path;  // 找到文件，返回路径
        }
    } catch (const fs::filesystem_error& e) {
        throw std::runtime_error("遍历目录失败：" + std::string(e.what()));
    }

    // 未找到文件
    return "";
}

std::string GetWriteFileName(const std::string& dir, const std::string& filename) {
    try {
        // 1. 检查并创建目录
        if (!fs::exists(dir)) {
            if (!fs::create_directories(dir)) {
                throw std::runtime_error("无法创建目录：" + dir);
            }
            std::cout << "已创建目录：" << dir << std::endl;
        }

        // 2. 拼接完整文件路径
        fs::path file_path = fs::path(dir) / filename;
        std::string full_path = file_path.string();
        return full_path;
    } catch (const fs::filesystem_error& e) {
        throw std::runtime_error("文件系统错误：" + std::string(e.what()));
    } catch (const std::exception& e) {
        throw std::runtime_error("操作失败：" + std::string(e.what()));
    }
    return "";
}

void WriteXmlFile(const std::string& file_path, RobotDescription& robot_description) {
    XMLDocument doc;

    // 添加XML声明（可选，但推荐）
    XMLDeclaration* decl = doc.NewDeclaration();
    doc.InsertFirstChild(decl);

    // 创建根节点 <robot>
    XMLElement* root = doc.NewElement("robot");
    root->SetAttribute("name", "FHrobot");
    root->SetAttribute("rootLinkIndex_", robot_description.rootLinkIndex_);
    root->SetAttribute("maxObbCnt", robot_description.maxObbCnt);
    root->SetAttribute("collision_structure_count", static_cast<int>(robot_description.collision_structures_.size()));
    root->SetAttribute("joint_count", static_cast<int>(robot_description.jointMap_.size()));
    doc.InsertEndChild(root);  // 将根节点添加到文档

    for (const auto& col_struct : robot_description.collision_structures_) {
        XMLElement* collisionElem = doc.NewElement("collision_model");
        collisionElem->SetAttribute("link_name", col_struct.link_name.c_str());
        collisionElem->SetAttribute("link_index", col_struct.link_index);
        collisionElem->SetAttribute("geometry_type", col_struct.geometry_type);
        collisionElem->SetAttribute("mesh_filename", col_struct.mesh_filename.c_str());
        collisionElem->SetAttribute("parent_link_index", col_struct.parent_link_index);
        collisionElem->SetAttribute("offset_x", col_struct.offset.x());
        collisionElem->SetAttribute("offset_y", col_struct.offset.y());
        collisionElem->SetAttribute("offset_z", col_struct.offset.z());
        collisionElem->SetAttribute("rotation_x", col_struct.rotation.x());
        collisionElem->SetAttribute("rotation_y", col_struct.rotation.y());
        collisionElem->SetAttribute("rotation_z", col_struct.rotation.z());
        collisionElem->SetAttribute("rotation_w", col_struct.rotation.w());

        collisionElem->SetAttribute("obb_count", static_cast<int>(col_struct.obbModelList.size()));
        for (size_t i = 0; i < col_struct.obbModelList.size(); ++i) {
            const OBB& obb = col_struct.obbModelList[i];
            XMLElement* obbElem = doc.NewElement("obb");
            obbElem->SetAttribute("index", col_struct.obbObbIndexList[i]);
            obbElem->SetAttribute("center_x", obb.center.x());
            obbElem->SetAttribute("center_y", obb.center.y());
            obbElem->SetAttribute("center_z", obb.center.z());
            obbElem->SetAttribute("half_extent_x", obb.halfExtent[0]);
            obbElem->SetAttribute("half_extent_y", obb.halfExtent[1]);
            obbElem->SetAttribute("half_extent_z", obb.halfExtent[2]);
            obbElem->SetAttribute("axis_0_x", obb.axis[0].x());
            obbElem->SetAttribute("axis_0_y", obb.axis[0].y());
            obbElem->SetAttribute("axis_0_z", obb.axis[0].z());
            obbElem->SetAttribute("axis_1_x", obb.axis[1].x());
            obbElem->SetAttribute("axis_1_y", obb.axis[1].y());
            obbElem->SetAttribute("axis_1_z", obb.axis[1].z());
            obbElem->SetAttribute("axis_2_x", obb.axis[2].x());
            obbElem->SetAttribute("axis_2_y", obb.axis[2].y());
            obbElem->SetAttribute("axis_2_z", obb.axis[2].z());

            obbElem->SetAttribute("max_radius", obb.maxRadius);
            obbElem->SetAttribute("self_min_project_x", obb.selfMinProject[0]);
            obbElem->SetAttribute("self_min_project_y", obb.selfMinProject[1]);
            obbElem->SetAttribute("self_min_project_z", obb.selfMinProject[2]);
            obbElem->SetAttribute("self_max_project_x", obb.selfMaxProject[0]);
            obbElem->SetAttribute("self_max_project_y", obb.selfMaxProject[1]);
            obbElem->SetAttribute("self_max_project_z", obb.selfMaxProject[2]);
            // 可根据需要添加轴向信息
            collisionElem->InsertEndChild(obbElem);
        }

        collisionElem->SetAttribute("joint_index_count", static_cast<int>(col_struct.jointIndexList.size()));
        for (size_t i = 0; i < col_struct.jointIndexList.size(); ++i) {
            XMLElement* jointIndexElem = doc.NewElement("joint_index");
            jointIndexElem->SetAttribute("index", col_struct.jointIndexList[i]);
            collisionElem->InsertEndChild(jointIndexElem);
        }

        root->InsertEndChild(collisionElem);
    }

    for (size_t i = 0; i < robot_description.jointMap_.size(); ++i) {
        const JointsModel& joint = robot_description.jointMap_[i];
        XMLElement* jointElem = doc.NewElement("joint_model");
        jointElem->SetAttribute("joint_name", joint.name.c_str());
        jointElem->SetAttribute("parent_link", joint.parent_link);
        jointElem->SetAttribute("child_link", joint.child_link);
        jointElem->SetAttribute("nextLinkOffset_x", joint.nextLinkOffset.x());
        jointElem->SetAttribute("nextLinkOffset_y", joint.nextLinkOffset.y());
        jointElem->SetAttribute("nextLinkOffset_z", joint.nextLinkOffset.z());
        jointElem->SetAttribute("joint_index", i);
        jointElem->SetAttribute("type", joint.type);
        jointElem->SetAttribute("axis_x", joint.axis.x());
        jointElem->SetAttribute("axis_y", joint.axis.y());
        jointElem->SetAttribute("axis_z", joint.axis.z());
        jointElem->SetAttribute("limit_lower", joint.limit_lower);
        jointElem->SetAttribute("limit_upper", joint.limit_upper);
        jointElem->SetAttribute("limit_effort", joint.limit_e);
        jointElem->SetAttribute("limit_velocity", joint.limit_v);
        jointElem->SetAttribute("rotation_x", joint.rotation.x());
        jointElem->SetAttribute("rotation_y", joint.rotation.y());
        jointElem->SetAttribute("rotation_z", joint.rotation.z());
        jointElem->SetAttribute("rotation_w", joint.rotation.w());
        root->InsertEndChild(jointElem);
    }

    // 4. 将XML内容写入文件
    XMLError error = doc.SaveFile(file_path.c_str());
    if (error != XML_SUCCESS) {
        throw std::runtime_error("写入XML文件失败:" + file_path + "（错误码：" + std::to_string(error) + "）");
    }

    printf("成功写入XML文件: %s\n", file_path.c_str());
}

int main(int argc, char *argv[])
{
    UNUSED(argc);
    UNUSED(argv);
    // 示例URDF和SRDF字符串
    std::string urdf_dir = "";
    std::string srdf_dir = "";
    std::string urdf_string = readFileToString(findFileInCurrentDir("FHrobot.urdf", urdf_dir));
    std::string srdf_string = readFileToString(findFileInCurrentDir("FHrobot.srdf", srdf_dir));

    printf("URDF路径: %s\n", urdf_dir.c_str());
    printf("SRDF路径: %s\n", srdf_dir.c_str());

    // 创建RobotDescription对象
    RobotDescription robot_description(urdf_string, srdf_string);

    std::string output_file = GetWriteFileName(urdf_dir, "FHrobot.xml");

    WriteXmlFile(output_file, robot_description);
    return 0;
}