// KinematicsReverse.cpp
#include "QuadrupedRobotSimulation/KinematicsReverse.h"
#include "QuadrupedRobotSimulation/graph_utils.hpp"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"

#include <iostream>
#include <sstream>
#include <iomanip>
#include <stdexcept>
#include "nlohmann/json.hpp"

using json = nlohmann::json;

bool ReverseKinematicsCalculator::IsSupporLF = false;
bool ReverseKinematicsCalculator::IsSupporLH = false;
// double ReverseKinematicsCalculator::BodyPosition_x = -3.24;
// double ReverseKinematicsCalculator::BodyPosition_y = -0.12;
// double ReverseKinematicsCalculator::BodyPosition_z = 560.4;
// double ReverseKinematicsCalculator::BodyPosition_qx = 0;
// double ReverseKinematicsCalculator::BodyPosition_qy = 0;
// double ReverseKinematicsCalculator::BodyPosition_qz = 0;
// double ReverseKinematicsCalculator::BodyPosition_qw = 1;
const std::string ReverseKinematicsCalculator::BodyCode = "7ec2aade-c45a-4bf8-8bb3-d1e0db5df9f6";

// 构造函数
ReverseKinematicsCalculator::ReverseKinematicsCalculator(std::shared_ptr<RobotGaitDataManager> manager)
{
    if (manager == nullptr)
    {
        // 使用默认数据
        _manager = std::make_shared<RobotGaitDataManager>();
        _manager->LoadDefaultData();
        _config = QuadrupedRobotConfiguration();
    }
    else
    {
        _config = QuadrupedRobotConfiguration(manager->GetGaitInfo(), manager->GetSystemParameters());
        _manager = manager;
    }

    LoadParametersFromConfig();
    ValidateConfiguration();
    CalculateDerivedParameters();
}

// 从配置加载参数
void ReverseKinematicsCalculator::LoadParametersFromConfig()
{
    // 基本几何参数
    A_point = _config.A_point;
    L10 = _config.L10;
    L20 = _config.L20;
    L30 = _config.L30;

    // 小腿连杆机构参数
    L21 = _config.L21;
    L22 = _config.L22;
    L23 = _config.L23;
    beta1 = _config.beta1;
    CC1 = _config.CC1;

    // 脚踝连杆机构参数
    L31 = _config.L31;
    C2C3_length = _config.C2C3_length;
    lead1 = _config.lead1;

    // 脚部构型参数
    D1_L_offset = _config.D1_L_offset;
    D2_L_offset = _config.D2_L_offset;
    C4_D2_offset = _config.C4_D2_offset;

    // 逆向运动学参数
    adjusted_shank_angles_factory = _config.adjusted_shank_angles_factory;

    // 初始C3C4距离
    initial_C3C4_LF = _config.initial_C3C4_LF;
    initial_C3C4_LH = _config.initial_C3C4_LH;
    initial_C3C4_RF = _config.initial_C3C4_RF;
    initial_C3C4_RH = _config.initial_C3C4_RH;
}

// 计算派生参数
void ReverseKinematicsCalculator::CalculateDerivedParameters()
{
    _BB2_BC_angle_rad = (360 - beta1) * PI / 180;
    _C1_B_length = L20 - CC1;

    // 计算CC4参考距离
    std::vector<double> C_ref = {0, L30};
    std::vector<double> C4_ref = {D2_L_offset, C4_D2_offset};

    _CC4_distance = std::sqrt(
        std::pow(C4_ref[0] - C_ref[0], 2) +
        std::pow(C4_ref[1] - C_ref[1], 2));
}

// 验证配置
void ReverseKinematicsCalculator::ValidateConfiguration()
{
    if (!_manager)
        throw std::runtime_error("RobotGaitDataManager is null");

    auto validation = _config.Validate();
    if (!validation.IsValid)
    {
        std::string error_msg = "配置参数验证失败: ";
        for (const auto &error : validation.Errors)
        {
            error_msg += error + "; ";
        }
        throw std::runtime_error(error_msg);
    }
}

// 更新配置
void ReverseKinematicsCalculator::UpdateConfiguration(const QuadrupedRobotConfiguration &newConfig)
{
    _config = newConfig;
    LoadParametersFromConfig();
    ValidateConfiguration();
    CalculateDerivedParameters();
}

// 计算点C（四杆机构解析解）
std::optional<Vector2D> ReverseKinematicsCalculator::CalculatePointC(
    const Vector2D &A, const Vector2D &D, const Vector2D &B, double l2, double l3)
{
    Vector2D BD = D - B;
    double d = BD.Length();

    // 确保三角形不等式成立
    if (d > l2 + l3 || d < std::abs(l2 - l3))
    {
        return std::nullopt; // 与Python一致：返回None
    }

    double a = (l2 * l2 - l3 * l3 + d * d) / (2.0 * d);
    double h_sq = l2 * l2 - a * a;

    // 处理负的h_sq
    if (h_sq < 0)
    {
        if (std::abs(h_sq) < 1e-10)
        {
            h_sq = 0.0;
        }
        else
        {
            return std::nullopt;
        }
    }

    double h = std::sqrt(h_sq);
    Vector2D u = BD.Normalize();
    Vector2D v = Vector2D(-u.Y, u.X); // 与Python一致：(-u.y, u.x)

    // 两个可能的解
    Vector2D C1 = B + u * a + v * h;
    Vector2D C2 = B + u * a - v * h;

    Vector2D C = SelectCPointByADSide(A, D, B, C1, C2);
    return C;
}

// 选择C点（确保C和B在AD同侧）
Vector2D ReverseKinematicsCalculator::SelectCPointByADSide(
    const Vector2D &A, const Vector2D &D, const Vector2D &B,
    const Vector2D &C1, const Vector2D &C2)
{
    Vector2D AD = D - A;
    Vector2D AB = B - A;
    Vector2D AC1 = C1 - A;
    Vector2D AC2 = C2 - A;

    double cross_ad_ab = AD.Cross(AB);
    double cross_ad_ac1 = AD.Cross(AC1);
    double cross_ad_ac2 = AD.Cross(AC2);

    bool c1_same_side = (cross_ad_ab * cross_ad_ac1 >= 0);
    bool c2_same_side = (cross_ad_ab * cross_ad_ac2 >= 0);

    // 与Python完全一致的选择逻辑
    if (c1_same_side && c2_same_side)
    {
        return C1; // 与Python一致：返回C1
    }
    else if (c1_same_side)
    {
        return C1;
    }
    else if (c2_same_side)
    {
        return C2;
    }
    else
    {
        // 都不在同一侧时，选择距离B更近的点
        double dist1 = Vector2D::Distance(B, C1);
        double dist2 = Vector2D::Distance(B, C2);
        return (dist1 < dist2) ? C1 : C2;
    }
}

// 根据大腿角度计算B点
std::vector<double> ReverseKinematicsCalculator::CalculateBFromThighAngleReverse(double thigh_angle_deg)
{
    // 与Python完全一致：actual_angle_deg = 270 - thigh_angle_deg
    double actual_angle_deg = 270 - thigh_angle_deg;
    double theta_rad = actual_angle_deg * DegToRad;

    double B_x = A_point[0] + L10 * std::cos(theta_rad);
    double B_y = A_point[1] + L10 * std::sin(theta_rad);

    return {B_x, B_y};
}

// 根据小腿角度计算B1点
std::vector<double> ReverseKinematicsCalculator::CalculateB1FromShankAngleReverse(
    const std::vector<double> &B_p, double shank_angle_deg)
{
    std::vector<double> AB_vector = {
        B_p[0] - A_point[0],
        B_p[1] - A_point[1]};

    double AB_length = std::sqrt(AB_vector[0] * AB_vector[0] + AB_vector[1] * AB_vector[1]);

    if (AB_length == 0)
        return A_point; // 返回A点

    std::vector<double> AB_unit = {
        AB_vector[0] / AB_length,
        AB_vector[1] / AB_length};

    // 与Python完全一致：shank_angle_deg = adjusted_shank_angles_factory - shank_angle_deg
    double adjusted_shank_angle_deg = adjusted_shank_angles_factory - shank_angle_deg;
    double theta1_rad = adjusted_shank_angle_deg * DegToRad;

    double cos_t = std::cos(theta1_rad);
    double sin_t = std::sin(theta1_rad);

    // 与Python完全一致的旋转计算
    double AB1_x = AB_unit[0] * cos_t - AB_unit[1] * sin_t;
    double AB1_y = AB_unit[0] * sin_t + AB_unit[1] * cos_t;

    std::vector<double> B1_point = {
        A_point[0] + L21 * AB1_x,
        A_point[1] + L21 * AB1_y};

    return B1_point;
}

// 根据四杆机构计算B2点
std::pair<std::vector<double>, std::string> ReverseKinematicsCalculator::CalculateB2FromFourbarReverse(
    const std::vector<double> &A_p, const std::vector<double> &B_p,
    const std::vector<double> &B1_p, double L22, double L23)
{
    Vector2D A(A_p[0], A_p[1]);
    Vector2D D(B_p[0], B_p[1]);   // Python中D对应B点
    Vector2D B(B1_p[0], B1_p[1]); // Python中B对应B1点

    auto C = CalculatePointC(A, D, B, L22, L23);

    if (!C.has_value())
    {
        // 与Python一致：返回中点作为近似解
        return std::make_pair(
            std::vector<double>{(A_p[0] + B_p[0]) / 2, (A_p[1] + B_p[1]) / 2},
            "approximate");
    }

    std::vector<double> B2_point = {C->X, C->Y};
    return std::make_pair(B2_point, "analytic_multiple");
}

// 根据B和B2计算C点
std::vector<double> ReverseKinematicsCalculator::CalculateCFromBAndB2Reverse(
    const std::vector<double> &B1, const std::vector<double> &B2, double L, double theta_deg)
{
    double theta = theta_deg * DegToRad; // theta_deg = beta1 = 166度

    std::vector<double> u = {B2[0] - B1[0], B2[1] - B1[1]};

    double cosTheta = std::cos(theta);
    double sinTheta = std::sin(theta);

    // 旋转矩阵乘法
    std::vector<double> v = {
        cosTheta * u[0] - sinTheta * u[1],
        sinTheta * u[0] + cosTheta * u[1]};

    double v_norm = std::sqrt(v[0] * v[0] + v[1] * v[1]);
    if (v_norm == 0)
        return B1; // 返回B1

    std::vector<double> v_unit = {v[0] / v_norm, v[1] / v_norm};
    std::vector<double> B1C = {v_unit[0] * L, v_unit[1] * L};

    std::vector<double> C = {B1[0] + B1C[0], B1[1] + B1C[1]};
    return C;
}

// 反向计算C1点
std::vector<double> ReverseKinematicsCalculator::CalculateC1PointReverse(
    const std::vector<double> &B_p, const std::vector<double> &C_p)
{
    std::vector<double> BC_vector = {C_p[0] - B_p[0], C_p[1] - B_p[1]};
    double BC_length = std::sqrt(BC_vector[0] * BC_vector[0] + BC_vector[1] * BC_vector[1]);

    if (BC_length == 0)
        return B_p; // 返回B点

    std::vector<double> BC_unit = {BC_vector[0] / BC_length, BC_vector[1] / BC_length};

    std::vector<double> C1_point = {
        B_p[0] + BC_unit[0] * _C1_B_length,
        B_p[1] + BC_unit[1] * _C1_B_length};

    return C1_point;
}

// 根据C1点计算C2点
std::vector<double> ReverseKinematicsCalculator::CalculateC2PointFromC1Reverse(
    const std::vector<double> &C1_p, const std::vector<double> &B_p,
    const std::vector<double> &C_point)
{
    std::vector<double> BC_vector = {C_point[0] - B_p[0], C_point[1] - B_p[1]};
    double BC_length = std::sqrt(BC_vector[0] * BC_vector[0] + BC_vector[1] * BC_vector[1]);

    if (BC_length == 0)
        return {C1_p[0] + L31, C1_p[1]}; // 与Python一致

    std::vector<double> BC_unit = {BC_vector[0] / BC_length, BC_vector[1] / BC_length};

    // 与Python一致：BC_perp = np.array([-BC_unit[1], BC_unit[0]])
    std::vector<double> BC_perp = {-BC_unit[1], BC_unit[0]};

    std::vector<double> C2_point = {
        C1_p[0] + BC_perp[0] * L31,
        C1_p[1] + BC_perp[1] * L31};

    return C2_point;
}

// 根据脚踝角度计算C3C4距离
double ReverseKinematicsCalculator::CalculateC3C4DistanceFromAnkleAngleReverse(
    double ankle_angle_deg, const std::string &leg_type)
{
    double initial_C3C4 = 0;

    if (leg_type == "LF")
        initial_C3C4 = initial_C3C4_LF;
    else if (leg_type == "LH")
        initial_C3C4 = initial_C3C4_LH;
    else if (leg_type == "RF")
        initial_C3C4 = initial_C3C4_RF;
    else if (leg_type == "RH")
        initial_C3C4 = initial_C3C4_RH;
    else
        throw std::invalid_argument("不支持的腿类型: " + leg_type);

    double ankle_angle_rad = ankle_angle_deg * DegToRad;
    double screw_displacement = (ankle_angle_rad / (2 * PI)) * lead1; // 与Python一致

    double C3C4_distance;
    if (leg_type == "LF" || leg_type == "LH")
    {
        // 左腿：距离随角度增加而增加
        C3C4_distance = initial_C3C4 + screw_displacement;
    }
    else
    {
        // 右腿：距离随角度增加而减少
        C3C4_distance = initial_C3C4 - screw_displacement;
    }

    // 与Python一致：return max(C3C4_distance, 1.0)
    return std::max(C3C4_distance, 1.0);
}

// 已知三角形三边和两个顶点，求第三个顶点
std::vector<double> ReverseKinematicsCalculator::FindThirdVertex(
    const std::vector<double> &C, const std::vector<double> &C2,
    double a, double b, double c)
{
    // 计算实际a值（C到C2的距离）
    double computed_a = std::sqrt(
        std::pow(C2[0] - C[0], 2) +
        std::pow(C2[1] - C[1], 2));

    // 如果长度不匹配，不修改a值（与Python一致）
    if (std::abs(computed_a - a) > 1e-9)
    {
        // 不处理，与Python一致
    }

    // 三角形不等式检查
    if (!(a + b > c && a + c > b && b + c > a))
    {
        throw std::invalid_argument("边长不满足三角形不等式");
    }

    // 余弦计算
    double cos_theta = (a * a + b * b - c * c) / (2 * a * b);
    cos_theta = std::max(-1.0, std::min(1.0, cos_theta)); // 与np.clip一致
    double theta = std::acos(cos_theta);

    std::vector<double> u = {
        (C2[0] - C[0]) / a,
        (C2[1] - C[1]) / a};

    // 旋转向量
    std::vector<double> rot_u1 = RotateVector(theta, u);  // 正向旋转
    std::vector<double> rot_u2 = RotateVector(-theta, u); // 反向旋转

    std::vector<double> C4_1 = {
        C[0] + b * rot_u1[0],
        C[1] + b * rot_u1[1]};

    std::vector<double> C4_2 = {
        C[0] + b * rot_u2[0],
        C[1] + b * rot_u2[1]};

    // 与Python一致：返回C4_2（第二个解）
    return C4_2;
}

// 旋转向量辅助函数
std::vector<double> ReverseKinematicsCalculator::RotateVector(
    double angle, const std::vector<double> &vector)
{
    double cosA = std::cos(angle);
    double sinA = std::sin(angle);

    return {
        cosA * vector[0] - sinA * vector[1],
        sinA * vector[0] + cosA * vector[1]};
}

// 根据C和C2点计算C4点
std::vector<double> ReverseKinematicsCalculator::CalculateC4PointFromCAndC2Reverse(
    const std::vector<double> &C_p, const std::vector<double> &C2_p,
    double ankle_angle_deg, const std::string &leg_type)
{
    // 与Python一致：右侧腿取反脚踝角度
    double adjusted_ankle_angle = ankle_angle_deg;
    if (leg_type == "RF" || leg_type == "RH")
    {
        adjusted_ankle_angle = -ankle_angle_deg;
    }

    double C3C4_distance = CalculateC3C4DistanceFromAnkleAngleReverse(adjusted_ankle_angle, leg_type);
    double C2C4_distance = std::sqrt(C2C3_length * C2C3_length + C3C4_distance * C3C4_distance);
    double CC2_distance = std::sqrt(CC1 * CC1 + L31 * L31);

    // 使用预计算的CC4_distance
    std::vector<double> C4_p = FindThirdVertex(C_p, C2_p, CC2_distance, _CC4_distance, C2C4_distance);
    return C4_p;
}

// 根据C和C4点计算L点
std::vector<double> ReverseKinematicsCalculator::CalculateLPointsFromCAndC4Reverse(
    const std::vector<double> &C, const std::vector<double> &C4)
{
    double LC4 = std::sqrt(C4_D2_offset * C4_D2_offset + D2_L_offset * D2_L_offset);
    double CC4_distance = std::sqrt(
        std::pow(C4[0] - C[0], 2) +
        std::pow(C4[1] - C[1], 2));

    std::vector<double> L_p = FindThirdVertex(C, C4, CC4_distance, L30, LC4);
    return L_p;
}

// 求直角三角形直角顶点
std::vector<double> ReverseKinematicsCalculator::FindRightAngleThirdVertex(
    const std::vector<double> &C, const std::vector<double> &L,
    double a, double b)
{
    std::vector<double> CL_vec = {L[0] - C[0], L[1] - C[1]};
    double CL_length = std::sqrt(CL_vec[0] * CL_vec[0] + CL_vec[1] * CL_vec[1]);

    // 如果长度不匹配，更新a（与Python一致）
    if (std::abs(CL_length - a) > 1e-9)
    {
        a = CL_length;
    }

    if (CL_length == 0)
    {
        throw std::invalid_argument("C和L不能是同一个点");
    }

    std::vector<double> CL_unit = {CL_vec[0] / CL_length, CL_vec[1] / CL_length};

    // 与Python一致的两个垂直方向
    std::vector<double> perp1 = {-CL_unit[1], CL_unit[0]};
    std::vector<double> perp2 = {CL_unit[1], -CL_unit[0]};

    std::vector<double> D1_1 = {
        L[0] + b * perp1[0],
        L[1] + b * perp1[1]};

    std::vector<double> D1_2 = {
        L[0] + b * perp2[0],
        L[1] + b * perp2[1]};

    // 与Python一致：返回D1_2
    return D1_2;
}

// 根据C和L点计算D1点
std::vector<double> ReverseKinematicsCalculator::CalculateD1PointsFromCAndLReverse(
    const std::vector<double> &C_p, const std::vector<double> &L_p)
{
    std::vector<double> D1_p = FindRightAngleThirdVertex(C_p, L_p, L30, D1_L_offset);
    return D1_p;
}

// 求直角三角形直角顶点D2
std::vector<double> ReverseKinematicsCalculator::FindRightAngleVertexD2(
    const std::vector<double> &L, const std::vector<double> &C4,
    double a, double b)
{
    std::vector<double> v = {C4[0] - L[0], C4[1] - L[1]};
    double d = std::sqrt(v[0] * v[0] + v[1] * v[1]);

    // 检查（只记录，不处理）
    if (std::abs(d * d - (a * a + b * b)) > 1e-9)
    {
        // 不处理，与Python一致
    }

    std::vector<double> u = {v[0] / d, v[1] / d};

    // 与Python一致的两个垂直方向
    std::vector<double> u_perp1 = {-u[1], u[0]};
    std::vector<double> u_perp2 = {u[1], -u[0]};

    double along = (b * b / d);
    double perp = (a * b / d);

    std::vector<double> D2_1 = {
        L[0] + along * u[0] + perp * u_perp1[0],
        L[1] + along * u[1] + perp * u_perp1[1]};

    std::vector<double> D2_2 = {
        L[0] + along * u[0] + perp * u_perp2[0],
        L[1] + along * u[1] + perp * u_perp2[1]};

    // 与Python一致：返回D2_2
    return D2_2;
}

// 根据L和C4点计算D2点
std::vector<double> ReverseKinematicsCalculator::CalculateD2PointsFromCAndLReverse(
    const std::vector<double> &L, const std::vector<double> &C4)
{
    std::vector<double> D2_p = FindRightAngleVertexD2(L, C4, C4_D2_offset, D2_L_offset);
    return D2_p;
}

// 求直角三角形直角顶点C3
std::vector<double> ReverseKinematicsCalculator::FindRightAngleVertex(
    const std::vector<double> &C2, const std::vector<double> &C4, double L)
{
    std::vector<double> u = {C4[0] - C2[0], C4[1] - C2[1]};
    double d = std::sqrt(u[0] * u[0] + u[1] * u[1]);

    if (d == 0)
        throw std::invalid_argument("C2和C4不能重合");

    if (L > d)
        throw std::invalid_argument("直角边L=" + std::to_string(L) + "不能大于斜边长度" + std::to_string(d));

    std::vector<double> u_perp = {-u[1], u[0]};

    // 与Python一致的计算
    double t_val = (L / d) * std::sqrt(std::max(0.0, 1.0 - L * L / (d * d)));

    double factor = L * L / (d * d);
    std::vector<double> v1 = {
        factor * u[0] + t_val * u_perp[0],
        factor * u[1] + t_val * u_perp[1]};

    std::vector<double> v2 = {
        factor * u[0] - t_val * u_perp[0],
        factor * u[1] - t_val * u_perp[1]};

    std::vector<double> C3_1 = {C2[0] + v1[0], C2[1] + v1[1]};
    std::vector<double> C3_2 = {C2[0] + v2[0], C2[1] + v2[1]};

    // 与Python一致：返回C3_1
    return C3_1;
}

// 根据C2和C4点计算C3点
std::vector<double> ReverseKinematicsCalculator::CalculateC3PointFromC2AndC4Reverse(
    const std::vector<double> &C2_point, const std::vector<double> &C4_point)
{
    std::vector<double> C3_point = FindRightAngleVertex(C2_point, C4_point, C2C3_length);
    return C3_point;
}

// 主函数：根据电机角度反向计算所有关键点位置
std::map<std::string, std::vector<double>> ReverseKinematicsCalculator::CalculateAllPointsFromMotorAnglesReverse(
    double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
    const std::string &leg_type, bool IsReset)
{
    // 与Python完全一致的右侧腿角度调整逻辑
    double adjusted_thigh_angle = thigh_angle_deg;
    double adjusted_shank_angle = shank_angle_deg;

    if (leg_type == "RF" || leg_type == "RH")
    {
        adjusted_thigh_angle = 360 - thigh_angle_deg;
        adjusted_thigh_angle = std::fmod(adjusted_thigh_angle, 360);
        adjusted_shank_angle = -shank_angle_deg;
    }

    // 与Python完全一致的计算顺序
    std::vector<double> B_point = CalculateBFromThighAngleReverse(adjusted_thigh_angle);
    std::vector<double> B1_point = CalculateB1FromShankAngleReverse(B_point, adjusted_shank_angle);

    auto b2_result = CalculateB2FromFourbarReverse(A_point, B_point, B1_point, L22, L23);
    std::vector<double> B2_point = b2_result.first;

    std::vector<double> C_point = CalculateCFromBAndB2Reverse(B_point, B2_point, L20, beta1);
    std::vector<double> C1_point = CalculateCFromBAndB2Reverse(B_point, B2_point, _C1_B_length, beta1);
    std::vector<double> C2_point = CalculateC2PointFromC1Reverse(C1_point, B_point, C_point);
    std::vector<double> C4_point = CalculateC4PointFromCAndC2Reverse(C_point, C2_point, ankle_angle_deg, leg_type);
    std::vector<double> L_point = CalculateLPointsFromCAndC4Reverse(C_point, C4_point);
    std::vector<double> D1_point = CalculateD1PointsFromCAndLReverse(C_point, L_point);
    std::vector<double> D2_point = CalculateD2PointsFromCAndLReverse(L_point, C4_point);
    std::vector<double> C3_point = CalculateC3PointFromC2AndC4Reverse(C2_point, C4_point);

    // 构建初始结果字典
    std::map<std::string, std::vector<double>> initial_points_dict = {
        {"A", A_point},
        {"B", B_point},
        {"B1", B1_point},
        {"B2", B2_point},
        {"C", C_point},
        {"C1", C1_point},
        {"C2", C2_point},
        {"C3", C3_point},
        {"C4", C4_point},
        {"D1", D1_point},
        {"D2", D2_point},
        {"L", L_point}};

    bool IsSuppor = IsSupportPoint(initial_points_dict);

    double deltX = 0.0;
    if (leg_type == "LF")
    {
        if (IsSupporLF)
        {
            auto old_points_dict = _manager->Get_old_initial_points_dictLF();
            auto it = old_points_dict.find("L");
            if (it != old_points_dict.end() && it->second.size() >= 1)
            {
                deltX = L_point[0] - it->second[0];
                if (std::abs(deltX) > 20)
                {
                    deltX = 0;
                }

                if (IsReset)
                {
                    deltX = 0;
                }
                _manager->SetRobotBody_x(_manager->GetRobotBody().x + deltX);
            }
        }
        IsSupporLF = IsSuppor;

        // 保存当前计算的点到管理器
        std::map<std::string, std::vector<double>> transformed_points_dict;
        double A_y = A_point[1];

        for (const auto &kvp : initial_points_dict)
        {
            const auto &original_point = kvp.second;
            std::vector<double> transformed_point = {
                original_point[0],
                A_y - original_point[1]};
            transformed_points_dict[kvp.first] = transformed_point;
        }

        _manager->Set_old_initial_points_dictLF(transformed_points_dict);
    }
    else if (leg_type == "LH")
    {
        if (IsSupporLH)
        {
            auto old_points_dict = _manager->Get_old_initial_points_dictLH();
            auto it = old_points_dict.find("L");
            if (it != old_points_dict.end() && it->second.size() >= 1)
            {
                deltX = L_point[0] - it->second[0];
                if (std::abs(deltX) > 20)
                {
                    deltX = 0;
                }

                if (IsReset)
                {
                    deltX = 0;
                }
                _manager->SetRobotBody_x(_manager->GetRobotBody().x + deltX);
            }
        }
        IsSupporLH = IsSuppor;

        // 保存当前计算的点到管理器
        std::map<std::string, std::vector<double>> transformed_points_dict;
        double A_y = A_point[1];

        for (const auto &kvp : initial_points_dict)
        {
            const auto &original_point = kvp.second;
            std::vector<double> transformed_point = {
                original_point[0],
                A_y - original_point[1]};
            transformed_points_dict[kvp.first] = transformed_point;
        }

        _manager->Set_old_initial_points_dictLH(transformed_points_dict);
        // old_L.clear();
        // old_initial_points_dictLH.clear();
    }

    // 最终的坐标变换
    std::map<std::string, std::vector<double>> final_transformed_points_dict;
    double A_y = A_point[1];

    for (const auto &kvp : initial_points_dict)
    {
        const auto &original_point = kvp.second;
        std::vector<double> transformed_point = {
            original_point[0],
            A_y - original_point[1]};
        final_transformed_points_dict[kvp.first] = transformed_point;
    }

    return final_transformed_points_dict;
}

// 判断是否为支撑点
bool ReverseKinematicsCalculator::IsSupportPoint(
    const std::map<std::string, std::vector<double>> &pointsDict,
    double groundHeight, double tolerance)
{
    std::vector<double> C = GetPointSafe(pointsDict, "C");
    std::vector<double> L = GetPointSafe(pointsDict, "L");
    std::vector<double> D1 = GetPointSafe(pointsDict, "D1");
    std::vector<double> D2 = GetPointSafe(pointsDict, "D2");
    std::vector<double> C4 = GetPointSafe(pointsDict, "C4");

    // 1. 检查Y坐标接近地面高度
    bool L_y_close_to_ground = std::abs(L[1] - groundHeight) <= tolerance;

    // 2. 检查C点在L点正上方
    double vertical_distance = std::abs(C[1] - L[1]);
    double horizontal_distance = std::abs(C[0] - L[0]);
    bool vertical_check = std::abs(vertical_distance - L30) <= tolerance;
    bool horizontal_check = horizontal_distance <= tolerance;
    bool C_above_L = vertical_check && horizontal_check;

    // 3. 检查脚部构型
    bool D1_y_close_to_ground = std::abs(D1[1] - groundHeight) <= tolerance;
    bool D2_y_close_to_ground = std::abs(D2[1] - groundHeight) <= tolerance;
    bool D1_D2_same_height = std::abs(D1[1] - D2[1]) <= tolerance;
    bool foot_contact = D1_y_close_to_ground && D2_y_close_to_ground && D1_D2_same_height;

    // 4. 检查C-L连线接近垂直
    bool CL_nearly_vertical;
    double CL_slope = std::numeric_limits<double>::infinity();

    if (std::abs(C[0] - L[0]) > 1e-6)
    {
        CL_slope = (C[1] - L[1]) / (C[0] - L[0]);
        CL_nearly_vertical = std::abs(CL_slope) > 10;
    }
    else
    {
        CL_nearly_vertical = true;
    }

    // 5. 检查C4点位置关系
    bool C4_above_D2 = std::abs(C4[0] - D2[0]) <= tolerance &&
                       std::abs(C4[1] - D2[1] - C4_D2_offset) <= tolerance;

    // 综合判断
    bool all_checks_passed = L_y_close_to_ground &&
                             C_above_L &&
                             foot_contact &&
                             CL_nearly_vertical &&
                             C4_above_D2;

    return all_checks_passed;
}

// 从字典安全获取点坐标
std::vector<double> ReverseKinematicsCalculator::GetPointSafe(
    const std::map<std::string, std::vector<double>> &pointsDict,
    const std::string &key)
{
    auto it = pointsDict.find(key);
    if (it != pointsDict.end() && !it->second.empty() && it->second.size() >= 2)
    {
        return it->second;
    }

    return {0.0, 0.0};
}

// 验证逆向计算结果
std::pair<bool, std::vector<std::string>> ReverseKinematicsCalculator::ValidateReverseCalculation(
    const std::map<std::string, std::vector<double>> &points_dict,
    double thigh_angle, double shank_angle, double ankle_angle)
{
    std::vector<std::string> errors;

    std::vector<double> A = points_dict.at("A");
    std::vector<double> B = points_dict.at("B");
    std::vector<double> B1 = points_dict.at("B1");
    std::vector<double> B2 = points_dict.at("B2");
    std::vector<double> C = points_dict.at("C");
    std::vector<double> C1 = points_dict.at("C1");
    std::vector<double> C2 = points_dict.at("C2");
    std::vector<double> C3 = points_dict.at("C3");
    std::vector<double> C4 = points_dict.at("C4");
    std::vector<double> D1 = points_dict.at("D1");
    std::vector<double> D2 = points_dict.at("D2");
    std::vector<double> L = points_dict.at("L");

    // C-C4距离检查
    double CC4_actual = std::sqrt(
        std::pow(C4[0] - C[0], 2) +
        std::pow(C4[1] - C[1], 2));

    if (std::abs(CC4_actual - _CC4_distance) > 1e-3)
    {
        errors.push_back("C-C4距离: 期望" + std::to_string(_CC4_distance) +
                         ", 实际" + std::to_string(CC4_actual));
    }

    // C-L距离检查
    double CL_actual = std::sqrt(
        std::pow(C[0] - L[0], 2) +
        std::pow(C[1] - L[1], 2));

    if (std::abs(CL_actual - L30) > 1e-3)
    {
        errors.push_back("C-L距离: 期望" + std::to_string(L30) +
                         ", 实际" + std::to_string(CL_actual));
    }

    // 杆长检查
    std::vector<std::tuple<std::string, std::vector<double>, std::vector<double>, double>> lengths_to_check = {
        {"AB", A, B, L10},
        {"BC", B, C, L20},
        {"AB1", A, B1, L21},
        {"B1B2", B1, B2, L22},
        {"BB2", B, B2, L23},
        {"CC1", C, C1, CC1},
        {"C1C2", C1, C2, L31},
        {"C2C3", C2, C3, C2C3_length}};

    for (const auto &check : lengths_to_check)
    {
        std::string name = std::get<0>(check);
        std::vector<double> p1 = std::get<1>(check);
        std::vector<double> p2 = std::get<2>(check);
        double expected = std::get<3>(check);

        double actual = std::sqrt(
            std::pow(p2[0] - p1[0], 2) +
            std::pow(p2[1] - p1[1], 2));

        double error = std::abs(actual - expected);

        if (error > 1e-3)
        {
            errors.push_back(name + ": 期望" + std::to_string(expected) +
                             ", 实际" + std::to_string(actual) +
                             ", 误差" + std::to_string(error));
        }
    }

    return {errors.empty(), errors};
}

// 批量逆向计算
std::vector<ReverseCalculationResult> ReverseKinematicsCalculator::BatchReverseCalculation(
    const std::map<std::string, std::vector<double>> &motor_data,
    const std::string &leg_type, std::optional<int> max_frames)
{
    // 检查必需的数据键
    if (motor_data.find("AB_angles") == motor_data.end() ||
        motor_data.find("AB1_AB_angles") == motor_data.end() ||
        motor_data.find("ankle_motor_angles") == motor_data.end() ||
        motor_data.find("frame_start_times") == motor_data.end())
    {
        throw std::invalid_argument("motor_data必须包含AB_angles、AB1_AB_angles、ankle_motor_angles和frame_start_times键");
    }

    const std::vector<double> &ab_angles = motor_data.at("AB_angles");
    const std::vector<double> &ab1_ab_angles = motor_data.at("AB1_AB_angles");
    const std::vector<double> &ankle_motor_angles = motor_data.at("ankle_motor_angles");
    const std::vector<double> &frame_start_times = motor_data.at("frame_start_times");

    int n_frames = ab_angles.size();
    if (max_frames.has_value())
    {
        n_frames = std::min(n_frames, max_frames.value());
    }

    std::vector<ReverseCalculationResult> all_reverse_points;
    int valid_count = 0;

    for (int i = 0; i < n_frames; i++)
    {
        try
        {
            double thigh_angle = ab_angles[i];
            double shank_angle = ab1_ab_angles[i];
            double ankle_angle = ankle_motor_angles[i];

            auto points = CalculateAllPointsFromMotorAnglesReverse(
                thigh_angle, shank_angle, ankle_angle, leg_type);

            auto validation_result = ValidateReverseCalculation(
                points, thigh_angle, shank_angle, ankle_angle);

            bool is_valid = validation_result.first;
            std::vector<std::string> errors = validation_result.second;

            if (is_valid)
                valid_count++;

            ReverseCalculationResult result;
            result.Frame = i;
            result.Time = frame_start_times[i];
            result.Points = points;
            result.Valid = is_valid;
            result.Errors = errors;

            all_reverse_points.push_back(result);
        }
        catch (const std::exception &e)
        {
            ReverseCalculationResult result;
            result.Frame = i;
            result.Time = frame_start_times[i];
            result.Valid = false;
            result.Errors = {e.what()};

            all_reverse_points.push_back(result);
        }
    }

    return all_reverse_points;
}

// 计算所有腿的JSON字符串
std::string ReverseKinematicsCalculator::CalculateAllPointsFromMotorAnglesJsonStr()
{
    // std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;

    auto paramDict = _manager->GetParamDictionary();

    if (paramDict.empty())
    {
        std::cout << "[DEBUG] 没有Param数据 json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;

        return "没有Param数据";
    }
    // debugPrintParamDict(paramDict);
    std::vector<C_ObjStates> objStates;

    for (const auto &kvp : paramDict)
    {
        std::string leg_type = kvp.first;
        LegParam legParam = kvp.second;

        double thigh_angle_deg = legParam.thigh_angle_deg;
        double shank_angle_deg = legParam.shank_angle_deg;
        double ankle_angle_deg = legParam.ankle_angle_deg;

        auto points_dict = CalculateAllPointsFromMotorAnglesReverse(
            thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);

        // std::cout << "计算得到的点数量: " << points_dict.size() << "\n";
        // std::cout << "包含的点: ";
        for (const auto &p : points_dict)
        {
            std::cout << p.first << " ";
        }
        // std::cout << "\n";

        // 改为：
        auto modelID = _manager->GetModelIDObject(); // 使用新方法

        auto leg = GetLegModelIDs(modelID, leg_type);

        for (const auto &legbar : leg.components)
        {
            std::string ModelID_uuid = legbar.uuid;
            // std::string ModelID_uuid = GetValueModelID(modelID, leg_type, legbar.first);
            auto p_name = StringUtils::splitByDelimiter(legbar.name);

            if (!p_name.second.empty())
            {
                std::string p_name1 = p_name.first;
                std::string p_name2 = p_name.second;

                auto p1 = points_dict[p_name1];
                auto p2 = points_dict[p_name2];

                auto c_ObjStates = PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(
                    ModelID_uuid, p1[0], p1[1], p2[0], p2[1]);

                objStates.push_back(c_ObjStates);
            }
        }
    }

    // 添加身体姿态
    C_ObjStates bodyState;
    bodyState.i = _manager->GetRobotBody().BodyCode;
    bodyState.tx = std::to_string(std::round(_manager->GetRobotBody().x / 1000 * 100000) / 100000);
    bodyState.ty = std::to_string(std::round(_manager->GetRobotBody().y / 1000 * 100000) / 100000);
    bodyState.tz = std::to_string(std::round(_manager->GetRobotBody().z / 1000 * 100000) / 100000);
    bodyState.qx = std::to_string(std::round(_manager->GetRobotBody().qx * 100000) / 100000);
    bodyState.qy = std::to_string(std::round(_manager->GetRobotBody().qy * 100000) / 100000);
    bodyState.qz = std::to_string(std::round(_manager->GetRobotBody().qz * 100000) / 100000);
    bodyState.qw = std::to_string(std::round(_manager->GetRobotBody().qw * 100000) / 100000);

    objStates.push_back(bodyState);

    C_Frames frame;
    frame.objStates = objStates;

    json j = frame;
    return j.dump();
}

// 计算单腿的JSON字符串
std::string ReverseKinematicsCalculator::CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
    double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
    const std::string &leg_type, bool IsReset)
{
    auto paramDict = _manager->GetParamDictionary();

    if (paramDict.empty())
    {
        return "没有Param数据";
    }

    std::vector<C_ObjStates> objStates;

    auto points_dict = CalculateAllPointsFromMotorAnglesReverse(
        thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type, IsReset);

    // auto modelID = _manager->GetModelID();
    // auto splitModelID = ModelIDSplitter::SplitModelIDToArrays(modelID, points_dict);

    // 改为：
    auto modelID = _manager->GetModelIDObject(); // 使用新方法
    // auto splitModelID = ModelIDSplitter::SplitModelIDToArrays(modelID, points_dict);

    // auto simpleDict = SplitModelIDExtensions::ToSimpleDictionary(splitModelID);

    auto leg = GetLegModelIDs(modelID, leg_type);

    for (const auto &legbar : leg.components)
    {
        std::string ModelID_uuid = legbar.uuid;
        // std::string ModelID_uuid = GetValueModelID(modelID, leg_type, legbar.first);
        auto p_name = StringUtils::splitByDelimiter(legbar.name);

        if (!p_name.second.empty())
        {
            std::string p_name1 = p_name.first;
            std::string p_name2 = p_name.second;

            auto p1 = points_dict[p_name1];
            auto p2 = points_dict[p_name2];

            auto c_ObjStates = PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(
                ModelID_uuid, p1[0], p1[1], p2[0], p2[1]);

            objStates.push_back(c_ObjStates);
        }
    }

    // 添加身体姿态
    C_ObjStates bodyState;
    bodyState.i = BodyCode;
    bodyState.tx = std::to_string(std::round(BodyPosition_x / 1000 * 100000) / 100000);
    bodyState.ty = std::to_string(std::round(BodyPosition_y / 1000 * 100000) / 100000);
    bodyState.tz = std::to_string(std::round(BodyPosition_z / 1000 * 100000) / 100000);
    bodyState.qx = std::to_string(std::round(BodyPosition_qx * 100000) / 100000);
    bodyState.qy = std::to_string(std::round(BodyPosition_qy * 100000) / 100000);
    bodyState.qz = std::to_string(std::round(BodyPosition_qz * 100000) / 100000);
    bodyState.qw = std::to_string(std::round(BodyPosition_qw * 100000) / 100000);

    objStates.push_back(bodyState);

    C_Frames frame;
    frame.objStates = objStates;

    json j = frame;
    return j.dump();
}

// 获取模型ID
// 在 KinematicsReverse.cpp 中更新 GetValueModelID 函数实现
// KinematicsReverse.cpp 中的 GetValueModelID 函数修改

// std::string ReverseKinematicsCalculator::GetValueModelID(
//     const ModelID &modelID, // 这是 RobotConfig.hpp 中的 ModelID
//     const std::string &legName, const std::string &key)
// {
//     const LegModelIDs *legComponents = nullptr;

//     if (legName == "LF")
//         legComponents = &modelID.LF;
//     else if (legName == "LH")
//         legComponents = &modelID.LH;
//     else if (legName == "RF")
//         legComponents = &modelID.RF;
//     else if (legName == "RH")
//         legComponents = &modelID.RH;
//     else
//         return "";

//     if (!legComponents)
//         return "";

//     // 在 legComponents->components 中查找
//     for (const auto &component : legComponents->components)
//     {
//         if (component.name == key) // 假设 key 对应 ComponentID 的 name
//         {
//             return component.uuid;
//         }
//     }

//     return "";
// }
// std::string ReverseKinematicsCalculator::GetValueModelID(
//     const LegModelIDs &legComponents, const std::string &key)
// {
//     const LegModelIDs *legComponents = nullptr;

//     for (const auto &component : legComponents->components)
//     {
//         if (component.name == key) // 假设 key 对应 ComponentID 的 name
//         {
//             return component.uuid;
//         }
//     }

//     return "";
// }
// 修改 GetLegModelIDs 函数实现（删除原来的错误返回）
const LegModelIDs &ReverseKinematicsCalculator::GetLegModelIDs(
    const ModelID &modelID,
    const std::string &legName)
{
    // 定义静态的空对象，用于返回默认值
    static LegModelIDs emptyLeg;

    if (legName == "LF")
        return modelID.LF;
    else if (legName == "LH")
        return modelID.LH;
    else if (legName == "RF")
        return modelID.RF;
    else if (legName == "RH")
        return modelID.RH;
    else
        return emptyLeg; // 返回空对象而不是字符串
}
void ReverseKinematicsCalculator::debugPrintParamDict(const std::map<std::string, LegParam> &paramDict) const
{
    std::cout << "共有 " << paramDict.size() << " 条腿:\n";

    for (const auto &kvp : paramDict)
    {
        std::cout << "  腿 " << kvp.first << ":\n";
        std::cout << "    thigh_angle_deg: " << kvp.second.thigh_angle_deg << "°\n";
        std::cout << "    shank_angle_deg: " << kvp.second.shank_angle_deg << "°\n";
        std::cout << "    ankle_angle_deg: " << kvp.second.ankle_angle_deg << "°\n";
    }

    if (paramDict.empty())
    {
        std::cout << "  [WARNING] paramDict 为空!\n";
    }
}

void ReverseKinematicsCalculator::debugPrintLegCalculation(
    const std::string &legName,
    const LegParam &legParam,
    const std::map<std::string, std::vector<double>> &points_dict,
    const ModelID &modelID) const
{
    std::cout << "  腿: " << legName << "\n";
    std::cout << "  角度参数: [" << legParam.thigh_angle_deg << "°, "
              << legParam.shank_angle_deg << "°, "
              << legParam.ankle_angle_deg << "°]\n";

    // 显示几个关键点
    std::vector<std::string> keyPoints = {"A", "B", "C", "L", "D1", "D2", "C4"};
    std::cout << "  关键点坐标:\n";
    for (const auto &pointName : keyPoints)
    {
        auto it = points_dict.find(pointName);
        if (it != points_dict.end() && it->second.size() >= 2)
        {
            std::cout << "    " << pointName << ": [" << it->second[0]
                      << ", " << it->second[1] << "]\n";
        }
    }

    // 显示 ModelID 信息
    const LegModelIDs *legComponents = nullptr;
    if (legName == "LF")
        legComponents = &modelID.LF;
    else if (legName == "LH")
        legComponents = &modelID.LH;
    else if (legName == "RF")
        legComponents = &modelID.RF;
    else if (legName == "RH")
        legComponents = &modelID.RH;

    if (legComponents)
    {
        std::cout << "  ModelID 组件数量: " << legComponents->components.size() << "\n";
        for (size_t i = 0; i < legComponents->components.size(); ++i)
        {
            std::cout << "    组件[" << i << "]: "
                      << legComponents->components[i].name
                      << " -> " << legComponents->components[i].uuid << "\n";
        }
    }
    else
    {
        std::cout << "  [ERROR] 未找到腿 " << legName << " 的 ModelID 组件\n";
    }
}

void ReverseKinematicsCalculator::debugPrintFinalJson(const std::vector<C_ObjStates> &objStates) const
{
    std::cout << "总共有 " << objStates.size() << " 个对象状态:\n\n";

    C_Frames frame;
    frame.objStates = objStates;

    json j = frame;
    std::string jsonStr = j.dump(2); // 使用缩进格式

    // 分割显示 JSON，避免过长
    std::cout << "生成的 JSON (前1000字符):\n";
    if (jsonStr.length() > 1000)
    {
        std::cout << jsonStr.substr(0, 1000) << "...\n";
        std::cout << "(JSON 总长度: " << jsonStr.length() << " 字符)\n";
    }
    else
    {
        std::cout << jsonStr << "\n";
    }

    std::cout << "\n对象状态详情:\n";
    int count = 0;
    for (const auto &obj : objStates)
    {
        std::cout << "\n  对象[" << count++ << "]:\n";
        std::cout << "    ID: " << obj.i << "\n";
        std::cout << "    位置: [" << obj.tx << ", " << obj.ty << ", " << obj.tz << "]\n";
        std::cout << "    四元数: [" << obj.qx << ", " << obj.qy << ", "
                  << obj.qz << ", " << obj.qw << "]\n";
    }
}
