// CompleteJsonExporter.cpp
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
#include "QuadrupedRobotSimulation/BaseClass.h"
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
#include <algorithm>
#include <cmath>
#include <functional>
#include <sstream>
#include <iomanip>
#include <limits>

// 主导出函数
std::string CompleteJsonExporter::ExportCompleteDataJsonString(const QuadrupedRobotSimulation &simulation)
{
    try
    {
        // 创建数据对象
        CompleteExportData exportData;

        // 基本信息
        exportData.GaitInfo = CreateGaitInfo(simulation);

        // 系统参数
        exportData.SystemParameters = CreateSystemParameters(simulation);

        // 轨迹数据（所有腿）
        exportData.TrajectoryData = CreateTrajectoryData(simulation);

        // 电机数据（所有腿）
        exportData.MotorData = CreateMotorData(simulation);

        // 约束检查数据
        exportData.ConstraintData = CreateConstraintData(simulation);

        // 统计信息
        exportData.Statistics = CreateStatistics(simulation);

        // 初始角度数据
        exportData.InitialAngles = CreateInitialAngles(simulation);

        // 相位信息
        exportData.PhaseInfo = CreatePhaseInfo(simulation);

        // 运动范围统计
        exportData.MotionRange = CreateMotionRange(simulation);

        // 序列化为JSON
        json j = exportData;
        return j.dump(2); // 缩进2个空格
    }
    catch (const std::exception &ex)
    {
        std::cerr << "导出JSON数据时出错: " << ex.what() << std::endl;
        throw;
    }
}

// 创建步态信息
GaitInfo CompleteJsonExporter::CreateGaitInfo(const QuadrupedRobotSimulation &simulation)
{
    GaitInfo gaitInfo;
    gaitInfo.GaitType = simulation.gait_type;
    gaitInfo.Period = simulation.T;
    gaitInfo.Frequency = simulation.f;
    gaitInfo.StepTime = simulation.step;
    gaitInfo.SupportTime = simulation.t_s;
    gaitInfo.SwingTime = simulation.t_w;
    gaitInfo.SupportRatio = simulation.support_ratio;
    gaitInfo.SwingRatio = simulation.swing_ratio;
    gaitInfo.TotalFrames = simulation.Trajectory_L.size();
    gaitInfo.TotalTime = simulation.Trajectory_L.size() * simulation.step;

    return gaitInfo;
}

// 创建系统参数
SystemParameters CompleteJsonExporter::CreateSystemParameters(const QuadrupedRobotSimulation &simulation)
{
    SystemParameters systemParams;

    // 基本参数
    systemParams.L10 = simulation.L10;
    systemParams.L20 = simulation.L20;
    systemParams.L30 = simulation.L30;

    // 步态设计参数
    systemParams.StepLength = simulation.s_l;
    systemParams.StepHeight = simulation.s_h;
    systemParams.X = simulation.X;
    systemParams.DeltaX = simulation.deltX;

    // 小腿连杆参数
    systemParams.L21 = simulation.L21;
    systemParams.L22 = simulation.L22;
    systemParams.L23 = simulation.L23;
    systemParams.Beta1 = simulation.beta1;
    systemParams.BB2_BC_Angle = simulation.BB2_BC_angle;
    systemParams.C1_B_Length = simulation.C1_B_length;

    // 脚踝连杆参数
    systemParams.CC1 = simulation.CC1;
    systemParams.L31 = simulation.L31;
    systemParams.C2C3_Length = simulation.C2C3_length;
    systemParams.Lead = simulation.lead1;

    // 脚部构型参数
    systemParams.D1_L_Offset = simulation.D1_L_offset;
    systemParams.D2_L_Offset = simulation.D2_L_offset;
    systemParams.C4_D2_Offset = simulation.C4_D2_offset;

    // 电机参数
    systemParams.ThighMotorReduction = simulation.thigh_motor_reduction;
    systemParams.ShankMotorReduction = simulation.shank_motor_reduction;
    systemParams.AnkleMotorReduction = simulation.ankle_motor_reduction;

    return systemParams;
}

// 创建轨迹数据
TrajectoryData CompleteJsonExporter::CreateTrajectoryData(const QuadrupedRobotSimulation &simulation)
{
    int n_frames = simulation.Trajectory_L.size();

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

    TrajectoryData trajectoryData;
    trajectoryData.TotalFrames = n_frames;
    trajectoryData.StepTime = simulation.step;

    // 为每条腿创建轨迹数据
    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
    std::vector<int> phaseShifts = {0, phase_LH, phase_RF, phase_RH};

    for (size_t i = 0; i < legCodes.size(); i++)
    {
        std::string legCode = legCodes[i];
        int phaseShift = phaseShifts[i];
        bool isRightLeg = legCode[0] == 'R'; // 检查是否以'R'开头

        LegTrajectory legTraj;
        legTraj.LegCode = legCode;
        legTraj.PhaseShift = phaseShift;
        legTraj.IsRightLeg = isRightLeg;

        for (int frame = 0; frame < n_frames; frame++)
        {
            int shiftedIdx = (frame + phaseShift) % n_frames;

            FrameData frameData;
            frameData.FrameNumber = frame;
            frameData.Time = frame * simulation.step;
            frameData.ShiftedFrameNumber = shiftedIdx;

            // 关键点坐标
            frameData.A_Point = Point2D(simulation.A_point[0], simulation.A_point[1]);
            frameData.B_Point = ArrayToPoint(simulation.Trajectory_B[shiftedIdx]);
            frameData.C_Point = ArrayToPoint(simulation.Trajectory_C[shiftedIdx]);
            frameData.L_Point = ArrayToPoint(simulation.Trajectory_L[shiftedIdx]);
            frameData.D1_Point = ArrayToPoint(simulation.Trajectory_D1[shiftedIdx]);
            frameData.D2_Point = ArrayToPoint(simulation.Trajectory_D2[shiftedIdx]);
            frameData.C4_Point = ArrayToPoint(simulation.Trajectory_C4[shiftedIdx]);
            frameData.B1_Point = ArrayToPoint(simulation.Trajectory_B1[shiftedIdx]);
            frameData.B2_Point = ArrayToPoint(simulation.Trajectory_B2[shiftedIdx]);
            frameData.C1_Point = ArrayToPoint(simulation.Trajectory_C1[shiftedIdx]);
            frameData.C2_Point = ArrayToPoint(simulation.Trajectory_C2[shiftedIdx]);
            frameData.C3_Point = ArrayToPoint(simulation.Trajectory_C3[shiftedIdx]);

            // 角度数据
            frameData.AB_Horizontal_Angle = simulation.AB_horizontal_angles[shiftedIdx];
            frameData.AB_BC_Angle = simulation.AB_BC_angles[shiftedIdx];
            frameData.BC_CL_Angle = simulation.BC_CL_angles[shiftedIdx];
            frameData.AB1_AB_Angle = simulation.AB1_AB_angles[shiftedIdx];

            // 距离数据
            frameData.C3_C4_Distance = simulation.C3_C4_distances[shiftedIdx];
            frameData.C2_C3_Distance = simulation.C2_C3_distances[shiftedIdx];
            frameData.C2_C4_Distance = simulation.C2_C4_distances[shiftedIdx];
            frameData.C2_C3_C4_Angle = simulation.C2_C3_C4_angles[shiftedIdx];

            // 如果是右侧腿，调整大腿角度（镜像变换）
            if (isRightLeg)
            {
                frameData.AB_Horizontal_Angle = 540 - frameData.AB_Horizontal_Angle;
                // 确保角度在0-360度范围内
                while (frameData.AB_Horizontal_Angle >= 360)
                {
                    frameData.AB_Horizontal_Angle -= 360;
                }
                while (frameData.AB_Horizontal_Angle < 0)
                {
                    frameData.AB_Horizontal_Angle += 360;
                }
            }

            legTraj.Frames.push_back(frameData);
        }

        trajectoryData.Legs[legCode] = legTraj;
    }

    return trajectoryData;
}

// 创建电机数据
MotorDataExport CompleteJsonExporter::CreateMotorData(const QuadrupedRobotSimulation &simulation)
{
    MotorDataExport motorData;
    motorData.StepTime = simulation.step;

    // 获取四条腿的电机数据
    std::map<std::string, MotorData> legsData = {
        {"LF", simulation.motor_data_LF},
        {"LH", simulation.motor_data_LH},
        {"RF", simulation.motor_data_RF},
        {"RH", simulation.motor_data_RH}};

    for (const auto &kvp : legsData)
    {
        std::string legCode = kvp.first;
        const MotorData &data = kvp.second;

        LegMotorData legMotorData;
        legMotorData.LegCode = legCode;

        int n_frames = data.frame_start_times.size();
        for (int i = 0; i < n_frames; i++)
        {
            MotorFrameData frameData;
            frameData.FrameNumber = i;
            frameData.StartTime = data.frame_start_times[i];
            frameData.EndTime = data.frame_end_times[i];

            // 大腿电机数据
            frameData.Thigh.Angle = data.AB_angles[i];
            frameData.Thigh.AngleIncrement = data.AB_angle_increments[i];
            frameData.Thigh.Speed = data.thigh_motor_speeds[i];

            // 小腿电机数据
            frameData.Shank.Angle = data.AB1_AB_angles[i];
            frameData.Shank.AngleIncrement = data.AB1_AB_angle_increments[i];
            frameData.Shank.Speed = data.shank_motor_speeds[i];

            // 脚踝电机数据
            frameData.Ankle.Angle = data.ankle_motor_angles[i];
            frameData.Ankle.AngleIncrement = data.ankle_motor_angle_increments[i];
            frameData.Ankle.Speed = data.ankle_motor_speeds[i];

            // 丝杠相关数据
            frameData.C3C4_Distance = data.C3C4_distances[i];
            frameData.C3C4_DistanceIncrement = data.C3C4_distance_increments[i];

            legMotorData.Frames.push_back(frameData);
        }

        motorData.Legs[legCode] = legMotorData;
    }

    return motorData;
}

// 创建约束检查数据
ConstraintData CompleteJsonExporter::CreateConstraintData(const QuadrupedRobotSimulation &simulation)
{
    const double AB1_AB_min = 23.0;
    const double AB1_AB_max = 130.0;
    const double C3_C4_min = 125.0;
    const double C3_C4_max = 205.0;

    ConstraintData constraintData;

    // 计算AB1-AB角度的绝对值的最大最小值
    double ab1_ab_min_actual = std::numeric_limits<double>::max();
    double ab1_ab_max_actual = std::numeric_limits<double>::lowest();
    bool ab1_ab_all_valid = true;

    for (double angle : simulation.AB1_AB_angles)
    {
        double abs_angle = std::abs(angle);
        ab1_ab_min_actual = std::min(ab1_ab_min_actual, abs_angle);
        ab1_ab_max_actual = std::max(ab1_ab_max_actual, abs_angle);

        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
        {
            ab1_ab_all_valid = false;
        }
    }

    // 计算C3-C4距离的最大最小值
    double c3c4_min_actual = std::numeric_limits<double>::max();
    double c3c4_max_actual = std::numeric_limits<double>::lowest();
    bool c3c4_all_valid = true;

    for (double distance : simulation.C3_C4_distances)
    {
        c3c4_min_actual = std::min(c3c4_min_actual, distance);
        c3c4_max_actual = std::max(c3c4_max_actual, distance);

        if (distance < C3_C4_min || distance > C3_C4_max)
        {
            c3c4_all_valid = false;
        }
    }

    // 计算CL距离的最大最小值
    double cl_min_actual = std::numeric_limits<double>::max();
    double cl_max_actual = std::numeric_limits<double>::lowest();
    bool cl_all_valid = true;

    for (double distance : simulation.CL_distances)
    {
        cl_min_actual = std::min(cl_min_actual, distance);
        cl_max_actual = std::max(cl_max_actual, distance);

        if (std::abs(distance - simulation.L30) >= 1e-6)
        {
            cl_all_valid = false;
        }
    }

    // 计算点积的最大最小值
    double dot_min_actual = std::numeric_limits<double>::max();
    double dot_max_actual = std::numeric_limits<double>::lowest();
    bool dot_all_valid = true;

    for (double dot_product : simulation.dot_products)
    {
        dot_min_actual = std::min(dot_min_actual, dot_product);
        dot_max_actual = std::max(dot_max_actual, dot_product);

        if (std::abs(dot_product) >= 1e-6)
        {
            dot_all_valid = false;
        }
    }

    // 设置约束数据
    constraintData.AB1_AB_Constraint.MinAllowed = AB1_AB_min;
    constraintData.AB1_AB_Constraint.MaxAllowed = AB1_AB_max;
    constraintData.AB1_AB_Constraint.MinActual = ab1_ab_min_actual;
    constraintData.AB1_AB_Constraint.MaxActual = ab1_ab_max_actual;
    constraintData.AB1_AB_Constraint.IsValid = ab1_ab_all_valid;
    constraintData.AB1_AB_Constraint.CheckType = "绝对值检查";

    constraintData.C3C4_Constraint.MinAllowed = C3_C4_min;
    constraintData.C3C4_Constraint.MaxAllowed = C3_C4_max;
    constraintData.C3C4_Constraint.MinActual = c3c4_min_actual;
    constraintData.C3C4_Constraint.MaxActual = c3c4_max_actual;
    constraintData.C3C4_Constraint.IsValid = c3c4_all_valid;

    constraintData.CL_Distance_Constraint.MinAllowed = simulation.L30;
    constraintData.CL_Distance_Constraint.MaxAllowed = simulation.L30;
    constraintData.CL_Distance_Constraint.MinActual = cl_min_actual;
    constraintData.CL_Distance_Constraint.MaxActual = cl_max_actual;
    constraintData.CL_Distance_Constraint.IsValid = cl_all_valid;

    constraintData.Perpendicularity_Constraint.MaxDotProduct = dot_max_actual;
    constraintData.Perpendicularity_Constraint.MinDotProduct = dot_min_actual;
    constraintData.Perpendicularity_Constraint.IsValid = dot_all_valid;

    // 收集约束违反信息
    std::vector<ConstraintViolation> violations;

    // 检查AB1-AB约束违反
    for (size_t i = 0; i < simulation.AB1_AB_angles.size(); i++)
    {
        double abs_angle = std::abs(simulation.AB1_AB_angles[i]);
        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
        {
            ConstraintViolation violation;
            violation.ConstraintType = "AB1_AB_Angle";
            violation.Frame = static_cast<int>(i);
            violation.Time = i * simulation.step;
            violation.Value = simulation.AB1_AB_angles[i];
            violation.AllowedRange = DoubleToString(AB1_AB_min) + "~" + DoubleToString(AB1_AB_max);
            violation.Severity = "Error";

            violations.push_back(violation);
        }
    }

    // 检查C3-C4约束违反
    for (size_t i = 0; i < simulation.C3_C4_distances.size(); i++)
    {
        double distance = simulation.C3_C4_distances[i];
        if (distance < C3_C4_min || distance > C3_C4_max)
        {
            ConstraintViolation violation;
            violation.ConstraintType = "C3C4_Distance";
            violation.Frame = static_cast<int>(i);
            violation.Time = i * simulation.step;
            violation.Value = distance;
            violation.AllowedRange = DoubleToString(C3_C4_min) + "~" + DoubleToString(C3_C4_max);
            violation.Severity = "Error";

            violations.push_back(violation);
        }
    }

    constraintData.ConstraintViolations = violations;

    return constraintData;
}

// 创建统计信息
StatisticsData CompleteJsonExporter::CreateStatistics(const QuadrupedRobotSimulation &simulation)
{
    StatisticsData statistics;

    // 计算角度统计
    double ab_horizontal_min = FindMin(simulation.AB_horizontal_angles);
    double ab_horizontal_max = FindMax(simulation.AB_horizontal_angles);

    double ab_bc_min = FindMin(simulation.AB_BC_angles);
    double ab_bc_max = FindMax(simulation.AB_BC_angles);

    double bc_cl_min = FindMin(simulation.BC_CL_angles);
    double bc_cl_max = FindMax(simulation.BC_CL_angles);

    double ab1_ab_min = FindMin(simulation.AB1_AB_angles);
    double ab1_ab_max = FindMax(simulation.AB1_AB_angles);

    // 角度统计
    statistics.AngleStatistics.AB_Horizontal.Min = ab_horizontal_min;
    statistics.AngleStatistics.AB_Horizontal.Max = ab_horizontal_max;
    statistics.AngleStatistics.AB_Horizontal.Range = ab_horizontal_max - ab_horizontal_min;

    statistics.AngleStatistics.AB_BC.Min = ab_bc_min;
    statistics.AngleStatistics.AB_BC.Max = ab_bc_max;
    statistics.AngleStatistics.AB_BC.Range = ab_bc_max - ab_bc_min;

    statistics.AngleStatistics.BC_CL.Min = bc_cl_min;
    statistics.AngleStatistics.BC_CL.Max = bc_cl_max;
    statistics.AngleStatistics.BC_CL.Range = bc_cl_max - bc_cl_min;

    statistics.AngleStatistics.AB1_AB.Min = ab1_ab_min;
    statistics.AngleStatistics.AB1_AB.Max = ab1_ab_max;
    statistics.AngleStatistics.AB1_AB.Range = ab1_ab_max - ab1_ab_min;

    // 计算距离统计
    double c3c4_min = FindMin(simulation.C3_C4_distances);
    double c3c4_max = FindMax(simulation.C3_C4_distances);

    double c2c3_min = FindMin(simulation.C2_C3_distances);
    double c2c3_max = FindMax(simulation.C2_C3_distances);

    double c2c4_min = FindMin(simulation.C2_C4_distances);
    double c2c4_max = FindMax(simulation.C2_C4_distances);

    // 距离统计
    statistics.DistanceStatistics.C3_C4.Min = c3c4_min;
    statistics.DistanceStatistics.C3_C4.Max = c3c4_max;
    statistics.DistanceStatistics.C3_C4.Range = c3c4_max - c3c4_min;

    statistics.DistanceStatistics.C2_C3.Min = c2c3_min;
    statistics.DistanceStatistics.C2_C3.Max = c2c3_max;
    statistics.DistanceStatistics.C2_C3.Range = c2c3_max - c2c3_min;

    statistics.DistanceStatistics.C2_C4.Min = c2c4_min;
    statistics.DistanceStatistics.C2_C4.Max = c2c4_max;
    statistics.DistanceStatistics.C2_C4.Range = c2c4_max - c2c4_min;

    // 计算电机速度统计（使用左前腿的数据）
    double thigh_min = FindMin(simulation.motor_data_LF.thigh_motor_speeds);
    double thigh_max = FindMax(simulation.motor_data_LF.thigh_motor_speeds);

    double shank_min = FindMin(simulation.motor_data_LF.shank_motor_speeds);
    double shank_max = FindMax(simulation.motor_data_LF.shank_motor_speeds);

    double ankle_min = FindMin(simulation.motor_data_LF.ankle_motor_speeds);
    double ankle_max = FindMax(simulation.motor_data_LF.ankle_motor_speeds);

    // 电机速度统计
    statistics.MotorSpeedStatistics.Thigh.Min = thigh_min;
    statistics.MotorSpeedStatistics.Thigh.Max = thigh_max;
    statistics.MotorSpeedStatistics.Thigh.Range = std::abs(thigh_max - thigh_min);

    statistics.MotorSpeedStatistics.Shank.Min = shank_min;
    statistics.MotorSpeedStatistics.Shank.Max = shank_max;
    statistics.MotorSpeedStatistics.Shank.Range = std::abs(shank_max - shank_min);

    statistics.MotorSpeedStatistics.Ankle.Min = ankle_min;
    statistics.MotorSpeedStatistics.Ankle.Max = ankle_max;
    statistics.MotorSpeedStatistics.Ankle.Range = std::abs(ankle_max - ankle_min);

    // 轨迹统计
    statistics.TrajectoryStatistics.TotalFrames = simulation.Trajectory_L.size();
    statistics.TrajectoryStatistics.TotalTime = simulation.Trajectory_L.size() * simulation.step;
    statistics.TrajectoryStatistics.SwingFrames = static_cast<int>(std::round(simulation.t_w / simulation.step));
    statistics.TrajectoryStatistics.SupportFrames = static_cast<int>(std::round(simulation.t_s / simulation.step));

    return statistics;
}

// 创建初始角度数据
InitialAnglesData CompleteJsonExporter::CreateInitialAngles(const QuadrupedRobotSimulation &simulation)
{
    InitialAnglesData initialAngles;

    // 获取原始初始角度
    std::map<std::string, RawInitialAngles> rawInitialAngles;

    rawInitialAngles["LF"] = RawInitialAngles{
        .Thigh = simulation.AB_horizontal_angles[0],
        .Shank = simulation.AB1_AB_angles[0],
        .Ankle = simulation.C3_C4_distances[0]};

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

    // 添加其他腿的初始角度
    rawInitialAngles["LH"] = RawInitialAngles{
        .Thigh = simulation.AB_horizontal_angles[phase_LH],
        .Shank = simulation.AB1_AB_angles[phase_LH],
        .Ankle = simulation.C3_C4_distances[phase_LH]};

    rawInitialAngles["RF"] = RawInitialAngles{
        .Thigh = simulation.AB_horizontal_angles[phase_RF],
        .Shank = simulation.AB1_AB_angles[phase_RF],
        .Ankle = simulation.C3_C4_distances[phase_RF]};

    rawInitialAngles["RH"] = RawInitialAngles{
        .Thigh = simulation.AB_horizontal_angles[phase_RH],
        .Shank = simulation.AB1_AB_angles[phase_RH],
        .Ankle = simulation.C3_C4_distances[phase_RH]};

    // 创建每条腿的初始角度数据
    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
    std::vector<MotorData> motorDatas = {
        simulation.motor_data_LF,
        simulation.motor_data_LH,
        simulation.motor_data_RF,
        simulation.motor_data_RH};

    for (size_t i = 0; i < legCodes.size(); i++)
    {
        std::string legCode = legCodes[i];
        const MotorData &motorData = motorDatas[i];
        const RawInitialAngles &rawAngles = rawInitialAngles[legCode];

        LegInitialAngles legInitialAngles;
        legInitialAngles.LegCode = legCode;

        // 调试初始标定位置（原始值）
        legInitialAngles.RawInitialAngles = rawAngles;

        // 标定后位置（调整后的值）
        legInitialAngles.CalibratedAngles.Thigh = motorData.AB_angles[0];
        legInitialAngles.CalibratedAngles.Shank = motorData.AB1_AB_angles[0];
        legInitialAngles.CalibratedAngles.Ankle = motorData.ankle_motor_angles[0];

        // 调整信息
        legInitialAngles.Adjustments.ThighAdjustment = motorData.AB_angles[0] - rawAngles.Thigh;
        legInitialAngles.Adjustments.ShankAdjustment = motorData.AB1_AB_angles[0] - rawAngles.Shank;
        legInitialAngles.Adjustments.AnkleAdjustment = motorData.ankle_motor_angles[0] - rawAngles.Ankle;

        initialAngles.Legs[legCode] = legInitialAngles;
    }

    return initialAngles;
}

// 创建相位信息
PhaseInfo CompleteJsonExporter::CreatePhaseInfo(const QuadrupedRobotSimulation &simulation)
{
    PhaseInfo phaseInfo;
    int n_frames = simulation.Trajectory_L.size();

    phaseInfo.GaitType = simulation.gait_type;
    phaseInfo.TimeLF = simulation.timeLF;
    phaseInfo.TimeLH = simulation.timeLH;
    phaseInfo.TimeRF = simulation.timeRF;
    phaseInfo.TimeRH = simulation.timeRH;
    phaseInfo.PhaseLF = 0;
    phaseInfo.PhaseLH = static_cast<int>(simulation.timeLH * n_frames);
    phaseInfo.PhaseRF = static_cast<int>(simulation.timeRF * n_frames);
    phaseInfo.PhaseRH = static_cast<int>(simulation.timeRH * n_frames);
    phaseInfo.SupportRatio = simulation.support_ratio;
    phaseInfo.SwingRatio = simulation.swing_ratio;
    phaseInfo.SupportTime = simulation.t_s;
    phaseInfo.SwingTime = simulation.t_w;

    return phaseInfo;
}

// 创建运动范围数据
MotionRangeData CompleteJsonExporter::CreateMotionRange(const QuadrupedRobotSimulation &simulation)
{
    MotionRangeData motionRange;

    // 为每条腿计算运动范围
    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
    std::vector<int> phaseShifts = {
        0,
        static_cast<int>(simulation.timeLH * simulation.Trajectory_L.size()),
        static_cast<int>(simulation.timeRF * simulation.Trajectory_L.size()),
        static_cast<int>(simulation.timeRH * simulation.Trajectory_L.size())};

    for (size_t i = 0; i < legCodes.size(); i++)
    {
        std::string legCode = legCodes[i];
        int phaseShift = phaseShifts[i];

        // 计算L点运动范围
        std::vector<double> x_values, y_values;
        for (size_t idx = 0; idx < simulation.Trajectory_L.size(); idx++)
        {
            int shiftedIdx = (idx + phaseShift) % simulation.Trajectory_L.size();
            const auto &point = simulation.Trajectory_L[shiftedIdx];
            x_values.push_back(point[0]);
            y_values.push_back(point[1]);
        }

        double x_min = FindMin(x_values);
        double x_max = FindMax(x_values);
        double y_min = FindMin(y_values);
        double y_max = FindMax(y_values);

        LegMotionRange legMotionRange;
        legMotionRange.LegCode = legCode;
        legMotionRange.PhaseShift = phaseShift;

        legMotionRange.L_Point.X.Min = x_min;
        legMotionRange.L_Point.X.Max = x_max;
        legMotionRange.L_Point.X.Range = x_max - x_min;

        legMotionRange.L_Point.Y.Min = y_min;
        legMotionRange.L_Point.Y.Max = y_max;
        legMotionRange.L_Point.Y.Range = y_max - y_min;

        legMotionRange.L_Point.TotalRange = std::sqrt(
            std::pow(x_max - x_min, 2) +
            std::pow(y_max - y_min, 2));

        motionRange.Legs[legCode] = legMotionRange;
    }

    return motionRange;
}

// 辅助函数：将数组转换为Point2D
Point2D CompleteJsonExporter::ArrayToPoint(const std::vector<double> &array)
{
    if (array.size() >= 2)
    {
        return Point2D(array[0], array[1]);
    }
    return Point2D(0.0, 0.0);
}

// 模板函数：查找最小值
template <typename T>
T CompleteJsonExporter::FindMin(const std::vector<T> &values)
{
    if (values.empty())
    {
        return T();
    }
    T min_val = values[0];
    for (const auto &val : values)
    {
        if (val < min_val)
        {
            min_val = val;
        }
    }
    return min_val;
}

// 模板函数：查找最大值
template <typename T>
T CompleteJsonExporter::FindMax(const std::vector<T> &values)
{
    if (values.empty())
    {
        return T();
    }
    T max_val = values[0];
    for (const auto &val : values)
    {
        if (val > max_val)
        {
            max_val = val;
        }
    }
    return max_val;
}

// 模板函数：检查所有值是否满足条件
template <typename T>
bool CompleteJsonExporter::All(const std::vector<T> &values, std::function<bool(const T &)> predicate)
{
    for (const auto &val : values)
    {
        if (!predicate(val))
        {
            return false;
        }
    }
    return true;
}

// 辅助函数：将double转换为字符串
std::string CompleteJsonExporter::DoubleToString(double value, int precision)
{
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(precision) << value;
    return oss.str();
}