#include "KinematicsWebAPI.h"

json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json &req_param)
{
    json res_data;

    if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
    {
        try
        {
            log("Handling Cmd_Kinematics_inverse_pose_str command");

            std::string pose_str = req_param.value("pose_str", "");
            std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
            std::string robot_uuid = req_param.value("robot_uuid", "default");

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                json result = robot->inversePoseStr(pose_str, q_init_str);
                if (result.empty())
                {
                    res_data = {{"error", "Inverse kinematics calculation failed"}};
                }
                else
                {
                    res_data = {{"joints", result}, {"success", true}};
                }
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
    {
        try
        {
            log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");

            std::string pose_str = req_param.value("pose_str", "");
            std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
            std::string robot_uuid = req_param.value("robot_uuid", "default");
            std::string steps_str = req_param.value("steps_str", "default");
            int steps = std::stoi(steps_str);

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                json result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
                if (result.empty())
                {
                    res_data = {{"error", "Inverse kinematics calculation failed"}};
                }
                else
                {
                    res_data = {{"joints", result}, {"success", true}};
                }
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
    {
        try
        {
            log("Handling Cmd_Kinematics_inverse_pose_str_NoDifference command");

            std::string pose_str = req_param.value("pose_str", "");
            std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
            std::string robot_uuid = req_param.value("robot_uuid", "default");

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                json result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
                if (result.empty())
                {
                    res_data = {{"error", "Inverse kinematics calculation failed"}};
                }
                else
                {
                    res_data = {{"joints", result}, {"success", true}};
                }
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
    {
        try
        {
            log("Handling Cmd_Kinematics_forward_pose_str command");

            std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
            std::string robot_uuid = req_param.value("robot_uuid", "default");

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                auto joints = robot->parseJointString(joints_str);
                if (joints.size() != 6)
                {
                    res_data = {{"error", "Invalid joints format"}};
                }
                else
                {
                    double tcp_pose[7];
                    if (robot->calculateFK_TCP(joints.data(), tcp_pose))
                    {
                        res_data = {
                            {"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
                            {"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
                            {"joints", joints},
                            {"success", true}};
                    }
                    else
                    {
                        res_data = {{"error", "Forward kinematics calculation failed"}};
                    }
                }
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
    {
        try
        {
            log("Handling Cmd_Kinematics_forward_all_joints command");

            std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
            std::string robot_uuid = req_param.value("robot_uuid", "default");

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                res_data = {
                    {"OPERATION", robot->handleKinematicsForwardAllJoints(joints_str)},
                    {"success", true}};
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_Kinematics_check_singularity")
    {
        try
        {
            log("Handling Cmd_Kinematics_check_singularity command");

            std::string joints_str = req_param.value("joints_str", req_param.value("q_init_str", "0,0,0,0,0,0"));
            std::string robot_uuid = req_param.value("robot_uuid", "default");
            double singular_threshold = req_param.value("singular_threshold", 1e-4);
            double warning_threshold = req_param.value("warning_threshold", 1e-2);
            double condition_threshold = req_param.value("condition_threshold", 1e6);
            double condition_warning_threshold = req_param.value("condition_warning_threshold", 1e4);

            auto robot = RobotManager::getRobot(robot_uuid);
            if (!robot || !robot->isInitialized())
            {
                res_data = {{"error", "Robot not found or not initialized"}};
            }
            else
            {
                res_data = robot->checkSingularity(
                    joints_str,
                    singular_threshold,
                    warning_threshold,
                    condition_threshold,
                    condition_warning_threshold);
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to check singularity: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_InitRobot")
    {
        try
        {
            log("Handling Cmd_InitRobot command");

            std::string urdf_base64 = req_param.value("urdf_base64", "");
            std::string uuid = req_param.value("robot_uuid", "");
            bool force_update = req_param.value("force_update", true);
            std::string urdf_content = utils::base64_to_urdf(urdf_base64);

            if (!utils::validate_urdf_base64(urdf_base64))
            {
                res_data = {
                    {"success", false},
                    {"message", "Invalid URDF format"},
                    {"timestamp", utils::get_current_time()}};
            }
            else
            {
                auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
                res_data = {
                    {"success", result.first},
                    {"message", result.second},
                    {"timestamp", utils::get_current_time()}};
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_GetRobot")
    {
        try
        {
            log("Handling Cmd_GetRobot command");

            std::string uuid = req_param.value("uuid", "");
            auto robot = RobotManager::getRobot(uuid);

            if (!robot)
            {
                res_data = {{"error", "Robot not found"}};
            }
            else
            {
                res_data = {
                    {"success", true},
                    {"uuid", uuid},
                    {"initialized", robot->isInitialized()},
                    {"joints_count", robot->getNumberOfJoints()},
                    {"timestamp", utils::get_current_time()}};
            }
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_RemoveRobot")
    {
        try
        {
            log("Handling Cmd_RemoveRobot command");

            std::string uuid = req_param.value("uuid", "");
            auto result = RobotManager::removeRobot(uuid);

            res_data = {
                {"success", result.first},
                {"message", result.second},
                {"timestamp", utils::get_current_time()}};
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_ListRobots")
    {
        try
        {
            log("Handling Cmd_ListRobots command");

            bool detail = req_param.value("detail", false);
            auto robots = RobotManager::listRobots(detail);

            res_data = {
                {"success", true},
                {"robots", robots},
                {"count", robots.size()},
                {"timestamp", utils::get_current_time()}};
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_SelectCraftTree")
    {
        try
        {
            log("Handling Cmd_SelectCraftTree command");
            std::string tree_id = req_param.value("tree_id", "");
            res_data = {
                {"success", true},
                {"tree_id", tree_id},
                {"message", "Craft tree selected successfully"},
                {"timestamp", utils::get_current_time()}};
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
        }
    }
    else if (req_cmd == "Cmd_AddOperationTree")
    {
        try
        {
            log("Handling Cmd_AddOperationTree command");
            std::string tree_name = req_param.value("name", "");
            json operations = req_param.value("operations", json::array());

            res_data = {
                {"success", true},
                {"tree_name", tree_name},
                {"operations_count", operations.size()},
                {"message", "Operation tree added successfully"},
                {"timestamp", utils::get_current_time()}};
        }
        catch (const std::exception &e)
        {
            res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
        }
    }

    return res_data;
}