// BaseClass.cpp - 修复type_error::create错误
#include "QuadrupedRobotSimulation/BaseClass.h"

// 为nlohmann/json添加std::optional支持
namespace nlohmann
{
    template <typename T>
    struct adl_serializer<std::optional<T>>
    {
        static void to_json(json &j, const std::optional<T> &opt)
        {
            if (opt.has_value())
            {
                j = opt.value();
            }
            else
            {
                j = nullptr;
            }
        }

        static void from_json(const json &j, std::optional<T> &opt)
        {
            if (j.is_null())
            {
                opt = std::nullopt;
            }
            else
            {
                opt = j.get<T>();
            }
        }
    };

    // 为 std::vector<std::map<std::string, std::string>> 添加特化
    template <>
    struct adl_serializer<std::vector<std::map<std::string, std::string>>>
    {
        static void to_json(json &j, const std::vector<std::map<std::string, std::string>> &v)
        {
            j = json::array();
            for (const auto &map_item : v)
            {
                json item = json::object();
                for (const auto &kvp : map_item)
                {
                    item[kvp.first] = kvp.second;
                }
                j.push_back(item);
            }
        }

        static void from_json(const json &j, std::vector<std::map<std::string, std::string>> &v)
        {
            if (!j.is_array())
            {
                // 修复：使用正确的参数
                throw json::type_error::create(302, "类型必须是数组", &j);
            }

            v.clear();
            for (const auto &item : j)
            {
                if (!item.is_object())
                {
                    // 修复：使用正确的参数
                    throw json::type_error::create(302, "数组元素必须是对象", &item);
                }

                std::map<std::string, std::string> map_item;
                for (auto it = item.begin(); it != item.end(); ++it)
                {
                    if (it.value().is_string())
                    {
                        map_item[it.key()] = it.value().get<std::string>();
                    }
                    else
                    {
                        map_item[it.key()] = it.value().dump();
                    }
                }
                v.push_back(map_item);
            }
        }
    };

    // 为 Vector2D 添加序列化支持
    template <>
    struct adl_serializer<Vector2D>
    {
        static void to_json(json &j, const Vector2D &v)
        {
            j = json{{"X", v.X}, {"Y", v.Y}};
        }

        static void from_json(const json &j, Vector2D &v)
        {
            if (j.contains("X"))
                j.at("X").get_to(v.X);
            if (j.contains("Y"))
                j.at("Y").get_to(v.Y);
        }
    };
    // 为 ModelID 添加序列化支持
    template <>
    struct adl_serializer<ModelID>
    {
        static void to_json(json &j, const ModelID &modelID)
        {
            j = json{
                {"LF", modelID.LF},
                {"LH", modelID.LH},
                {"RF", modelID.RF},
                {"RH", modelID.RH}};
        }

        static void from_json(const json &j, ModelID &modelID)
        {
            if (j.contains("LF"))
            {
                j.at("LF").get_to(modelID.LF);
            }
            if (j.contains("LH"))
            {
                j.at("LH").get_to(modelID.LH);
            }
            if (j.contains("RF"))
            {
                j.at("RF").get_to(modelID.RF);
            }
            if (j.contains("RH"))
            {
                j.at("RH").get_to(modelID.RH);
            }
        }
    };
}
const std::string RobotBaseJson = R"({
        "GaitInfo": {
            "GaitType": "trot",
            "Period": 0.8,
            "Frequency": 50.0,
            "StepTime": 0.02,
            "SupportTime": 0.4,
            "SwingTime": 0.4,
            "SupportRatio": 0.5,
            "SwingRatio": 0.5,
            "TotalFrames": 40,
            "TotalTime": 0.8
        },
        "SystemParameters": {
            "A_x": 0.0,
            "A_y": 500.0,
            "L10": 300.0,
            "L20": 286.0,
            "L30": 55.0,
            "StepLength": 60.0,
            "StepHeight": 50.0,
            "X": 30.0,
            "DeltaX": 0.0,
            "L21": 70.0,
            "L22": 292.0,
            "L23": 80.0,
            "Beta1": 166.0,
            "BB2_BC_Angle": 3.3859387489,
            "C1_B_Length": 107.0,
            "CC1": 179.0,
            "L31": 32.0,
            "C2C3_Length": 32.0,
            "Lead": 10.0,
            "D1_L_Offset": 90.0,
            "D2_L_Offset": 50.0,
            "C4_D2_Offset": 18.0,
            "ThighMotorReduction": 1.0,
            "ShankMotorReduction": 1.0,
            "AnkleMotorReduction": 1.0
        },
        "ModelID": {
            "LF": [
                {
                    "A_B": "6b6243e3-42f6-422f-953d-e5067defe496"
                },
                {
                    "A_B1": "64cb1022-d31c-47dd-93ba-05d5e809f5fc"
                },
                {
                    "B1_B2": "ea2129dc-7e67-409f-a179-0b92d4ebfde7"
                },
                {
                    "B_C": "a29577a4-2585-4dd0-8a42-e4119946c48d"
                },
                {
                    "C2_C3": "7c934822-4b62-458a-a7b6-e4db93494e98"
                },
                {
                    "C4_C3": "4350a961-2d7b-476e-89e2-626ae87cd770"
                },
                {
                    "C_C4": "b00c3442-081e-4436-96f8-9d1f26ffc3ba"
                }
            ],
            "LH": [
                {
                    "A_B": "18465e41-5edc-41b3-9ffe-629df5f01f54"
                },
                {
                    "A_B1": "2b2c8edc-d0f6-43e0-8879-11f8f5ae9062"
                },
                {
                    "B1_B2": "77168aa6-d713-482f-8783-e50a7cac201b"
                },
                {
                    "B_C": "e9f937e5-1c8c-42f0-8c9a-6573fca012fd"
                },
                {
                    "C2_C3": "04bdfae0-f6b4-40b5-a6c6-0f269065845b"
                },
                {
                    "C4_C3": "a84326a9-fec7-4b0d-abea-9fd28e72a898"
                },
                {
                    "C_C4": "19f64283-19b0-4672-bb83-144c20d0902e"
                }
            ],
            "RF": [
                {
                    "A_B": "40ecce28-6457-4052-a0d7-a1ca2b2a758e"
                },
                {
                    "A_B1": "1d4c294a-ea5b-4f31-a8d7-978fe5d070cc"
                },
                {
                    "B1_B2": "d67149a8-b9a8-4538-a387-0f01c97793dc"
                },
                {
                    "B_C": "6ee6e0ea-6b43-4d80-b852-b33643448e94"
                },
                {
                    "C2_C3": "099975df-0a2f-4c9d-b415-bc98e61d3cf6"
                },
                {
                    "C4_C3": "5b106343-30ff-4d2d-a3b7-3ffc90bf0019"
                },
                {
                    "C_C4": "b5f275f9-8f49-44aa-9dcf-859ddf0fc9c7"
                }
            ],
            "RH": [
                {
                    "A_B": "643c80b9-222d-4617-b3c8-82d4db7b692e"
                },
                {
                    "A_B1": "4d04f0dd-8528-46b0-b205-8e0433e4c7bb"
                },
                {
                    "B1_B2": "18a5d033-bd98-4ed7-96ed-a70fd21c44b1"
                },
                {
                    "B_C": "2e9c91a9-c427-45fa-8ca4-be55b7916574"
                },
                {
                    "C2_C3": "0dc7124d-e86e-413a-9743-57bc65dde135"
                },
                {
                    "C4_C3": "9fca5ec6-46d1-4e50-8a8c-b2931c73594d"
                },
                {
                    "C_C4": "00369f21-f96d-43a8-821c-3fc8c6ce9c03"
                }
            ]
        },
        "Param": {
            "LF": {
                "thigh_angle_deg": -37.4784578338516,
                "shank_angle_deg": -22.4917532451628,
                "ankle_angle_deg": 0.0
            },
            "LH": {
                "thigh_angle_deg": -34.0564623655754,
                "shank_angle_deg": -22.1353106546916,
                "ankle_angle_deg": 0.0
            },
            "RF": {
                "thigh_angle_deg": 34.0564623655754,
                "shank_angle_deg": 22.1353106546916,
                "ankle_angle_deg": 0.0
            },
            "RH": {
                "thigh_angle_deg": 37.4784578338516,
                "shank_angle_deg": 22.4917532451628,
                "ankle_angle_deg": 0.0
            }
        },
        "RobotBody": {
            "BodyCode": "7ec2aade-c45a-4bf8-8bb3-d1e0db5df9f6",
            "x": 0,
            "y": 0,
            "z": 560,
            "qx": 0,
            "qy": 0,
            "qz": 0,
            "qw": 1
        },
        "t_percentage": 1
    }
)";

// CheckResultItem 序列化
void to_json(json &j, const CheckResultItem &item)
{
    j = json{
        {"Condition", item.Condition},
        {"Passed", item.Passed},
        {"AdditionalData", item.AdditionalData}};

    // 处理optional字段
    if (item.Actual.has_value())
    {
        j["Actual"] = item.Actual.value();
    }
    else
    {
        j["Actual"] = nullptr;
    }

    if (item.Expected.has_value())
    {
        j["Expected"] = item.Expected.value();
    }
    else
    {
        j["Expected"] = nullptr;
    }

    if (item.Tolerance.has_value())
    {
        j["Tolerance"] = item.Tolerance.value();
    }
    else
    {
        j["Tolerance"] = nullptr;
    }
}

void from_json(const json &j, CheckResultItem &item)
{
    if (j.contains("Condition"))
    {
        j.at("Condition").get_to(item.Condition);
    }

    if (j.contains("Passed"))
    {
        j.at("Passed").get_to(item.Passed);
    }

    if (j.contains("Actual") && !j["Actual"].is_null())
    {
        item.Actual = j["Actual"].get<double>();
    }
    else
    {
        item.Actual = std::nullopt;
    }

    if (j.contains("Expected") && !j["Expected"].is_null())
    {
        item.Expected = j["Expected"].get<double>();
    }
    else
    {
        item.Expected = std::nullopt;
    }

    if (j.contains("Tolerance") && !j["Tolerance"].is_null())
    {
        item.Tolerance = j["Tolerance"].get<double>();
    }
    else
    {
        item.Tolerance = std::nullopt;
    }

    if (j.contains("AdditionalData"))
    {
        j.at("AdditionalData").get_to(item.AdditionalData);
    }
}

// 在 BaseClass.cpp 中添加以下代码（在文件末尾或其他合适位置）

// // RobotGaitRequest 的反序列化实现
// 实现序列化函数
void to_json(json &j, const RobotGaitRequest &r)
{
    j = json::object();
    j["req_param"] = r.req_param;
}

void from_json(const json &j, RobotGaitRequest &r)
{
    if (j.is_object() && j.contains("req_param"))
    {
        j.at("req_param").get_to(r.req_param);
    }
    else if (j.is_object())
    {
        // 如果直接包含 req_param 的字段
        try
        {
            j.get_to(r.req_param);
        }
        catch (...)
        {
            r.req_param = RequestParameters{};
        }
    }
}
// SupportCheckResult 实现
SupportCheckResult::SupportCheckResult() : IsSupport(false)
{
    Checks = std::map<std::string, CheckResultItem>();
    Reasons = std::vector<std::string>();
    FailedChecks = std::vector<std::string>();
}

// 如果没有定义M_PI，则定义它
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// Point2D 实现
Point2D::Point2D(double x, double y) : X(x), Y(y) {}

void to_json(json &j, const Point2D &p)
{
    j = json{{"X", p.X}, {"Y", p.Y}};
}

void from_json(const json &j, Point2D &p)
{
    if (j.contains("X"))
    {
        j.at("X").get_to(p.X);
    }
    if (j.contains("Y"))
    {
        j.at("Y").get_to(p.Y);
    }
}

// MotorData 实现
MotorData::MotorData(int n_frames)
{
    if (n_frames > 0)
    {
        Resize(n_frames);
    }
}

void MotorData::Resize(int n_frames)
{
    frame_start_times.resize(n_frames);
    frame_end_times.resize(n_frames);
    AB_angles.resize(n_frames);
    AB_angle_increments.resize(n_frames);
    thigh_motor_speeds.resize(n_frames);
    AB1_AB_angles.resize(n_frames);
    AB1_AB_angle_increments.resize(n_frames);
    shank_motor_speeds.resize(n_frames);
    C3C4_distances.resize(n_frames);
    C3C4_distance_increments.resize(n_frames);
    ankle_motor_angles.resize(n_frames);
    ankle_motor_angle_increments.resize(n_frames);
    ankle_motor_speeds.resize(n_frames);
}

MotorData MotorData::Clone() const
{
    MotorData clone;
    clone.frame_start_times = frame_start_times;
    clone.frame_end_times = frame_end_times;
    clone.AB_angles = AB_angles;
    clone.AB_angle_increments = AB_angle_increments;
    clone.thigh_motor_speeds = thigh_motor_speeds;
    clone.AB1_AB_angles = AB1_AB_angles;
    clone.AB1_AB_angle_increments = AB1_AB_angle_increments;
    clone.shank_motor_speeds = shank_motor_speeds;
    clone.C3C4_distances = C3C4_distances;
    clone.C3C4_distance_increments = C3C4_distance_increments;
    clone.ankle_motor_angles = ankle_motor_angles;
    clone.ankle_motor_angle_increments = ankle_motor_angle_increments;
    clone.ankle_motor_speeds = ankle_motor_speeds;
    return clone;
}

void to_json(json &j, const MotorData &md)
{
    j = json{
        {"frame_start_times", md.frame_start_times},
        {"frame_end_times", md.frame_end_times},
        {"AB_angles", md.AB_angles},
        {"AB_angle_increments", md.AB_angle_increments},
        {"thigh_motor_speeds", md.thigh_motor_speeds},
        {"AB1_AB_angles", md.AB1_AB_angles},
        {"AB1_AB_angle_increments", md.AB1_AB_angle_increments},
        {"shank_motor_speeds", md.shank_motor_speeds},
        {"C3C4_distances", md.C3C4_distances},
        {"C3C4_distance_increments", md.C3C4_distance_increments},
        {"ankle_motor_angles", md.ankle_motor_angles},
        {"ankle_motor_angle_increments", md.ankle_motor_angle_increments},
        {"ankle_motor_speeds", md.ankle_motor_speeds}};
}

void from_json(const json &j, MotorData &md)
{
    if (j.contains("frame_start_times"))
    {
        j.at("frame_start_times").get_to(md.frame_start_times);
    }
    if (j.contains("frame_end_times"))
    {
        j.at("frame_end_times").get_to(md.frame_end_times);
    }
    if (j.contains("AB_angles"))
    {
        j.at("AB_angles").get_to(md.AB_angles);
    }
    if (j.contains("AB_angle_increments"))
    {
        j.at("AB_angle_increments").get_to(md.AB_angle_increments);
    }
    if (j.contains("thigh_motor_speeds"))
    {
        j.at("thigh_motor_speeds").get_to(md.thigh_motor_speeds);
    }
    if (j.contains("AB1_AB_angles"))
    {
        j.at("AB1_AB_angles").get_to(md.AB1_AB_angles);
    }
    if (j.contains("AB1_AB_angle_increments"))
    {
        j.at("AB1_AB_angle_increments").get_to(md.AB1_AB_angle_increments);
    }
    if (j.contains("shank_motor_speeds"))
    {
        j.at("shank_motor_speeds").get_to(md.shank_motor_speeds);
    }
    if (j.contains("C3C4_distances"))
    {
        j.at("C3C4_distances").get_to(md.C3C4_distances);
    }
    if (j.contains("C3C4_distance_increments"))
    {
        j.at("C3C4_distance_increments").get_to(md.C3C4_distance_increments);
    }
    if (j.contains("ankle_motor_angles"))
    {
        j.at("ankle_motor_angles").get_to(md.ankle_motor_angles);
    }
    if (j.contains("ankle_motor_angle_increments"))
    {
        j.at("ankle_motor_angle_increments").get_to(md.ankle_motor_angle_increments);
    }
    if (j.contains("ankle_motor_speeds"))
    {
        j.at("ankle_motor_speeds").get_to(md.ankle_motor_speeds);
    }
}

// ModelIDSplitter 实现
// 修改QuadrupedRobotConfiguration中的SplitModelIDToArrays函数
// BaseClass.cpp - 修正 ModelIDSplitter 的实现
SplitModelID ModelIDSplitter::SplitModelIDToArrays(
    const ModelID &modelID, // 已修正为 ModelID 类型
    const std::map<std::string, std::vector<double>> &pointsDict)
{
    if (pointsDict.empty())
    {
        throw std::invalid_argument("points_dict不能为空");
    }

    SplitModelID result;
    std::set<std::string> validKeys;
    for (const auto &kvp : pointsDict)
    {
        validKeys.insert(kvp.first);
    }

    return result;
}
std::vector<std::map<std::string, std::vector<std::string>>>
ModelIDSplitter::SplitLegDictionaryDataToArrays(
    const std::vector<std::map<std::string, std::string>> &legData,
    const std::set<std::string> &validKeys)
{

    std::vector<std::map<std::string, std::vector<std::string>>> result;

    for (const auto &dict : legData)
    {
        auto splitDict = SplitDictionaryToArrays(dict, validKeys);
        if (!splitDict.empty())
        {
            result.push_back(splitDict);
        }
    }

    return result;
}

std::map<std::string, std::vector<std::string>>
ModelIDSplitter::SplitDictionaryToArrays(
    const std::map<std::string, std::string> &dict,
    const std::set<std::string> &validKeys)
{

    std::map<std::string, std::vector<std::string>> result;

    for (const auto &kvp : dict)
    {
        auto splitKeys = SplitKey(kvp.first, validKeys);
        if (!splitKeys.empty())
        {
            result[kvp.first] = splitKeys;
        }
    }

    return result;
}

std::vector<std::string> ModelIDSplitter::SplitKey(
    const std::string &compositeKey,
    const std::set<std::string> &validKeys)
{

    std::vector<std::string> result;

    if (compositeKey.empty())
    {
        return result;
    }

    size_t i = 0;
    while (i < compositeKey.length())
    {
        std::string matchedKey;

        for (int length = 4; length >= 1; length--)
        {
            if (i + length <= compositeKey.length())
            {
                std::string potentialKey = compositeKey.substr(i, length);
                if (validKeys.find(potentialKey) != validKeys.end())
                {
                    matchedKey = potentialKey;
                    break;
                }
            }
        }

        if (!matchedKey.empty())
        {
            result.push_back(matchedKey);
            i += matchedKey.length();
        }
        else
        {
            result.push_back(compositeKey.substr(i, 1));
            i++;
        }
    }

    return result;
}

// QuadrupedRobotConfiguration 实现
QuadrupedRobotConfiguration::QuadrupedRobotConfiguration()
    : A_point({0.0, 500.0}),
      L10(300.0), L20(286.0), L30(55.0),
      L21(70.0), L22(292.0), L23(80.0),
      beta1(166.0), CC1(179.0),
      L31(32.0), C2C3_length(32.0), lead1(10.0),
      s_l(60.0), s_h(50.0), X(30.0),
      D1_L_offset(90.0), D2_L_offset(50.0), C4_D2_offset(18.0),
      T(0.8), f(50.0), gait_type("trot"),
      support_ratio(0.5), swing_ratio(0.5),
      thigh_motor_reduction(1.0), shank_motor_reduction(1.0), ankle_motor_reduction(1.0),
      adjusted_shank_angles_factory(99.41097105),
      initial_C3C4_LF(177.312342), initial_C3C4_LH(168.8321),
      initial_C3C4_RF(168.833), initial_C3C4_RH(177.312342),
      BB2_BC_angle_rad(0.0), C1_B_length(0.0), CC4_reference_distance(0.0),
      deltX(0.0), step(0.0), t_s(0.0), t_w(0.0),
      timeLF(0.0), timeLH(0.0), timeRF(0.0), timeRH(0.0)
{
    CalculateDerivedParameters();
}

QuadrupedRobotConfiguration::QuadrupedRobotConfiguration(
    const GaitInfo &gaitInfo, const SystemParameters &sysParams)
    : QuadrupedRobotConfiguration()
{
    FromGaitInfoAndSystemParameters(gaitInfo, sysParams);
}

void QuadrupedRobotConfiguration::FromGaitInfoAndSystemParameters(
    const GaitInfo &gaitInfo, const SystemParameters &sysParams)
{
    FromSystemParameters(sysParams);
    FromGaitInfo(gaitInfo);
    CalculateDerivedParameters();

    // 调试输出
    // std::cout << "\n[DEBUG] 调用FromGaitInfoAndSystemParameters后配置信息:" << std::endl;
    // debugPrint();
}

void QuadrupedRobotConfiguration::CalculateDerivedParameters()
{
    BB2_BC_angle_rad = (360.0 - beta1) * M_PI / 180.0;
    C1_B_length = L20 - CC1;
    deltX = s_l / 2.0 - X;
    step = 1.0 / f;

    if (t_s <= 0 || t_w <= 0)
    {
        t_s = T * support_ratio;
        t_w = T * swing_ratio;
    }

    std::vector<double> C_ref = {0.0, L30};
    std::vector<double> C4_ref = {D2_L_offset, C4_D2_offset};
    CC4_reference_distance = std::sqrt(
        std::pow(C4_ref[0] - C_ref[0], 2) +
        std::pow(C4_ref[1] - C_ref[1], 2));
}

void QuadrupedRobotConfiguration::FromSystemParameters(const SystemParameters &sysParams)
{
    A_point = {sysParams.A_x, sysParams.A_y};
    L10 = sysParams.L10;
    L20 = sysParams.L20;
    L30 = sysParams.L30;
    s_l = sysParams.StepLength;
    s_h = sysParams.StepHeight;
    X = sysParams.X;
    L21 = sysParams.L21;
    L22 = sysParams.L22;
    L23 = sysParams.L23;
    beta1 = sysParams.Beta1;
    CC1 = sysParams.CC1;
    L31 = sysParams.L31;
    C2C3_length = sysParams.C2C3_Length;
    lead1 = sysParams.Lead;
    D1_L_offset = sysParams.D1_L_Offset;
    D2_L_offset = sysParams.D2_L_Offset;
    C4_D2_offset = sysParams.C4_D2_Offset;
    thigh_motor_reduction = sysParams.ThighMotorReduction;
    shank_motor_reduction = sysParams.ShankMotorReduction;
    ankle_motor_reduction = sysParams.AnkleMotorReduction;
}

void QuadrupedRobotConfiguration::FromGaitInfo(const GaitInfo &gaitInfo)
{
    T = gaitInfo.Period;
    f = gaitInfo.Frequency;
    gait_type = gaitInfo.GaitType;

    if (gaitInfo.StepTime > 0)
    {
        step = gaitInfo.StepTime;
    }

    if (gaitInfo.SupportTime > 0 && gaitInfo.SwingTime > 0)
    {
        t_s = gaitInfo.SupportTime;
        t_w = gaitInfo.SwingTime;
    }

    if (gaitInfo.SupportRatio > 0 && gaitInfo.SwingRatio > 0)
    {
        support_ratio = gaitInfo.SupportRatio;
        swing_ratio = gaitInfo.SwingRatio;
    }
}

QuadrupedRobotConfiguration::ValidationResult::ValidationResult()
    : IsValid(false)
{
    Errors = std::vector<std::string>();
    Warnings = std::vector<std::string>();
}

// 在BaseClass.cpp中实现
void QuadrupedRobotConfiguration::debugPrint() const
{
    std::cout << "========================================" << std::endl;
    std::cout << "QuadrupedRobotConfiguration DEBUG PRINT" << std::endl;
    std::cout << "========================================" << std::endl;

    // 基本几何参数
    std::cout << "=== 基本几何参数 ===" << std::endl;
    std::cout << "  A_point: [" << A_point[0] << ", " << A_point[1] << "]" << std::endl;
    std::cout << "  L10: " << L10 << std::endl;
    std::cout << "  L20: " << L20 << std::endl;
    std::cout << "  L30: " << L30 << std::endl;

    // 小腿连杆机构参数
    std::cout << "\n=== 小腿连杆机构参数 ===" << std::endl;
    std::cout << "  L21: " << L21 << std::endl;
    std::cout << "  L22: " << L22 << std::endl;
    std::cout << "  L23: " << L23 << std::endl;
    std::cout << "  beta1: " << beta1 << "°" << std::endl;
    std::cout << "  CC1: " << CC1 << std::endl;

    // 脚踝连杆机构参数
    std::cout << "\n=== 脚踝连杆机构参数 ===" << std::endl;
    std::cout << "  L31: " << L31 << std::endl;
    std::cout << "  C2C3_length: " << C2C3_length << std::endl;
    std::cout << "  lead1: " << lead1 << std::endl;

    // 步态控制参数
    std::cout << "\n=== 步态控制参数 ===" << std::endl;
    std::cout << "  s_l (步长): " << s_l << std::endl;
    std::cout << "  s_h (步高): " << s_h << std::endl;
    std::cout << "  X: " << X << std::endl;

    // 脚部构型参数
    std::cout << "\n=== 脚部构型参数 ===" << std::endl;
    std::cout << "  D1_L_offset: " << D1_L_offset << std::endl;
    std::cout << "  D2_L_offset: " << D2_L_offset << std::endl;
    std::cout << "  C4_D2_offset: " << C4_D2_offset << std::endl;

    // 步态周期参数
    std::cout << "\n=== 步态周期参数 ===" << std::endl;
    std::cout << "  T (周期): " << T << " s" << std::endl;
    std::cout << "  f (频率): " << f << " Hz" << std::endl;
    std::cout << "  gait_type: " << gait_type << std::endl;
    std::cout << "  support_ratio: " << support_ratio << std::endl;
    std::cout << "  swing_ratio: " << swing_ratio << std::endl;

    // 电机参数
    std::cout << "\n=== 电机参数 ===" << std::endl;
    std::cout << "  thigh_motor_reduction: " << thigh_motor_reduction << std::endl;
    std::cout << "  shank_motor_reduction: " << shank_motor_reduction << std::endl;
    std::cout << "  ankle_motor_reduction: " << ankle_motor_reduction << std::endl;

    // 逆向运动学特有参数
    std::cout << "\n=== 逆向运动学参数 ===" << std::endl;
    std::cout << "  adjusted_shank_angles_factory: " << adjusted_shank_angles_factory << "°" << std::endl;

    // 初始C3C4距离
    std::cout << "\n=== 初始C3C4距离 ===" << std::endl;
    std::cout << "  initial_C3C4_LF: " << initial_C3C4_LF << std::endl;
    std::cout << "  initial_C3C4_LH: " << initial_C3C4_LH << std::endl;
    std::cout << "  initial_C3C4_RF: " << initial_C3C4_RF << std::endl;
    std::cout << "  initial_C3C4_RH: " << initial_C3C4_RH << std::endl;

    // 计算得到的中间变量
    std::cout << "\n=== 计算得到的中间变量 ===" << std::endl;
    std::cout << "  BB2_BC_angle_rad: " << BB2_BC_angle_rad << " rad ("
              << BB2_BC_angle_rad * 180.0 / M_PI << "°)" << std::endl;
    std::cout << "  C1_B_length: " << C1_B_length << std::endl;
    std::cout << "  CC4_reference_distance: " << CC4_reference_distance << std::endl;
    std::cout << "  deltX: " << deltX << std::endl;
    std::cout << "  step (时间步长): " << step << " s" << std::endl;
    std::cout << "  t_s (支撑时间): " << t_s << " s" << std::endl;
    std::cout << "  t_w (摆动时间): " << t_w << " s" << std::endl;

    // 相位时间
    std::cout << "\n=== 相位时间 ===" << std::endl;
    std::cout << "  timeLF: " << timeLF << " s" << std::endl;
    std::cout << "  timeLH: " << timeLH << " s" << std::endl;
    std::cout << "  timeRF: " << timeRF << " s" << std::endl;
    std::cout << "  timeRH: " << timeRH << " s" << std::endl;

    // 验证配置
    std::cout << "\n=== 配置验证结果 ===" << std::endl;
    auto validation = Validate();
    std::cout << "  有效性: " << (validation.IsValid ? "有效" : "无效") << std::endl;

    if (!validation.Errors.empty())
    {
        std::cout << "  错误信息:" << std::endl;
        for (const auto &error : validation.Errors)
        {
            std::cout << "    - " << error << std::endl;
        }
    }

    if (!validation.Warnings.empty())
    {
        std::cout << "  警告信息:" << std::endl;
        for (const auto &warning : validation.Warnings)
        {
            std::cout << "    - " << warning << std::endl;
        }
    }

    // 关键计算检查
    std::cout << "\n=== 关键计算检查 ===" << std::endl;
    std::cout << "  支撑比例 + 摆动比例: " << (support_ratio + swing_ratio)
              << " (期望: 1.0)" << std::endl;
    std::cout << "  周期时间: " << T << " s, 步数: " << (T / step) << std::endl;

    if (T > 0 && step > 0)
    {
        int calculated_frames = static_cast<int>(T / step);
        std::cout << "  计算帧数: " << calculated_frames << std::endl;
    }

    // 杆长关系检查
    std::cout << "\n=== 杆长关系检查 ===" << std::endl;
    std::cout << "  L20 - CC1 = " << L20 << " - " << CC1 << " = " << C1_B_length << std::endl;
    std::cout << "  C1_B_length 应为正数: " << (C1_B_length > 0 ? "是" : "否") << std::endl;

    // 四杆机构检查
    std::cout << "  四杆机构杆长: L21=" << L21 << ", L22=" << L22
              << ", L23=" << L23 << std::endl;

    std::cout << "========================================" << std::endl;
    std::cout << "END QuadrupedRobotConfiguration DEBUG PRINT" << std::endl;
    std::cout << "========================================" << std::endl;
}

QuadrupedRobotConfiguration::ValidationResult
QuadrupedRobotConfiguration::Validate() const
{
    ValidationResult result;

    if (A_point.size() != 2)
    {
        result.Errors.push_back("A点坐标无效");
    }

    if (L10 <= 0)
        result.Errors.push_back("L10必须大于0");
    if (L20 <= 0)
        result.Errors.push_back("L20必须大于0");
    if (L30 <= 0)
        result.Errors.push_back("L30必须大于0");
    if (L21 <= 0)
        result.Errors.push_back("L21必须大于0");
    if (L22 <= 0)
        result.Errors.push_back("L22必须大于0");
    if (L23 <= 0)
        result.Errors.push_back("L23必须大于0");
    if (L31 <= 0)
        result.Errors.push_back("L31必须大于0");

    if (beta1 < 0 || beta1 > 360)
    {
        result.Warnings.push_back("beta1角度(" + std::to_string(beta1) + "°)可能超出正常范围");
    }

    if (T <= 0)
        result.Errors.push_back("步态周期时间T必须大于0");
    if (f <= 0)
        result.Errors.push_back("控制频率f必须大于0");
    if (step <= 0)
        result.Errors.push_back("时间步长step必须大于0");
    if (t_s <= 0)
        result.Errors.push_back("支撑时间t_s必须大于0");
    if (t_w <= 0)
        result.Errors.push_back("摆动时间t_w必须大于0");
    if (support_ratio <= 0 || support_ratio >= 1)
    {
        result.Errors.push_back("支撑比例support_ratio必须在0-1之间，当前值: " + std::to_string(support_ratio));
    }
    if (std::abs(support_ratio + swing_ratio - 1.0) > 1e-10)
    {
        result.Errors.push_back("支撑比例(" + std::to_string(support_ratio) +
                                ")和摆动比例(" + std::to_string(swing_ratio) + ")之和必须为1");
    }

    if (s_l <= 0)
        result.Warnings.push_back("步长为0，可能没有移动");
    if (s_h < 0)
        result.Warnings.push_back("步高为负数，脚部轨迹可能异常");

    if (thigh_motor_reduction <= 0)
        result.Errors.push_back("大腿电机减速比必须大于0");
    if (shank_motor_reduction <= 0)
        result.Errors.push_back("小腿电机减速比必须大于0");
    if (ankle_motor_reduction <= 0)
        result.Errors.push_back("脚踝电机减速比必须大于0");

    if (deltX < -s_l / 2.0 || deltX > s_l / 2.0)
    {
        result.Warnings.push_back("x方向偏移deltX(" + std::to_string(deltX) +
                                  ")可能超出正常范围[-" + std::to_string(s_l / 2.0) +
                                  ", " + std::to_string(s_l / 2.0) + "]");
    }

    result.IsValid = result.Errors.empty();
    return result;
}

void to_json(json &j, const QuadrupedRobotConfiguration &config)
{
    j = json{
        {"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},
        {"s_l", config.s_l},
        {"s_h", config.s_h},
        {"X", config.X},
        {"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},
        {"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},
        {"BB2_BC_angle_rad", config.BB2_BC_angle_rad},
        {"C1_B_length", config.C1_B_length},
        {"CC4_reference_distance", config.CC4_reference_distance},
        {"deltX", config.deltX},
        {"step", config.step},
        {"t_s", config.t_s},
        {"t_w", config.t_w},
        {"timeLF", config.timeLF},
        {"timeLH", config.timeLH},
        {"timeRF", config.timeRF},
        {"timeRH", config.timeRH}};
}

void from_json(const json &j, QuadrupedRobotConfiguration &config)
{
    if (j.contains("A_point"))
    {
        j.at("A_point").get_to(config.A_point);
    }
    if (j.contains("L10"))
    {
        j.at("L10").get_to(config.L10);
    }
    if (j.contains("L20"))
    {
        j.at("L20").get_to(config.L20);
    }
    if (j.contains("L30"))
    {
        j.at("L30").get_to(config.L30);
    }
    if (j.contains("L21"))
    {
        j.at("L21").get_to(config.L21);
    }
    if (j.contains("L22"))
    {
        j.at("L22").get_to(config.L22);
    }
    if (j.contains("L23"))
    {
        j.at("L23").get_to(config.L23);
    }
    if (j.contains("beta1"))
    {
        j.at("beta1").get_to(config.beta1);
    }
    if (j.contains("CC1"))
    {
        j.at("CC1").get_to(config.CC1);
    }
    if (j.contains("L31"))
    {
        j.at("L31").get_to(config.L31);
    }
    if (j.contains("C2C3_length"))
    {
        j.at("C2C3_length").get_to(config.C2C3_length);
    }
    if (j.contains("lead1"))
    {
        j.at("lead1").get_to(config.lead1);
    }
    if (j.contains("s_l"))
    {
        j.at("s_l").get_to(config.s_l);
    }
    if (j.contains("s_h"))
    {
        j.at("s_h").get_to(config.s_h);
    }
    if (j.contains("X"))
    {
        j.at("X").get_to(config.X);
    }
    if (j.contains("D1_L_offset"))
    {
        j.at("D1_L_offset").get_to(config.D1_L_offset);
    }
    if (j.contains("D2_L_offset"))
    {
        j.at("D2_L_offset").get_to(config.D2_L_offset);
    }
    if (j.contains("C4_D2_offset"))
    {
        j.at("C4_D2_offset").get_to(config.C4_D2_offset);
    }
    if (j.contains("T"))
    {
        j.at("T").get_to(config.T);
    }
    if (j.contains("f"))
    {
        j.at("f").get_to(config.f);
    }
    if (j.contains("gait_type"))
    {
        j.at("gait_type").get_to(config.gait_type);
    }
    if (j.contains("support_ratio"))
    {
        j.at("support_ratio").get_to(config.support_ratio);
    }
    if (j.contains("swing_ratio"))
    {
        j.at("swing_ratio").get_to(config.swing_ratio);
    }
    if (j.contains("thigh_motor_reduction"))
    {
        j.at("thigh_motor_reduction").get_to(config.thigh_motor_reduction);
    }
    if (j.contains("shank_motor_reduction"))
    {
        j.at("shank_motor_reduction").get_to(config.shank_motor_reduction);
    }
    if (j.contains("ankle_motor_reduction"))
    {
        j.at("ankle_motor_reduction").get_to(config.ankle_motor_reduction);
    }
    if (j.contains("adjusted_shank_angles_factory"))
    {
        j.at("adjusted_shank_angles_factory").get_to(config.adjusted_shank_angles_factory);
    }
    if (j.contains("initial_C3C4_LF"))
    {
        j.at("initial_C3C4_LF").get_to(config.initial_C3C4_LF);
    }
    if (j.contains("initial_C3C4_LH"))
    {
        j.at("initial_C3C4_LH").get_to(config.initial_C3C4_LH);
    }
    if (j.contains("initial_C3C4_RF"))
    {
        j.at("initial_C3C4_RF").get_to(config.initial_C3C4_RF);
    }
    if (j.contains("initial_C3C4_RH"))
    {
        j.at("initial_C3C4_RH").get_to(config.initial_C3C4_RH);
    }
    if (j.contains("BB2_BC_angle_rad"))
    {
        j.at("BB2_BC_angle_rad").get_to(config.BB2_BC_angle_rad);
    }
    if (j.contains("C1_B_length"))
    {
        j.at("C1_B_length").get_to(config.C1_B_length);
    }
    if (j.contains("CC4_reference_distance"))
    {
        j.at("CC4_reference_distance").get_to(config.CC4_reference_distance);
    }
    if (j.contains("deltX"))
    {
        j.at("deltX").get_to(config.deltX);
    }
    if (j.contains("step"))
    {
        j.at("step").get_to(config.step);
    }
    if (j.contains("t_s"))
    {
        j.at("t_s").get_to(config.t_s);
    }
    if (j.contains("t_w"))
    {
        j.at("t_w").get_to(config.t_w);
    }
    if (j.contains("timeLF"))
    {
        j.at("timeLF").get_to(config.timeLF);
    }
    if (j.contains("timeLH"))
    {
        j.at("timeLH").get_to(config.timeLH);
    }
    if (j.contains("timeRF"))
    {
        j.at("timeRF").get_to(config.timeRF);
    }
    if (j.contains("timeRH"))
    {
        j.at("timeRH").get_to(config.timeRH);
    }
}

// RobotGaitDataManager 实现
std::mutex RobotGaitDataManager::_lock;

RobotGaitDataManager::RobotGaitDataManager()
{
    LoadDefaultData();
}

const RobotGaitRequest &RobotGaitDataManager::GetData() const
{
    std::lock_guard<std::mutex> lock(_lock);
    return _data;
}

void RobotGaitDataManager::SetData(const RobotGaitRequest &data)
{
    std::lock_guard<std::mutex> lock(_lock);
    _data = data;
}

void RobotGaitDataManager::LoadFromJson(const nlohmann::json &j, bool useDefaultForMissing)
{
    std::lock_guard<std::mutex> lock(_lock);
    try
    {

        // std::cout << "[DEBUG] LoadFromJson  001：" << std::endl;
        // _data.req_param.debugPrint();

        // _data.req_param = RequestParameters::fromJsonString(jsonStr);

        // 更新对象（只更新GaitType、Period和t_percentage）
        RequestParameters::updateFromJson(j, _data.req_param);

        // 显示所有解析的数据
        // std::cout << "[DEBUG] 解析完成，显示详细数据：" << std::endl;
        // std::cout << "[DEBUG] LoadFromJson  002：" << std::endl;
        // _data.req_param.debugPrint();
    }
    catch (const std::exception &e)
    {
        std::cerr << "JSON解析错误: " << e.what() << std::endl;
        LoadDefaultData();
    }
}

void RobotGaitDataManager::LoadDefaultData()
{
    std::string jsonInput = RobotBaseJson;
    // std::cerr << "LoadDefaultData JSON解析: RobotBaseJson  " << jsonStr << std::endl;

    nlohmann::json j;
    j = nlohmann::json::parse(jsonInput);
    RequestParameters::updateFromJson(j, _data.req_param);
    ;

    // _data.req_param.debugPrint();
    // std::cout << "加载BaseData中的默认数据..." << std::endl;
    // _data = CreateDefaultRobotGaitRequest();
}

GaitInfo RobotGaitDataManager::GetGaitInfo() const
{
    return _data.req_param.GaitInfo;
}

SystemParameters RobotGaitDataManager::GetSystemParameters() const
{
    return _data.req_param.SystemParameters;
}

ModelID RobotGaitDataManager::GetModelIDObject() const // 新增方法
{
    return _data.req_param.ModelID;
}

LegParameters RobotGaitDataManager::GetParam() const
{
    return _data.req_param.Param;
}

RobotBody RobotGaitDataManager::GetRobotBody() const
{
    return _data.req_param.RobotBody;
}

void RobotGaitDataManager::SetRobotBody_x(const double x)
{
    _data.req_param.RobotBody.x = x;
}
std::map<std::string, LegParam> RobotGaitDataManager::GetParamDictionary() const
{
    std::map<std::string, LegParam> paramDict;
    auto param = _data.req_param.Param;

    paramDict["LF"] = param.LF;
    paramDict["LH"] = param.LH;
    paramDict["RF"] = param.RF;
    paramDict["RH"] = param.RH;

    return paramDict;
}

std::map<std::string, std::vector<double>> RobotGaitDataManager::Get_old_initial_points_dictLF() const
{
    return _data.req_param.old_initial_points_dictLF;
}

std::map<std::string, std::vector<double>> RobotGaitDataManager::Get_old_initial_points_dictLH() const
{
    return _data.req_param.old_initial_points_dictLH;
}

void RobotGaitDataManager::Set_old_initial_points_dictLF(const std::map<std::string, std::vector<double>> &L_point)
{
    _data.req_param.old_initial_points_dictLF = L_point;
}

void RobotGaitDataManager::Set_old_initial_points_dictLH(const std::map<std::string, std::vector<double>> &L_point)
{
    _data.req_param.old_initial_points_dictLH = L_point;
}

RobotGaitRequest RobotGaitDataManager::CreateDefaultRobotGaitRequest() const
{
    RobotGaitRequest request;

    // 默认GaitInfo
    request.req_param.GaitInfo = GaitInfo{
        "trot", 0.8, 50.0, 0.02, 0.4, 0.4, 0.5, 0.5, 40, 0.8};

    // 默认SystemParameters
    request.req_param.SystemParameters = SystemParameters{
        0.0, 500.0, 300.0, 286.0, 55.0, 60.0, 50.0, 30.0, 0.0, 70.0, 292.0, 80.0,
        166.0, 3.3859387489, 107.0, 179.0, 32.0, 32.0, 10.0, 90.0, 50.0, 18.0,
        1.0, 1.0, 1.0};

    // 默认ModelID - 使用新的结构
    request.req_param.ModelID = ModelID{
        {}, // LF
        {}, // LH
        {}, // RF
        {}  // RH
    };
    // 默认Param
    request.req_param.Param = LegParameters{
        LegParam{-49.1033472630546, 7.50678016014155, -183.115048990549},
        LegParam{-38.4042396745178, -9.70116099369837, 107.000051175049},
        LegParam{38.4042396745178, 9.70116099369837, 107.000051175049},
        LegParam{49.1033472630546, -7.50678016014155, -183.115048990549}};

    // 默认RobotBody
    request.req_param.RobotBody = RobotBody{
        "7ec2aade-c45a-4bf8-8bb3-d1e0db5df9f6",
        -3.24, -0.12, 560.4, 0.0, 0.0, 0.0, 1.0};

    request.req_param.t_percentage = 1.0;
    request.req_param.RobotID = "0";

    return request;
}

RobotGaitRequest RobotGaitDataManager::FillMissingDataWithDefaults(const RobotGaitRequest &data) const
{
    RobotGaitRequest result = data;
    auto defaultRequest = CreateDefaultRobotGaitRequest();

    if (result.req_param.GaitInfo.GaitType.empty())
    {
        result.req_param.GaitInfo.GaitType = defaultRequest.req_param.GaitInfo.GaitType;
    }
    if (result.req_param.GaitInfo.Period == 0.0)
    {
        result.req_param.GaitInfo.Period = defaultRequest.req_param.GaitInfo.Period;
    }
    if (result.req_param.GaitInfo.Frequency == 0.0)
    {
        result.req_param.GaitInfo.Frequency = defaultRequest.req_param.GaitInfo.Frequency;
    }

    if (result.req_param.SystemParameters.L10 == 0.0)
    {
        result.req_param.SystemParameters = defaultRequest.req_param.SystemParameters;
    }

    // if (result.req_param.ModelID.empty())
    // {
    //     result.req_param.ModelID = defaultRequest.req_param.ModelID;
    // }

    return result;
}

bool RobotGaitDataManager::IsDefaultValue(double value)
{
    return std::abs(value) < 0.000001;
}

// SimulationStatistics 实现
SimulationStatistics::SimulationStatistics(const QuadrupedRobotConfiguration &config)
{
    TotalFrames = 40;
    TotalTime = TotalFrames * config.step;

    MinABAngle = -50;
    MaxABAngle = 50;
    MinBCAngle = 30;
    MaxBCAngle = 150;
    MinAB1ABAngle = 23;
    MaxAB1ABAngle = 130;
    MinC3C4Distance = 125;
    MaxC3C4Distance = 205;
    MaxThighSpeed = 100;
    MaxShankSpeed = 100;
    MaxAnkleSpeed = 100;
}

std::string SimulationStatistics::ToString() const
{
    std::stringstream ss;
    ss << "仿真统计:\n"
       << "总帧数: " << TotalFrames << "\n"
       << "总时间: " << std::fixed << std::setprecision(3) << TotalTime << "s\n"
       << "AB角度范围: " << std::fixed << std::setprecision(1) << MinABAngle << "° ~ " << MaxABAngle << "°\n"
       << "BC角度范围: " << MinBCAngle << "° ~ " << MaxBCAngle << "°\n"
       << "θ1角度范围: " << MinAB1ABAngle << "° ~ " << MaxAB1ABAngle << "°\n"
       << "C3-C4距离范围: " << MinC3C4Distance << "mm ~ " << MaxC3C4Distance << "mm\n"
       << "最大大腿速度: " << MaxThighSpeed << "rpm\n"
       << "最大小腿速度: " << MaxShankSpeed << "rpm\n"
       << "最大脚踝速度: " << MaxAnkleSpeed << "rpm";
    return ss.str();
}

void to_json(json &j, const SimulationStatistics &stats)
{
    j = json{
        {"TotalFrames", stats.TotalFrames},
        {"TotalTime", stats.TotalTime},
        {"MinABAngle", stats.MinABAngle},
        {"MaxABAngle", stats.MaxABAngle},
        {"MinBCAngle", stats.MinBCAngle},
        {"MaxBCAngle", stats.MaxBCAngle},
        {"MinAB1ABAngle", stats.MinAB1ABAngle},
        {"MaxAB1ABAngle", stats.MaxAB1ABAngle},
        {"MinC3C4Distance", stats.MinC3C4Distance},
        {"MaxC3C4Distance", stats.MaxC3C4Distance},
        {"MaxThighSpeed", stats.MaxThighSpeed},
        {"MaxShankSpeed", stats.MaxShankSpeed},
        {"MaxAnkleSpeed", stats.MaxAnkleSpeed}};
}

void from_json(const json &j, SimulationStatistics &stats)
{
    if (j.contains("TotalFrames"))
    {
        j.at("TotalFrames").get_to(stats.TotalFrames);
    }
    if (j.contains("TotalTime"))
    {
        j.at("TotalTime").get_to(stats.TotalTime);
    }
    if (j.contains("MinABAngle"))
    {
        j.at("MinABAngle").get_to(stats.MinABAngle);
    }
    if (j.contains("MaxABAngle"))
    {
        j.at("MaxABAngle").get_to(stats.MaxABAngle);
    }
    if (j.contains("MinBCAngle"))
    {
        j.at("MinBCAngle").get_to(stats.MinBCAngle);
    }
    if (j.contains("MaxBCAngle"))
    {
        j.at("MaxBCAngle").get_to(stats.MaxBCAngle);
    }
    if (j.contains("MinAB1ABAngle"))
    {
        j.at("MinAB1ABAngle").get_to(stats.MinAB1ABAngle);
    }
    if (j.contains("MaxAB1ABAngle"))
    {
        j.at("MaxAB1ABAngle").get_to(stats.MaxAB1ABAngle);
    }
    if (j.contains("MinC3C4Distance"))
    {
        j.at("MinC3C4Distance").get_to(stats.MinC3C4Distance);
    }
    if (j.contains("MaxC3C4Distance"))
    {
        j.at("MaxC3C4Distance").get_to(stats.MaxC3C4Distance);
    }
    if (j.contains("MaxThighSpeed"))
    {
        j.at("MaxThighSpeed").get_to(stats.MaxThighSpeed);
    }
    if (j.contains("MaxShankSpeed"))
    {
        j.at("MaxShankSpeed").get_to(stats.MaxShankSpeed);
    }
    if (j.contains("MaxAnkleSpeed"))
    {
        j.at("MaxAnkleSpeed").get_to(stats.MaxAnkleSpeed);
    }
}

// SplitModelIDExtensions 实现
std::map<std::string, std::map<std::string, std::vector<std::string>>>
SplitModelIDExtensions::ToSimpleDictionary(const SplitModelID &splitModelID)
{
    std::map<std::string, std::map<std::string, std::vector<std::string>>> result;

    if (!splitModelID.LF.empty())
    {
        result["LF"] = MergeLegData(splitModelID.LF);
    }
    if (!splitModelID.LH.empty())
    {
        result["LH"] = MergeLegData(splitModelID.LH);
    }
    if (!splitModelID.RF.empty())
    {
        result["RF"] = MergeLegData(splitModelID.RF);
    }
    if (!splitModelID.RH.empty())
    {
        result["RH"] = MergeLegData(splitModelID.RH);
    }

    return result;
}

std::map<std::string, std::vector<std::string>>
SplitModelIDExtensions::MergeLegData(const std::vector<std::map<std::string, std::vector<std::string>>> &legData)
{
    std::map<std::string, std::vector<std::string>> result;

    for (const auto &dict : legData)
    {
        for (const auto &kvp : dict)
        {
            if (result.find(kvp.first) == result.end())
            {
                result[kvp.first] = kvp.second;
            }
            else
            {
                result[kvp.first].insert(result[kvp.first].end(),
                                         kvp.second.begin(),
                                         kvp.second.end());
            }
        }
    }

    return result;
}

// 实用函数
Point2D ArrayToPoint(const std::vector<double> &array)
{
    if (array.size() >= 2)
    {
        return Point2D(array[0], array[1]);
    }
    return Point2D();
}
