// KinematicsHelper.cpp
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
#include <iostream>
#include <sstream>
#include <iomanip>

// 声明网络发送函数（需要根据实际网络库实现）
extern void SendMsg(const std::string &msg);
static std::unordered_map<std::string, std::shared_ptr<RobotGaitDataManager>> instanceMap;

void KinematicsHelper::SimRobot()
{
    double thigh_angle_deg = 0;
    double shank_angle_deg = 0;
    double ankle_angle_deg = 0;
    std::string leg_type = "LF";

    auto manager = KinematicsHelper::RobotGaitDataManagerFromJson();
    auto simulation = std::make_shared<ReverseKinematicsCalculator>(manager);

    nlohmann::json jsonData = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();

    try
    {
        // 使用不同的变量名避免冲突
        CompleteExportData exportData = jsonData.get<CompleteExportData>();

        int pointCount = exportData.MotorData.Legs.at("LH").Frames.size();

        for (int i = 0; i < pointCount; i++)
        {
            // 左前腿
            leg_type = "LF";
            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;

            std::string c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);

            // 发送消息（需要实现SendMsg函数）
            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);

            // 左后腿
            leg_type = "LH";
            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;

            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);

            // 右前腿
            leg_type = "RF";
            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;

            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);

            // 右后腿
            leg_type = "RH";
            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;

            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);

            // 等待200毫秒
            Wait(200);
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error in SimRobot: " << e.what() << std::endl;
    }
}

json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput)
{
    std::string jsonStr = "{}";
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    ReverseKinematicsCalculator simulation(manager);
    jsonStr = simulation.CalculateAllPointsFromMotorAnglesJsonStr();
    json jsonObj = json::parse(jsonStr);
    return jsonObj;
}
json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput)
{
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    QuadrupedRobotConfiguration config(manager->GetGaitInfo(), manager->GetSystemParameters());
    QuadrupedRobotSimulation simulation(config);
    std::string jsonStr = simulation.CalculateAllTrajectoriesJsonString();
    json jsonObj = json::parse(jsonStr);
    return jsonObj;
}
std::string KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
    double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
    const std::string &leg_type, const std::string &jsonInput)
{
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    ReverseKinematicsCalculator simulation(manager);
    std::string c_Frames_jsonStr = simulation.CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
        thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
    return c_Frames_jsonStr;
}

std::shared_ptr<RobotGaitDataManager> KinematicsHelper::RobotGaitDataManagerFromJson(const std::string &jsonInput, const std::string &robotID)
{

    std::string RobotID;
    RobotID = robotID;
    // 尝试解析JSON
    nlohmann::json j;
    j = nlohmann::json::parse(jsonInput);

        if (j.contains("RobotID"))
    {
        RobotID = j.at("RobotID").get<std::string>();
    }
    auto it = instanceMap.find(RobotID);
    if (it != instanceMap.end())
    {
        // 已存在：重新加载默认数据
        auto manager = it->second;
        if (jsonInput.empty())
        {
            manager->LoadDefaultData();
        }
        else
        {
            manager->LoadFromJson(j);
        }
        return manager;
    }
    else
    {

        // 不存在：创建新的并加载默认数据
        auto manager = std::make_shared<RobotGaitDataManager>();

        manager->LoadDefaultData();

        manager->LoadFromJson(jsonInput);

        instanceMap[robotID] = manager;
        return manager;
    }
    auto manager = std::make_shared<RobotGaitDataManager>();

    return manager;
}

QuadrupedRobotConfiguration KinematicsHelper::CreateConfigFromJson(const std::string &jsonInput)
{
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    return QuadrupedRobotConfiguration(manager->GetGaitInfo(), manager->GetSystemParameters());
}

std::string KinematicsHelper::PerformForwardKinematics(const QuadrupedRobotConfiguration &config)
{
    QuadrupedRobotSimulation simulation(config);
    std::string jsonStr = simulation.CalculateAllTrajectories_JsonStr();
    return jsonStr;
}

std::string KinematicsHelper::ExportCompleteData(const QuadrupedRobotConfiguration &config)
{
    QuadrupedRobotSimulation simulation(config);
    return CompleteJsonExporter::ExportCompleteDataJsonString(simulation);
}

std::vector<ReverseCalculationResult> KinematicsHelper::BatchReverseCalculation(
    const std::map<std::string, std::vector<double>> &motor_data,
    const std::string &leg_type,
    std::optional<int> max_frames,
    const std::string &jsonInput)
{
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    ReverseKinematicsCalculator calculator(manager);
    return calculator.BatchReverseCalculation(motor_data, leg_type, max_frames);
}

SupportCheckResult KinematicsHelper::CheckSupportPoint(
    const std::map<std::string, std::vector<double>> &pointsDict,
    double groundHeight,
    double tolerance,
    const std::string &jsonInput)
{
    auto manager = RobotGaitDataManagerFromJson(jsonInput);
    ReverseKinematicsCalculator calculator(manager);

    // 注意：原C#代码中IsSupportPoint返回bool，但BaseClass.h中有SupportCheckResult结构
    // 这里需要根据实际实现调整
    bool isSupport = calculator.IsSupportPoint(pointsDict, groundHeight, tolerance);

    SupportCheckResult result;
    result.IsSupport = isSupport;
    // 这里可以添加更多的检查结果填充逻辑

    return result;
}

P_OPERATION KinematicsHelper::MakeOperation(
    int frameRate,
    const std::string &modelCode,
    const std::vector<std::vector<double>> &positions,
    const std::vector<std::vector<double>> &quaternions)
{
    return P_OPERATION_Func::MakeOperation(frameRate, modelCode, positions, quaternions);
}

RobotGaitRequest KinematicsHelper::CreateDefaultRequest()
{
    RobotGaitRequest request;

    // 设置默认步态信息
    request.req_param.GaitInfo.GaitType = "walk";
    request.req_param.GaitInfo.Period = 2.0;
    request.req_param.GaitInfo.Frequency = 50.0;
    request.req_param.GaitInfo.StepTime = 0.02;
    request.req_param.GaitInfo.SupportTime = 1.0;
    request.req_param.GaitInfo.SwingTime = 1.0;
    request.req_param.GaitInfo.SupportRatio = 0.5;
    request.req_param.GaitInfo.SwingRatio = 0.5;
    request.req_param.GaitInfo.TotalFrames = 100;
    request.req_param.GaitInfo.TotalTime = 2.0;

    // 设置默认系统参数
    request.req_param.SystemParameters.A_x = 0.0;
    request.req_param.SystemParameters.A_y = 0.0;
    request.req_param.SystemParameters.L10 = 100.0;
    request.req_param.SystemParameters.L20 = 100.0;
    request.req_param.SystemParameters.L30 = 50.0;
    request.req_param.SystemParameters.StepLength = 200.0;
    request.req_param.SystemParameters.StepHeight = 50.0;
    request.req_param.SystemParameters.X = 50.0;
    request.req_param.SystemParameters.DeltaX = 0.0;
    request.req_param.SystemParameters.L21 = 80.0;
    request.req_param.SystemParameters.L22 = 60.0;
    request.req_param.SystemParameters.L23 = 70.0;
    request.req_param.SystemParameters.Beta1 = 30.0;
    request.req_param.SystemParameters.BB2_BC_Angle = 30.0;
    request.req_param.SystemParameters.C1_B_Length = 50.0;
    request.req_param.SystemParameters.CC1 = 40.0;
    request.req_param.SystemParameters.L31 = 60.0;
    request.req_param.SystemParameters.C2C3_Length = 50.0;
    request.req_param.SystemParameters.Lead = 5.0;
    request.req_param.SystemParameters.D1_L_Offset = 20.0;
    request.req_param.SystemParameters.D2_L_Offset = 20.0;
    request.req_param.SystemParameters.C4_D2_Offset = 15.0;
    request.req_param.SystemParameters.ThighMotorReduction = 10.0;
    request.req_param.SystemParameters.ShankMotorReduction = 10.0;
    request.req_param.SystemParameters.AnkleMotorReduction = 10.0;

    // 设置默认机器人身体位置
    request.req_param.RobotBody.BodyCode = "Body";
    request.req_param.RobotBody.x = 0.0;
    request.req_param.RobotBody.y = 0.0;
    request.req_param.RobotBody.z = 300.0;
    request.req_param.RobotBody.qx = 0.0;
    request.req_param.RobotBody.qy = 0.0;
    request.req_param.RobotBody.qz = 0.0;
    request.req_param.RobotBody.qw = 1.0;

    // 设置默认模型ID
    // 这里可以根据需要添加默认模型ID

    // 设置默认参数
    request.req_param.Param.LF.thigh_angle_deg = -49.1033472630546;
    request.req_param.Param.LF.shank_angle_deg = 7.50678016014155;
    request.req_param.Param.LF.ankle_angle_deg = -183.115048990549;

    request.req_param.Param.LH.thigh_angle_deg = -38.4042396745178;
    request.req_param.Param.LH.shank_angle_deg = -9.70116099369837;
    request.req_param.Param.LH.ankle_angle_deg = 107.000051175049;

    request.req_param.Param.RF.thigh_angle_deg = 38.4042396745178;
    request.req_param.Param.RF.shank_angle_deg = 9.70116099369837;
    request.req_param.Param.RF.ankle_angle_deg = 107.000051175049;

    request.req_param.Param.RH.thigh_angle_deg = 49.1033472630546;
    request.req_param.Param.RH.shank_angle_deg = -7.50678016014155;
    request.req_param.Param.RH.ankle_angle_deg = -183.115048990549;

    return request;
}

RobotGaitRequest KinematicsHelper::LoadAndValidateJson(const std::string &jsonInput)
{
    if (jsonInput.empty())
    {
        return CreateDefaultRequest();
    }

    try
    {

        RobotGaitRequest request;
        request.req_param = RequestParameters::fromJsonString(jsonInput);
        return request;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error parsing JSON: " << e.what() << std::endl;
        return CreateDefaultRequest();
    }
}