#include <chrono>
#include <map>
#include <memory>
#include <set>
#include <string>

#include "obstacle_nav2/nav2_profile_loader.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

namespace obstacle_nav2
{
namespace
{

std::string setParametersServiceName(const std::string & node_name)
{
  if (node_name.empty()) {
    return "/set_parameters";
  }
  if (node_name.front() == '/') {
    return node_name + "/set_parameters";
  }
  return "/" + node_name + "/set_parameters";
}

std::string listParametersServiceName(const std::string & node_name)
{
  if (node_name.empty()) {
    return "/list_parameters";
  }
  if (node_name.front() == '/') {
    return node_name + "/list_parameters";
  }
  return "/" + node_name + "/list_parameters";
}

}  // namespace

class Nav2ProfileTuner : public rclcpp::Node
{
public:
  Nav2ProfileTuner()
  : Node("nav2_profile_tuner")
  {
    const auto profile_10_path = declare_parameter<std::string>("profile_10_yaml", "");
    const auto profile_11_path = declare_parameter<std::string>("profile_11_yaml", "");
    const auto trigger_topic = declare_parameter<std::string>("trigger_topic", "/sign4return");

    if (profile_10_path.empty() || profile_11_path.empty()) {
      throw std::runtime_error("profile_10_yaml and profile_11_yaml must both be set");
    }

    profiles_[10] = loadNav2Profile(profile_10_path);
    profiles_[11] = loadNav2Profile(profile_11_path);

    trigger_sub_ = create_subscription<std_msgs::msg::Int32>(
      trigger_topic, 10,
      [this](const std_msgs::msg::Int32::SharedPtr msg) {
        handleTrigger(msg->data);
      });

    retry_timer_ = create_wall_timer(
      std::chrono::milliseconds(500),
      [this]() {
        tryApplyPendingProfile();
      });

    requestProfile(10);
  }

private:
  using SetParameters = rcl_interfaces::srv::SetParameters;
  using ListParameters = rcl_interfaces::srv::ListParameters;

  void handleTrigger(const int32_t command)
  {
    if (command == 10 || command == 11) {
      requestProfile(command);
      return;
    }
    RCLCPP_DEBUG(get_logger(), "Ignoring sign4return=%d", command);
  }

  void requestProfile(const int command)
  {
    if (active_profile_ == command && !has_pending_profile_) {
      return;
    }
    pending_profile_ = command;
    has_pending_profile_ = true;
    RCLCPP_INFO(get_logger(), "Queued Nav2 profile %d", command);
    tryApplyPendingProfile();
  }

  void tryApplyPendingProfile()
  {
    if (!has_pending_profile_) {
      return;
    }

    const auto profile_it = profiles_.find(pending_profile_);
    if (profile_it == profiles_.end()) {
      RCLCPP_ERROR(get_logger(), "No Nav2 profile loaded for command %d", pending_profile_);
      has_pending_profile_ = false;
      return;
    }

    for (const auto & node_set : profile_it->second.nodes) {
      const auto set_service_name = setParametersServiceName(node_set.node_name);
      auto & set_client = set_clients_[set_service_name];
      if (!set_client) {
        set_client = create_client<SetParameters>(set_service_name);
      }
      if (!set_client->wait_for_service(std::chrono::milliseconds(0))) {
        RCLCPP_WARN_THROTTLE(
          get_logger(), *get_clock(), 3000,
          "Waiting for parameter service %s", set_service_name.c_str());
        return;
      }

      const auto list_service_name = listParametersServiceName(node_set.node_name);
      auto & list_client = list_clients_[list_service_name];
      if (!list_client) {
        list_client = create_client<ListParameters>(list_service_name);
      }
      if (!list_client->wait_for_service(std::chrono::milliseconds(0))) {
        RCLCPP_WARN_THROTTLE(
          get_logger(), *get_clock(), 3000,
          "Waiting for parameter service %s", list_service_name.c_str());
        return;
      }
    }

    const auto command = pending_profile_;
    for (const auto & node_set : profile_it->second.nodes) {
      const auto list_service_name = listParametersServiceName(node_set.node_name);
      auto request = std::make_shared<ListParameters::Request>();
      request->depth = ListParameters::Request::DEPTH_RECURSIVE;

      list_clients_.at(list_service_name)->async_send_request(
        request,
        [this, node_set](rclcpp::Client<ListParameters>::SharedFuture future) {
          const auto response = future.get();
          const std::set<std::string> declared_names(
            response->result.names.begin(), response->result.names.end());
          const auto parameters = filterDeclaredParameters(node_set.parameters, declared_names);

          if (parameters.empty()) {
            RCLCPP_WARN(
              get_logger(), "No declared parameters from profile for %s",
              node_set.node_name.c_str());
            return;
          }

          auto set_request = std::make_shared<SetParameters::Request>();
          set_request->parameters.reserve(parameters.size());
          for (const auto & parameter : parameters) {
            set_request->parameters.push_back(parameter.to_parameter_msg());
          }

          const auto set_service_name = setParametersServiceName(node_set.node_name);
          set_clients_.at(set_service_name)->async_send_request(
            set_request,
            [this, node_name = node_set.node_name](
              rclcpp::Client<SetParameters>::SharedFuture set_future) {
              const auto set_response = set_future.get();
              for (const auto & result : set_response->results) {
                if (!result.successful) {
                  RCLCPP_WARN(
                    get_logger(), "Parameter set failed on %s: %s",
                    node_name.c_str(), result.reason.c_str());
                }
              }
            });
        });
    }

    active_profile_ = command;
    has_pending_profile_ = false;
    RCLCPP_INFO(get_logger(), "Applied Nav2 profile %d", active_profile_);
  }

  std::map<int, Nav2Profile> profiles_;
  std::map<std::string, rclcpp::Client<SetParameters>::SharedPtr> set_clients_;
  std::map<std::string, rclcpp::Client<ListParameters>::SharedPtr> list_clients_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr trigger_sub_;
  rclcpp::TimerBase::SharedPtr retry_timer_;
  int active_profile_{0};
  int pending_profile_{0};
  bool has_pending_profile_{false};
};

}  // namespace obstacle_nav2

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  try {
    rclcpp::spin(std::make_shared<obstacle_nav2::Nav2ProfileTuner>());
  } catch (const std::exception & e) {
    RCLCPP_FATAL(rclcpp::get_logger("nav2_profile_tuner"), "%s", e.what());
    rclcpp::shutdown();
    return 1;
  }
  rclcpp::shutdown();
  return 0;
}
