#ifndef BRAIN_CEREBRUM_NODE_HPP_
#define BRAIN_CEREBRUM_NODE_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/service.hpp>
#include <unordered_map>
#include <vector>
#include <string>
#include <atomic>
#include <unordered_set>
#include <optional>
#include <mutex>
#include <filesystem>
#include "brain/action_registry.hpp"
#include <behaviortree_cpp/bt_factory.h>
#include "brain/bt_registry.hpp"
#include "brain/skill_manager.hpp"
#include "brain/constants.hpp"
#include "brain/epoch_filter.hpp"
#include <std_srvs/srv/trigger.hpp>
#include "brain_interfaces/msg/skill_call.hpp"
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>


namespace brain
{

class CerebrumNode : public rclcpp::Node
{
public:
  /**
   * @brief Construct the Cerebrum node.
   *
   * Responsibilities:
   *  - Load skill definitions (YAML) and register corresponding BT leaf actions.
   *  - Provide timers to (a) periodically choose / rebuild a skill sequence BT and (b) tick it.
   *  - Funnel sequence execution through a single ExecuteBtAction toward the cerebellum (executor).
   *  - Expose a trigger service to force an immediate rebuild.
   *
   * @param options Optional ROS 2 node options.
   * @thread_safety Construction is single-threaded; no concurrent access occurs during ctor.
   */
  explicit CerebrumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

  /**
   * @brief Send a single action goal by its registered name using the ActionClientRegistry.
   * @param name Action client key (must have been previously registered).
   * @thread_safety Thread-safe if underlying ActionClientRegistry::send is thread-safe (read-only state).
   */
  void SendAction(const std::string & name);

  /**
   * @brief Register a custom BehaviorTree node type (template wrapper that forwards to BTRegistry).
   * @tparam T Concrete BT node class.
   * @param type_name Name under which the node type will be registered.
   */
  template<typename T>
  void RegisterBtNode(const std::string & type_name) {bt_registry_->register_bt_node<T>(type_name);}
  /**
   * @brief Register a leaf BT action with its handler callbacks.
   * @param type_name Node type string (convention: <SkillName>_H).
   * @param handlers Callback bundle (on_start/on_running/on_halted).
   * @thread_safety Should be invoked during initialization before concurrent ticking.
   */
  void RegisterBtAction(const std::string & type_name, const BTActionHandlers & handlers);
  /**
   * @brief Register a sequence specification with a symbolic name.
   * @param seq_name Logical sequence key.
   * @param seq Sequence specification (ordered vector of BTActionSpec).
   * @thread_safety Typically called from model logic prior to UpdateBehaviorTree; not thread-safe if BT rebuild is concurrent.
   */
  void RegisterBtSequence(const std::string & seq_name, const BTSequenceSpec & seq)
  {
    bt_registry_->register_bt_sequence(seq_name, seq);
  }
  /**
   * @brief Build a new behavior tree instance from a registered sequence.
   * @param seq_name Sequence key.
   * @return true if build succeeds; false otherwise.
   * @thread_safety Not thread-safe with concurrent ticking; intended for controlled rebuild phases.
   */
  bool BuildTreeFromSequence(const std::string & seq_name)
  {
    return bt_registry_->build_tree_from_sequence(seq_name, tree_);
  }
  /**
   * @brief Mark a sequence as active (does not perform a rebuild).
   * @param seq_name Sequence key.
   * @thread_safety Requires external synchronization with UpdateBehaviorTree / ticking.
   */
  void SetActiveSequence(const std::string & seq_name) {active_sequence_ = seq_name;}

  /**
   * @brief Send ExecuteBtAction through its dedicated action client.
   * @param bt_action_name Typically brain::kExecuteBtActionName.
   * @thread_safety Thread-safe if ActionClientRegistry::send is; goal building uses immutable snapshot of sequence.
   */
  void SendExecuteBt(const std::string & bt_action_name);

  // ---- Public utility types & helpers (exposed for unit tests) ----
  enum class ExecResultCode { UNKNOWN, SUCCESS, FAILURE, CANCELLED };

  /**
   * @brief Pick up to count random action-capable skills.
   * @param count Maximum number of distinct skills to return.
   * @return Vector of selected skill names (size <= count).
   * @thread_safety Reads skill metadata; safe after initialization if SkillManager is immutable.
   */
  std::vector<std::string> PickRandomActionSkills(std::size_t count);

  /**
   * @brief Parse a delimited list into unique tokens (order preserved, duplicates removed).
   * @param raw Raw input string.
   * @return Vector of unique tokens in first-occurrence order.
   * @thread_safety Pure function.
   */
  static std::vector<std::string> ParseListString(const std::string & raw);

  /**
   * @brief Classify textual result detail (prefix or code= token) into ExecResultCode.
   * @param detail Input detail string.
   * @return Parsed ExecResultCode enumeration.
   * @thread_safety Pure function.
   */
  static ExecResultCode ParseResultDetail(const std::string & detail);

private:

  /**
   * Declare ROS 2 parameters used by this node.
   *
   * Declaration is separated from parameter retrieval so the logic for
   * computing defaults (e.g., locating the skills file) happens after all
   * declarations exist. Idempotent only during construction; not intended for
   * repeated invocation at runtime.
   */
  void DeclareParameters();

  /**
   * Resolve the skills definition YAML path.
   *
   * If the 'skills_file' parameter is empty, attempts to fall back to the
   * installed package share directory (brain/config/robot_skills.yaml).
   *
   * Returns: Absolute or relative path string (may be empty on failure).
   */
  std::string ResolveSkillsFile(std::string skills_file_path);

  /**
   * Load skill definitions from a YAML file via SkillManager.
   *
   * Silently returns if the provided path is empty. Emits a warning log if
   * parsing/loading fails. Does not throw.
   *
   * @param skills_file Path to YAML file (may be empty => no-op).
   */
  void LoadSkills(const std::string & skills_file);

  /**
   * Retrieve parameter values and apply post-processing.
   *
   * Tasks:
   *  - Clamp random_skill_pick_count_ to >= 1.
   *  - Parse allowed_action_skills_ list.
   *  - Parse and store fixed skill sequence.
   * Logs summary information for visibility. Assumes all parameters already
   * declared via DeclareParameters().
   */
  void LoadParameters();

  /**
   * Create external service interfaces (currently a Trigger to force BT rebuild).
   *
   * The rebuild service coordinates with atomic flags to avoid unsafe rebuilds
   * when disallowed. Does not itself perform heavy work; instead triggers the
   * same path as the periodic task.
   */
  void CreateServices();

  /** Declare per-skill instance parameter entries used to customize outgoing calls. */
  void UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
  /** Declare per-instance parameter entries used to customize a specific BT node occurrence. */
  void DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
  /** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
  brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
  /** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
  brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name, const std::optional<std::string> & instance_name) const;
  /** Install on_set_parameters callback to validate and accept dynamic updates. */
  void SetupDynamicParameterHandling();

  BT::Tree tree_;
  BT::BehaviorTreeFactory factory_;
  std::unique_ptr<ActionClientRegistry> action_registry_;
  std::unique_ptr<BTRegistry> bt_registry_;
  std::unique_ptr<SkillManager> skill_manager_;
  std::string active_sequence_;
  std::string share_directory_;
  std::string robot_skill_file_path_;
  std::string bt_config_file_path_;
  std::string bt_params_file_path_;
  std::filesystem::file_time_type bt_xml_last_write_time_{}; // last observed mod time (for hot reload)
  rclcpp::TimerBase::SharedPtr task_timer_;
  rclcpp::TimerBase::SharedPtr bt_timer_;
  // Service to trigger immediate rebuild of sequence + BT
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr rebuild_service_;
  std::atomic<bool> bt_updating_{false};
  std::atomic<bool> bt_pending_run_{false};
  // Encapsulated epoch+skill filter
  EpochSkillFilter epoch_filter_;
  // Current sequence skills (kept in original order for logging / statistics).
  std::vector<std::string> current_sequence_skills_;
  // Mutex that protects both per_node_exec_ and epoch_skills_ updates.
  std::mutex exec_mutex_;
  // Removed legacy global pending_bt_action_name_: each node now stores its own skill_name.

  struct PerNodeExecInfo
  {
    bool in_flight {false};
    bool result {false}; // legacy bool success flag for backward compatibility
    rclcpp::Time start_time;
    std::string skill_name; ///< Associated skill name
    double last_progress {0.0};
    enum class CancelState { NONE, REQUESTED, CONFIRMED } cancel_state {CancelState::NONE};
    ExecResultCode result_code {ExecResultCode::UNKNOWN};
    bool awaiting_result {false};              ///< 已收到 END(SUCCESS) 反馈，等待最终 action result
    rclcpp::Time end_feedback_time;            ///< 记录 END(SUCCESS) 到达时间
  };
  std::unordered_map<std::string, PerNodeExecInfo> per_node_exec_;
  // Set of already registered action client names (prevents duplicate registration & duplicate feedback).
  std::unordered_set<std::string> registered_actions_;
  // Per-BT-node execution timeout (seconds).
  double node_timeout_sec_ {5.0};
  // Warning threshold: END(SUCCESS) 与最终 result 回调之间超过该秒数则输出警告日志。
  double result_wait_warn_sec_ {2.0};
  // Allow rebuilding the BT while it is still running (default false to avoid preemption).
  bool allow_bt_rebuild_during_execution_ {false};

  // Parameter: number of random skills to pick (exposed as 'random_skill_pick_count').
  std::size_t random_skill_pick_count_{2};

  // Optional whitelist of skill names allowed to be randomly selected; empty => unrestricted.
  std::vector<std::string> allowed_action_skills_;
  // Fixed skill sequence (if set, random selection is skipped entirely).
  std::optional<std::vector<std::string>> fixed_skill_sequence_;
  /**
   * @brief Cancel the currently active ExecuteBtAction goal (if any).
   * @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
   */
  void CancelActiveExecuteBtGoal();
  /**
   * @brief (Legacy) Update per-epoch allowed skills. Currently a no-op (handled by EpochSkillFilter).
   * @param skills Ignored.
   * @thread_safety Safe (no state mutation).
   */
  void SetEpochSkills(const std::vector<std::string> & skills); // transitional; will delegate to epoch_filter_

  /**
   * @brief Rebuild the behavior tree from the active sequence.
   * @post On success: epoch incremented, per-node state reset, bt_pending_run_ = true.
   * @thread_safety Guarded by atomic flag bt_updating_; not re-entrant.
   */
  void UpdateBehaviorTree();
  /**
   * @brief Build the behavior tree from an XML file.
   * @param xml_file Path to XML file.
   * @thread_safety Not thread-safe with concurrent ticking; intended for controlled rebuild phases.
   */
  void BuildBehaviorTreeFromFile(const std::string & xml_file);
  /**
   * @brief Tick the behavior tree root if a run is pending.
   * @thread_safety Called from a timer callback; not thread-safe against concurrent UpdateBehaviorTree (atomic gating only).
   */
  void ExecuteBehaviorTree();
  /**
   * @brief Ensure ExecuteBtAction client is registered (idempotent).
   * @thread_safety Initialization routine; avoid concurrent calls.
   */
  void RegisterActionClient();
  /**
   * @brief Periodic task to select a new sequence (model-driven) then trigger rebuild.
   * @thread_safety Timer callback; coordinates with UpdateBehaviorTree via atomics and mutex for sequence list.
   */
  void CerebrumTask();

  /**
   * @brief Pick up to count random skills that expose an action interface (filtered by whitelist if provided).
   * @param count Maximum number of skills to return.
   * @return Vector of distinct skill names (size <= count).
   */
  /**
   * @brief Convert ordered skill names into a BT sequence of per-skill nodes.
   *
   * Each skill becomes a <Skill>_H leaf. Unlike the earlier unified-node
   * experiment, we now send one ExecuteBtAction goal per skill (goal contains
   * only that skill) so BT node status aligns with the underlying action
   * result. This preserves fine-grained visibility while avoiding duplicated
   * sequencing inside a single action invocation.
   * @param names Ordered skill list.
   * @return BTSequenceSpec object preserving order.
   * @thread_safety Pure function.
   */
  BTSequenceSpec MakeSequenceFromSkills(const std::vector<std::string> & names);
  /**
   * @brief Build a minimal XML representation of a sequence for diagnostics.
   * @param seq Sequence specification.
   * @return XML string containing a root <Sequence> with leaf nodes.
   * @thread_safety Pure function.
   */
  std::string BuildXmlFromSequence(const BTSequenceSpec & seq) const;
  /**
   * @brief Choose and activate a skill sequence (fixed preferred over random selection).
   * @thread_safety Uses mutex to update current_sequence_skills_.
   */
  void RunVlmModel();
  /**
   * @brief Register per-skill BT action handlers (idempotent per skill).
   *
   * Each handler now directly triggers an ExecuteBtAction goal containing only
   * its skill (single-skill override) instead of only the first node sending a
   * multi-skill goal.
   */
  void RegisterSkillBtActions();

  // Temporary override used so the generic ExecuteBtAction goal builder can
  // send a single-skill goal when a per-skill BT node starts.
  std::optional<std::string> single_skill_goal_override_;
  // When a per-skill BT node starts, capture its BT node instance name for per-instance params.
  std::optional<std::string> single_skill_instance_override_;

  // Handle for dynamic parameter updates (cerebrum.bt.<skill>.*)
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;

};

} // namespace brain

#endif // BRAIN_CEREBRUM_NODE_HPP_
