#include "component_manager/component_manager.hpp"
#include "component_manager/node_component.hpp"
#include "component_manager/publisher_component.hpp"
#include "component_manager/subscriber_component.hpp"
#include "component_manager/service_component.hpp"
#include "component_manager/client_component.hpp"
#include "glog/logging.h"

#include "rcl/remap.h"
#include <iostream>

namespace env_manager
{
namespace component_manager
{

ComponentManager::ComponentManager(std::weak_ptr<rclcpp::Executor> executor)
    : _executor(executor)
{
}

static rclcpp::NodeOptions create_options(
    const config::NodeOption &node_opts,
    const std::string &node_name,
    const std::string &ns)
{
    rclcpp::NodeOptions opts;
    std::vector<std::string> args = {"--ros-args", 
        "--disable-rosout-logs", 
        "--disable-stdout-logs", 
        "--enable-external-lib-logs",
        "--log-level", "WARN",
    };
    args.push_back("-r");
    args.push_back("__ns:=/" + ns);
    args.push_back("-r");
    args.push_back("__node:=" + node_name);
    args.push_back("-r");
    
    switch (node_opts.type) 
    {
        case config::NodeOption::NodeType::Publisher:
            args.push_back(DEFAULT_PUB_TOPIC_NAME + ":=" + node_opts.name);
            break; 
        case config::NodeOption::NodeType::Subscriber:
            args.push_back(DEFAULT_SUB_TOPIC_NAME + ":=" + node_opts.name);
            break; 
        case config::NodeOption::NodeType::Service:
            args.push_back(DEFAULT_SERVICE_NAME + ":=" + node_opts.name);
            break; 
        case config::NodeOption::NodeType::Client:
            args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name);
            break;
        case config::NodeOption::NodeType::Node:
            args.erase(args.end());
            break;
    }

    opts.arguments(args);

    return opts;
}

static rclcpp::NodeOptions create_default_options(const std::string &ns)
{
    rclcpp::NodeOptions opts;
    opts.arguments({
        "--ros-args", "-r", std::string("__ns:=/" + ns), 
        "--disable-rosout-logs", 
        "--disable-stdout-logs", 
        "--enable-external-lib-logs",
        "--log-level", "FATAL",
        });

    return opts;
}

void ComponentManager::register_components(
    const std::map<std::string, config::ComponentOption> &comps, 
    const std::map<std::string, config::NodeOption> &nodes,
    const std::string& ns)
{
    if (comps.empty())
        return; 

    for (const auto &[name, comp]: comps)
    {
        auto class_name = "rclcpp_components::NodeFactoryTemplate<" + comp.class_name + ">";
                
        LOG(INFO) << "Provide lib: " << comp.library << " namespace: " + ns;
        auto loader = new class_loader::ClassLoader(comp.library);
        auto classes = 
            loader->getAvailableClasses<rclcpp_components::NodeFactory>();

        for (auto clazz: classes)
        {
            if (clazz != class_name)
                continue;
            rclcpp::NodeOptions opts = create_default_options(ns);
            if (clazz == class_name)
            {
                auto node_opts = nodes.at(name);
                opts = create_options(node_opts, name, ns);
            }
            LOG(INFO) << "Create instance of class: " << clazz;
            auto node_factory = 
                loader->createInstance<rclcpp_components::NodeFactory>(clazz);
            auto wrapper = node_factory->create_node_instance(opts);
            auto node = wrapper.get_node_base_interface();

            _node_wrappers.insert({name, wrapper});
            _nodes.insert({name, node});
            if (auto exec = _executor.lock())
                exec->add_node(node);
        }

        _loaders.push_back(loader);
    }
}

void ComponentManager::remap_components_namespace(const std::string &ns)
{
    char* nms = const_cast<char*>(ns.c_str());
    for (auto& [name, wrapper]: _node_wrappers)
    {
        auto node = (rclcpp::Node*)wrapper.get_node_instance().get(); 
        auto opts = node->get_node_options();
        auto ret = rcl_remap_node_namespace(
            &opts.get_rcl_node_options()->arguments, 
            NULL, node->get_name(), rcl_get_default_allocator(), 
            &nms); 
        
        if (ret == RCL_RET_OK)
            LOG(INFO) << "Succesfully remap node with ns: " + ns;
    }
    
    //std::logic_error("Not implemented." + ns);
}

void ComponentManager::remove_components_from_executor()
{
    if (_nodes.empty())
    {
        LOG(WARNING) << "Unable to remove nodes from executor.";
        return;
    }

    if (auto exec = _executor.lock())
    {
        for (const auto &[node_name, node]: _nodes)
        {
            LOG(INFO) << "Remove node '" << node_name << "' from executor.";
            exec->remove_node(node);
        }
    }
}

} // namespace env_manager
} // namespace component_manager