#include "navigation.hpp"
#include "logger.hpp"

namespace custom {

Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) {
    subSlamInfo = ChannelSubscriberPtr(
        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamInfoTopic));
    subSlamInfo->InitChannel(
        std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1);
    
    subSlamKeyInfo = ChannelSubscriberPtr(
        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamKeyInfoTopic));
    subSlamKeyInfo->InitChannel(
        std::bind(&Navigation::slamKeyInfoHandler, this, std::placeholders::_1), 1);
}

Navigation::~Navigation() {}

void Navigation::Init() {
    try {
        SetApiVersion(SLAM_API_VERSION);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_POSE_NAV_PL);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_PAUSE_NAV);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_RESUME_NAV);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_STOP_NODE);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_MAPPING_PL);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_END_MAPPING_PL);
        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_RELOCATION_PL);
        LOG_INFO("Navigation client for service '" + SLAM_SERVICE_NAME + "' initialized.");
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to initialize navigation client: " + std::string(e.what()));
    }
}

bool Navigation::callSlamService(int api_id, const nlohmann::json& data) {
    nlohmann::json request_json;
    request_json["data"] = data;
    std::string request_str = request_json.dump();
    std::string response_str;

    LOG_INFO("Calling slam service api_id: " + std::to_string(api_id) + ", request: " + request_str);
    
    int ret = this->Call(api_id, request_str, response_str);

    if (ret != 0) {
        LOG_ERROR("Slam service request failed with SDK error code: " + std::to_string(ret));
        return false;
    }
    
    LOG_INFO("Slam service response: " + response_str);

    try {
        nlohmann::json response_json = nlohmann::json::parse(response_str);
        if (response_json.contains("succeed") && response_json["succeed"].get<bool>()) {
            LOG_INFO("Slam service call for api_id " + std::to_string(api_id) + " succeeded. Info: " + response_json.value("info", ""));
            return true;
        } else {
            LOG_ERROR("Slam service call for api_id " + std::to_string(api_id) + " failed. Error code: " + 
                      std::to_string(response_json.value("errorCode", -1)) + ", Info: " + response_json.value("info", ""));
            return false;
        }
    } catch (const nlohmann::json::parse_error& e) {
        LOG_ERROR("Failed to parse slam service response: " + std::string(e.what()));
        return false;
    }
}

void Navigation::slamInfoHandler(const void *message) {
    std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message;
    nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data());

    // Check for errors
    if (jsonData["errorCode"] != 0) {
        LOG_WARN("SLAM info error: " + jsonData.value("info", ""));
        return;
    }

    // Process pose information
    if (jsonData["type"] == "pos_info") {
        currentPose.x = jsonData["data"]["currentPose"]["x"];
        currentPose.y = jsonData["data"]["currentPose"]["y"];
        currentPose.z = jsonData["data"]["currentPose"]["z"];
        currentPose.q_x = jsonData["data"]["currentPose"]["q_x"];
        currentPose.q_y = jsonData["data"]["currentPose"]["q_y"];
        currentPose.q_z = jsonData["data"]["currentPose"]["q_z"];
        currentPose.q_w = jsonData["data"]["currentPose"]["q_w"];
        LOG_INFO("Current pose updated: x=" + std::to_string(currentPose.x) + 
                 ", y=" + std::to_string(currentPose.y) + 
                 ", z=" + std::to_string(currentPose.z));
    }
}

void Navigation::slamKeyInfoHandler(const void *message) {
    std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message;
    nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data());

    // Check for errors
    if (jsonData["errorCode"] != 0) {
        LOG_WARN("SLAM key info error: " + jsonData.value("info", ""));
        return;
    }

    // Process task results
    if (jsonData["type"] == "task_result") {
        is_arrived = jsonData["data"]["is_arrived"];
        std::string targetNodeName = jsonData["data"]["targetNodeName"];
        if (is_arrived) {
            LOG_INFO("Arrived at target: " + targetNodeName);
        } else {
            LOG_WARN("Failed to reach target: " + targetNodeName + ". Please help!");
        }
    }
}


bool Navigation::startMapping() {
    nlohmann::json data;
    data["slam_type"] = "indoor";
    return callSlamService(ROBOT_API_ID_START_MAPPING_PL, data);
}

bool Navigation::endMapping(const std::string& address) {
    nlohmann::json data;
    data["address"] = address;
    return callSlamService(ROBOT_API_ID_END_MAPPING_PL, data);
}

bool Navigation::initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address) {
    nlohmann::json data;
    data["x"] = x;
    data["y"] = y;
    data["z"] = z;
    data["q_x"] = q_x;
    data["q_y"] = q_y;
    data["q_z"] = q_z;
    data["q_w"] = q_w;
    data["address"] = address;
    return callSlamService(ROBOT_API_ID_START_RELOCATION_PL, data);
}

bool Navigation::poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed) {
    nlohmann::json data;
    data["targetPose"]["x"] = x;
    data["targetPose"]["y"] = y;
    data["targetPose"]["z"] = z;
    data["targetPose"]["q_x"] = q_x;
    data["targetPose"]["q_y"] = q_y;
    data["targetPose"]["q_z"] = q_z;
    data["targetPose"]["q_w"] = q_w;
    data["mode"] = mode;
    data["speed"] = speed;
    return callSlamService(ROBOT_API_ID_POSE_NAV_PL, data);
}

bool Navigation::pauseNavigation() {
    nlohmann::json data;
    return callSlamService(ROBOT_API_ID_PAUSE_NAV, data);
}

bool Navigation::resumeNavigation() {
    nlohmann::json data;
    return callSlamService(ROBOT_API_ID_RESUME_NAV, data);
}

bool Navigation::closeSlam() {
    nlohmann::json data;
    return callSlamService(ROBOT_API_ID_STOP_NODE, data);
}

} // namespace custom