#include <j7s_diagnostics_ros/conversions.hpp>

std::string convertString(const rust::String & rust)
{
    auto copy = rust;
    return std::string(copy.c_str());
}

j7s_diagnostics_cxx::DiagnosticLevel convertLevel(const uint8_t rosLevel)
{
    switch (rosLevel)
    {
        case diagnostic_msgs::msg::DiagnosticStatus::OK:
            return j7s_diagnostics_cxx::DiagnosticLevel::OK;
        case diagnostic_msgs::msg::DiagnosticStatus::WARN:
            return j7s_diagnostics_cxx::DiagnosticLevel::WARN;
        case diagnostic_msgs::msg::DiagnosticStatus::ERROR:
            return j7s_diagnostics_cxx::DiagnosticLevel::ERROR;
        case diagnostic_msgs::msg::DiagnosticStatus::STALE:
        default:
            return j7s_diagnostics_cxx::DiagnosticLevel::STALE;
    }
}

uint8_t convertLevel(const j7s_diagnostics_cxx::DiagnosticLevel & level)
{
    switch (level)
    {
        case j7s_diagnostics_cxx::DiagnosticLevel::OK:
            return diagnostic_msgs::msg::DiagnosticStatus::OK;
        case j7s_diagnostics_cxx::DiagnosticLevel::WARN:
            return diagnostic_msgs::msg::DiagnosticStatus::WARN;
        case j7s_diagnostics_cxx::DiagnosticLevel::ERROR:
            return diagnostic_msgs::msg::DiagnosticStatus::ERROR;
        case j7s_diagnostics_cxx::DiagnosticLevel::STALE:
        case j7s_diagnostics_cxx::DiagnosticLevel::UNSET:
        default:
            return diagnostic_msgs::msg::DiagnosticStatus::STALE;
    }
}

std::vector<diagnostic_msgs::msg::KeyValue> convertValues(
    const rust::Box<j7s_diagnostics_cxx::StringMap> & map)
{
    std::vector<diagnostic_msgs::msg::KeyValue> toReturn;

    const rust::Vec<rust::String> keys = map->keys();
    if (keys.empty())
    {
        return toReturn;
    }

    for (const auto & key : keys)
    {
        const auto value = map->get(key);
        diagnostic_msgs::msg::KeyValue msg;
        msg.key = convertString(key);
        msg.value = std::string(value->value().c_str());
        toReturn.emplace_back(msg);
    }

    return toReturn;
}

rust::Box<j7s_diagnostics_cxx::StringMap> convertValues(
    const std::vector<diagnostic_msgs::msg::KeyValue> & map)
{
    rust::Box<j7s_diagnostics_cxx::StringMap> toReturn = j7s_diagnostics_cxx::StringMap::build();

    for (const auto & pair : map)
    {
        toReturn->insert(rust::String(pair.key), rust::String(pair.value));
    }

    return toReturn;
}

j7s_diagnostics_cxx::DiagnosticStatus fromMsg(const diagnostic_msgs::msg::DiagnosticStatus & msg)
{
    auto toReturn = j7s_diagnostics_cxx::DiagnosticStatus::build();
    toReturn.level = convertLevel(msg.level);
    toReturn.name = rust::String(msg.name);
    toReturn.message = rust::String(msg.message);
    toReturn.hardware_id = rust::String(msg.hardware_id);
    toReturn.values = convertValues(msg.values);

    return toReturn;
}

rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> fromMsg(
    const diagnostic_msgs::msg::DiagnosticArray & msg)
{
    rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> toReturn;
    toReturn.reserve(msg.status.size());

    for (const auto & rosMsg : msg.status)
    {
        toReturn.emplace_back(fromMsg(rosMsg));
    }
    return toReturn;
}

diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::DiagnosticStatus & status)
{
    diagnostic_msgs::msg::DiagnosticStatus toReturn;
    toReturn.level = convertLevel(status.level);
    toReturn.name = convertString(status.name);
    toReturn.message = convertString(status.message);
    toReturn.hardware_id = convertString(status.hardware_id);
    toReturn.values = convertValues(status.values);

    return toReturn;
}

diagnostic_msgs::msg::DiagnosticArray toMsg(
    const rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> & statuses)
{
    diagnostic_msgs::msg::DiagnosticArray msg;
    msg.status.reserve(statuses.size());

    for (const auto & status : statuses)
    {
        msg.status.emplace_back(toMsg(status));
    }
    return msg;
}