#pragma once

#include <rclcpp/rclcpp.hpp>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "brain/action_registry.hpp"
#include "brain/bt_registry.hpp"

#include "brain/skill_config.hpp"

namespace brain
{
/**
 * @brief Specification of a skill as parsed from YAML.
 *
 * Fields capture metadata, dependency declarations and interface exposure. Interface strings use
 * the convention "BaseName.action" or "BaseName.service" to distinguish interface kinds.
 */
struct SkillSpec
{
  std::string name;
  std::string version;
  std::string description;
  std::vector<std::string> required_skills;
  std::vector<std::string> interfaces;  // e.g. "BaseName.action" / "BaseName.service"
  // Optional per-interface default topic overrides keyed by full token (e.g. "Arm.action").
  std::unordered_map<std::string, std::string> interface_default_topics;
};

/**
 * @brief Loads skill specifications and registers related action/service interfaces.
 *
 * Responsibilities:
 *  1. Parse a YAML description file (robot_skills.yaml or equivalent).
 *  2. Register action clients and service clients for declared interfaces.
 *  3. Provide enumeration utilities and type-safe access to service clients.
 *
 * BehaviorTree action node registration is handled elsewhere; this class only prepares interface bindings.
 */
class SkillManager
{
public:
  /**
   * @param node ROS 2 node pointer (not owned).
   * @param action_clients Action client registry used to register skill actions.
   * @param bt Behavior tree registry reference (not used for node creation directly here).
   */
  SkillManager(rclcpp::Node * node, ActionClientRegistry * action_clients, BTRegistry * bt)
  : node_(node), action_clients_(action_clients), bt_(bt)
  {
    if (node_) {
      constexpr auto prefix_param = "skill_topic_prefix";
      if (!node_->has_parameter(prefix_param)) {
        node_->declare_parameter<std::string>(prefix_param, "");
      }
    }
  }

  /**
   * @brief Load skill specifications from a YAML file.
   * @param yaml_path File system path to YAML document.
   * @return true on success, false if parsing or validation fails.
   */
  bool load_from_file(const std::string & yaml_path);

  /**
   * @brief Access loaded skills mapping.
   * @return Map of skill name -> SkillSpec.
   */
  const std::unordered_map<std::string, SkillSpec> & skills() const {return skills_;}

  /**
   * @brief Enumerate action oriented skills.
   * @return Vector of (skill_name, action_base) pairs.
   */
  std::vector<std::pair<std::string, std::string>> enumerate_action_skills() const;

private:
  rclcpp::Node * node_ {nullptr};
  ActionClientRegistry * action_clients_ {nullptr};
  BTRegistry * bt_ {nullptr};
  std::unordered_map<std::string, SkillSpec> skills_;
  std::vector<rclcpp::ClientBase::SharedPtr> service_clients_;
  std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client
  std::unordered_map<std::string, std::function<void(const std::string &, const std::string &)>>
    action_override_registrars_;
  std::unordered_map<std::string, std::function<bool(
    const std::string & skill_name,
    std::string & detail)>> service_hook_registrars_;

  void register_interfaces_(const SkillSpec & s);

public:

  // Public helpers for registrars to avoid accessing private members directly from free functions
  template<typename ActionT>
  void register_action_for_base(
    const std::string & base,
    const std::string & topic,
    const std::string & internal_skill)
  {
    register_action_client_default<ActionT>(base, topic, internal_skill);
  }

  template<typename ServiceT>
  void register_service_client(
    const std::string & topic,
    const std::string & internal_skill)
  {
    // Try reuse existing client if already created under either key
    std::shared_ptr<rclcpp::Client<ServiceT>> typed_existing;
    auto it_topic = service_client_map_.find(topic);
    if (it_topic != service_client_map_.end()) {
      typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_topic->second);
    }
    if (!typed_existing) {
      auto it_name = service_client_map_.find(internal_skill);
      if (it_name != service_client_map_.end()) {
        typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_name->second);
      }
    }
    auto client = typed_existing ? typed_existing : node_->create_client<ServiceT>(topic);
    if (!typed_existing) {
      service_clients_.push_back(client);
    }
    service_client_map_[internal_skill] = client;
    service_client_map_[topic] = client;
  }

  template<typename ServiceT>
  struct ServiceHookOptions
  {
    using ClientPtr = std::shared_ptr<rclcpp::Client<ServiceT>>;
    using RequestPtr = std::shared_ptr<typename ServiceT::Request>;
    using ResponsePtr = std::shared_ptr<typename ServiceT::Response>;

    std::function<RequestPtr(const std::string & skill_name)> make_request;
    std::function<std::chrono::nanoseconds(const std::string & skill_name)> wait_timeout;
    std::function<std::chrono::nanoseconds(const std::string & skill_name)> call_timeout;
    std::function<void(const std::string & skill_name, const ClientPtr & client)> on_before_call;
    std::function<bool(
      const std::string & skill_name,
      const ClientPtr & client,
      const ResponsePtr & response,
      std::string & detail)> on_response;
  };
  template<typename ActionT>
  struct ActionHookOptions
  {
    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
    std::function<typename ActionT::Goal(const std::string & skill_name)> make_goal;
    std::function<void(
      const std::string & skill_name,
      typename GoalHandle::SharedPtr,
      const std::shared_ptr<const typename ActionT::Feedback>)> on_feedback;
    std::function<void(
      const std::string & skill_name,
      const typename GoalHandle::WrappedResult &)> on_result;
    std::function<void(
      const std::string & skill_name,
      const std::shared_ptr<GoalHandle> &)> on_goal_response;
  };

  template<typename ActionT>
  void configure_action_hooks(const std::string & base, ActionHookOptions<ActionT> options)
  {
    action_override_registrars_[base] =
      [this,
      make_goal = std::move(options.make_goal),
      on_feedback = std::move(options.on_feedback),
      on_result = std::move(options.on_result),
      on_goal_response = std::move(options.on_goal_response)](
      const std::string & topic, const std::string & internal_skill) {
        std::function<typename ActionT::Goal()> make_goal_cb;
        if (make_goal) {
          make_goal_cb = [make_goal, internal_skill]() {return make_goal(internal_skill);};
        }
        using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
        std::function<void(
          typename GoalHandle::SharedPtr,
          const std::shared_ptr<const typename ActionT::Feedback>)> feedback_cb;
        if (on_feedback) {
          feedback_cb = [on_feedback, internal_skill](
              typename GoalHandle::SharedPtr handle,
              const std::shared_ptr<const typename ActionT::Feedback> feedback) {
              on_feedback(internal_skill, handle, feedback);
            };
        }
        std::function<void(const typename GoalHandle::WrappedResult &)> result_cb;
        if (on_result) {
          result_cb = [on_result, internal_skill](const typename GoalHandle::WrappedResult & res) {
              on_result(internal_skill, res);
            };
        }
        std::function<void(const std::shared_ptr<GoalHandle> &)> goal_response_cb;
        if (on_goal_response) {
          goal_response_cb = [on_goal_response, internal_skill](
              const std::shared_ptr<GoalHandle> & handle) {
              on_goal_response(internal_skill, handle);
            };
        }
        register_action_client_with_overrides<ActionT>(
          topic, internal_skill, make_goal_cb, feedback_cb, result_cb, goal_response_cb);
      };
  }

  template<typename ServiceT>
  void configure_service_hooks(const std::string & base, ServiceHookOptions<ServiceT> options)
  {
    service_hook_registrars_[base] = [this, options = std::move(options)](
        const std::string & skill_name,
        std::string & detail) -> bool {
        auto client = this->get_typed_service_client<ServiceT>(skill_name);
        if (!client) {
          detail = "Service client missing";
          return false;
        }
        const auto wait_timeout = options.wait_timeout ?
          options.wait_timeout(skill_name) : std::chrono::seconds(2);
        if (!client->wait_for_service(wait_timeout)) {
          detail = "Service unavailable";
          return false;
        }
        if (options.on_before_call) {
          options.on_before_call(skill_name, client);
        }
        auto request = options.make_request ?
          options.make_request(skill_name) : std::make_shared<typename ServiceT::Request>();
        const auto call_timeout = options.call_timeout ?
          options.call_timeout(skill_name) : std::chrono::seconds(5);
        auto future = client->async_send_request(request);
        if (future.wait_for(call_timeout) != std::future_status::ready) {
          detail = "Service call timeout";
          return false;
        }
        auto response = future.get();
        if (!response) {
          detail = "Null response";
          return false;
        }
        if (options.on_response) {
          return options.on_response(skill_name, client, response, detail);
        }
        detail = "success";
        return true;
      };
  }

  bool invoke_service_hook(
    const std::string & base,
    const std::string & skill_name,
    std::string & detail) const
  {
    auto it = service_hook_registrars_.find(base);
    if (it == service_hook_registrars_.end()) {
      return false;
    }
    return it->second(skill_name, detail);
  }

  /**
   * @brief Retrieve a typed service client for a named skill if available.
   * @tparam T Service type.
   * @param skill_name Registered skill name.
   * @return Shared pointer to service client or nullptr if not present or type mismatch.
   */
  template<typename T>
  std::shared_ptr<rclcpp::Client<T>> get_typed_service_client(const std::string & skill_name) const
  {
    auto it = service_client_map_.find(skill_name);
    if (it == service_client_map_.end()) {return nullptr;}
    return std::dynamic_pointer_cast<rclcpp::Client<T>>(it->second);
  }

private:
  template<typename ActionT>
  void register_action_client_default(
    const std::string & base,
    const std::string & topic,
    const std::string & internal_skill)
  {
    auto it = action_override_registrars_.find(base);
    if (it != action_override_registrars_.end()) {
      it->second(topic, internal_skill);
      RCLCPP_INFO(node_->get_logger(), "Overridden action client registration: %s", base.c_str());
      return;
    }
    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
    register_action_client_with_overrides<ActionT>(
      topic,
      internal_skill,
      std::function<typename ActionT::Goal()>{},
      std::function<void(typename GoalHandle::SharedPtr,
      const std::shared_ptr<const typename ActionT::Feedback>)>{},
      std::function<void(const typename GoalHandle::WrappedResult &)> {},
      std::function<void(const std::shared_ptr<GoalHandle> &)> {});
  }

  template<typename ActionT>
  void register_action_client_with_overrides(
    const std::string & topic,
    const std::string & internal_skill,
    const std::function<typename ActionT::Goal()> & make_goal_override,
    const std::function<void(typename ActionClientRegistry::GoalHandle<ActionT>::SharedPtr,
    const std::shared_ptr<const typename ActionT::Feedback>)> & on_feedback_override,
    const std::function<void(const typename ActionClientRegistry::GoalHandle<ActionT>::WrappedResult &)> & on_result_override,
    const std::function<void(const std::shared_ptr<typename ActionClientRegistry::GoalHandle<ActionT>> &)> & on_goal_response_override)
  {
    if (!action_clients_) {return;}
    using Goal = typename ActionT::Goal;
    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
    std::function<Goal()> make_goal = make_goal_override ? make_goal_override : []() {return Goal{};};
    auto feedback_cb = on_feedback_override;
    auto goal_response_cb = on_goal_response_override;
    std::function<void(const typename GoalHandle::WrappedResult &)> result_cb = on_result_override;
    if (!result_cb) {
      result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
          if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
            SkillActionTrait<ActionT>::success(*res.result)) {
            RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
          } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
            RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
          } else {
            std::string detail;
            if (res.result) {
              detail = SkillActionTrait<ActionT>::message(*res.result);
            } else {
              detail = "result unavailable";
            }
            RCLCPP_ERROR(
              node_->get_logger(), "action failed: %s (%s)", internal_skill.c_str(), detail.c_str());
          }
        };
    }
    action_clients_->register_client<ActionT>(
      topic,
      make_goal,
      feedback_cb,
      result_cb,
      goal_response_cb);
  }
};

/**
 * @brief Recursive compile-time dispatcher that matches a skill name to an action type and invokes send_and_wait.
 * @tparam I Current tuple index (internal use, defaults to 0).
 * @tparam TupleT Tuple type containing action types (e.g., SkillActionTypes).
 * @tparam RegistryT Registry providing template send_and_wait<ActionT>(...).
 * @param skill Canonical skill name to match (SkillActionTrait<ActionT>::skill_name).
 * @param topic Action topic name used when calling the registry.
 * @param reg Pointer to registry instance.
 * @param logger Logger for diagnostics.
 * @param timeout Wait timeout for the action result.
 * @param detail Populated with success/failure detail message.
 * @return true if execution succeeded and result success flag is true; false otherwise.
 */
template<size_t I = 0, typename TupleT, typename RegistryT>
bool dispatch_skill_action(
  const std::string & skill, const std::string & topic, RegistryT * reg,
  rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
{
  if constexpr (I == std::tuple_size<TupleT>::value) {
    return false;
  } else {
    using ActionT = std::tuple_element_t<I, TupleT>;
    if (skill == SkillActionTrait<ActionT>::skill_name) {
      auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
      if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
        bool ok = SkillActionTrait<ActionT>::success(*opt->result);
        detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
          "result flag false");
        return ok;
      }
      detail = "action failed";
      return false;
    }
    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
  }
}

} // namespace brain
