// KinematicsSimulation.cpp
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
#include <functional>
#include <numeric>
#include <iomanip>
#include <sstream>

// 构造函数
QuadrupedRobotSimulation::QuadrupedRobotSimulation(const QuadrupedRobotConfiguration &config)
    : _config(config)
{
    LoadParametersFromConfig();
    InitializeParameters();
}

QuadrupedRobotSimulation::QuadrupedRobotSimulation()
    : QuadrupedRobotSimulation(QuadrupedRobotConfiguration()) {}

// 从配置加载参数
void QuadrupedRobotSimulation::LoadParametersFromConfig()
{
    // 基本几何参数
    A_x = _config.A_point[0];
    A_y = _config.A_point[1];
    A_point = _config.A_point;
    L10 = _config.L10;
    L20 = _config.L20;
    L30 = _config.L30;
    s_l = _config.s_l;
    s_h = _config.s_h;
    X = _config.X;

    // 小腿连杆机构参数
    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;

    // 步态控制参数
    T = _config.T;
    f = _config.f;
    gait_type = _config.gait_type;
    support_ratio = _config.support_ratio;
    swing_ratio = _config.swing_ratio;

    // 电机参数
    thigh_motor_reduction = _config.thigh_motor_reduction;
    shank_motor_reduction = _config.shank_motor_reduction;
    ankle_motor_reduction = _config.ankle_motor_reduction;

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

    // 步态相位时间
    timeLF = _config.timeLF;
    timeLH = _config.timeLH;
    timeRF = _config.timeRF;
    timeRH = _config.timeRH;
}

// 初始化参数
void QuadrupedRobotSimulation::InitializeParameters()
{
    // 如果A_point为空，则创建
    if (A_point.empty() || A_point.size() != 2)
    {
        A_point = {A_x, A_y};
    }
    else
    {
        A_point[0] = A_x;
        A_point[1] = A_y;
    }

    BB2_BC_angle = (360 - beta1) * PI / 180; // 转换为弧度
    C1_B_length = L20 - CC1;
    deltX = s_l / 2 - X;
    step = 1.0 / f; // 时间步长 = 1/频率

    // 设置步态相位时间
    SetGaitParameters();

    t_s = T * support_ratio;
    t_w = T * swing_ratio;
}

// 设置步态参数
void QuadrupedRobotSimulation::SetGaitParameters()
{
    if (gait_type == "walk")
    { // 行走步态
        timeLF = 0;
        timeLH = 1.0 / 4;     // 90度相位差
        timeRF = 2.0 / 4;     // 180度相位差
        timeRH = 3.0 / 4;     // 270度相位差
        support_ratio = 0.75; // 75%支撑，25%摆动
        swing_ratio = 1 - support_ratio;
    }
    else if (gait_type == "trot")
    { // 对角步态
        timeLF = 0;
        timeLH = 1.0 / 2; // 对角同相
        timeRF = 1.0 / 2; // 对角同相
        timeRH = 0;
        support_ratio = 0.5; // 50%支撑，50%摆动
        swing_ratio = 1 - support_ratio;
    }
    else if (gait_type == "standup")
    { // 蹲起步态
        timeLF = 0;
        timeLH = 0; // 无相位差
        timeRF = 0;
        timeRH = 0;
        support_ratio = 0.2; // 20%支撑，80%摆动
        swing_ratio = 1 - support_ratio;
    }
    else
    {
        throw std::invalid_argument("不支持的步态类型 '" + gait_type + "'");
    }

    // 重新计算支撑和摆动时间
    t_s = T * support_ratio;
    t_w = T * swing_ratio;
}

// 主计算函数
std::string QuadrupedRobotSimulation::CalculateAllTrajectories_JsonStr()
{
    try
    {
        CalculateTrajectoryL_V2();                                        // 计算L点轨迹
        CalculateTrajectoryC();                                           // 计算C点轨迹
        CalculateTrajectoryB();                                           // 计算B点轨迹
        CalculateAllPoints();                                             // 计算所有辅助点
        CalculateAllAngles();                                             // 计算所有角度
        CheckConstraints();                                               // 检查机构约束
        CalculateMotorData();                                             // 计算电机数据
        return CompleteJsonExporter::ExportCompleteDataJsonString(*this); // 导出数据
    }
    catch (const std::exception &ex)
    {
        throw std::runtime_error("计算过程中出错: " + std::string(ex.what()));
    }
}

void QuadrupedRobotSimulation::CalculateAllTrajectories()
{
    try
    {
        CalculateTrajectoryL_V2(); // 计算L点轨迹
        CalculateTrajectoryC();    // 计算C点轨迹
        CalculateTrajectoryB();    // 计算B点轨迹
        CalculateAllPoints();      // 计算所有辅助点
        CalculateAllAngles();      // 计算所有角度
        CheckConstraints();        // 检查机构约束
        CalculateMotorData();      // 计算电机数据
    }
    catch (const std::exception &ex)
    {
        throw std::runtime_error("计算过程中出错: " + std::string(ex.what()));
    }
}

std::string QuadrupedRobotSimulation::CalculateAllTrajectoriesJsonString()
{
    std::string jsonStr = "{}";
    try
    {
        CalculateTrajectoryL_V2();                                           // 计算L点轨迹
        CalculateTrajectoryC();                                              // 计算C点轨迹
        CalculateTrajectoryB();                                              // 计算B点轨迹
        CalculateAllPoints();                                                // 计算所有辅助点
        CalculateAllAngles();                                                // 计算所有角度
        CheckConstraints();                                                  // 检查机构约束
        CalculateMotorData();                                                // 计算电机数据
        jsonStr = CompleteJsonExporter::ExportCompleteDataJsonString(*this); // 导出数据
    }
    catch (const std::exception &ex)
    {
        throw std::runtime_error("计算过程中出错: " + std::string(ex.what()));
    }
    return jsonStr;
}

// 计算L点轨迹
void QuadrupedRobotSimulation::CalculateTrajectoryL_V2()
{
    Trajectory_L.clear();

    int n_swing = PythonRoundExact(t_w / step);   // 摆动阶段点数
    int n_support = PythonRoundExact(t_s / step); // 支撑阶段点数

    // 贝塞尔曲线控制点
    std::vector<double> P0 = {-s_l / 2 - deltX, 0};         // 起点
    std::vector<double> P1 = {-s_l / 2 - deltX, 1.2 * s_h}; // 控制点1
    std::vector<double> P2 = {s_l / 2 - deltX, 1.8 * s_h};  // 控制点2
    std::vector<double> P3 = {s_l / 2 - deltX, 0};          // 终点

    auto B = Zeros(n_swing, 2);                      // 摆动轨迹点
    std::vector<double> t = Linspace(0, 1, n_swing); // 归一化时间参数

    // 计算贝塞尔曲线点
    for (int i = 0; i < n_swing; i++)
    {
        double ti = t[i];
        double one_minus_t = 1 - ti;
        double B_x = pow(one_minus_t, 3) * P0[0] +
                     3 * pow(one_minus_t, 2) * ti * P1[0] +
                     3 * one_minus_t * pow(ti, 2) * P2[0] +
                     pow(ti, 3) * P3[0];
        double B_y = pow(one_minus_t, 3) * P0[1] +
                     3 * pow(one_minus_t, 2) * ti * P1[1] +
                     3 * one_minus_t * pow(ti, 2) * P2[1] +
                     pow(ti, 3) * P3[1];
        B[i][0] = B_x;
        B[i][1] = B_y;
    }

    auto A = Zeros(n_support, 2); // 支撑轨迹点
    std::vector<double> xA = Linspace(-s_l / 2 - deltX, s_l / 2 - deltX, n_support);

    // 支撑阶段：直线运动
    for (int i = 0; i < n_support; i++)
    {
        A[i][0] = xA[i];
        A[i][1] = 0;
    }

    auto tr_B1 = Flip(B);                    // 翻转摆动轨迹
    auto trajectoryArray = VStack(tr_B1, A); // 合并摆动和支撑轨迹

    // 转换为列表格式
    Trajectory_L = trajectoryArray;
}

// 计算C点轨迹
void QuadrupedRobotSimulation::CalculateTrajectoryC()
{
    Trajectory_C.clear();
    CL_distances.clear();
    dot_products.clear();

    int n_total = Trajectory_L.size();
    int n_swing = round(t_w / step);

    // 获取支撑阶段起始和结束位置
    std::vector<double> support_end_L = Trajectory_L[0];
    std::vector<double> support_start_L = Trajectory_L[n_swing - 1];

    auto support_end_points = CalculateSupportPoints(support_end_L);
    auto support_start_points = CalculateSupportPoints(support_start_L);

    std::vector<double> C_end = support_end_points[0];
    std::vector<double> C_start = support_start_points[0];

    std::vector<double> B_end = Calculate_B_Position(A_point, C_end, L10, L20);
    std::vector<double> B_start = Calculate_B_Position(A_point, C_start, L10, L20);

    double BCL_angle_end = Calculate_BCL_Angle(B_end, C_end, support_end_L);
    double BCL_angle_start = Calculate_BCL_Angle(B_start, C_start, support_start_L);

    // 生成摆动阶段的BCL角度序列（非线性变化）
    std::vector<double> swing_BCL_angles;
    for (int i = 0; i < n_swing; i++)
    {
        double t_val = static_cast<double>(i) / (n_swing - 1);
        double target_angle = NonlinearAngleFunction(t_val, BCL_angle_end * 180 / PI,
                                                     BCL_angle_start * 180 / PI,
                                                     "quadratic_ease_out", 3.0) *
                              PI / 180;
        swing_BCL_angles.push_back(target_angle);
    }

    double prev_angle = atan2(C_end[1] - support_end_L[1], C_end[0] - support_end_L[0]);

    // 计算摆动阶段的C点
    for (int i = 0; i < n_swing; i++)
    {
        std::vector<double> C_point;
        if (i == 0)
        {
            C_point = C_end;
        }
        else if (i == n_swing - 1)
        {
            C_point = CalculateSwingPointWithConstraints(Trajectory_L[i],
                                                         swing_BCL_angles[i], Trajectory_C[i - 1], prev_angle);
        }
        else
        {
            C_point = CalculateSwingPointWithConstraints(Trajectory_L[i],
                                                         swing_BCL_angles[i], Trajectory_C[i - 1], prev_angle);
        }

        Trajectory_C.push_back(C_point);
        double cl_distance = sqrt(pow(C_point[0] - Trajectory_L[i][0], 2) +
                                  pow(C_point[1] - Trajectory_L[i][1], 2));
        CL_distances.push_back(cl_distance);
    }

    // 计算支撑阶段的C点
    for (int i = n_swing; i < n_total; i++)
    {
        auto support_points = CalculateSupportPoints(Trajectory_L[i]);
        std::vector<double> C_point = support_points[0];
        Trajectory_C.push_back(C_point);
        double cl_distance = sqrt(pow(C_point[0] - Trajectory_L[i][0], 2) +
                                  pow(C_point[1] - Trajectory_L[i][1], 2));
        CL_distances.push_back(cl_distance);
    }

    RecalculateAllPoints();
}

// 计算支撑阶段的点
std::vector<std::vector<double>> QuadrupedRobotSimulation::CalculateSupportPoints(const std::vector<double> &L_point)
{
    double x_l = L_point[0];
    double y_l = L_point[1];

    std::vector<double> C = {x_l, y_l + L30};                         // C点在L点正上方
    std::vector<double> D2 = {x_l + D2_L_offset, y_l};                // D2点在L点右侧
    std::vector<double> D1 = {x_l - D1_L_offset, y_l};                // D1点在L点左侧
    std::vector<double> C4 = {x_l + D2_L_offset, y_l + C4_D2_offset}; // C4点在D2点上方

    return {C, D1, D2, C4};
}

// 计算B点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_B_Position(const std::vector<double> &A,
                                                                   const std::vector<double> &C,
                                                                   double AB_length, double BC_length)
{
    double AC_distance = sqrt(pow(C[0] - A[0], 2) + pow(C[1] - A[1], 2));

    // 检查三角形是否可能形成
    if (AC_distance > AB_length + BC_length || AC_distance < fabs(AB_length - BC_length))
    {
        return {(A[0] + C[0]) / 2, (A[1] + C[1]) / 2}; // 返回中点近似
    }

    std::vector<double> AC_vector = {C[0] - A[0], C[1] - A[1]};
    std::vector<double> AC_unit = {AC_vector[0] / AC_distance, AC_vector[1] / AC_distance};

    // 余弦定理计算夹角
    double cos_angle = (pow(AB_length, 2) + pow(AC_distance, 2) - pow(BC_length, 2)) /
                       (2 * AB_length * AC_distance);
    cos_angle = std::max(-1.0, std::min(1.0, cos_angle)); // 确保在有效范围内
    double angle = acos(cos_angle);

    // 旋转AC向量得到AB向量
    double cosA = cos(angle);
    double sinA = sin(angle);
    double AB_x = AB_length * (cosA * AC_unit[0] - sinA * AC_unit[1]);
    double AB_y = AB_length * (sinA * AC_unit[0] + cosA * AC_unit[1]);

    double B_x = A[0] + AB_x;
    double B_y = A[1] + AB_y;
    return {B_x, B_y};
}

// 计算B-C-L夹角
double QuadrupedRobotSimulation::Calculate_BCL_Angle(const std::vector<double> &B_point,
                                                     const std::vector<double> &C_point,
                                                     const std::vector<double> &L_point)
{
    std::vector<double> BC_vector = {C_point[0] - B_point[0], C_point[1] - B_point[1]};
    std::vector<double> CL_vector = {L_point[0] - C_point[0], L_point[1] - C_point[1]};

    // 点积公式计算夹角余弦
    double dot_product = BC_vector[0] * CL_vector[0] + BC_vector[1] * CL_vector[1];
    double norm_BC = sqrt(BC_vector[0] * BC_vector[0] + BC_vector[1] * BC_vector[1]);
    double norm_CL = sqrt(CL_vector[0] * CL_vector[0] + CL_vector[1] * CL_vector[1]);

    if (norm_BC > 0 && norm_CL > 0)
    {
        double cos_angle = dot_product / (norm_BC * norm_CL);
        cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
        return acos(cos_angle);
    }
    return 0.0;
}

// 计算满足约束的摆动阶段C点位置
std::vector<double> QuadrupedRobotSimulation::CalculateSwingPointWithConstraints(const std::vector<double> &L_point,
                                                                                 double target_BCL_angle,
                                                                                 const std::vector<double> &prev_C,
                                                                                 double &prev_angle)
{
    double x_l = L_point[0];
    double y_l = L_point[1];
    int max_iterations = 100;
    double tolerance = 1e-8;

    double current_angle;
    if (!prev_C.empty())
    {
        double dx = prev_C[0] - x_l;
        double dy = prev_C[1] - y_l;
        double norm = sqrt(dx * dx + dy * dy);
        current_angle = norm > 1e-10 ? atan2(dy, dx) : prev_angle;
    }
    else
    {
        current_angle = PI / 2; // 默认从正上方开始
    }

    // 初始C点候选位置
    std::vector<double> C_candidate = {
        x_l + L30 * cos(current_angle),
        y_l + L30 * sin(current_angle)};

    // 迭代优化：调整C点位置使BCL角度接近目标值
    for (int iteration = 0; iteration < max_iterations; iteration++)
    {
        std::vector<double> B_candidate = Calculate_B_Position(A_point, C_candidate, L10, L20);
        double current_BCL_angle = Calculate_BCL_Angle(B_candidate, C_candidate, L_point);
        double angle_error = current_BCL_angle - target_BCL_angle;
        if (fabs(angle_error) < tolerance)
            break;
        double angle_adjustment = -0.5 * angle_error; // 调整系数
        current_angle += angle_adjustment;
        C_candidate = {
            x_l + L30 * cos(current_angle),
            y_l + L30 * sin(current_angle)};
    }

    prev_angle = current_angle;
    return C_candidate;
}

// 重新计算所有点
void QuadrupedRobotSimulation::RecalculateAllPoints()
{
    Trajectory_D1.clear();
    Trajectory_D2.clear();
    Trajectory_C4.clear();

    int n_total = Trajectory_L.size();

    for (int i = 0; i < n_total; i++)
    {
        std::vector<double> C = Trajectory_C[i];
        std::vector<double> L = Trajectory_L[i];

        // 确保C-L距离为L30（数值调整）
        std::vector<double> CL_vector = {C[0] - L[0], C[1] - L[1]};
        double current_CL_distance = sqrt(CL_vector[0] * CL_vector[0] + CL_vector[1] * CL_vector[1]);
        if (fabs(current_CL_distance - L30) > 1e-10)
        {
            CL_vector[0] = CL_vector[0] / current_CL_distance * L30;
            CL_vector[1] = CL_vector[1] / current_CL_distance * L30;
            C = {L[0] + CL_vector[0], L[1] + CL_vector[1]};
            Trajectory_C[i] = C;
        }

        // 重新计算CL向量
        CL_vector = {C[0] - L[0], C[1] - L[1]};

        // 计算与CL垂直的单位向量（右侧）
        std::vector<double> perp_vector_right = {CL_vector[1], -CL_vector[0]};
        double perp_norm = sqrt(perp_vector_right[0] * perp_vector_right[0] +
                                perp_vector_right[1] * perp_vector_right[1]);

        if (perp_norm > 0)
        {
            perp_vector_right[0] /= perp_norm;
            perp_vector_right[1] /= perp_norm;
        }
        else
        {
            perp_vector_right = {1, 0}; // 默认向右
        }

        // 计算D2点（右侧）
        std::vector<double> D2 = {
            L[0] + perp_vector_right[0] * D2_L_offset,
            L[1] + perp_vector_right[1] * D2_L_offset};

        // 计算D1点（左侧，与D2相反方向）
        std::vector<double> perp_vector_left = {-perp_vector_right[0], -perp_vector_right[1]};
        std::vector<double> D1 = {
            L[0] + perp_vector_left[0] * D1_L_offset,
            L[1] + perp_vector_left[1] * D1_L_offset};

        // 计算D2-L向量
        std::vector<double> D2L_vector = {D2[0] - L[0], D2[1] - L[1]};

        // 计算与D2-L垂直的向量（向上）
        std::vector<double> perp_up_vector = {-D2L_vector[1], D2L_vector[0]};
        double perp_up_norm = sqrt(perp_up_vector[0] * perp_up_vector[0] +
                                   perp_up_vector[1] * perp_up_vector[1]);

        if (perp_up_norm > 0)
        {
            perp_up_vector[0] /= perp_up_norm;
            perp_up_vector[1] /= perp_up_norm;
        }
        else
        {
            perp_up_vector = {0, 1}; // 默认向上
        }

        // 计算C4点（在D2点上方）
        std::vector<double> C4 = {
            D2[0] + perp_up_vector[0] * C4_D2_offset,
            D2[1] + perp_up_vector[1] * C4_D2_offset};

        // 存储结果
        Trajectory_D1.push_back(D1);
        Trajectory_D2.push_back(D2);
        Trajectory_C4.push_back(C4);

        // 验证垂直性约束：C-L与D2-L的点积应为0
        CL_vector = {C[0] - L[0], C[1] - L[1]};
        D2L_vector = {D2[0] - L[0], D2[1] - L[1]};
        double dot_product = CL_vector[0] * D2L_vector[0] + CL_vector[1] * D2L_vector[1];
        dot_products.push_back(dot_product);
    }
}

// 计算B点轨迹
void QuadrupedRobotSimulation::CalculateTrajectoryB()
{
    Trajectory_B.clear();
    for (const auto &C_point : Trajectory_C)
    {
        std::vector<double> B_point = Calculate_B_Position(A_point, C_point, L10, L20);
        Trajectory_B.push_back(B_point);
    }
}

// 计算所有辅助点
void QuadrupedRobotSimulation::CalculateAllPoints()
{
    Trajectory_B1.clear();
    Trajectory_B2.clear();
    Trajectory_C1.clear();
    Trajectory_C2.clear();
    Trajectory_C3.clear();
    C3_C4_distances.clear();
    C2_C3_distances.clear();
    C2_C4_distances.clear();
    C2_C3_C4_angles.clear();

    for (size_t i = 0; i < Trajectory_B.size(); i++)
    {
        // 计算B2点
        std::vector<double> B2_point = Calculate_B2_Position(Trajectory_B[i], Trajectory_C[i], L23, BB2_BC_angle);
        Trajectory_B2.push_back(B2_point);

        // 计算B1点
        std::vector<double> B1_point = Calculate_B1_Position(A_point, B2_point, L21, L22);
        Trajectory_B1.push_back(B1_point);

        // 计算C1点
        std::vector<double> C1_point = Calculate_C1_Position(Trajectory_B[i], Trajectory_C[i], C1_B_length);
        Trajectory_C1.push_back(C1_point);

        // 计算C2点
        std::vector<double> C2_point = Calculate_C2_Position(C1_point, Trajectory_B[i], Trajectory_C[i], L31);
        Trajectory_C2.push_back(C2_point);

        // 计算C3点
        std::vector<double> C3_point = Calculate_C3_Position(C2_point, Trajectory_C4[i], C2C3_length);
        Trajectory_C3.push_back(C3_point);

        // 计算距离
        double c2_c3 = Distance(C2_point, C3_point);
        double c3_c4 = Distance(C3_point, Trajectory_C4[i]);
        double c2_c4 = Distance(C2_point, Trajectory_C4[i]);

        C2_C3_distances.push_back(c2_c3);
        C3_C4_distances.push_back(c3_c4);
        C2_C4_distances.push_back(c2_c4);

        // 计算∠C2C3C4角度
        double angle = Calculate_C2C3C4_Angle(C2_point, C3_point, Trajectory_C4[i]);
        C2_C3_C4_angles.push_back(angle);
    }
}

// 计算B2点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_B2_Position(const std::vector<double> &B,
                                                                    const std::vector<double> &C,
                                                                    double B_B2_length, double BB2_BC_angle)
{
    std::vector<double> BC_vector = {C[0] - B[0], C[1] - B[1]};
    double BC_angle = atan2(BC_vector[1], BC_vector[0]); // BC方向角度
    double B2_angle = BC_angle + BB2_BC_angle;           // B2相对于BC的角度

    double B2_x = B[0] + B_B2_length * cos(B2_angle);
    double B2_y = B[1] + B_B2_length * sin(B2_angle);
    return {B2_x, B2_y};
}

// 计算B1点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_B1_Position(const std::vector<double> &A,
                                                                    const std::vector<double> &B2,
                                                                    double A_B1_length, double B1B2_length)
{
    double AB2_distance = sqrt(pow(B2[0] - A[0], 2) + pow(B2[1] - A[1], 2));

    // 检查三角形是否可能形成
    if (AB2_distance > A_B1_length + B1B2_length ||
        AB2_distance < fabs(A_B1_length - B1B2_length))
    {
        return {(A[0] + B2[0]) / 2, (A[1] + B2[1]) / 2}; // 返回中点近似
    }

    std::vector<double> AB2_vector = {B2[0] - A[0], B2[1] - A[1]};
    std::vector<double> AB2_unit = {AB2_vector[0] / AB2_distance, AB2_vector[1] / AB2_distance};

    // 余弦定理计算夹角
    double cos_angle = (pow(A_B1_length, 2) + pow(AB2_distance, 2) - pow(B1B2_length, 2)) /
                       (2 * A_B1_length * AB2_distance);
    cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
    double angle = acos(cos_angle);

    // 旋转AB2向量得到AB1向量
    double cosA = cos(angle);
    double sinA = sin(angle);
    double AB1_x = A_B1_length * (cosA * AB2_unit[0] - sinA * AB2_unit[1]);
    double AB1_y = A_B1_length * (sinA * AB2_unit[0] + cosA * AB2_unit[1]);

    double B1_x = A[0] + AB1_x;
    double B1_y = A[1] + AB1_y;
    return {B1_x, B1_y};
}

// 计算C1点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_C1_Position(const std::vector<double> &B,
                                                                    const std::vector<double> &C,
                                                                    double C1_B_length)
{
    std::vector<double> BC_vector = {C[0] - B[0], C[1] - B[1]};
    double BC_length = sqrt(pow(BC_vector[0], 2) + pow(BC_vector[1], 2));
    std::vector<double> BC_unit;
    if (BC_length > 0)
    {
        BC_unit = {BC_vector[0] / BC_length, BC_vector[1] / BC_length};
    }
    else
    {
        BC_unit = {1, 0}; // 默认方向
    }

    double C1_x = B[0] + BC_unit[0] * C1_B_length;
    double C1_y = B[1] + BC_unit[1] * C1_B_length;
    return {C1_x, C1_y};
}

// 计算C2点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_C2_Position(const std::vector<double> &C1,
                                                                    const std::vector<double> &B,
                                                                    const std::vector<double> &C,
                                                                    double C1C2_length)
{
    std::vector<double> BC_vector = {C[0] - B[0], C[1] - B[1]};
    double BC_length = sqrt(pow(BC_vector[0], 2) + pow(BC_vector[1], 2));
    std::vector<double> BC_unit;
    if (BC_length > 0)
    {
        BC_unit = {BC_vector[0] / BC_length, BC_vector[1] / BC_length};
    }
    else
    {
        BC_unit = {1, 0}; // 默认方向
    }

    // 垂直于BC的单位向量（右侧）
    std::vector<double> BC_perp = {-BC_unit[1], BC_unit[0]};

    double C2_x = C1[0] + BC_perp[0] * C1C2_length;
    double C2_y = C1[1] + BC_perp[1] * C1C2_length;
    return {C2_x, C2_y};
}

// 计算C3点位置
std::vector<double> QuadrupedRobotSimulation::Calculate_C3_Position(const std::vector<double> &C2,
                                                                    const std::vector<double> &C4,
                                                                    double C2C3_length)
{
    std::vector<double> C2C4_vector = {C4[0] - C2[0], C4[1] - C2[1]};
    double C2C4_length = sqrt(pow(C2C4_vector[0], 2) + pow(C2C4_vector[1], 2));

    // 如果C2C4长度小于C2C3长度，无法形成直角三角形
    if (C2C4_length < C2C3_length)
    {
        return {C2[0] + C2C3_length, C2[1]}; // 返回C2右侧水平点近似
    }

    std::vector<double> C2C4_unit = {C2C4_vector[0] / C2C4_length, C2C4_vector[1] / C2C4_length};
    std::vector<double> C2C4_perp = {-C2C4_unit[1], C2C4_unit[0]}; // 垂直于C2C4的向量

    // 使用向量投影方法求解C3点
    double a = pow(C2C3_length, 2) / C2C4_length;
    double b = sqrt(pow(C2C3_length, 2) - pow(a, 2));

    double C3_x = C2[0] + a * C2C4_unit[0] + b * C2C4_perp[0];
    double C3_y = C2[1] + a * C2C4_unit[1] + b * C2C4_perp[1];
    return {C3_x, C3_y};
}

// 计算∠C2C3C4角度
double QuadrupedRobotSimulation::Calculate_C2C3C4_Angle(const std::vector<double> &C2,
                                                        const std::vector<double> &C3,
                                                        const std::vector<double> &C4)
{
    std::vector<double> C2C3_vector = {C3[0] - C2[0], C3[1] - C2[1]};
    std::vector<double> C3C4_vector = {C4[0] - C3[0], C4[1] - C3[1]};

    double norm_C2C3 = sqrt(C2C3_vector[0] * C2C3_vector[0] + C2C3_vector[1] * C2C3_vector[1]);
    double norm_C3C4 = sqrt(C3C4_vector[0] * C3C4_vector[0] + C3C4_vector[1] * C3C4_vector[1]);

    if (norm_C2C3 > 0 && norm_C3C4 > 0)
    {
        double dot_product = C2C3_vector[0] * C3C4_vector[0] + C2C3_vector[1] * C3C4_vector[1];
        double cos_angle = dot_product / (norm_C2C3 * norm_C3C4);
        cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
        double angle_rad = acos(cos_angle);
        return angle_rad * 180 / PI; // 转换为度
    }
    return 90; // 默认90度
}

// 计算两点间距离
double QuadrupedRobotSimulation::Distance(const std::vector<double> &p1, const std::vector<double> &p2)
{
    return sqrt(pow(p2[0] - p1[0], 2) + pow(p2[1] - p1[1], 2));
}

// 计算所有角度
void QuadrupedRobotSimulation::CalculateAllAngles()
{
    AB_horizontal_angles.clear();
    AB_BC_angles.clear();
    BC_CL_angles.clear();
    AB1_AB_angles.clear();

    for (size_t i = 0; i < Trajectory_B.size(); i++)
    {
        // 1. 计算AB连杆与水平夹角（调整到机器人坐标系）
        std::vector<double> AB_vector = {
            Trajectory_B[i][0] - A_point[0],
            Trajectory_B[i][1] - A_point[1]};
        double AB_horizontal_angle = atan2(AB_vector[1], AB_vector[0]);
        double AB_horizontal_angle_deg = AB_horizontal_angle * 180 / PI;
        if (AB_horizontal_angle_deg < 0)
            AB_horizontal_angle_deg += 360;
        double adjusted_AB_angle = 270 - AB_horizontal_angle_deg; // 调整到机器人坐标系
        if (adjusted_AB_angle > 180)
            adjusted_AB_angle -= 360;
        else if (adjusted_AB_angle < -180)
            adjusted_AB_angle += 360;
        AB_horizontal_angles.push_back(adjusted_AB_angle);

        // 2. 计算AB连杆与BC连杆夹角
        std::vector<double> BC_vector = {
            Trajectory_C[i][0] - Trajectory_B[i][0],
            Trajectory_C[i][1] - Trajectory_B[i][1]};
        double dot_product_AB_BC = AB_vector[0] * BC_vector[0] + AB_vector[1] * BC_vector[1];
        double AB_length_current = sqrt(AB_vector[0] * AB_vector[0] + AB_vector[1] * AB_vector[1]);
        double BC_length_current = sqrt(BC_vector[0] * BC_vector[0] + BC_vector[1] * BC_vector[1]);
        double AB_BC_angle_deg = 0;
        if (AB_length_current > 1e-10 && BC_length_current > 1e-10)
        {
            double cos_angle = dot_product_AB_BC / (AB_length_current * BC_length_current);
            cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
            double AB_BC_angle_rad = acos(cos_angle);
            AB_BC_angle_deg = AB_BC_angle_rad * 180 / PI;
        }
        double adjusted_BC_angle = 180 - AB_BC_angle_deg; // 调整到机器人坐标系
        AB_BC_angles.push_back(adjusted_BC_angle);

        // 3. 计算BC连杆与C-L连线夹角
        std::vector<double> CL_vector = {
            Trajectory_L[i][0] - Trajectory_C[i][0],
            Trajectory_L[i][1] - Trajectory_C[i][1]};
        double dot_product_BC_CL = BC_vector[0] * CL_vector[0] + BC_vector[1] * CL_vector[1];
        double CL_length_current = sqrt(CL_vector[0] * CL_vector[0] + CL_vector[1] * CL_vector[1]);
        double BC_CL_angle_deg = 90;
        if (BC_length_current > 1e-10 && CL_length_current > 1e-10)
        {
            double cos_angle = dot_product_BC_CL / (BC_length_current * CL_length_current);
            cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
            double BC_CL_angle_rad = acos(cos_angle);
            BC_CL_angle_deg = BC_CL_angle_rad * 180 / PI;
        }
        BC_CL_angles.push_back(BC_CL_angle_deg);

        // 4. 计算AB1连杆与AB连杆夹角θ1
        std::vector<double> AB1_vector = {
            Trajectory_B1[i][0] - A_point[0],
            Trajectory_B1[i][1] - A_point[1]};
        double dot_product_AB_AB1 = AB_vector[0] * AB1_vector[0] + AB_vector[1] * AB1_vector[1];
        double AB1_length_current = sqrt(AB1_vector[0] * AB1_vector[0] + AB1_vector[1] * AB1_vector[1]);
        double AB_AB1_angle_deg = 0;
        if (AB_length_current > 1e-10 && AB1_length_current > 1e-10)
        {
            double cos_angle = dot_product_AB_AB1 / (AB_length_current * AB1_length_current);
            cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
            double AB_AB1_angle_rad = acos(cos_angle);
            AB_AB1_angle_deg = AB_AB1_angle_rad * 180 / PI;
        }
        double adjusted_AB1_angle = 0 - AB_AB1_angle_deg; // 调整到机器人坐标系
        AB1_AB_angles.push_back(adjusted_AB1_angle);
    }
}

// 检查机构约束
void QuadrupedRobotSimulation::CheckConstraints()
{
    const double AB1_AB_min = 23;  // θ1最小允许角度
    const double AB1_AB_max = 130; // θ1最大允许角度
    const double C3_C4_min = 125;  // C3-C4最小允许距离
    const double C3_C4_max = 205;  // C3-C4最大允许距离

    bool ab1_ab_valid = true;
    bool c3c4_valid = true;

    std::vector<std::string> ab1_ab_errors;
    std::vector<std::string> c3c4_errors;

    // 检查θ1角度约束
    for (size_t i = 0; i < AB1_AB_angles.size(); i++)
    {
        double abs_angle = fabs(AB1_AB_angles[i]);
        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
        {
            ab1_ab_valid = false;
            std::stringstream ss;
            ss << "帧 " << i << ": 角度=" << std::fixed << std::setprecision(2)
               << AB1_AB_angles[i] << "°, 绝对值=" << abs_angle
               << "°, 时间=" << i * step << "s";
            ab1_ab_errors.push_back(ss.str());
        }
    }

    // 检查C3-C4距离约束
    for (size_t i = 0; i < C3_C4_distances.size(); i++)
    {
        double distance = C3_C4_distances[i];
        if (distance < C3_C4_min || distance > C3_C4_max)
        {
            c3c4_valid = false;
            std::stringstream ss;
            ss << "帧 " << i << ": 距离=" << std::fixed << std::setprecision(2)
               << distance << "mm, 时间=" << i * step << "s";
            c3c4_errors.push_back(ss.str());
        }
    }

    // 如果超出约束范围，抛出异常
    if (!ab1_ab_valid)
    {
        throw std::runtime_error("AB1与AB夹角θ1超出允许范围，机构锁死不能运动!");
    }

    if (!c3c4_valid)
    {
        throw std::runtime_error("C3-C4距离L33超出允许范围，机构锁死不能运动!");
    }
}

// 计算电机数据
void QuadrupedRobotSimulation::CalculateMotorData()
{
    int n_frames = Trajectory_L.size();

    // 计算相位偏移帧数
    int phase_LH = static_cast<int>(timeLH * n_frames);
    int phase_RF = static_cast<int>(timeRF * n_frames);
    int phase_RH = static_cast<int>(timeRH * n_frames);

    std::cout << "步态类型: " << gait_type << std::endl;
    std::cout << "总帧数: " << n_frames << std::endl;
    std::cout << "相位偏移（帧）: LF=0, LH=" << phase_LH
              << ", RF=" << phase_RF << ", RH=" << phase_RH << std::endl;

    // 1. 计算左前腿基准数据
    motor_data_LF_raw = CalculateIncrementsAndVelocities();

    // 2. 调整左前腿的电机数据
    motor_data_LF = AdjustMotorAngles(motor_data_LF_raw,
                                      motor_data_LF_raw.frame_start_times, step);

    // 3. 计算其他三条腿的原始数据
    motor_data_LH_raw = CalculateOtherLegData(motor_data_LF_raw, phase_LH, false);
    motor_data_RF_raw = CalculateOtherLegData(motor_data_LF_raw, phase_RF, true);
    motor_data_RH_raw = CalculateOtherLegData(motor_data_LF_raw, phase_RH, true);

    // 4. 调整其他腿的电机数据
    motor_data_LH = AdjustMotorAngles(motor_data_LH_raw,
                                      motor_data_LH_raw.frame_start_times, step);
    motor_data_RF = AdjustMotorAngles(motor_data_RF_raw,
                                      motor_data_RF_raw.frame_start_times, step);
    motor_data_RH = AdjustMotorAngles(motor_data_RH_raw,
                                      motor_data_RH_raw.frame_start_times, step);

    // 5. 调整左前腿的脚踝电机角度
    motor_data_LF = AdjustAnkleAngles(motor_data_LF);

    // 6. 调整其他腿的脚踝电机角度
    motor_data_LH = AdjustAnkleAngles(motor_data_LH);
    motor_data_RF = AdjustAnkleAngles(motor_data_RF);
    motor_data_RH = AdjustAnkleAngles(motor_data_RH);

    // 7. 对右侧小腿数据进行取反操作
    motor_data_RF = ReverseShankData(motor_data_RF, step, shank_motor_reduction);
    motor_data_RH = ReverseShankData(motor_data_RH, step, shank_motor_reduction);
}

// 计算增量值和速度
MotorData QuadrupedRobotSimulation::CalculateIncrementsAndVelocities()
{
    int n_frames = Trajectory_L.size();
    MotorData data(n_frames);
    double cumulative_ankle_angle = 0; // 脚踝电机累积角度

    for (int i = 0; i < n_frames; i++)
    {
        // 时间信息
        data.frame_start_times[i] = i * step;
        data.frame_end_times[i] = (i + 1) * step;

        // 大腿电机数据
        data.AB_angles[i] = AB_horizontal_angles[i];

        if (i == 0)
        {
            data.AB_angle_increments[i] = 0;
        }
        else
        {
            // 计算角度增量（考虑角度循环）
            double angle_diff = data.AB_angles[i] - data.AB_angles[i - 1];
            if (angle_diff > 180)
                angle_diff -= 360;
            else if (angle_diff < -180)
                angle_diff += 360;
            data.AB_angle_increments[i] = angle_diff;
        }

        // 大腿电机角速度 (rpm) = 角度增量/360/时间步长×60×减速比
        data.thigh_motor_speeds[i] = (data.AB_angle_increments[i] / 360 / step) * 60 * thigh_motor_reduction;

        // 小腿电机数据
        data.AB1_AB_angles[i] = AB1_AB_angles[i];

        if (i == 0)
        {
            data.AB1_AB_angle_increments[i] = 0;
        }
        else
        {
            data.AB1_AB_angle_increments[i] = data.AB1_AB_angles[i] - data.AB1_AB_angles[i - 1];
        }

        // 小腿电机角速度 (rpm)
        data.shank_motor_speeds[i] = (data.AB1_AB_angle_increments[i] / 360 / step) * 60 * shank_motor_reduction;

        // 脚踝电机数据
        data.C3C4_distances[i] = C3_C4_distances[i];

        if (i == 0)
        {
            data.C3C4_distance_increments[i] = 0;
        }
        else
        {
            data.C3C4_distance_increments[i] = data.C3C4_distances[i] - data.C3C4_distances[i - 1];
        }

        // 脚踝电机角度增量 = 距离增量/导程×360
        data.ankle_motor_angle_increments[i] = data.C3C4_distance_increments[i] / lead1 * 360;
        cumulative_ankle_angle += data.ankle_motor_angle_increments[i];
        data.ankle_motor_angles[i] = cumulative_ankle_angle;

        // 脚踝电机角速度 (rpm) = 距离增量/导程/时间步长×60×减速比
        data.ankle_motor_speeds[i] = (data.C3C4_distance_increments[i] / lead1 / step) * 60 * ankle_motor_reduction;
    }

    return data;
}

// 计算其他腿的数据
MotorData QuadrupedRobotSimulation::CalculateOtherLegData(const MotorData &baseData, int shift, bool isRightLeg)
{
    int n_frames = baseData.frame_start_times.size();
    MotorData shifted(n_frames);

    // 移位函数
    auto shift_array = [&](const std::vector<double> &array, int shiftFrames) -> std::vector<double>
    {
        if (shiftFrames == 0)
            return array;
        std::vector<double> result(array.size());
        for (size_t i = 0; i < array.size(); i++)
        {
            result[(i + shiftFrames) % array.size()] = array[i];
        }
        return result;
    };

    // 获取数组的副本以便修改
    std::vector<double> AB_angles_shifted = shift_array(baseData.AB_angles, shift);
    std::vector<double> AB_angle_increments_shifted = shift_array(baseData.AB_angle_increments, shift);
    std::vector<double> thigh_motor_speeds_shifted = shift_array(baseData.thigh_motor_speeds, shift);
    std::vector<double> AB1_AB_angles_shifted = shift_array(baseData.AB1_AB_angles, shift);
    std::vector<double> AB1_AB_angle_increments_shifted = shift_array(baseData.AB1_AB_angle_increments, shift);
    std::vector<double> shank_motor_speeds_shifted = shift_array(baseData.shank_motor_speeds, shift);
    std::vector<double> C3C4_distances_shifted = shift_array(baseData.C3C4_distances, shift);
    std::vector<double> C3C4_distance_increments_shifted = shift_array(baseData.C3C4_distance_increments, shift);
    std::vector<double> ankle_motor_angles_shifted = shift_array(baseData.ankle_motor_angles, shift);
    std::vector<double> ankle_motor_angle_increments_shifted = shift_array(baseData.ankle_motor_angle_increments, shift);
    std::vector<double> ankle_motor_speeds_shifted = shift_array(baseData.ankle_motor_speeds, shift);

    // 时间数据不需要移位
    shifted.frame_start_times = baseData.frame_start_times;
    shifted.frame_end_times = baseData.frame_end_times;

    // 如果是右侧腿，进行镜像变换
    if (isRightLeg)
    {
        // 1. 大腿相关数据：进行镜像变换
        std::vector<double> angles_mirrored(n_frames);
        for (int i = 0; i < n_frames; i++)
        {
            angles_mirrored[i] = 360 - AB_angles_shifted[i];
            angles_mirrored[i] = fmod(angles_mirrored[i], 360);
        }

        // 重新计算角度增量
        std::vector<double> increments(n_frames);
        increments[0] = 0;
        for (int i = 1; i < n_frames; i++)
        {
            double angle_diff = angles_mirrored[i] - angles_mirrored[i - 1];
            if (angle_diff > 180)
                angle_diff -= 360;
            else if (angle_diff < -180)
                angle_diff += 360;
            increments[i] = angle_diff;
        }

        // 重新计算大腿电机角速度
        std::vector<double> speeds(n_frames);
        speeds[0] = 0;
        for (int i = 1; i < n_frames; i++)
        {
            speeds[i] = (increments[i] / 360 / step) * 60 * thigh_motor_reduction;
        }

        // 分配镜像后的大腿数据
        shifted.AB_angles = angles_mirrored;
        shifted.AB_angle_increments = increments;
        shifted.thigh_motor_speeds = speeds;

        // 2. 小腿相关数据：保持不变
        shifted.AB1_AB_angles = AB1_AB_angles_shifted;
        shifted.AB1_AB_angle_increments = AB1_AB_angle_increments_shifted;
        shifted.shank_motor_speeds = shank_motor_speeds_shifted;

        // 3. 脚踝相关数据：保持不变
        shifted.C3C4_distances = C3C4_distances_shifted;
        shifted.C3C4_distance_increments = C3C4_distance_increments_shifted;
        shifted.ankle_motor_angles = ankle_motor_angles_shifted;
        shifted.ankle_motor_angle_increments = ankle_motor_angle_increments_shifted;
        shifted.ankle_motor_speeds = ankle_motor_speeds_shifted;
    }
    else
    {
        // 左侧腿：直接使用移位后的数据
        shifted.AB_angles = AB_angles_shifted;
        shifted.AB_angle_increments = AB_angle_increments_shifted;
        shifted.thigh_motor_speeds = thigh_motor_speeds_shifted;
        shifted.AB1_AB_angles = AB1_AB_angles_shifted;
        shifted.AB1_AB_angle_increments = AB1_AB_angle_increments_shifted;
        shifted.shank_motor_speeds = shank_motor_speeds_shifted;
        shifted.C3C4_distances = C3C4_distances_shifted;
        shifted.C3C4_distance_increments = C3C4_distance_increments_shifted;
        shifted.ankle_motor_angles = ankle_motor_angles_shifted;
        shifted.ankle_motor_angle_increments = ankle_motor_angle_increments_shifted;
        shifted.ankle_motor_speeds = ankle_motor_speeds_shifted;
    }

    return shifted;
}

// 调整电机角度
MotorData QuadrupedRobotSimulation::AdjustMotorAngles(const MotorData &motorData,
                                                      const std::vector<double> &frame_start_times,
                                                      double step)
{
    MotorData adjustedData = motorData.Clone();
    int n_frames = motorData.AB_angles.size();

    // 调整大腿角度：直接使用角度
    for (int i = 0; i < n_frames; i++)
    {
        adjustedData.AB_angles[i] = motorData.AB_angles[i];
    }

    // 重新计算大腿角度增量
    adjustedData.AB_angle_increments[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        double angle_diff = adjustedData.AB_angles[i] - adjustedData.AB_angles[i - 1];
        if (angle_diff > 180)
            angle_diff -= 360;
        else if (angle_diff < -180)
            angle_diff += 360;
        adjustedData.AB_angle_increments[i] = angle_diff;
    }

    // 重新计算大腿电机角速度
    adjustedData.thigh_motor_speeds[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        adjustedData.thigh_motor_speeds[i] = (adjustedData.AB_angle_increments[i] / 360 / step) * 60 * thigh_motor_reduction;
    }

    // 调整小腿角度：加上99.41097105
    for (int i = 0; i < n_frames; i++)
    {
        adjustedData.AB1_AB_angles[i] = motorData.AB1_AB_angles[i] + 99.41097105;
    }

    // 重新计算小腿角度增量
    adjustedData.AB1_AB_angle_increments[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        double angle_diff = adjustedData.AB1_AB_angles[i] - adjustedData.AB1_AB_angles[i - 1];
        adjustedData.AB1_AB_angle_increments[i] = angle_diff;
    }

    // 重新计算小腿电机角速度
    adjustedData.shank_motor_speeds[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        adjustedData.shank_motor_speeds[i] = (adjustedData.AB1_AB_angle_increments[i] / 360 / step) * 60 * shank_motor_reduction;
    }

    // 调整脚踝角度：使用原始值
    for (int i = 0; i < n_frames; i++)
    {
        adjustedData.ankle_motor_angles[i] = motorData.ankle_motor_angles[i];
    }

    // 重新计算脚踝角度增量
    adjustedData.ankle_motor_angle_increments[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        double angle_diff = adjustedData.ankle_motor_angles[i] - adjustedData.ankle_motor_angles[i - 1];
        adjustedData.ankle_motor_angle_increments[i] = angle_diff;
    }

    // 重新计算脚踝电机角速度
    adjustedData.ankle_motor_speeds[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        adjustedData.ankle_motor_speeds[i] = (adjustedData.ankle_motor_angle_increments[i] / 360 / step) * 60 * ankle_motor_reduction;
    }

    return adjustedData;
}

// 对右侧小腿数据进行取反操作
MotorData QuadrupedRobotSimulation::ReverseShankData(const MotorData &motorData,
                                                     double step,
                                                     double shank_motor_reduction)
{
    MotorData reversedData = motorData.Clone();
    int n_frames = motorData.AB1_AB_angles.size();

    // 小腿角度取反
    for (int i = 0; i < n_frames; i++)
    {
        reversedData.AB1_AB_angles[i] = -motorData.AB1_AB_angles[i];
    }

    // 重新计算小腿角度增量
    reversedData.AB1_AB_angle_increments[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        double angle_diff = reversedData.AB1_AB_angles[i] - reversedData.AB1_AB_angles[i - 1];
        reversedData.AB1_AB_angle_increments[i] = angle_diff;
    }

    // 重新计算小腿电机角速度
    reversedData.shank_motor_speeds[0] = 0;
    for (int i = 1; i < n_frames; i++)
    {
        reversedData.shank_motor_speeds[i] = (reversedData.AB1_AB_angle_increments[i] / 360 / step) * 60 * shank_motor_reduction;
    }

    return reversedData;
}

// 调整脚踝电机角度
MotorData QuadrupedRobotSimulation::AdjustAnkleAngles(const MotorData &data)
{
    MotorData adjusted = data.Clone();
    int n = data.ankle_motor_angles.size();
    double initial_ankle_angle = data.ankle_motor_angles[0];

    // 减去初始角度
    for (int i = 0; i < n; i++)
    {
        adjusted.ankle_motor_angles[i] = data.ankle_motor_angles[i] - initial_ankle_angle;
    }

    // 重新计算角度增量和速度
    adjusted.ankle_motor_angle_increments[0] = 0;
    adjusted.ankle_motor_speeds[0] = 0;

    for (int i = 1; i < n; i++)
    {
        double angle_diff = adjusted.ankle_motor_angles[i] - adjusted.ankle_motor_angles[i - 1];
        adjusted.ankle_motor_angle_increments[i] = angle_diff;
        adjusted.ankle_motor_speeds[i] = (angle_diff / 360 / step) * 60 * ankle_motor_reduction;
    }

    return adjusted;
}

// 数学辅助函数
double QuadrupedRobotSimulation::NonlinearAngleFunction(double t, double start_angle_deg,
                                                        double end_angle_deg,
                                                        const std::string &function_type,
                                                        double curvature)
{
    if (function_type == "quadratic_ease_out")
    { // 二次缓出：开始快，结束慢
        double quadratic_t = 1 - pow(1 - t, 2);
        return start_angle_deg + (end_angle_deg - start_angle_deg) * quadratic_t;
    }
    else if (function_type == "ease_in_out")
    { // 缓入缓出：开始慢，中间快，结束慢
        if (t < 0.5)
        {
            double ease_t = 2 * pow(t, 2);
            return start_angle_deg + (end_angle_deg - start_angle_deg) * ease_t;
        }
        else
        {
            double ease_t = 1 - 2 * pow(1 - t, 2);
            return start_angle_deg + (end_angle_deg - start_angle_deg) * ease_t;
        }
    }
    else
    { // 线性变化
        return start_angle_deg + (end_angle_deg - start_angle_deg) * t;
    }
}

std::vector<double> QuadrupedRobotSimulation::Linspace(double start, double end, int num)
{
    if (num <= 0)
        return {};
    if (num == 1)
        return {(start + end) / 2};
    std::vector<double> result(num);
    double stepSize = (end - start) / (num - 1);
    for (int i = 0; i < num; i++)
    {
        result[i] = start + i * stepSize;
    }
    return result;
}

std::vector<std::vector<double>> QuadrupedRobotSimulation::Zeros(int rows, int cols)
{
    return std::vector<std::vector<double>>(rows, std::vector<double>(cols, 0.0));
}

std::vector<std::vector<double>> QuadrupedRobotSimulation::Flip(const std::vector<std::vector<double>> &array)
{
    int rows = array.size();
    if (rows == 0)
        return {};
    int cols = array[0].size();
    std::vector<std::vector<double>> result(rows, std::vector<double>(cols, 0.0));
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            result[rows - 1 - i][j] = array[i][j];
        }
    }
    return result;
}

std::vector<std::vector<double>> QuadrupedRobotSimulation::VStack(const std::vector<std::vector<double>> &array1,
                                                                  const std::vector<std::vector<double>> &array2)
{
    int rows1 = array1.size();
    int rows2 = array2.size();
    if (rows1 == 0)
        return array2;
    if (rows2 == 0)
        return array1;

    int cols = array1[0].size();
    if (array2[0].size() != cols)
    {
        throw std::invalid_argument("数组列数不一致");
    }

    std::vector<std::vector<double>> result(rows1 + rows2, std::vector<double>(cols, 0.0));
    for (int i = 0; i < rows1; i++)
    {
        result[i] = array1[i];
    }
    for (int i = 0; i < rows2; i++)
    {
        result[rows1 + i] = array2[i];
    }
    return result;
}

int QuadrupedRobotSimulation::PythonRoundExact(double value)
{
    double floor_val = floor(value);
    double ceil_val = ceil(value);
    double mid = (floor_val + ceil_val) / 2;
    double epsilon = 1e-14;
    if (fabs(value - mid) < epsilon)
    {
        return static_cast<int>(floor_val) % 2 == 0 ? static_cast<int>(floor_val) : static_cast<int>(ceil_val);
    }
    return static_cast<int>(round(value));
}

std::vector<double> QuadrupedRobotSimulation::ShiftArray(const std::vector<double> &array, int shiftFrames)
{
    if (shiftFrames == 0)
        return array;
    std::vector<double> result(array.size());
    for (size_t i = 0; i < array.size(); i++)
    {
        result[(i + shiftFrames) % array.size()] = array[i];
    }
    return result;
}
