#include "RobotManager.h"
#include <sstream>
#include <iomanip>
#include <iostream>
#include <functional>

// 初始化静态成员
std::unordered_map<std::string, std::shared_ptr<Robot>> RobotManager::_kinematics_table;
std::unordered_map<std::string, std::string> RobotManager::_urdf_hashes;
std::recursive_mutex RobotManager::_lock;

// 哈希函数
std::string calculateHash(const std::string& input) {
    // 使用标准库的哈希函数组合
    std::hash<std::string> hasher;
    size_t h1 = hasher(input);
    size_t h2 = hasher(input + "salt"); // 添加盐值增加唯一性

    std::stringstream ss;
    ss << std::hex << h1 << h2;
    return ss.str();
}

bool RobotManager::_validateKinematics(const std::shared_ptr<Robot>& robot) {
    if (!robot || !robot->isInitialized()) {
        return false;
    }

    try {
        // 简单的验证：检查正向运动学计算
        double test_joints[6] = { 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
        double tcp_pose[7];

        return robot->calculateFK_TCP(test_joints, tcp_pose);
    }
    catch (const std::exception& e) {
        std::cerr << "Robot validation failed: " << e.what() << std::endl;
        return false;
    }
}

std::pair<bool, std::string> RobotManager::initRobot(const std::string& urdf_robot,
    const std::string& robot_uuid,
    bool force_update) {
    std::lock_guard<std::recursive_mutex> lock(_lock);

    try {
        // 创建机器人实例
        auto robot = std::make_shared<Robot>();

        // 初始化机器人
        if (!robot->initRobot(urdf_robot)) {
            return std::make_pair(false, "Failed to initialize robot from URDF");
        }

        std::string actual_uuid = robot_uuid;
        if (actual_uuid.empty()) {
            // 如果没有提供UUID，使用URDF内容的哈希作为UUID
            actual_uuid = calculateHash(urdf_robot);
        }

        // 计算URDF哈希
        std::string urdf_hash = calculateHash(urdf_robot);

        // 检查是否需要更新
        if (!force_update && _urdf_hashes.find(actual_uuid) != _urdf_hashes.end() &&
            _urdf_hashes[actual_uuid] == urdf_hash) {
            return std::make_pair(true, "Content unchanged, skipping update");
        }

        // 验证运动学
        if (!_validateKinematics(robot)) {
            return std::make_pair(false, "URDF validation failed");
        }

        // 更新表
        _kinematics_table[actual_uuid] = robot;
        _urdf_hashes[actual_uuid] = urdf_hash;

        bool existed = (_urdf_hashes.find(actual_uuid) != _urdf_hashes.end());
        std::string action = existed ? "updated" : "added";
        return std::make_pair(true, "Successfully " + action + " robot " + actual_uuid);
    }
    catch (const std::exception& e) {
        return std::make_pair(false, "Operation failed: " + std::string(e.what()));
    }
}

std::unordered_map<std::string, std::pair<bool, std::string>>
RobotManager::batchInit(const std::unordered_map<std::string, std::string>& robot_specs) {
    std::lock_guard<std::recursive_mutex> lock(_lock);

    std::unordered_map<std::string, std::pair<bool, std::string>> results;

    // 使用传统的迭代器语法
    for (auto it = robot_specs.begin(); it != robot_specs.end(); ++it) {
        results[it->first] = initRobot(it->second, it->first);
    }

    return results;
}

std::shared_ptr<Robot> RobotManager::getRobot(const std::string& robot_uuid) {
    std::lock_guard<std::recursive_mutex> lock(_lock);

    auto it = _kinematics_table.find(robot_uuid);
    if (it != _kinematics_table.end()) {
        return it->second;
    }
    return nullptr;
}

std::pair<bool, std::string> RobotManager::removeRobot(const std::string& robot_uuid) {
    std::lock_guard<std::recursive_mutex> lock(_lock);

    if (_kinematics_table.find(robot_uuid) == _kinematics_table.end()) {
        return std::make_pair(false, "Robot " + robot_uuid + " does not exist");
    }

    _kinematics_table.erase(robot_uuid);
    _urdf_hashes.erase(robot_uuid);

    return std::make_pair(true, "Successfully removed robot " + robot_uuid);
}

std::unordered_map<std::string, std::string> RobotManager::listRobots(bool detail) {
    std::lock_guard<std::recursive_mutex> lock(_lock);

    std::unordered_map<std::string, std::string> result;

    if (!detail) {
        for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
            result[it->first] = "Robot Instance";
        }
        return result;
    }

    for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
        const std::string& uuid = it->first;
        const auto& robot = it->second;

        std::stringstream info;
        info << "Joints: " << robot->getNumberOfJoints() << ", "
            << "Status: " << (_validateKinematics(robot) ? "valid" : "invalid") << ", "
            << "Hash: " << _urdf_hashes.at(uuid).substr(0, 8) + "...";

        result[uuid] = info.str();
    }

    return result;
}

size_t RobotManager::getRobotCount() {
    std::lock_guard<std::recursive_mutex> lock(_lock);
    return _kinematics_table.size();
}

void RobotManager::clearAll() {
    std::lock_guard<std::recursive_mutex> lock(_lock);
    _kinematics_table.clear();
    _urdf_hashes.clear();
}

bool RobotManager::containsRobot(const std::string& robot_uuid) {
    std::lock_guard<std::recursive_mutex> lock(_lock);
    return _kinematics_table.find(robot_uuid) != _kinematics_table.end();
}