#include "QuadrupedRobotSimulation/RobotConfig.hpp"
// 统一的更新函数：手动从JSON更新现有对象
RequestParameters &RequestParameters::updateFromJson(const nlohmann::json &j, RequestParameters &req_param)
{
    if (j.is_null() || !j.is_object())
        return req_param; // 修正：返回传入的引用

    try
    {
        // 更新 GaitInfo
        if (j.contains("GaitInfo"))
        {
            const auto &gaitJson = j.at("GaitInfo");
            if (gaitJson.is_object())
            {
                if (gaitJson.contains("GaitType"))
                    req_param.GaitInfo.GaitType = gaitJson.at("GaitType").get<std::string>();
                if (gaitJson.contains("Period"))
                    req_param.GaitInfo.Period = gaitJson.at("Period").get<double>();
                if (gaitJson.contains("Frequency"))
                    req_param.GaitInfo.Frequency = gaitJson.at("Frequency").get<double>();
                if (gaitJson.contains("StepTime"))
                    req_param.GaitInfo.StepTime = gaitJson.at("StepTime").get<double>();
                if (gaitJson.contains("SupportTime"))
                    req_param.GaitInfo.SupportTime = gaitJson.at("SupportTime").get<double>();
                if (gaitJson.contains("SwingTime"))
                    req_param.GaitInfo.SwingTime = gaitJson.at("SwingTime").get<double>();
                if (gaitJson.contains("SupportRatio"))
                    req_param.GaitInfo.SupportRatio = gaitJson.at("SupportRatio").get<double>();
                if (gaitJson.contains("SwingRatio"))
                    req_param.GaitInfo.SwingRatio = gaitJson.at("SwingRatio").get<double>();
                if (gaitJson.contains("TotalFrames"))
                    req_param.GaitInfo.TotalFrames = gaitJson.at("TotalFrames").get<int>();
                if (gaitJson.contains("TotalTime"))
                    req_param.GaitInfo.TotalTime = gaitJson.at("TotalTime").get<double>();
            }
        }

        // 更新 SystemParameters
        if (j.contains("SystemParameters"))
        {
            const auto &sysJson = j.at("SystemParameters");
            if (sysJson.is_object())
            {
                if (sysJson.contains("A_x"))
                    req_param.SystemParameters.A_x = sysJson.at("A_x").get<double>();
                if (sysJson.contains("A_y"))
                    req_param.SystemParameters.A_y = sysJson.at("A_y").get<double>();
                if (sysJson.contains("L10"))
                    req_param.SystemParameters.L10 = sysJson.at("L10").get<double>();
                if (sysJson.contains("L20"))
                    req_param.SystemParameters.L20 = sysJson.at("L20").get<double>();
                if (sysJson.contains("L30"))
                    req_param.SystemParameters.L30 = sysJson.at("L30").get<double>();
                if (sysJson.contains("StepLength"))
                    req_param.SystemParameters.StepLength = sysJson.at("StepLength").get<double>();
                if (sysJson.contains("StepHeight"))
                    req_param.SystemParameters.StepHeight = sysJson.at("StepHeight").get<double>();
                if (sysJson.contains("X"))
                    req_param.SystemParameters.X = sysJson.at("X").get<double>();
                if (sysJson.contains("DeltaX"))
                    req_param.SystemParameters.DeltaX = sysJson.at("DeltaX").get<double>();
                if (sysJson.contains("L21"))
                    req_param.SystemParameters.L21 = sysJson.at("L21").get<double>();
                if (sysJson.contains("L22"))
                    req_param.SystemParameters.L22 = sysJson.at("L22").get<double>();
                if (sysJson.contains("L23"))
                    req_param.SystemParameters.L23 = sysJson.at("L23").get<double>();
                if (sysJson.contains("Beta1"))
                    req_param.SystemParameters.Beta1 = sysJson.at("Beta1").get<double>();
                if (sysJson.contains("BB2_BC_Angle"))
                    req_param.SystemParameters.BB2_BC_Angle = sysJson.at("BB2_BC_Angle").get<double>();
                if (sysJson.contains("C1_B_Length"))
                    req_param.SystemParameters.C1_B_Length = sysJson.at("C1_B_Length").get<double>();
                if (sysJson.contains("CC1"))
                    req_param.SystemParameters.CC1 = sysJson.at("CC1").get<double>();
                if (sysJson.contains("L31"))
                    req_param.SystemParameters.L31 = sysJson.at("L31").get<double>();
                if (sysJson.contains("C2C3_Length"))
                    req_param.SystemParameters.C2C3_Length = sysJson.at("C2C3_Length").get<double>();
                if (sysJson.contains("Lead"))
                    req_param.SystemParameters.Lead = sysJson.at("Lead").get<double>();
                if (sysJson.contains("D1_L_Offset"))
                    req_param.SystemParameters.D1_L_Offset = sysJson.at("D1_L_Offset").get<double>();
                if (sysJson.contains("D2_L_Offset"))
                    req_param.SystemParameters.D2_L_Offset = sysJson.at("D2_L_Offset").get<double>();
                if (sysJson.contains("C4_D2_Offset"))
                    req_param.SystemParameters.C4_D2_Offset = sysJson.at("C4_D2_Offset").get<double>();
                if (sysJson.contains("ThighMotorReduction"))
                    req_param.SystemParameters.ThighMotorReduction = sysJson.at("ThighMotorReduction").get<double>();
                if (sysJson.contains("ShankMotorReduction"))
                    req_param.SystemParameters.ShankMotorReduction = sysJson.at("ShankMotorReduction").get<double>();
                if (sysJson.contains("AnkleMotorReduction"))
                    req_param.SystemParameters.AnkleMotorReduction = sysJson.at("AnkleMotorReduction").get<double>();
            }
        }

        // 更新 ModelID
        if (j.contains("ModelID"))
        {
            const auto &modelJson = j.at("ModelID");
            if (modelJson.is_object())
            {
                // 更新 LF
                if (modelJson.contains("LF"))
                {
                    const auto &lfJson = modelJson.at("LF");
                    if (lfJson.is_array())
                    {
                        req_param.ModelID.LF.components.clear();
                        for (const auto &item : lfJson)
                        {
                            if (item.is_object())
                            {
                                ComponentID comp;
                                auto it = item.begin();
                                if (it != item.end())
                                {
                                    comp.name = it.key();
                                    comp.uuid = it.value().get<std::string>();
                                    req_param.ModelID.LF.components.push_back(comp);
                                }
                            }
                        }
                    }
                }

                // 更新 LH
                if (modelJson.contains("LH"))
                {
                    const auto &lhJson = modelJson.at("LH");
                    if (lhJson.is_array())
                    {
                        req_param.ModelID.LH.components.clear();
                        for (const auto &item : lhJson)
                        {
                            if (item.is_object())
                            {
                                ComponentID comp;
                                auto it = item.begin();
                                if (it != item.end())
                                {
                                    comp.name = it.key();
                                    comp.uuid = it.value().get<std::string>();
                                    req_param.ModelID.LH.components.push_back(comp);
                                }
                            }
                        }
                    }
                }

                // 更新 RF
                if (modelJson.contains("RF"))
                {
                    const auto &rfJson = modelJson.at("RF");
                    if (rfJson.is_array())
                    {
                        req_param.ModelID.RF.components.clear();
                        for (const auto &item : rfJson)
                        {
                            if (item.is_object())
                            {
                                ComponentID comp;
                                auto it = item.begin();
                                if (it != item.end())
                                {
                                    comp.name = it.key();
                                    comp.uuid = it.value().get<std::string>();
                                    req_param.ModelID.RF.components.push_back(comp);
                                }
                            }
                        }
                    }
                }

                // 更新 RH
                if (modelJson.contains("RH"))
                {
                    const auto &rhJson = modelJson.at("RH");
                    if (rhJson.is_array())
                    {
                        req_param.ModelID.RH.components.clear();
                        for (const auto &item : rhJson)
                        {
                            if (item.is_object())
                            {
                                ComponentID comp;
                                auto it = item.begin();
                                if (it != item.end())
                                {
                                    comp.name = it.key();
                                    comp.uuid = it.value().get<std::string>();
                                    req_param.ModelID.RH.components.push_back(comp);
                                }
                            }
                        }
                    }
                }
            }
        }

        // 更新 Param (LegParameters)
        if (j.contains("Param"))
        {
            const auto &paramJson = j.at("Param");
            if (paramJson.is_object())
            {
                // 更新 LF
                if (paramJson.contains("LF"))
                {
                    const auto &lfJson = paramJson.at("LF");
                    if (lfJson.is_object())
                    {
                        if (lfJson.contains("thigh_angle_deg"))
                            req_param.Param.LF.thigh_angle_deg = lfJson.at("thigh_angle_deg").get<double>();
                        if (lfJson.contains("shank_angle_deg"))
                            req_param.Param.LF.shank_angle_deg = lfJson.at("shank_angle_deg").get<double>();
                        if (lfJson.contains("ankle_angle_deg"))
                            req_param.Param.LF.ankle_angle_deg = lfJson.at("ankle_angle_deg").get<double>();
                    }
                }

                // 更新 LH
                if (paramJson.contains("LH"))
                {
                    const auto &lhJson = paramJson.at("LH");
                    if (lhJson.is_object())
                    {
                        if (lhJson.contains("thigh_angle_deg"))
                            req_param.Param.LH.thigh_angle_deg = lhJson.at("thigh_angle_deg").get<double>();
                        if (lhJson.contains("shank_angle_deg"))
                            req_param.Param.LH.shank_angle_deg = lhJson.at("shank_angle_deg").get<double>();
                        if (lhJson.contains("ankle_angle_deg"))
                            req_param.Param.LH.ankle_angle_deg = lhJson.at("ankle_angle_deg").get<double>();
                    }
                }

                // 更新 RF
                if (paramJson.contains("RF"))
                {
                    const auto &rfJson = paramJson.at("RF");
                    if (rfJson.is_object())
                    {
                        if (rfJson.contains("thigh_angle_deg"))
                            req_param.Param.RF.thigh_angle_deg = rfJson.at("thigh_angle_deg").get<double>();
                        if (rfJson.contains("shank_angle_deg"))
                            req_param.Param.RF.shank_angle_deg = rfJson.at("shank_angle_deg").get<double>();
                        if (rfJson.contains("ankle_angle_deg"))
                            req_param.Param.RF.ankle_angle_deg = rfJson.at("ankle_angle_deg").get<double>();
                    }
                }

                // 更新 RH
                if (paramJson.contains("RH"))
                {
                    const auto &rhJson = paramJson.at("RH");
                    if (rhJson.is_object())
                    {
                        if (rhJson.contains("thigh_angle_deg"))
                            req_param.Param.RH.thigh_angle_deg = rhJson.at("thigh_angle_deg").get<double>();
                        if (rhJson.contains("shank_angle_deg"))
                            req_param.Param.RH.shank_angle_deg = rhJson.at("shank_angle_deg").get<double>();
                        if (rhJson.contains("ankle_angle_deg"))
                            req_param.Param.RH.ankle_angle_deg = rhJson.at("ankle_angle_deg").get<double>();
                    }
                }
            }
        }

        // 更新 RobotBody
        if (j.contains("RobotBody"))
        {
            const auto &bodyJson = j.at("RobotBody");
            if (bodyJson.is_object())
            {
                if (bodyJson.contains("BodyCode"))
                    req_param.RobotBody.BodyCode = bodyJson.at("BodyCode").get<std::string>();
                if (bodyJson.contains("x"))
                    req_param.RobotBody.x = bodyJson.at("x").get<double>();
                if (bodyJson.contains("y"))
                    req_param.RobotBody.y = bodyJson.at("y").get<double>();
                if (bodyJson.contains("z"))
                    req_param.RobotBody.z = bodyJson.at("z").get<double>();
                if (bodyJson.contains("qx"))
                    req_param.RobotBody.qx = bodyJson.at("qx").get<double>();
                if (bodyJson.contains("qy"))
                    req_param.RobotBody.qy = bodyJson.at("qy").get<double>();
                if (bodyJson.contains("qz"))
                    req_param.RobotBody.qz = bodyJson.at("qz").get<double>();
                if (bodyJson.contains("qw"))
                    req_param.RobotBody.qw = bodyJson.at("qw").get<double>();
            }
        }

        // 更新 t_percentage
        if (j.contains("t_percentage"))
        {
            req_param.t_percentage = j.at("t_percentage").get<double>();
        }
        if (j.contains("RobotID"))
        {
            req_param.RobotID = j.at("RobotID").get<std::string>();
        }

        return req_param; // 返回引用
    }
    catch (const nlohmann::json::exception &e)
    {
        std::cerr << "[ERROR] 更新RequestParameters失败: " << e.what() << std::endl;
        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
        throw;
    }
}
// GaitInfo 序列化/反序列化实现
void to_json(nlohmann::json &j, const GaitInfo &g)
{
    j = nlohmann::json{
        {"GaitType", g.GaitType},
        {"Period", g.Period},
        {"Frequency", g.Frequency},
        {"StepTime", g.StepTime},
        {"SupportTime", g.SupportTime},
        {"SwingTime", g.SwingTime},
        {"SupportRatio", g.SupportRatio},
        {"SwingRatio", g.SwingRatio},
        {"TotalFrames", g.TotalFrames},
        {"TotalTime", g.TotalTime}};
}

void from_json(const nlohmann::json &j, GaitInfo &g)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("GaitType"))
        j.at("GaitType").get_to(g.GaitType);
    if (j.contains("Period"))
        j.at("Period").get_to(g.Period);
    if (j.contains("Frequency"))
        j.at("Frequency").get_to(g.Frequency);
    if (j.contains("StepTime"))
        j.at("StepTime").get_to(g.StepTime);
    if (j.contains("SupportTime"))
        j.at("SupportTime").get_to(g.SupportTime);
    if (j.contains("SwingTime"))
        j.at("SwingTime").get_to(g.SwingTime);
    if (j.contains("SupportRatio"))
        j.at("SupportRatio").get_to(g.SupportRatio);
    if (j.contains("SwingRatio"))
        j.at("SwingRatio").get_to(g.SwingRatio);
    if (j.contains("TotalFrames"))
        j.at("TotalFrames").get_to(g.TotalFrames);
    if (j.contains("TotalTime"))
        j.at("TotalTime").get_to(g.TotalTime);
}

// SystemParameters 序列化/反序列化实现
void to_json(nlohmann::json &j, const SystemParameters &s)
{
    j = nlohmann::json{
        {"A_x", s.A_x},
        {"A_y", s.A_y},
        {"L10", s.L10},
        {"L20", s.L20},
        {"L30", s.L30},
        {"StepLength", s.StepLength},
        {"StepHeight", s.StepHeight},
        {"X", s.X},
        {"DeltaX", s.DeltaX},
        {"L21", s.L21},
        {"L22", s.L22},
        {"L23", s.L23},
        {"Beta1", s.Beta1},
        {"BB2_BC_Angle", s.BB2_BC_Angle},
        {"C1_B_Length", s.C1_B_Length},
        {"CC1", s.CC1},
        {"L31", s.L31},
        {"C2C3_Length", s.C2C3_Length},
        {"Lead", s.Lead},
        {"D1_L_Offset", s.D1_L_Offset},
        {"D2_L_Offset", s.D2_L_Offset},
        {"C4_D2_Offset", s.C4_D2_Offset},
        {"ThighMotorReduction", s.ThighMotorReduction},
        {"ShankMotorReduction", s.ShankMotorReduction},
        {"AnkleMotorReduction", s.AnkleMotorReduction}};
}

void from_json(const nlohmann::json &j, SystemParameters &s)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("A_x"))
        j.at("A_x").get_to(s.A_x);
    if (j.contains("A_y"))
        j.at("A_y").get_to(s.A_y);
    if (j.contains("L10"))
        j.at("L10").get_to(s.L10);
    if (j.contains("L20"))
        j.at("L20").get_to(s.L20);
    if (j.contains("L30"))
        j.at("L30").get_to(s.L30);
    if (j.contains("StepLength"))
        j.at("StepLength").get_to(s.StepLength);
    if (j.contains("StepHeight"))
        j.at("StepHeight").get_to(s.StepHeight);
    if (j.contains("X"))
        j.at("X").get_to(s.X);
    if (j.contains("DeltaX"))
        j.at("DeltaX").get_to(s.DeltaX);
    if (j.contains("L21"))
        j.at("L21").get_to(s.L21);
    if (j.contains("L22"))
        j.at("L22").get_to(s.L22);
    if (j.contains("L23"))
        j.at("L23").get_to(s.L23);
    if (j.contains("Beta1"))
        j.at("Beta1").get_to(s.Beta1);
    if (j.contains("BB2_BC_Angle"))
        j.at("BB2_BC_Angle").get_to(s.BB2_BC_Angle);
    if (j.contains("C1_B_Length"))
        j.at("C1_B_Length").get_to(s.C1_B_Length);
    if (j.contains("CC1"))
        j.at("CC1").get_to(s.CC1);
    if (j.contains("L31"))
        j.at("L31").get_to(s.L31);
    if (j.contains("C2C3_Length"))
        j.at("C2C3_Length").get_to(s.C2C3_Length);
    if (j.contains("Lead"))
        j.at("Lead").get_to(s.Lead);
    if (j.contains("D1_L_Offset"))
        j.at("D1_L_Offset").get_to(s.D1_L_Offset);
    if (j.contains("D2_L_Offset"))
        j.at("D2_L_Offset").get_to(s.D2_L_Offset);
    if (j.contains("C4_D2_Offset"))
        j.at("C4_D2_Offset").get_to(s.C4_D2_Offset);
    if (j.contains("ThighMotorReduction"))
        j.at("ThighMotorReduction").get_to(s.ThighMotorReduction);
    if (j.contains("ShankMotorReduction"))
        j.at("ShankMotorReduction").get_to(s.ShankMotorReduction);
    if (j.contains("AnkleMotorReduction"))
        j.at("AnkleMotorReduction").get_to(s.AnkleMotorReduction);
}

// ComponentID 序列化/反序列化实现
void to_json(nlohmann::json &j, const ComponentID &c)
{
    j = nlohmann::json{{c.name, c.uuid}};
}

void from_json(const nlohmann::json &j, ComponentID &c)
{
    if (j.is_null() || !j.is_object())
        return;

    auto it = j.begin();
    if (it != j.end())
    {
        c.name = it.key();
        c.uuid = it.value();
    }
}

// LegModelIDs 序列化/反序列化实现
void to_json(nlohmann::json &j, const LegModelIDs &l)
{

    j = nlohmann::json::array();
    for (const auto &component : l.components)
    {
        j.push_back(component);
    }
}

void from_json(const nlohmann::json &j, LegModelIDs &l)
{
    l.components.clear();
    if (j.is_null() || !j.is_array())
        return;

    for (const auto &item : j)
    {
        ComponentID comp;
        from_json(item, comp);
        l.components.push_back(comp);
    }
}

// ModelID 序列化/反序列化实现
void to_json(nlohmann::json &j, const ModelID &m)
{
    j = nlohmann::json{
        {"LF", m.LF},
        {"LH", m.LH},
        {"RF", m.RF},
        {"RH", m.RH}};
}

void from_json(const nlohmann::json &j, ModelID &m)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("LF"))
        j.at("LF").get_to(m.LF);
    if (j.contains("LH"))
        j.at("LH").get_to(m.LH);
    if (j.contains("RF"))
        j.at("RF").get_to(m.RF);
    if (j.contains("RH"))
        j.at("RH").get_to(m.RH);
}

// LegParam 序列化/反序列化实现
void to_json(nlohmann::json &j, const LegParam &l)
{
    j = nlohmann::json{
        {"thigh_angle_deg", l.thigh_angle_deg},
        {"shank_angle_deg", l.shank_angle_deg},
        {"ankle_angle_deg", l.ankle_angle_deg}};
}

void from_json(const nlohmann::json &j, LegParam &l)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("thigh_angle_deg"))
        j.at("thigh_angle_deg").get_to(l.thigh_angle_deg);
    if (j.contains("shank_angle_deg"))
        j.at("shank_angle_deg").get_to(l.shank_angle_deg);
    if (j.contains("ankle_angle_deg"))
        j.at("ankle_angle_deg").get_to(l.ankle_angle_deg);
}

// LegParameters 序列化/反序列化实现
void to_json(nlohmann::json &j, const LegParameters &l)
{
    j = nlohmann::json{
        {"LF", l.LF},
        {"LH", l.LH},
        {"RF", l.RF},
        {"RH", l.RH}};
}

void from_json(const nlohmann::json &j, LegParameters &l)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("LF"))
        j.at("LF").get_to(l.LF);
    if (j.contains("LH"))
        j.at("LH").get_to(l.LH);
    if (j.contains("RF"))
        j.at("RF").get_to(l.RF);
    if (j.contains("RH"))
        j.at("RH").get_to(l.RH);
}

// RobotBody 序列化/反序列化实现
void to_json(nlohmann::json &j, const RobotBody &r)
{
    j = nlohmann::json{
        {"BodyCode", r.BodyCode},
        {"x", r.x},
        {"y", r.y},
        {"z", r.z},
        {"qx", r.qx},
        {"qy", r.qy},
        {"qz", r.qz},
        {"qw", r.qw}};
}

void from_json(const nlohmann::json &j, RobotBody &r)
{
    if (j.is_null() || !j.is_object())
        return;

    if (j.contains("BodyCode"))
        j.at("BodyCode").get_to(r.BodyCode);
    if (j.contains("x"))
        j.at("x").get_to(r.x);
    if (j.contains("y"))
        j.at("y").get_to(r.y);
    if (j.contains("z"))
        j.at("z").get_to(r.z);
    if (j.contains("qx"))
        j.at("qx").get_to(r.qx);
    if (j.contains("qy"))
        j.at("qy").get_to(r.qy);
    if (j.contains("qz"))
        j.at("qz").get_to(r.qz);
    if (j.contains("qw"))
        j.at("qw").get_to(r.qw);
}

// RequestParameters 序列化/反序列化实现
void to_json(nlohmann::json &j, const RequestParameters &c)
{
    j = nlohmann::json{
        {"GaitInfo", c.GaitInfo},
        {"SystemParameters", c.SystemParameters},
        {"ModelID", c.ModelID},
        {"Param", c.Param},
        {"RobotBody", c.RobotBody},
        {"RobotID", c.RobotID},
        {"t_percentage", c.t_percentage}};
}

void from_json(const nlohmann::json &j, RequestParameters &c)
{
    if (j.is_null() || !j.is_object())
        return;

    try
    {
        if (j.contains("GaitInfo"))
            j.at("GaitInfo").get_to(c.GaitInfo);
        if (j.contains("SystemParameters"))
            j.at("SystemParameters").get_to(c.SystemParameters);
        if (j.contains("ModelID"))
            j.at("ModelID").get_to(c.ModelID);
        if (j.contains("Param"))
            j.at("Param").get_to(c.Param);
        if (j.contains("RobotBody"))
            j.at("RobotBody").get_to(c.RobotBody);
        if (j.contains("t_percentage"))
            j.at("t_percentage").get_to(c.t_percentage);
        if (j.contains("RobotID"))
            j.at("RobotID").get_to(c.RobotID);
    }
    catch (const nlohmann::json::exception &e)
    {
        std::cerr << "[ERROR] 解析RequestParameters失败: " << e.what() << std::endl;
        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
        throw;
    }
}
void RequestParameters::debugPrint() const
{
    std::cout << "========================================" << std::endl;
    std::cout << "RequestParameters DEBUG PRINT" << std::endl;
    std::cout << "========================================" << std::endl;

    // GaitInfo
    std::cout << "=== GaitInfo ===" << std::endl;
    std::cout << "  GaitType:     " << GaitInfo.GaitType << std::endl;
    std::cout << "  Period:       " << GaitInfo.Period << std::endl;
    std::cout << "  Frequency:    " << GaitInfo.Frequency << std::endl;
    std::cout << "  StepTime:     " << GaitInfo.StepTime << std::endl;
    std::cout << "  SupportTime:  " << GaitInfo.SupportTime << std::endl;
    std::cout << "  SwingTime:    " << GaitInfo.SwingTime << std::endl;
    std::cout << "  SupportRatio: " << GaitInfo.SupportRatio << std::endl;
    std::cout << "  SwingRatio:   " << GaitInfo.SwingRatio << std::endl;
    std::cout << "  TotalFrames:  " << GaitInfo.TotalFrames << std::endl;
    std::cout << "  TotalTime:    " << GaitInfo.TotalTime << std::endl;

    // SystemParameters (关键字段)
    std::cout << "\n=== SystemParameters (key fields) ===" << std::endl;
    std::cout << "  A_x: " << SystemParameters.A_x << std::endl;
    std::cout << "  A_y: " << SystemParameters.A_y << std::endl;
    std::cout << "  L10: " << SystemParameters.L10 << std::endl;
    std::cout << "  L20: " << SystemParameters.L20 << std::endl;
    std::cout << "  L30: " << SystemParameters.L30 << std::endl;
    std::cout << "  StepLength: " << SystemParameters.StepLength << std::endl;
    std::cout << "  StepHeight: " << SystemParameters.StepHeight << std::endl;
    std::cout << "  X: " << SystemParameters.X << std::endl;
    std::cout << "  DeltaX: " << SystemParameters.DeltaX << std::endl;
    std::cout << "  L21: " << SystemParameters.L21 << std::endl;
    std::cout << "  L22: " << SystemParameters.L22 << std::endl;
    std::cout << "  L23: " << SystemParameters.L23 << std::endl;
    std::cout << "  Beta1: " << SystemParameters.Beta1 << std::endl;
    std::cout << "  BB2_BC_Angle: " << SystemParameters.BB2_BC_Angle << std::endl;
    std::cout << "  C1_B_Length: " << SystemParameters.C1_B_Length << std::endl;
    std::cout << "  CC1: " << SystemParameters.CC1 << std::endl;
    std::cout << "  L31: " << SystemParameters.L31 << std::endl;
    std::cout << "  C2C3_Length: " << SystemParameters.C2C3_Length << std::endl;
    std::cout << "  Lead: " << SystemParameters.Lead << std::endl;
    std::cout << "  D1_L_Offset: " << SystemParameters.D1_L_Offset << std::endl;
    std::cout << "  D2_L_Offset: " << SystemParameters.D2_L_Offset << std::endl;
    std::cout << "  C4_D2_Offset: " << SystemParameters.C4_D2_Offset << std::endl;
    std::cout << "  ThighMotorReduction: " << SystemParameters.ThighMotorReduction << std::endl;
    std::cout << "  ShankMotorReduction: " << SystemParameters.ShankMotorReduction << std::endl;
    std::cout << "  AnkleMotorReduction: " << SystemParameters.AnkleMotorReduction << std::endl;

    // ModelID详细内容
    std::cout << "\n=== ModelID Details ===" << std::endl;

    // LF
    std::cout << "  LF [" << ModelID.LF.components.size() << " components]:" << std::endl;
    for (size_t i = 0; i < ModelID.LF.components.size(); ++i)
    {
        std::cout << "    [" << i << "] " << ModelID.LF.components[i].name
                  << ": " << ModelID.LF.components[i].uuid << std::endl;
    }

    // LH
    std::cout << "  LH [" << ModelID.LH.components.size() << " components]:" << std::endl;
    for (size_t i = 0; i < ModelID.LH.components.size(); ++i)
    {
        std::cout << "    [" << i << "] " << ModelID.LH.components[i].name
                  << ": " << ModelID.LH.components[i].uuid << std::endl;
    }

    // RF
    std::cout << "  RF [" << ModelID.RF.components.size() << " components]:" << std::endl;
    for (size_t i = 0; i < ModelID.RF.components.size(); ++i)
    {
        std::cout << "    [" << i << "] " << ModelID.RF.components[i].name
                  << ": " << ModelID.RF.components[i].uuid << std::endl;
    }

    // RH
    std::cout << "  RH [" << ModelID.RH.components.size() << " components]:" << std::endl;
    for (size_t i = 0; i < ModelID.RH.components.size(); ++i)
    {
        std::cout << "    [" << i << "] " << ModelID.RH.components[i].name
                  << ": " << ModelID.RH.components[i].uuid << std::endl;
    }

    // LegParameters
    std::cout << "\n=== Param (LegParameters) ===" << std::endl;

    std::cout << "  LF:" << std::endl;
    std::cout << "    thigh_angle_deg: " << Param.LF.thigh_angle_deg << std::endl;
    std::cout << "    shank_angle_deg: " << Param.LF.shank_angle_deg << std::endl;
    std::cout << "    ankle_angle_deg: " << Param.LF.ankle_angle_deg << std::endl;

    std::cout << "  LH:" << std::endl;
    std::cout << "    thigh_angle_deg: " << Param.LH.thigh_angle_deg << std::endl;
    std::cout << "    shank_angle_deg: " << Param.LH.shank_angle_deg << std::endl;
    std::cout << "    ankle_angle_deg: " << Param.LH.ankle_angle_deg << std::endl;

    std::cout << "  RF:" << std::endl;
    std::cout << "    thigh_angle_deg: " << Param.RF.thigh_angle_deg << std::endl;
    std::cout << "    shank_angle_deg: " << Param.RF.shank_angle_deg << std::endl;
    std::cout << "    ankle_angle_deg: " << Param.RF.ankle_angle_deg << std::endl;

    std::cout << "  RH:" << std::endl;
    std::cout << "    thigh_angle_deg: " << Param.RH.thigh_angle_deg << std::endl;
    std::cout << "    shank_angle_deg: " << Param.RH.shank_angle_deg << std::endl;
    std::cout << "    ankle_angle_deg: " << Param.RH.ankle_angle_deg << std::endl;

    // RobotBody
    std::cout << "\n=== RobotBody ===" << std::endl;
    std::cout << "  BodyCode: " << RobotBody.BodyCode << std::endl;
    std::cout << "  x: " << RobotBody.x << std::endl;
    std::cout << "  y: " << RobotBody.y << std::endl;
    std::cout << "  z: " << RobotBody.z << std::endl;
    std::cout << "  qx: " << RobotBody.qx << std::endl;
    std::cout << "  qy: " << RobotBody.qy << std::endl;
    std::cout << "  qz: " << RobotBody.qz << std::endl;
    std::cout << "  qw: " << RobotBody.qw << std::endl;

    // Other parameters
    std::cout << "\n=== Other Parameters  ===" << std::endl;
    std::cout << "  t_percentage: " << t_percentage << std::endl;
    std::cout << "  RobotID: " << RobotID << std::endl;

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

// RequestParameters 成员函数实现
RequestParameters RequestParameters::fromJsonString(const std::string &jsonStr)
{
    try
    {
        auto j = nlohmann::json::parse(jsonStr);

        // 如果不包含req_param，尝试直接解析
        if (j.contains("GaitInfo") || j.contains("SystemParameters") ||
            j.contains("ModelID") || j.contains("Param") || j.contains("RobotBody") || j.contains("t_percentage") || j.contains("RobotID"))
        {
            // 看起来是直接RequestParameters格式
            return j.get<RequestParameters>();
        }
        else
        {

            if (j.contains("req_param") && !j["req_param"].is_null())
            {
                // req_param是一个嵌套的JSON对象，直接获取它
                auto req_param_json = j["req_param"];
                if (req_param_json.contains("GaitInfo") || req_param_json.contains("SystemParameters") ||
                    req_param_json.contains("ModelID") || req_param_json.contains("Param") || req_param_json.contains("RobotBody") || req_param_json.contains("t_percentage") || req_param_json.contains("RobotID"))
                {
                    // 看起来是直接RequestParameters格式
                    return req_param_json.get<RequestParameters>();
                }
            }
        }

        // 返回默认对象
        return RequestParameters();
    }
    catch (const nlohmann::json::exception &e)
    {
        std::cerr << "[ERROR] JSON解析失败: " << e.what() << std::endl;
        return RequestParameters(); // 返回默认对象
    }
    catch (...)
    {
        std::cerr << "[ERROR] 未知错误解析JSON" << std::endl;
        return RequestParameters(); // 返回默认对象
    }
}

RequestParameters RequestParameters::fromJsonFile(const std::string &filename)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        throw std::runtime_error("Cannot open file: " + filename);
    }

    std::stringstream buffer;
    buffer << file.rdbuf();
    return fromJsonString(buffer.str());
}

std::string RequestParameters::toJsonString() const
{
    nlohmann::json j = *this;
    return j.dump(2); // 缩进2个空格
}

bool RequestParameters::saveToFile(const std::string &filename) const
{
    try
    {
        std::ofstream file(filename);
        if (!file.is_open())
        {
            return false;
        }
        file << toJsonString();
        return true;
    }
    catch (...)
    {
        return false;
    }
}