#pragma once

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

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

#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/leg_control.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"

#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge

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"
};

/**
 * @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:
  /**
   * @brief Construct with external registry references.
   * @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) {}

  /**
   * @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_; // 保存 service client 生命周期
  std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client

  void register_interfaces_(const SkillSpec & s);

public:
  /**
   * @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);
  }
};


/**
 * @brief Primary template (unspecialized) for skill action trait extraction.
 * Specializations must define: skill_name (constexpr const char*), success(result), message(result).
 */
template<typename ActionT>
struct SkillActionTrait; // intentionally undefined primary template

template<>
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
{
  static constexpr const char * skill_name = "ArmSpaceControl";
  static bool success(const interfaces::action::ArmSpaceControl::Result & r) {return r.success;}
  static std::string message(const interfaces::action::ArmSpaceControl::Result & r)
  {
    return r.message;
  }
};
template<>
struct SkillActionTrait<interfaces::action::HandControl>
{
  static constexpr const char * skill_name = "HandControl";
  static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<interfaces::action::LegControl>
{
  static constexpr const char * skill_name = "LegControl";
  static bool success(const interfaces::action::LegControl::Result & r) {return r.success;}
  static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<interfaces::action::VisionGraspObject>
{
  static constexpr const char * skill_name = "VisionGraspObject";
  static bool success(const interfaces::action::VisionGraspObject::Result & r) {return r.success;}
  static std::string message(const interfaces::action::VisionGraspObject::Result & r)
  {
    return r.message;
  }
};

/**
 * @brief Tuple listing of all supported skill action types for dispatcher iteration.
 */
using SkillActionTypes = std::tuple<
  interfaces::action::ArmSpaceControl,
  interfaces::action::HandControl,
  interfaces::action::LegControl,
  interfaces::action::VisionGraspObject
>;

/**
 * @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
