#ifndef CEREBELLUM_CEREBELLUM_NODE_HPP_
#define CEREBELLUM_CEREBELLUM_NODE_HPP_

#include <atomic>
#include <limits>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <yaml-cpp/yaml.h>
#include "brain_interfaces/action/execute_bt_action.hpp"
#include "brain_interfaces/msg/skill_call.hpp"
#include "brain/action_registry.hpp"
#include "brain/skill_manager.hpp"
#include <smacc2/smacc_signal_detector.hpp>
#include "brain/sm_cerebellum.hpp"  // for CerebellumData::ExecResult
// For from_yaml<T>(YAML::Node)
#include "brain/payload_converters.hpp"

namespace brain
{

class CerebellumNode : public rclcpp::Node
{
public:
  /**
   * @brief Construct the CerebellumNode.
   *
   * Responsibilities:
   *  - Initialize action server/client registries and skill manager.
   *  - Declare & load parameters (timeouts, abort policy, stats interval).
   *  - Load skills YAML (either user-specified or default share path).
   *  - Create statistics publisher & periodic timer (if enabled).
   *  - Register ExecuteBtAction server for external skill sequence execution.
   *  - Optionally spin up the SMACC2 state machine execution context.
   * @param options (optional) ROS 2 node options.
   */
  explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  // Generic action registry to support multiple actions via registration
  std::unique_ptr<ActionServerRegistry> action_registry_;
  std::unique_ptr<ActionClientRegistry> action_clients_;
  std::unique_ptr<SkillManager> skill_manager_;
  std::unique_ptr<BTRegistry> bt_registry_;

  // SMACC2 execution context
  std::unique_ptr<smacc2::SmExecution> sm_exec_;

  // Parameters: abort policy, default action timeout, specific grasp timeout
  bool abort_on_failure_ {true};
  double default_action_timeout_sec_ {60.0};
  double vision_grasp_object_timeout_sec_ {120.0};
  std::string map_save_url_ {"/tmp/humanoid_map"};
  std::string map_save_image_format_ {"pgm"};
  double map_save_free_thresh_ {0.25};
  double map_save_occupied_thresh_ {0.65};
  std::string map_load_url_ {};
  double nav_goal_yaw_offset_ {0.0};
  double nav_goal_distance_tolerance_ {0.5};
  bool slam_enable_mapping_default_ {true};

  // Optional per-skill override timeout (skill_name -> seconds)
  std::unordered_map<std::string, double> skill_timeouts_;

  // Statistics: per-skill success/failure counters & sequence totals
  std::unordered_map<std::string, size_t> skill_success_counts_;
  std::unordered_map<std::string, size_t> skill_failure_counts_;
  size_t total_sequence_success_ {0};
  size_t total_sequence_failure_ {0};
  rclcpp::TimerBase::SharedPtr stats_timer_;
  double stats_log_interval_sec_ {60.0};
  std::mutex stats_mutex_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
  std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
  std::string target_frame_{"bottle"};
  int64_t command_id_{0};
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::string arm_target_frame_{"base_link_leftarm"};
  double arm_transform_timeout_sec_{0.2};

  // Current accepted sequence epoch (from client) used to filter/reject stale goals
  std::atomic<uint32_t> current_epoch_{0};
  // Latest ExecuteBtAction calls (Plan A minimal support); guarded by implicit action server thread confinement
  std::vector<brain_interfaces::msg::SkillCall> last_calls_;

  // Lookup the SkillCall for a given skill name in the latest goal, if any
  const brain_interfaces::msg::SkillCall * FindCallForSkill(const std::string & name) const
  {
    for (const auto & c : last_calls_) {
      if (c.name == name) return &c;
    }
    return nullptr;
  }
  std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
  std::atomic<bool> nav2_last_goal_succeeded_{false};

  // ---- helpers ----
  /**
   * @brief Try to parse the current goal's per-call YAML payload into a typed object.
   * 
   * Looks up the latest ExecuteBtAction.goal.calls entry matching skill_name, and if
   * payload_yaml is non-empty attempts to YAML::Load and convert via brain::from_yaml<T>.
   * On success, writes into 'out' and returns true; on failure returns false and leaves 'out' unchanged.
   */
  template<typename T>
  bool TryParseCallPayload(const std::string & skill_name, T & out) const
  {
    if (last_calls_.empty()) {return false;}
    for (const auto & c : last_calls_) {
      if (c.name == skill_name && !c.payload_yaml.empty()) {
        try {
          auto node = YAML::Load(c.payload_yaml);
          out = brain::from_yaml<T>(node);
          RCLCPP_INFO(rclcpp::Node::get_logger(), "[%s] payload_yaml parse success", skill_name.c_str());
          return true;
        } catch (const std::exception & e) {
          RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
            skill_name.c_str(), e.what());
        }
        break;
      }
    }
    return false;
  }
  /** @brief Install application-specific action hook overrides for skill manager. */
  void ConfigureActionHooks();
  void ConfigureArmSpaceControlHooks();
  void ConfigureArmHooks();
  void ConfigureHandControlHooks();
  void ConfigureCameraTakePhotoHooks();
  void ConfigureLegControlHooks();
  void ConfigureVisionGraspObjectHooks();
  void ConfigureSlamModeHooks();
  void ConfigureNavigateToPoseHooks();
  void ConfigureMoveWaistHooks();
  void ConfigureMoveLegHooks();
  void ConfigureMoveWheelHooks();
  void ConfigureMoveHomeHooks();

  /** @brief Install application-specific service hook overrides. */
  void ConfigureServiceHooks();
  void ConfigureVisionObjectRecognitionHooks();
  void ConfigureMapLoadHooks();
  void ConfigureMapSaveHooks();

  /**
   * @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
   *
   * Strips an optional trailing "_H" suffix from each token (behavior-tree leaf naming
   * convention) but otherwise preserves order and duplicates.
   * Thread-safety: const, no shared state usage.
   * @param raw Input string e.g. "ArmMove,HandOpen  VisionGraspObject_H".
   * @return Vector of parsed skill names with suffix removed.
   */
  std::vector<std::string> ParseSkillSequence(const std::string & raw) const;

  /**
   * @brief Convert CamelCase to snake_case (ASCII letters only).
   * Thread-safety: const.
   * @param s Skill name, e.g. "VisionGraspObject".
   * @return snake_case representation, e.g. "vision_grasp_object".
   */
  std::string ToSnake(const std::string & s) const;

  /**
   * @brief Compute timeout for a skill.
   * Precedence: explicit per-skill override > special-case (VisionGraspObject) > default.
   * Thread-safety: const (reads immutable maps after construction phase).
   * @param skill Skill name.
   * @return Timeout in seconds.
   */
  double GetTimeoutForSkill(const std::string & skill) const;

  /**
   * @brief Publish a single feedback stage message (START, RUN, WAIT, END).
   * Thread-safety: safe (ROS2 publisher is thread-safe; uses atomic epoch).
   * @param gh Goal handle.
   * @param stage Stage label.
   * @param current_skill Current skill name.
   * @param progress Fractional sequence progress in [0,1].
   * @param detail Additional free-form detail (may be empty).
   */
  void PublishFeedbackStage(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & gh,
    const std::string & stage,
    const std::string & current_skill,
    float progress,
    const std::string & detail) const;
  /**
   * @brief Execute one skill (action or service) updating detail and emitting feedback.
   * Dispatches to ExecuteActionSkill / ExecuteServiceSkill based on interface kind.
   * @param skill Skill name.
   * @param chosen_topic Topic / action client key used for actions.
   * @param spec Skill specification metadata.
   * @param detail [out] Filled with result or error detail.
   * @param index 0-based index of this skill in the sequence.
   * @param total_skills Total number of skills in the sequence.
   * @param goal_handle ExecuteBtAction goal handle.
   * @return true if successful, false otherwise.
   */
  bool ExecuteSingleSkill(
    const std::string & skill, const std::string & chosen_topic,
    const SkillSpec & spec, std::string & detail, int index, int total_skills,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Determine interface kind from first interface entry.
   * Example: some_pkg/DoThing.action => "action".
   * @param spec Skill specification.
   * @return Interface kind token ("action", "service" or empty).
   */
  std::string GetInterfaceKind(const SkillSpec & spec) const;

  /**
   * @brief Wait (with retries) for action server readiness emitting WAIT feedback.
   * @param topic Action client topic key.
   * @param skill Skill name for feedback context.
   * @param base_progress Progress before this skill's execution.
   * @param goal_handle Goal handle.
   * @param detail [out] Error detail if unavailable.
   * @return true if server becomes available before retry exhaustion.
   */
  bool WaitForActionServer(
    const std::string & topic,
    const std::string & skill,
    float base_progress,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
    std::string & detail) const;
  /**
   * @brief Execute an action-type skill.
   * Orchestrates START feedback, action server wait, worker thread dispatch, and progress loop.
   * @param skill Skill name.
   * @param topic Resolved action client topic.
   * @param spec Skill specification (currently unused for actions beyond interface detection).
   * @param detail [out] Detail / diagnostics.
   * @param index Skill index in sequence.
   * @param total_skills Total skills count.
   * @param goal_handle Goal handle for feedback.
   * @return true if action succeeds.
   */
  bool ExecuteActionSkill(
    const std::string & skill,
    const std::string & topic,
    const SkillSpec & spec,
    std::string & detail,
    int index,
    int total_skills,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
   * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
   * @param skill Skill name.
   * @param topic Action topic.
   * @param timeout_ns Timeout duration (nanoseconds).
   * @param index Skill index.
   * @param total_skills Total skills count.
   * @param goal_handle Goal handle for feedback publishing.
   * @param detail [out] Detailed status text.
   * @return true if worker reports success.
   */
  bool RunActionSkillWithProgress(
    const std::string & skill,
    const std::string & topic,
    std::chrono::nanoseconds timeout_ns,
    int index,
    int total_skills,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
    std::string & detail);

  /**
   * @brief Execute a service-based skill using SkillManager-configured hooks.
   * @param skill Skill name.
  * @param spec Skill specification (interfaces used to resolve service base).
  * @param detail [out] Detail / diagnostics.
  * @param start_progress Sequence progress prior to executing the service.
  * @param goal_handle Goal handle (used for feedback publishing).
   * @return true if successful.
   */
  bool ExecuteServiceSkill(
    const std::string & skill,
    const SkillSpec & spec,
    std::string & detail,
    float start_progress,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Record per-skill success/failure counts (thread-safe via mutex).
   * @param skill Skill name.
   * @param success True if skill succeeded.
   */
  void RecordSkillResult(const std::string & skill, bool success);
  /** @brief Periodically log and publish aggregate statistics snapshot. */
  void LogStatsPeriodic();
  /**
   * @brief Process an accepted ExecuteBtAction goal end-to-end.
   * Validates goal, executes skills in order (respecting abort policy), publishes END feedback.
   * This function is now wrapped into the state machine via execute_fn; kept private for internal call.
   * @param goal_handle Goal handle.
   */
  void RunExecuteBtGoal(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);

  /**
   * @brief Execute the full skill sequence building a CerebellumData::ExecResult summary.
   * (Refactored facade used by execute_fn so state machine can own lifecycle.)
   * @param goal_handle ExecuteBtAction goal handle.
   * @param skills Parsed ordered skill list.
   * @return CerebellumData::ExecResult aggregated result.
   */
  brain::CerebellumData::ExecResult ExecuteSequence(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
    const std::vector<std::string> & skills);
  // --- Decomposed helpers for RunExecuteBtGoal ---
  /**
   * @brief Validate goal and parse action_name into skill list.
   * @param goal_handle Goal handle.
   * @param skills_out [out] Parsed skills.
   * @return true if goal is valid; false otherwise (goal aborted internally).
   */
  bool ValidateAndParseGoal(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
    std::vector<std::string> & skills_out) const;
  /**
   * @brief Resolve action topic name for a skill.
   * Prefers snake_case, falls back to original name if client registered directly.
   * @param skill Skill name.
   * @return Topic / action client key.
   */
  std::string ResolveTopicForSkill(const std::string & skill) const;
  /**
   * @brief Finalize goal with summary result (success or abort) and log message.
   * @param goal_handle Goal handle.
   * @param overall_ok True if whole sequence succeeded.
   * @param succeeded_skills Number of succeeded skills.
   * @param total_skills Total skills count.
   * @param summary_stream Accumulated summary text.
   */
  [[deprecated("Result finalization now handled inside state machine complete_cb; kept for backward compatibility")]]
  void FinalizeExecuteBtResult(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
    bool overall_ok,
    int32_t succeeded_skills,
    int32_t total_skills,
    const std::ostringstream & summary_stream);
  /** @brief Declare parameters and load initial values. */
  void DeclareAndLoadParameters();
  /** @brief Load skill definitions from YAML file (path via parameter or default). */
  void LoadSkillsFile();
  /** @brief Setup periodic stats timer and publisher if interval > 0. */
  void SetupStatsTimerAndPublisher();
  /** @brief Register ExecuteBtAction server with goal/execute handlers. */
  void SetupExecuteBtServer();

  /** @brief 将检测到的姿态转换到机械臂参考坐标系。 */
  bool TransformPoseToArmFrame(
    const geometry_msgs::msg::PoseStamped & source,
    geometry_msgs::msg::PoseStamped & transformed) const;

  // NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.

};

} // namespace brain

#endif // CEREBELLUM_CEREBELLUM_NODE_HPP_
