#include "Robot.h"
#include <iostream>
#include <stdexcept>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/tree.hpp>

// #include <cmath>
#include <algorithm>
#include <kdl/path_roundedcomposite.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <regex>

using namespace KDL;

using namespace std;

Robot::Robot() : m_initialized(false), fkSolver(nullptr), ikVelSolver(nullptr),
                 ikSolverNR(nullptr), ikSolverLMA(nullptr)
{
    // 构造函数中显式初始化全部求解器指针。
}
Robot::Robot(const std::string &urdfString) : m_initialized(false),
                                              fkSolver(nullptr),
                                              ikVelSolver(nullptr),
                                              ikSolverNR(nullptr),
                                              ikSolverLMA(nullptr)
{
    initRobot(urdfString);
}

Robot::~Robot()
{
    delete fkSolver;
    delete ikVelSolver;
    delete ikSolverNR;
    delete ikSolverLMA;
}

// 根据 URDF 初始化机器人，并提取关节与 child link 的 UUID 映射。
bool Robot::initRobot(const std::string &urdfString)
{
    try
    {
        KDL::Tree tree;

        // 从 URDF 字符串解析 KDL Tree。
        if (!kdl_parser::treeFromString(urdfString, tree))
        {
            std::cerr << "Failed to parse URDF from string" << std::endl;
            return false;
        }

        // 提取 base 到 tool0 的运动学链。
        if (!tree.getChain("base", "tool0", kinematicChain))
        {
            std::cerr << "Failed to get chain from base to tool0" << std::endl;
            return false;
        }

        // 当前逻辑只支持 6 轴机器人链。
        if (kinematicChain.getNrOfJoints() != 6)
        {
            std::cerr << "Robot chain must have exactly 6 joints, got: "
                      << kinematicChain.getNrOfJoints() << std::endl;
            return false;
        }

        // 解析 joint 到 child link UUID 的映射。
        if (!parseJointChildLinkUuidsFromUrdf(urdfString))
        {
            std::cerr << "Warning: Failed to parse joint child link UUIDs from URDF" << std::endl;
        }

        // 初始化 FK / IK 求解器。
        fkSolver = new KDL::ChainFkSolverPos_recursive(kinematicChain);
        ikVelSolver = new KDL::ChainIkSolverVel_pinv(kinematicChain);
        ikSolverNR = new KDL::ChainIkSolverPos_NR(kinematicChain, *fkSolver, *ikVelSolver, 100, 1e-6);

        // LMA 求解器按调用时动态构造，便于传入不同收敛参数。
        m_initialized = true;
        std::cout << "Robot initialized successfully with "
                  << kinematicChain.getNrOfJoints() << " joints" << std::endl;
        numberOfJoints = getNumberOfJoints();
        return true;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception during robot initialization: " << e.what() << std::endl;
        return false;
    }
}
// 根据序号生成标准关节名。
std::string Robot::getJointNameByIndex(int jointIndex)
{
    if (jointIndex < 1)
    {
        return "";
    }

    std::stringstream ss;
    ss << "joint_" << jointIndex;
    return ss.str();
}

// 根据关节序号获取 child link UUID。
std::string Robot::getJointUuidByIndex(int jointIndex) const
{
    std::string jointName = getJointNameByIndex(jointIndex);
    if (jointName.empty())
    {
        return "";
    }

    return getJointChildLinkUuid(jointName);
}

// 获取当前已经解析到的全部关节序号。
std::vector<int> Robot::getAvailableJointIndices() const
{
    std::vector<int> indices;

    for (const auto &pair : jointChildLinkUuidMap)
    {
        const std::string &jointName = pair.first;

        // 只统计 `joint_X` 这类标准格式。
        if (jointName.find("joint_") == 0)
        {
            try
            {
                // 取出序号部分。
                std::string indexStr = jointName.substr(6); // "joint_".length() = 6
                int index = std::stoi(indexStr);
                indices.push_back(index);
            }
            catch (const std::exception &e)
            {
                // 非数字后缀直接忽略。
                continue;
            }
        }
    }

    // 排序并去重。
    std::sort(indices.begin(), indices.end());
    indices.erase(std::unique(indices.begin(), indices.end()), indices.end());

    return indices;
}
std::string Robot::findJointUuidByAnyFormat(int jointIndex) const
{
    if (jointIndex < 1)
        return "";

    // 兼容多种可能的关节命名格式。
    std::vector<std::string> possibleNames;

    // 格式 1: `joint_X`
    std::stringstream ss1;
    ss1 << "joint_" << jointIndex;
    possibleNames.push_back(ss1.str());

    // 格式 2: `jointX`
    std::stringstream ss2;
    ss2 << "joint" << jointIndex;
    possibleNames.push_back(ss2.str());

    // 格式 3: `joint_XX`
    std::stringstream ss3;
    ss3 << "joint_" << std::setw(2) << std::setfill('0') << jointIndex;
    possibleNames.push_back(ss3.str());

    // 逐个尝试匹配。
    for (const auto &name : possibleNames)
    {
        auto it = jointChildLinkUuidMap.find(name);
        if (it != jointChildLinkUuidMap.end())
        {
            return it->second;
        }
    }

    return "";
}
// 解析 URDF 中的 joint 与 child link UUID 映射。
bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
{
    try
    {
        jointChildLinkUuidMap.clear();

        // 使用正则提取 joint 标签及其 child link UUID。
        // `[\\s\\S]*?` 用于跨行匹配 joint 标签内部内容。
        std::regex jointRegex(R"(<joint\s+name=\"([^\"]+)\"[^>]*>[\\s\\S]*?<child\s+link=\"([^\"]+)\"\s+uuid=\"([^\"]+)\")");
        std::smatch matches;
        std::string::const_iterator searchStart(urdfString.cbegin());

        // 输出解析过程，便于排查 URDF 结构问题。
        std::cout << "Parsing URDF for joint child link UUIDs..." << std::endl;

        bool foundAny = false;
        while (std::regex_search(searchStart, urdfString.cend(), matches, jointRegex))
        {
            if (matches.size() == 4)
            {
                std::string jointName = matches[1].str();
                std::string childLinkName = matches[2].str();
                std::string uuid = matches[3].str();
                jointChildLinkUuidMap[jointName] = uuid;

                // 输出匹配结果。
                std::cout << "Found joint: " << jointName
                          << " -> Child link: " << childLinkName
                          << " -> UUID: " << uuid << std::endl;
                foundAny = true;
            }
            searchStart = matches.suffix().first;
        }

        // 如果首轮没有命中，则退化为更宽松的解析方式。
        if (!foundAny)
        {
            std::cout << "Trying alternative parsing method..." << std::endl;

            // 方案二：分别匹配 joint 名称和 child link。
            std::regex jointNameRegex(R"(<joint\s+name=\"([^\"]+)\")");
            std::regex childLinkRegex(R"(<child\s+link=\"([^\"]+)\"\s+uuid=\"([^\"]+)\")");

            std::smatch nameMatches;
            std::smatch childMatches;

            std::string::const_iterator nameStart = urdfString.cbegin();
            while (std::regex_search(nameStart, urdfString.cend(), nameMatches, jointNameRegex))
            {
                if (nameMatches.size() == 2)
                {
                    std::string jointName = nameMatches[1].str();

                    // 在 joint 标签范围内继续查找 child link。
                    std::string::const_iterator jointStart = nameMatches[0].first;
                    std::string::const_iterator jointEnd;

                    // 找到 joint 标签结束位置。
                    std::regex jointEndRegex(R"(</joint>)");
                    std::smatch endMatch;
                    if (std::regex_search(jointStart, urdfString.cend(), endMatch, jointEndRegex))
                    {
                        jointEnd = endMatch[0].second;

                        // 在 joint 片段中提取 child link UUID。
                        std::string jointContent(jointStart, jointEnd);
                        if (std::regex_search(jointContent, childMatches, childLinkRegex))
                        {
                            if (childMatches.size() == 3)
                            {
                                std::string childLinkName = childMatches[1].str();
                                std::string uuid = childMatches[2].str();
                                jointChildLinkUuidMap[jointName] = uuid;

                                std::cout << "Found joint: " << jointName
                                          << " -> Child link: " << childLinkName
                                          << " -> UUID: " << uuid << std::endl;
                                foundAny = true;
                            }
                        }
                    }

                    nameStart = nameMatches.suffix().first;
                }
            }
        }

        // 如果最终仍未找到任何关节映射，则输出 URDF 片段辅助排查。
        if (jointChildLinkUuidMap.empty())
        {
            std::cerr << "No joint child link UUIDs found in URDF" << std::endl;

            // 仅打印前 500 个字符，避免日志过长。
            std::cout << "First 500 characters of URDF:" << std::endl;
            std::cout << urdfString.substr(0, 500) << std::endl;

            return false;
        }

        std::cout << "Successfully parsed " << jointChildLinkUuidMap.size()
                  << " joint child link UUIDs" << std::endl;
        return true;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception parsing joint child link UUIDs: " << e.what() << std::endl;
        return false;
    }
}
// 根据关节名称获取 child link UUID。
std::string Robot::getJointChildLinkUuid(const std::string &jointName) const
{
    auto it = jointChildLinkUuidMap.find(jointName);
    if (it != jointChildLinkUuidMap.end())
    {
        return it->second;
    }
    return ""; // 返回空字符串表示未找到。
}

bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double resultJoints[6])
{
    if (!m_initialized)
    {
        std::cerr << "Robot not initialized" << std::endl;
        return false;
    }

    try
    {
        // 将输入位姿转换为 KDL Frame。
        KDL::Vector position(pose[0], pose[1], pose[2]);
        KDL::Rotation rotation = KDL::Rotation::Quaternion(pose[3], pose[4], pose[5], pose[6]);
        KDL::Frame targetFrame(rotation, position);

        KDL::JntArray initialJoints(6);
        for (int i = 0; i < 6; i++)
        {
            initialJoints(i) = iniJ[i];
        }

        KDL::JntArray result(6);

        // 执行 NR 逆运动学求解。
        int status = ikSolverNR->CartToJnt(initialJoints, targetFrame, result);

        if (status >= 0)
        {
            // 将结果写回输出数组。
            for (int i = 0; i < 6; i++)
            {
                resultJoints[i] = result(i);
            }
            return true;
        }
        else
        {
            std::cerr << "NR 逆运动学求解失败，状态码: " << status << std::endl;
            return false;
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "NR 逆运动学求解异常: " << e.what() << std::endl;
        return false;
    }
}

bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double resultJoints[6],
                            double eps, int maxiter, double eps_joints)
{
    if (!m_initialized)
    {
        std::cerr << "Robot not initialized" << std::endl;
        return false;
    }

    try
    {
        // 每次调用时动态构造 LMA 求解器，便于传入不同参数。
        KDL::ChainIkSolverPos_LMA ikSolverLMA(kinematicChain, eps, maxiter, eps_joints);

        // 将输入位姿转换为 KDL Frame。
        KDL::Vector position(pose[0], pose[1], pose[2]);
        KDL::Rotation rotation = KDL::Rotation::Quaternion(pose[3], pose[4], pose[5], pose[6]);
        KDL::Frame targetFrame(rotation, position);

        KDL::JntArray initialJoints(6);
        for (int i = 0; i < 6; i++)
        {
            initialJoints(i) = iniJ[i];
        }

        KDL::JntArray result(6);

        // 执行 LMA 逆运动学求解。
        int status = ikSolverLMA.CartToJnt(initialJoints, targetFrame, result);

        if (status >= 0)
        {
            // 将结果写回输出数组。
            for (int i = 0; i < 6; i++)
            {
                resultJoints[i] = result(i);
            }
            return true;
        }
        else
        {
            std::cerr << "LMA 逆运动学求解失败，状态码: " << status << std::endl;
            return false;
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "LMA 逆运动学求解异常: " << e.what() << std::endl;
        return false;
    }
}

bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7])
{
    if (!m_initialized)
    {
        std::cerr << "Robot not initialized" << std::endl;
        return false;
    }

    try
    {
        // 将关节数组转换为 KDL 关节对象。
        KDL::JntArray jointArray(6);
        for (int i = 0; i < 6; i++)
        {
            jointArray(i) = joints[i];
        }

        KDL::Frame tcpFrame;

        // 计算 TCP 正运动学结果。
        int status = fkSolver->JntToCart(jointArray, tcpFrame);

        if (status >= 0)
        {
            // 提取位置。
            tcpPose[0] = tcpFrame.p.x();
            tcpPose[1] = tcpFrame.p.y();
            tcpPose[2] = tcpFrame.p.z();

            // 提取四元数。
            double x, y, z, w;
            tcpFrame.M.GetQuaternion(x, y, z, w);
            tcpPose[3] = x;
            tcpPose[4] = y;
            tcpPose[5] = z;
            tcpPose[6] = w;

            return true;
        }
        else
        {
            std::cerr << "TCP 正运动学计算失败，状态码: " << status << std::endl;
            return false;
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "TCP 正运动学计算异常: " << e.what() << std::endl;
        return false;
    }
}
// bool Robot::forwardKinematics(double* inJoint, double* outFrame)
// const double joints[6], double jointPoses[42]
bool Robot::forwardKinematics(const double inJoint[6], double outFrame[42])
{
    Frame F_result;

    ChainFkSolverPos_recursive fkSolver(kinematicChain);
    JntArray q(6);

    q(0) = inJoint[0];
    q(1) = inJoint[1];
    q(2) = inJoint[2];
    q(3) = inJoint[3];
    q(4) = inJoint[4];
    q(5) = inJoint[5];

    for (int i = 0; i < 6; i++)
    {

        F_result = kinematicChain.getSegment(i + 1).pose(q(i));

        outFrame[7 * i + 0] = F_result.p.data[0];
        outFrame[7 * i + 1] = F_result.p.data[1];
        outFrame[7 * i + 2] = F_result.p.data[2];

        // double roll;
        // double pitch;
        // double yaw;

        // F_result.M.GetRPY(roll, pitch, yaw);
        // outFrame[6 * i + 3] = roll;
        // outFrame[6 * i + 4] = pitch;
        // outFrame[6 * i + 5] = yaw;

        double x;
        double y;
        double z;
        double w;
        F_result.M.GetQuaternion(x, y, z, w);
        outFrame[7 * i + 3] = x;
        outFrame[7 * i + 4] = y;
        outFrame[7 * i + 5] = z;
        outFrame[7 * i + 6] = w;
    }
    return true;
}

bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], double jointPoses[42])
{
    if (!m_initialized)
    {
        std::cerr << "Robot not initialized" << std::endl;
        return false;
    }

    try
    {
        // 将关节数组转换为 KDL 关节对象。
        KDL::JntArray jointArray(6);
        for (int i = 0; i < 6; i++)
        {
            jointArray(i) = joints[i];
        }

        // 逐段计算每个关节节点的位姿。
        for (int segIndex = 1; segIndex <= 6; segIndex++)
        {
            KDL::Frame frame;

            // 计算当前段的关节位姿。
            int status = fkSolver->JntToCart(jointArray, frame, segIndex);

            if (status >= 0)
            {
                int baseIndex = (segIndex - 1) * 7;

                // 提取位置。
                jointPoses[baseIndex + 0] = frame.p.x();
                jointPoses[baseIndex + 1] = frame.p.y();
                jointPoses[baseIndex + 2] = frame.p.z();

                // 提取四元数。
                double x, y, z, w;
                frame.M.GetQuaternion(x, y, z, w);
                jointPoses[baseIndex + 3] = x;
                jointPoses[baseIndex + 4] = y;
                jointPoses[baseIndex + 5] = z;
                jointPoses[baseIndex + 6] = w;
            }
            else
            {
                std::cerr << "关节 " << segIndex << " 正运动学计算失败，状态码: " << status << std::endl;
                return false;
            }
        }

        return true;
    }
    catch (const std::exception &e)
    {
        std::cerr << "全部关节正运动学计算异常: " << e.what() << std::endl;
        return false;
    }
}

/**
 * @brief 计算四元数之间的夹角差，用于轨迹步数估算。
 */

double Robot::quaternionAngleDifference(const double q1[4], const double q2[4])
{
    double dot = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
    dot = std::max(-1.0, min(1.0, dot));
    return 2 * std::acos(fabs(dot));
}

int Robot::calculateAutoSteps(const double startPose[7], const double endPose[7],
                              int minSteps = 10, int maxSteps = 100,
                              double positionResolution = 0.01,
                              double orientationResolution = 0.1)
{
    // 计算位置距离。
    double dx = endPose[0] - startPose[0];
    double dy = endPose[1] - startPose[1];
    double dz = endPose[2] - startPose[2];
    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);

    // 计算姿态差异。
    double startQuat[4] = {startPose[3], startPose[4], startPose[5], startPose[6]};
    double endQuat[4] = {endPose[3], endPose[4], endPose[5], endPose[6]};
    double angleDiff = quaternionAngleDifference(startQuat, endQuat);

    // 分别根据位置和姿态分辨率估算步数。
    int stepsFromPosition = static_cast<int>(ceil(distance / positionResolution));
    int stepsFromOrientation = static_cast<int>(ceil(angleDiff / orientationResolution));

    // 取较大值，并限制在最小/最大步数范围内。
    int autoSteps = max(stepsFromPosition, stepsFromOrientation);
    autoSteps = max(minSteps, min(maxSteps, autoSteps));

    return autoSteps;
}

int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7],
                              double outputPoses[][7], int npoint)
{

    if (!m_initialized)
    {
        cerr << "Robot not initialized" << endl;
        return -1;
    }

    // 未指定步数时自动估算。
    if (npoint <= 0)
    {
        npoint = calculateAutoSteps(pose1, pose2);
        cout << "自动计算轨迹点数: " << npoint << endl;
    }

    if (npoint <= 0)
    {
        cerr << "轨迹点数量必须大于 0" << endl;
        return -1;
    }

    try
    {
        // 将数组位姿转换为 KDL Frame。
        KDL::Vector pos1(pose1[0], pose1[1], pose1[2]);
        KDL::Rotation rot1 = KDL::Rotation::Quaternion(pose1[3], pose1[4], pose1[5], pose1[6]);
        KDL::Frame f1(rot1, pos1);

        KDL::Vector pos2(pose2[0], pose2[1], pose2[2]);
        KDL::Rotation rot2 = KDL::Rotation::Quaternion(pose2[3], pose2[4], pose2[5], pose2[6]);
        KDL::Frame f2(rot2, pos2);

        // 使用圆角路径插补并配合单轴旋转插值。
        // KDL::Path_RoundedComposite* path = new KDL::Path_RoundedComposite(
        //    0.002,  // 有效圆角半径
        //    0.01    // 几何公差
        //    // 旋转插值器使用单轴插值
        //);

        Path_RoundedComposite *path = new Path_RoundedComposite(0.002, 0.01, new RotationalInterpolation_SingleAxis());

        path->Add(f1);
        path->Add(f2);

        // 完成路径构造。
        path->Finish();
        // if (!path->Finish()) {
        //     cerr << "路径构造失败" << endl;
        //     delete path;
        //     return -1;
        // }

        // 设置速度曲线。
        KDL::VelocityProfile *velpref = new KDL::VelocityProfile_Trap(0.5, 0.1);
        velpref->SetProfile(0, path->PathLength());

        // 生成轨迹对象。
        KDL::Trajectory *traject = new KDL::Trajectory_Segment(path, velpref);

        double duration = traject->Duration();
        double timeStep = duration / npoint;

        int frameIndex = 0;

        // 采样轨迹点。
        for (int i = 0; i < npoint; i++)
        {
            double time = i * timeStep;
            if (i == npoint - 1)
            {
                time = duration; // 确保最后一个点落在终点。
            }

            KDL::Frame current_pose = traject->Pos(time);

            // 转回 `[x, y, z, qx, qy, qz, qw]` 数组格式。
            outputPoses[frameIndex][0] = current_pose.p.x();
            outputPoses[frameIndex][1] = current_pose.p.y();
            outputPoses[frameIndex][2] = current_pose.p.z();

            double x, y, z, w;
            current_pose.M.GetQuaternion(x, y, z, w);
            outputPoses[frameIndex][3] = x;
            outputPoses[frameIndex][4] = y;
            outputPoses[frameIndex][5] = z;
            outputPoses[frameIndex][6] = w;

            frameIndex++;
        }

        // 释放轨迹对象，内部会一并清理 path 和 velpref。
        delete traject;

        return frameIndex;
    }
    catch (const exception &e)
    {
        cerr << "轨迹规划异常: " << e.what() << endl;
        return -1;
    }
}

/**
 * @brief 生成位姿轨迹，输出为二维向量版本。
 */
int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7],
                              std::vector<std::vector<double>> &outputPoses, int npoint)
{

    if (!m_initialized)
    {
        std::cerr << "Robot not initialized" << std::endl;
        return -1;
    }

    // 未指定步数时自动估算。
    if (npoint <= 0)
    {
        npoint = calculateAutoSteps(pose1, pose2);
        std::cout << "自动计算轨迹点数: " << npoint << std::endl;
    }

    if (npoint <= 0)
    {
        std::cerr << "无效的轨迹点数量" << std::endl;
        return -1;
    }

    try
    {
        // 将数组位姿转换为 KDL Frame。
        KDL::Vector pos1(pose1[0], pose1[1], pose1[2]);
        KDL::Rotation rot1 = KDL::Rotation::Quaternion(pose1[3], pose1[4], pose1[5], pose1[6]);
        KDL::Frame f1(rot1, pos1);

        KDL::Vector pos2(pose2[0], pose2[1], pose2[2]);
        KDL::Rotation rot2 = KDL::Rotation::Quaternion(pose2[3], pose2[4], pose2[5], pose2[6]);
        KDL::Frame f2(rot2, pos2);

        // 构造路径与轨迹对象。
        Path_RoundedComposite *path = new Path_RoundedComposite(0.002, 0.01, new RotationalInterpolation_SingleAxis());

        path->Add(f1);
        path->Add(f2);
        path->Finish();
        // if (!path->Finish()) {
        //     std::cerr << "路径构造失败" << std::endl;
        //     delete path;
        //     return -1;
        // }

        KDL::VelocityProfile *velpref = new KDL::VelocityProfile_Trap(0.5, 0.1);
        velpref->SetProfile(0, path->PathLength());

        KDL::Trajectory *traject = new KDL::Trajectory_Segment(path, velpref);

        double duration = traject->Duration();
        double timeStep = duration / npoint;

        // 预分配输出空间，减少 push_back 过程中的扩容成本。
        outputPoses.clear();
        outputPoses.reserve(npoint);

        // 采样轨迹点。
        for (int i = 0; i < npoint; i++)
        {
            double time = (i == npoint - 1) ? duration : i * timeStep;
            KDL::Frame current_pose = traject->Pos(time);

            std::vector<double> point(7);
            point[0] = current_pose.p.x();
            point[1] = current_pose.p.y();
            point[2] = current_pose.p.z();

            double x, y, z, w;
            current_pose.M.GetQuaternion(x, y, z, w);
            point[3] = x;
            point[4] = y;
            point[5] = z;
            point[6] = w;

            outputPoses.push_back(std::move(point));
        }

        // 释放轨迹对象，内部会一并清理 path 和 velpref。
        delete traject;

        return outputPoses.size();
    }
    catch (const std::exception &e)
    {
        std::cerr << "轨迹规划异常: " << e.what() << std::endl;
        return -1;
    }
}

/**
 * @brief 生成位姿轨迹，输入输出都使用 `std::vector`。
 */
int Robot::trajectoryPlanning(const std::vector<double> &pose1,
                              const std::vector<double> &pose2,
                              std::vector<std::vector<double>> &outputPoses,
                              int npoint)
{

    if (pose1.size() != 7 || pose2.size() != 7)
    {
        std::cerr << "输入位姿必须是 7 维数组" << std::endl;
        return -1;
    }

    // 转换为数组后复用主实现。
    double pose1_arr[7], pose2_arr[7];
    std::copy(pose1.begin(), pose1.end(), pose1_arr);
    std::copy(pose2.begin(), pose2.end(), pose2_arr);

    return trajectoryPlanning(pose1_arr, pose2_arr, outputPoses, npoint);
}

/**
 * @brief 使用 LMA 求解逆运动学，输出关节角向量。
 */
std::vector<double> Robot::inverse(const double pose[7], const double iniJ[6],
                                   double eps, int maxiter, double eps_joints)
{

    std::vector<double> result;

    // 先检查机器人是否已经完成初始化。
    if (!isInitialized())
    {
        std::cerr << "错误: 机器人尚未初始化" << std::endl;
        return result;
    }

    // 校验输入参数。
    if (pose == nullptr || iniJ == nullptr)
    {
        std::cerr << "错误: 输入参数不能为 nullptr" << std::endl;
        return result;
    }

    double resultJoints[6];

    // 调用底层 LMA 求解器。
    bool success = calculateIK_LMA(pose, iniJ, resultJoints, eps, maxiter, eps_joints);

    if (success)
    {
        // 将结果拷贝到 vector。
        result.reserve(6);
        for (int i = 0; i < 6; i++)
        {
            result.push_back(resultJoints[i]);
        }
    }
    else
    {
        // 求解失败时返回空结果。
        result.clear();
        std::cerr << "错误: 逆运动学求解失败" << std::endl;
    }

    return result;
}

/**
 * @brief `std::vector` 版本的逆运动学接口。
 */
std::vector<double> Robot::inverse(const std::vector<double> &pose,
                                   const std::vector<double> &iniJ,
                                   double eps, int maxiter, double eps_joints)
{

    if (pose.size() != 7)
    {
        std::cerr << "错误: 目标位姿应为 7 维，当前为 " << pose.size() << " 维" << std::endl;
        return std::vector<double>();
    }

    if (iniJ.size() != 6)
    {
        std::cerr << "错误: 初始关节角应为 6 维，当前为 " << iniJ.size() << " 维" << std::endl;
        return std::vector<double>();
    }

    // 转为数组后复用主实现。
    return inverse(pose.data(), iniJ.data(), eps, maxiter, eps_joints);
}

/**
 * @brief 使用零位初始关节角进行逆解。
 */
std::vector<double> Robot::inverse(const double pose[7],
                                   double eps, int maxiter, double eps_joints)
{

    // 使用零位关节角作为默认初值。
    double zeroJoints[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    return inverse(pose, zeroJoints, eps, maxiter, eps_joints);
}

std::vector<double> Robot::inverse(const std::vector<double> &pose,
                                   double eps, int maxiter, double eps_joints)
{

    if (pose.size() != 7)
    {
        std::cerr << "错误: 目标位姿应为 7 维" << std::endl;
        return std::vector<double>();
    }

    std::vector<double> zeroJoints(6, 0.0);
    return inverse(pose, zeroJoints, eps, maxiter, eps_joints);
}

// 对输入姿态逐点逆解，不执行轨迹插补。
json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::string &q_init_str)
{
    // 准备输出 JSON 数组。
    json joints_poses_array = json::array();
    json joints_array = json::array();

    std::vector<std::vector<double>> posePoints = parsePoseString(pose_str);
    if (posePoints.empty())
    {
        return joints_poses_array; // 返回空数组。
    }

    std::vector<double> initJoints = parseJointString(q_init_str);
    std::vector<std::vector<double>> allJointSolutions;

    if (posePoints.size() == 1)
    {
        std::vector<double> inverseSolution = inverse(posePoints[0], initJoints);

        // 单点逆解成功则直接返回。
        if (!inverseSolution.empty())
        {
            // 转为 JSON 数组。
            joints_array = inverseSolution;
            joints_poses_array.push_back(joints_array);
            return joints_poses_array;
        }
        else
        {
            return joints_poses_array; // 返回空数组。
        }
    }
    else
    {
        for (size_t i = 0; i < posePoints.size() - 1; i++)
        {
            std::vector<double> startPose = posePoints[i];
            std::vector<double> inverseSolution = inverse(startPose, initJoints);

            // 逆解成功则更新当前初值。
            if (!inverseSolution.empty())
            {
                allJointSolutions.push_back(inverseSolution);
                initJoints = inverseSolution; // 用上一点结果作为下一点初值。
            }
            else
            {
                std::cerr << "逆解失败" << std::endl;
                // 失败时复用上一次成功结果，避免轨迹中断。
                if (!allJointSolutions.empty())
                {
                    allJointSolutions.push_back(allJointSolutions.back());
                }
                else
                {
                    allJointSolutions.push_back(initJoints);
                }
            }
        }

        // 汇总全部关节解。
        for (const auto &jointSolution : allJointSolutions)
        {
            joints_array = jointSolution;
            joints_poses_array.push_back(joints_array);
        }

        return joints_poses_array;
    }
}

// 对姿态段进行自动插补后，再逐点做逆解。
json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_init_str)
{
    // 准备输出 JSON 数组。
    json joints_poses_array = json::array();
    json joints_array = json::array();

    std::vector<std::vector<double>> posePoints = parsePoseString(pose_str);
    if (posePoints.empty())
    {
        return joints_poses_array; // 返回空数组。
    }

    std::vector<double> initJoints = parseJointString(q_init_str);
    std::vector<std::vector<double>> allJointSolutions;

    if (posePoints.size() == 1)
    {
        std::vector<double> inverseSolution = inverse(posePoints[0], initJoints);

        // 单点逆解成功则直接返回。
        if (!inverseSolution.empty())
        {
            // 转为 JSON 数组。
            joints_array = inverseSolution;
            joints_poses_array.push_back(joints_array);
            return joints_poses_array;
        }
        else
        {
            return joints_poses_array; // 返回空数组。
        }
    }
    else
    {
        for (size_t i = 0; i < posePoints.size() - 1; i++)
        {
            std::vector<double> startPose = posePoints[i];
            std::vector<double> endPose = posePoints[i + 1];

            int steps = calculateAutoSteps(startPose, endPose, 10, 100, 0.01, 0.1);
            std::vector<std::vector<double>> interpolatedPoses;
            int pointCount = trajectoryPlanning(startPose, endPose, interpolatedPoses, steps);

            if (pointCount <= 0)
            {
                std::cerr << "轨迹规划失败" << std::endl;
                continue;
            }

            for (const auto &pose : interpolatedPoses)
            {
                std::vector<double> inverseSolution = inverse(pose, initJoints);

                // 逆解成功则更新当前初值。
                if (!inverseSolution.empty())
                {
                    allJointSolutions.push_back(inverseSolution);
                    initJoints = inverseSolution; // 用上一点结果作为下一点初值。
                }
                else
                {
                    std::cerr << "逆解失败" << std::endl;
                    // 失败时复用上一次成功结果，避免轨迹中断。
                    if (!allJointSolutions.empty())
                    {
                        allJointSolutions.push_back(allJointSolutions.back());
                    }
                    else
                    {
                        allJointSolutions.push_back(initJoints);
                    }
                }
            }
        }

        // 汇总全部关节解。
        for (const auto &jointSolution : allJointSolutions)
        {
            joints_array = jointSolution;
            joints_poses_array.push_back(joints_array);
        }

        return joints_poses_array;
    }
}

// 对两点姿态按指定步数插补后，逐点计算逆解。
json Robot::inversePoseStr2PSteps(const std::string &pose_str, const std::string &q_init_str, const std::int32_t steps)
{
    // 准备输出 JSON 数组。
    json joints_poses_array = json::array();
    json joints_array = json::array();

    std::vector<std::vector<double>> posePoints = parsePoseString(pose_str);
    if (posePoints.empty())
    {
        return joints_poses_array; // 返回空数组。
    }

    std::vector<double> initJoints = parseJointString(q_init_str);
    std::vector<std::vector<double>> allJointSolutions;

    if (posePoints.size() == 1)
    {
        std::vector<double> inverseSolution = inverse(posePoints[0], initJoints);

        // 单点逆解成功则直接返回。
        if (!inverseSolution.empty())
        {
            // 转为 JSON 数组。
            joints_array = inverseSolution;
            joints_poses_array.push_back(joints_array);
            return joints_poses_array;
        }
        else
        {
            return joints_poses_array; // 返回空数组。
        }
    }
    else
    {

        std::vector<double> startPose = posePoints[0];
        std::vector<double> endPose = posePoints[1];
        std::vector<std::vector<double>> interpolatedPoses;
        int pointCount = trajectoryPlanning(startPose, endPose, interpolatedPoses, steps);
        for (const auto &pose : interpolatedPoses)
        {
            std::vector<double> inverseSolution = inverse(pose, initJoints);

            // 逆解成功则更新当前初值。
            if (!inverseSolution.empty())
            {
                allJointSolutions.push_back(inverseSolution);
                initJoints = inverseSolution; // 用上一点结果作为下一点初值。
            }
            else
            {
                std::cerr << "逆解失败" << std::endl;
                // 失败时复用上一次成功结果，避免轨迹中断。
                if (!allJointSolutions.empty())
                {
                    allJointSolutions.push_back(allJointSolutions.back());
                }
                else
                {
                    allJointSolutions.push_back(initJoints);
                }
            }
        }

        // 汇总全部关节解。
        for (const auto &jointSolution : allJointSolutions)
        {
            joints_array = jointSolution;
            joints_poses_array.push_back(joints_array);
        }

        return joints_poses_array;
    }
}

std::vector<std::vector<double>> Robot::parsePoseString(const std::string &pose_str)
{
    std::vector<std::vector<double>> result;
    std::vector<std::string> poseStrings;
    size_t start = 0;
    size_t end = pose_str.find(';');

    while (end != std::string::npos)
    {
        poseStrings.push_back(pose_str.substr(start, end - start));
        start = end + 1;
        end = pose_str.find(';', start);
    }
    poseStrings.push_back(pose_str.substr(start));

    for (const auto &poseString : poseStrings)
    {
        if (poseString.empty())
            continue;

        std::vector<double> values;
        size_t valueStart = 0;
        size_t valueEnd = poseString.find(',');

        while (valueEnd != std::string::npos)
        {
            std::string valueStr = poseString.substr(valueStart, valueEnd - valueStart);
            if (!valueStr.empty())
            {
                values.push_back(stod(valueStr));
            }
            valueStart = valueEnd + 1;
            valueEnd = poseString.find(',', valueStart);
        }

        std::string lastValue = poseString.substr(valueStart);
        if (!lastValue.empty())
        {
            values.push_back(stod(lastValue));
        }

        if (values.size() != 7)
        {
            throw invalid_argument("每个姿态都必须包含 7 个值 (x,y,z,qx,qy,qz,qw)");
        }

        result.push_back(values);
    }

    return result;
}

/// <summary>
/// "0.374, 0, 0.63,0, 0, 0;0.4626,0.2209,0.3340,0.148283,0.435718,0.079062"
/// </summary>
/// <param name="joint_str"></param>
/// <returns></returns>
std::vector<std::vector<double>> Robot::parseJointListString(const std::string &joint_str)
{
    std::vector<std::vector<double>> result;
    std::vector<std::string> poseStrings;
    size_t start = 0;
    size_t end = joint_str.find(';');

    while (end != std::string::npos)
    {
        poseStrings.push_back(joint_str.substr(start, end - start));
        start = end + 1;
        end = joint_str.find(';', start);
    }
    poseStrings.push_back(joint_str.substr(start));

    for (const auto &poseString : poseStrings)
    {
        if (poseString.empty())
            continue;

        std::vector<double> values;
        size_t valueStart = 0;
        size_t valueEnd = poseString.find(',');

        while (valueEnd != std::string::npos)
        {
            std::string valueStr = poseString.substr(valueStart, valueEnd - valueStart);
            if (!valueStr.empty())
            {
                values.push_back(stod(valueStr));
            }
            valueStart = valueEnd + 1;
            valueEnd = poseString.find(',', valueStart);
        }

        std::string lastValue = poseString.substr(valueStart);
        if (!lastValue.empty())
        {
            values.push_back(stod(lastValue));
        }

        if (values.size() != 6)
        {
            throw invalid_argument("每组关节角都必须包含 6 个值 (j1,j2,j3,j4,j5,j6)");
        }

        result.push_back(values);
    }

    return result;
}

std::vector<double> Robot::parseJointString(const std::string &joint_str)
{
    std::vector<double> result;
    size_t start = 0;
    size_t end = joint_str.find(',');

    while (end != std::string::npos)
    {
        std::string valueStr = joint_str.substr(start, end - start);
        if (!valueStr.empty())
        {
            result.push_back(stod(valueStr));
        }
        start = end + 1;
        end = joint_str.find(',', start);
    }

    std::string lastValue = joint_str.substr(start);
    if (!lastValue.empty())
    {
        result.push_back(stod(lastValue));
    }

    return result;
}

// OPERATION  frames  time objStates  i,tx,ty,tz,qx,qy,qz,qw
json Robot::kinematicsForwardAllJointsList(std::vector<double> joints)
{
    try
    {
        constexpr int VALUES_PER_JOINT = 7;
        const int total_values = numberOfJoints * VALUES_PER_JOINT;
        std::vector<double> joint_poses(total_values);

        if (!forwardKinematics(joints.data(), joint_poses.data()))
        {
            return json{{"error", "Forward kinematics calculation for all joints failed"}};
        }

        json result;
        // json joints_array = json::array();
        // json joints_array1 = json::array();
        // json joints_array_id = json::array();
        // json joints_array1_id = json::array();
        json joints_array_objStates = json::array();

        // 输出前端需要的 objStates 结构。
        for (int i = 0; i < numberOfJoints; i++)
        {
            const int base_index = i * VALUES_PER_JOINT;
            std::string id = getJointUuidByIndex(i + 1);

            json objStates =
                {
                    {"i", id},
                    {"tx", joint_poses[base_index + 0]},
                    {"ty", joint_poses[base_index + 1]},
                    {"tz", joint_poses[base_index + 2]},
                    {"qx", joint_poses[base_index + 3]},
                    {"qy", joint_poses[base_index + 4]},
                    {"qz", joint_poses[base_index + 5]},
                    {"qw", joint_poses[base_index + 6]}};

            joints_array_objStates.push_back(objStates);
        }

        json objStates_time =
            {
                {"time", "0.020000"},
                {"objStates", joints_array_objStates}

            };

        // 返回按时间帧组织的结果。
        return {
            //{"array_format", joints_array},
            //{"key_value_format", joints_array1},
            //{"array_format_id", joints_array_id},
            //{"key_value_format_id", joints_array1_id},
            {"objStates_time", objStates_time}};
    }
    catch (const std::exception &e)
    {
        return json{{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
    }
}

// OPERATION  frames  time objStates  i,tx,ty,tz,qx,qy,qz,qw

json Robot::handleKinematicsForwardAllJoints(const std::string &joints_str)
{
    try
    {
        auto joints = parseJointListString(joints_str);
        if (joints.empty())
        {
            return json{{"error", "Empty joints input"}};
        }

        json frames = json::array();

        for (size_t i = 0; i < joints.size(); i++)
        {
            json result = kinematicsForwardAllJointsList(joints[i]);

            // 遇到错误则立即返回。
            if (result.contains("error"))
            {
                return result; // 直接返回错误信息。
            }

            frames.push_back(result["objStates_time"]);
        }
        json OPERATION = {{"frames", frames}};
        return {

            {"OPERATION", OPERATION},
            {"success", true},
            {"count", joints.size()}};
    }

    catch (const std::exception &e)
    {
        return json{{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
    }
}

json Robot::handleKinematicsForwardAllJoints_objStates(const std::string &joints_str)
{
    try
    {
        auto joints = parseJointListString(joints_str);
        if (joints.empty())
        {
            return json{{"error", "Empty joints input"}};
        }

        json joints_poses_array = json::array();
        json joints_poses_array1 = json::array();
        json joints_poses_array_id = json::array();
        json joints_poses_array1_id = json::array();

        for (size_t i = 0; i < joints.size(); i++)
        {
            json result = kinematicsForwardAllJointsList(joints[i]);

            // 遇到错误则立即返回。
            if (result.contains("error"))
            {
                return result; // 直接返回错误信息。
            }

            joints_poses_array.push_back(result["array_format"]);
            joints_poses_array1.push_back(result["key_value_format"]);

            joints_poses_array_id.push_back(result["array_format_id"]);
            joints_poses_array1_id.push_back(result["key_value_format_id"]);
        }

        return {
            {"objStates", joints_poses_array1_id},
        };
    }

    catch (const std::exception &e)
    {
        return json{{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
    }
}

int Robot::calculateAutoSteps(const std::vector<double> &startPose, const std::vector<double> &endPose,
                              int minSteps, int maxSteps,
                              double positionResolution, double orientationResolution)
{
    double dx = endPose[0] - startPose[0];
    double dy = endPose[1] - startPose[1];
    double dz = endPose[2] - startPose[2];
    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);

    double angleDiff = quaternionAngleDifference(
        {startPose[3], startPose[4], startPose[5], startPose[6]},
        {endPose[3], endPose[4], endPose[5], endPose[6]});

    int stepsFromPosition = static_cast<int>(ceil(distance / positionResolution));
    int stepsFromOrientation = static_cast<int>(ceil(angleDiff / orientationResolution));

    int autoSteps = max(stepsFromPosition, stepsFromOrientation);
    autoSteps = max(minSteps, min(maxSteps, autoSteps));

    return autoSteps;
}

double Robot::quaternionAngleDifference(const std::vector<double> &q1, const std::vector<double> &q2)
{
    double dot = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
    dot = max(-1.0, min(1.0, dot));
    return 2 * std::acos(fabs(dot));
}
