#include "KinematicsWebAPI.h"
#include "URDFStrings.h"

#include <cassert>
#include <ctime>
#include <iostream>
#include <string>

namespace
{
bool isRobotCommand(const std::string &req_cmd)
{
    return req_cmd == "Cmd_Kinematics_inverse_pose_str" ||
           req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps" ||
           req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference" ||
           req_cmd == "Cmd_Kinematics_forward_pose_str" ||
           req_cmd == "Cmd_Kinematics_forward_all_joints" ||
           req_cmd == "Cmd_Kinematics_check_singularity" ||
           req_cmd == "Cmd_InitRobot" ||
           req_cmd == "Cmd_GetRobot" ||
           req_cmd == "Cmd_RemoveRobot" ||
           req_cmd == "Cmd_ListRobots" ||
           req_cmd == "Cmd_SelectCraftTree" ||
           req_cmd == "Cmd_AddOperationTree";
}

// 从业务返回体推导顶层 API 是否成功，避免把业务错误包装成成功响应。
bool isBusinessResultSuccess(const json &res_data)
{
    if (!res_data.is_object())
    {
        return true;
    }

    if (res_data.contains("success") && res_data["success"].is_boolean())
    {
        return res_data["success"].get<bool>();
    }

    return !res_data.contains("error");
}

// 提取业务错误信息，用于同步顶层 msg 字段。
std::string getBusinessResultMessage(const json &res_data, const std::string &fallback)
{
    if (isBusinessResultSuccess(res_data))
    {
        return fallback;
    }

    if (res_data.contains("message") && res_data["message"].is_string())
    {
        return res_data["message"].get<std::string>();
    }

    if (res_data.contains("error") && res_data["error"].is_string())
    {
        return res_data["error"].get<std::string>();
    }

    return fallback.empty() ? "Business command failed" : fallback;
}
} // namespace

KinematicsWebAPI::KinematicsWebAPI()
{
    init_func();
}

std::string KinematicsWebAPI::init_func()
{
    std::string urdf_string = URDFStrings::abb120_urdf;
    RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
    RobotManager::initRobot(urdf_string, "abb_irb120_3_58");

    return utils::create_api_response(true, 200, "init_func", "", "", "").dump();
}

std::string KinematicsWebAPI::func(std::string sanitized_body)
{
    log("func called with body: " + sanitized_body.substr(0, 100));
    assert(!sanitized_body.empty() && "sanitized_body should not be empty");

    try
    {
        json request_json;
        try
        {
            request_json = json::parse(sanitized_body);
        }
        catch (const std::exception &)
        {
            return utils::create_api_response(false, 400, "Invalid JSON format", "", "", "").dump();
        }

        const std::string msg = request_json.value("msg", "");
        const std::string req_code = request_json.value("req_code", "");
        const std::string req_from = request_json.value("req_from", "");
        const std::string req_cmd = request_json.value("req_cmd", "");
        const json req_param = request_json.value("req_param", json::object());

        json res_data = dispatchCommand(req_cmd, req_param);
        const bool business_success = isBusinessResultSuccess(res_data);
        const int code = business_success ? 0 : 1000;
        const std::string response_msg = getBusinessResultMessage(res_data, msg);
        return utils::create_api_response(business_success, code, response_msg, req_code, req_from, req_cmd, res_data).dump();
    }
    catch (const std::exception &e)
    {
        return utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "").dump();
    }
}

json KinematicsWebAPI::dispatchCommand(const std::string &req_cmd, const json &req_param)
{
    if (isRobotCommand(req_cmd))
    {
        return handleRobotCommand(req_cmd, req_param);
    }

    if (req_cmd == "Cmd_Spc")
    {
        return handleSpcCommand(req_cmd, req_param);
    }

    if (req_cmd == "Cmd_FourBar_Simulate" ||
        req_cmd == "Cmd_FourBar_CrankSlider" ||
        req_cmd == "Cmd_FourBar_CrankSlider_Simulate")
    {
        return handleFourBarCommand(req_cmd, req_param);
    }

    if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles" ||
        req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
    {
        return handleQuadrupedCommand(req_cmd, req_param);
    }

    return createUnknownCommandResponse(req_cmd, req_param);
}

json KinematicsWebAPI::createUnknownCommandResponse(const std::string &req_cmd, const json &req_param) const
{
    return {
        {"success", false},
        {"error", "Unknown command: " + req_cmd},
        {"received_params", req_param},
        {"timestamp", std::time(nullptr)}};
}

void KinematicsWebAPI::log(const std::string &message)
{
    std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;

    if (onLog)
    {
        onLog("[" + getCurrentTimestamp() + "] " + message);
    }
}

std::string KinematicsWebAPI::getCurrentTimestamp()
{
    return utils::get_current_timestamp();
}

bool KinematicsWebAPI::is_running() const
{
    return running_;
}