// KinematicsHelper.h
#ifndef KINEMATICS_HELPER_H
#define KINEMATICS_HELPER_H

#include "QuadrupedRobotSimulation/BaseClass.h"
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
#include "QuadrupedRobotSimulation/KinematicsReverse.h"
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
#include <string>
#include <memory>
#include <chrono>
#include <thread>

/**
 * @brief 工具类：提供便捷的正逆解计算方法
 */
class KinematicsHelper
{
public:
    /**
     * @brief 模拟机器人完整运动
     */
    static void SimRobot();

    /**
     * @brief 输入三个电机的值，返回各个连杆的姿态
     * @param jsonInput JSON输入字符串（可选）
     * @return JSON格式的所有点计算结果
     */
    static json QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput = "");
    /**
     * @brief 执行正向运动学计算：计算所有轨迹
     * @param jsonInput JSON输入字符串（可选）
     * @return JSON格式的正向运动学计算结果
     */
    static json QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput = "");
    /**
     * @brief 单腿运动计算
     * @param thigh_angle_deg 大腿电机角度（度）
     * @param shank_angle_deg 小腿电机角度（度）
     * @param ankle_angle_deg 脚踝电机角度（度）
     * @param leg_type 腿类型："LF"、"LH"、"RF"、"RH"
     * @param json JSON输入字符串（可选）
     * @return JSON格式的单腿计算结果
     */
    static std::string QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
        double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
        const std::string &leg_type = "LF", const std::string &json = "");

    /**
     * @brief 获得RobotGaitDataManager机器人管理句柄
     * @param json JSON输入字符串（可选）
     * @return 共享指针指向RobotGaitDataManager对象
     */
    static std::shared_ptr<RobotGaitDataManager> RobotGaitDataManagerFromJson(const std::string &jsonInput = "", const std::string &robotID = "0");

    /**
     * @brief 从JSON字符串创建配置对象
     * @param jsonInput JSON输入字符串（可选）
     * @return QuadrupedRobotConfiguration对象
     */
    static QuadrupedRobotConfiguration CreateConfigFromJson(const std::string &jsonInput = "");

    /**
     * @brief 根据机器人的机构数据和机构类型，计算机器人四个腿的所有轨迹数据
     * @param config 机器人配置
     * @return JSON格式的正向运动学计算结果
     */
    static std::string PerformForwardKinematics(const QuadrupedRobotConfiguration &config);

    /**
     * @brief 导出完整数据为JSON字符串
     * @param config 机器人配置
     * @return JSON格式的完整导出数据
     */
    static std::string ExportCompleteData(const QuadrupedRobotConfiguration &config);

    /**
     * @brief 批量逆向计算
     * @param motor_data 电机数据映射
     * @param leg_type 腿类型
     * @param max_frames 最大帧数（可选）
     * @param jsonInput 配置JSON（可选）
     * @return 逆向计算结果向量
     */
    static std::vector<ReverseCalculationResult> BatchReverseCalculation(
        const std::map<std::string, std::vector<double>> &motor_data,
        const std::string &leg_type = "LF",
        std::optional<int> max_frames = std::nullopt,
        const std::string &jsonInput = "");

    /**
     * @brief 计算并验证支撑点
     * @param pointsDict 点字典
     * @param groundHeight 地面高度
     * @param tolerance 容差
     * @param json 配置JSON（可选）
     * @return 支撑点检查结果
     */
    static SupportCheckResult CheckSupportPoint(
        const std::map<std::string, std::vector<double>> &pointsDict,
        double groundHeight = 0.0,
        double tolerance = 1.0,
        const std::string &json = "");

    /**
     * @brief 生成线框播放数据
     * @param frameRate 帧率
     * @param modelCode 模型代码
     * @param positions 位置数组
     * @param quaternions 四元数数组
     * @return P_OPERATION对象
     */
    static P_OPERATION MakeOperation(
        int frameRate,
        const std::string &modelCode,
        const std::vector<std::vector<double>> &positions,
        const std::vector<std::vector<double>> &quaternions);

    /**
     * @brief 等待函数（用于模拟延迟）
     * @param milliseconds 等待的毫秒数
     */
    static void Wait(int milliseconds)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
    }

private:
    /**
     * @brief 从JSON创建默认配置
     * @return 默认机器人步态请求
     */
    static RobotGaitRequest CreateDefaultRequest();

    /**
     * @brief 加载并验证JSON数据
     * @param json JSON字符串
     * @return 验证后的RobotGaitRequest对象
     */
    static RobotGaitRequest LoadAndValidateJson(const std::string &jsonInput);
};

#endif // KINEMATICS_HELPER_H