#ifndef ROBOTMANAGER_H
#define ROBOTMANAGER_H

#include "Robot.h"
#include <string>
#include <unordered_map>
#include <mutex>
#include <vector>
#include <memory>
#include <functional>

class RobotManager
{
private:
    // 机器人实例表 <uuid, Robot实例>
    static std::unordered_map<std::string, std::shared_ptr<Robot>> _kinematics_table;

    // URDF哈希表 <uuid, urdf_hash>
    static std::unordered_map<std::string, std::string> _urdf_hashes;

    // 线程安全锁
    static std::recursive_mutex _lock;

    /**
     * @brief 验证机器人运动学实例
     * @param robot 机器人实例
     * @return 验证成功返回true
     */
    static bool _validateKinematics(const std::shared_ptr<Robot> &robot);

public:
    // 删除拷贝构造函数和赋值运算符
    RobotManager() = delete;
    RobotManager(const RobotManager &) = delete;
    RobotManager &operator=(const RobotManager &) = delete;

    /**
     * @brief 初始化或更新机器人实例
     * @param urdf_robot URDF描述字符串
     * @param robot_uuid 机器人唯一标识符（可选）
     * @param force_update 是否强制更新（忽略内容变更检查）
     * @return 成功状态和操作消息
     */
    static std::pair<bool, std::string> initRobot(const std::string &urdf_robot,
                                                  const std::string &robot_uuid = "",
                                                  bool force_update = true);

    /**
     * @brief 批量初始化机器人
     * @param robot_specs {uuid: urdf} 字典
     * @return 每个UUID的操作结果
     */
    static std::unordered_map<std::string, std::pair<bool, std::string>>
    batchInit(const std::unordered_map<std::string, std::string> &robot_specs);

    /**
     * @brief 获取机器人实例
     * @param robot_uuid 机器人UUID
     * @return 机器人实例的shared_ptr，如果不存在返回nullptr
     */
    static std::shared_ptr<Robot> getRobot(const std::string &robot_uuid);

    /**
     * @brief 移除机器人
     * @param robot_uuid 机器人UUID
     * @return 成功状态和操作消息
     */
    static std::pair<bool, std::string> removeRobot(const std::string &robot_uuid);

    /**
     * @brief 列出所有机器人
     * @param detail 是否返回详细信息
     * @return 机器人信息字典
     */
    static std::unordered_map<std::string, std::string> listRobots(bool detail = false);

    /**
     * @brief 获取机器人数量
     * @return 当前管理的机器人数量
     */
    static size_t getRobotCount();

    /**
     * @brief 清空所有机器人实例
     */
    static void clearAll();

    /**
     * @brief 检查机器人是否存在
     * @param robot_uuid 机器人UUID
     * @return 存在返回true
     */
    static bool containsRobot(const std::string &robot_uuid);

    /**
     * @brief 线程安全的操作执行
     * @param func 要执行的操作函数
     * @return 操作结果
     */
    template <typename Func, typename... Args>
    static auto executeSafely(Func &&func, Args &&...args)
        -> decltype(func(std::forward<Args>(args)...))
    {
        std::lock_guard<std::recursive_mutex> lock(_lock);
        return func(std::forward<Args>(args)...);
    }
};

#endif // ROBOTMANAGER_H