// Copyright (c) 2025
// This file implements the CerebellumNode which hosts execution infrastructure
// for skill/action/service orchestration.

// Own header first.
#include "brain/cerebellum_node.hpp"

// C / C++ standard library.
#include <chrono>
#include <algorithm>
#include <cctype>
#include <cmath>
#include <future>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include <vector>

// Third-party / external libraries (ROS2, ament).
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

// Project headers.
#include "brain/action_registry.hpp"
#include "brain/sm_cerebellum.hpp"
#include "brain/constants.hpp"

// Skill interfaces (clients are registered by SkillManager).
#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/leg_control.hpp"
#include "interfaces/action/arm.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
#include "brain/payload_converters.hpp"
#include "interfaces/action/slam_mode.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/map_load.hpp"
#include <std_msgs/msg/string.hpp>
#include <tf2/exceptions.h>
#include <tf2/time.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

namespace brain
{
static thread_local int tls_arm_body_id = -1;

/**
 * @brief Construct the CerebellumNode.
 *
 * Initializes internal registries (action server/client, skill manager), declares and
 * loads parameters, loads the skills YAML, sets up statistics publisher/timer, registers
 * the ExecuteBtAction action server, and optionally launches the SMACC2 state machine
 * unless disabled at compile time via BRAIN_DISABLE_SM.
 * @param options Optional ROS 2 node options.
 */
CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
: Node("cerebellum_node", options)
{
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  tf_buffer_->setUsingDedicatedThread(true);
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
  action_registry_ = std::make_unique<ActionServerRegistry>(this);
  action_clients_ = std::make_unique<ActionClientRegistry>(this);
  skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);

  DeclareAndLoadParameters();
  // Register action/service hook overrides first so SkillManager will pick them up
  ConfigureActionHooks();
  ConfigureServiceHooks();
  // Then load skills which will register action/service clients and honor the overrides
  LoadSkillsFile();
  SetupStatsTimerAndPublisher();
  SetupExecuteBtServer();

#ifndef BRAIN_DISABLE_SM
  if (!sm_exec_) {
    sm_exec_.reset(smacc2::run_async<brain::SmCerebellum>());
    RCLCPP_INFO(this->get_logger(), "SMACC2 SmCerebellum started");
  }
#else
  RCLCPP_INFO(
    this->get_logger(), "SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
#endif
}

/**
 * @brief Configure action hooks for arm control.
 * 
 */
void CerebellumNode::ConfigureArmHooks()
{
  // Arm
  SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::Arm::Goal goal{};
      // Plan A: per-call YAML overrides
      (void)TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal);

      RCLCPP_INFO(
        this->get_logger(), "Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
        goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
        goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);


      goal.body_id = (tls_arm_body_id == 0 || tls_arm_body_id == 1) ? tls_arm_body_id : 0;   // LEFT_ARM=0, RIGHT_ARM=1
      goal.data_type = 3; //ARM_COMMAND_TYPE_POSE_DIRECT_MOVE
      goal.command_id = ++command_id_;

      if (target_pose_.empty()) {
        RCLCPP_WARN(this->get_logger(), "[%s] 未检测到目标物体，使用默认位置", skill_name.c_str());
        // default position
        // goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
        goal.data_array = {0.222853, 0.514124, 0.261742,
          -0.65115316, 0.05180144, -0.19539139, 0.73153153};
        goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
      } else {
        RCLCPP_INFO(this->get_logger(), "[%s] 使用检测到的目标物体位置", skill_name.c_str());
        const auto pose_it = target_pose_.find(target_frame_);
        if (pose_it == target_pose_.end()) {
          RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
            skill_name.c_str(), target_frame_.c_str());
          // goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
          goal.data_array = {0.222853, 0.514124, 0.261742,
            -0.65115316, 0.05180144, -0.19539139, 0.73153153};
          goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
        } else {
          geometry_msgs::msg::PoseStamped pose_in_arm;
          const bool transformed = TransformPoseToArmFrame(pose_it->second, pose_in_arm);
          const auto & pose_ref = transformed ? pose_in_arm : pose_it->second;
          if (!transformed) {
            RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
              skill_name.c_str(), pose_ref.header.frame_id.c_str());
          } else {
            RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
              skill_name.c_str(), arm_target_frame_.c_str());
          }
          goal.data_array = {
            pose_ref.pose.position.x,
            pose_ref.pose.position.y,
            pose_ref.pose.position.z,
            pose_ref.pose.orientation.x,
            pose_ref.pose.orientation.y,
            pose_ref.pose.orientation.z,
            pose_ref.pose.orientation.w
          };
          goal.frame_time_stamp = pose_ref.header.stamp.sec * 1000000000LL
            + pose_ref.header.stamp.nanosec;
        }
        target_pose_.clear();
      }
      goal.data_length = static_cast<int32_t>(goal.data_array.size());

      RCLCPP_INFO(this->get_logger(), "[%s] 目标参数 body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
        skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
        goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);

      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
      const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
      if (!feedback) {
        RCLCPP_WARN(this->get_logger(), "[%s] feedback 为空", skill_name.c_str());
        return;
      }
      (void)feedback;
      //TODO :
      RCLCPP_INFO(this->get_logger(), "[%s] command_id: %ld, current progress: %d", 
        skill_name.c_str(), feedback->command_id, feedback->int_lenth);

    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
      const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0);
      const std::string message = res.result ? std::string("action end") : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] command_id %ld 完成: %s", skill_name.c_str(), res.result->command_id, message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
}

/**
 * @brief Configure action hooks for hand control.
 * 
 */
void CerebellumNode::ConfigureHandControlHooks()
{
  // HandControl
  SkillManager::ActionHookOptions<interfaces::action::HandControl> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::HandControl::Goal goal{};
      // Plan A: per-call YAML overrides
      (void)TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal);
      const std::string param = "hand_control.target_pose";
      if (!this->has_parameter(param)) {
        this->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
      }
      const auto values = this->get_parameter(param).as_double_array();
      RCLCPP_INFO(this->get_logger(), "[%s] 目标参数 size=%zu", skill_name.c_str(), values.size());
      (void)values;
      goal.mode = 1;
      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::SharedPtr,
      const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
      if (!feedback) {return;}
      (void)feedback;
      //TODO :
      RCLCPP_INFO(this->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
}

/**
 * @brief Configure action hooks for waist movement.
 * 
 */
void CerebellumNode::ConfigureMoveWaistHooks()
{
  // MoveWaist
  SkillManager::ActionHookOptions<interfaces::action::MoveWaist> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::MoveWaist::Goal goal{};
      // Plan A: load per-call YAML payload into goal if present
      (void)TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal);

      RCLCPP_INFO(this->get_logger(), "[%s] move_pitch_degree: %f, move_yaw_degree: %f", skill_name.c_str(), goal.move_pitch_degree, goal.move_yaw_degree);
      
      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::SharedPtr,
      const std::shared_ptr<const interfaces::action::MoveWaist::Feedback> & feedback) {
      if (!feedback) {return;}
      //TODO
      
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
}

/**
 * @brief Configure action hooks for leg movement.
 * 
 */
void CerebellumNode::ConfigureMoveLegHooks()
{
  // MoveLeg
  SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::MoveLeg::Goal goal{};
      (void)TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal);
      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::SharedPtr,
      const std::shared_ptr<const interfaces::action::MoveLeg::Feedback> & feedback) {
      if (!feedback) {return;}
      //TODO
      
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
}

/**
 * @brief Configure action hooks for wheel movement.
 * 
 */
void CerebellumNode::ConfigureMoveWheelHooks()
{
  // MoveWheel
  SkillManager::ActionHookOptions<interfaces::action::MoveWheel> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::MoveWheel::Goal goal{};
      // Plan A: per-call YAML overrides
      (void)TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal);

      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::SharedPtr,
      const std::shared_ptr<const interfaces::action::MoveWheel::Feedback> & feedback) {
      if (!feedback) {return;}
      //TODO
      
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
}

/**
 * @brief Configure action hooks for home movement.
 * 
 */
void CerebellumNode::ConfigureMoveHomeHooks()
{
  // MoveHome
  SkillManager::ActionHookOptions<interfaces::action::MoveHome> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::MoveHome::Goal goal{};
      // Plan A: per-call YAML overrides (even though goal is empty)
      (void)TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal);

      return goal;
    };
  hooks.on_feedback = [this](
      const std::string & skill_name,
      ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::SharedPtr,
      const std::shared_ptr<const interfaces::action::MoveHome::Feedback> & feedback) {
      if (!feedback) {return;}
      //TODO
      
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
}

/**
 * @brief Configure action hooks for various movements.
 * 
 */
void CerebellumNode::ConfigureActionHooks()
{
  if (!skill_manager_) {
    RCLCPP_WARN(this->get_logger(), "SkillManager 不可用，跳过动作钩子配置");
    return;
  }
  ConfigureArmHooks();
  ConfigureHandControlHooks();
  ConfigureCameraTakePhotoHooks();
  ConfigureMoveWaistHooks();
  ConfigureMoveLegHooks();
  ConfigureMoveWheelHooks();
  ConfigureMoveHomeHooks();
}

/**
 * @brief Configure action hooks for vision object recognition.
 * 
 */
void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
{
  using VisionSrv = interfaces::srv::VisionObjectRecognition;
  {
    SkillManager::ServiceHookOptions<VisionSrv> hooks;
    hooks.wait_timeout = [](const std::string &) {
        return std::chrono::seconds(2);
      };
    hooks.call_timeout = [](const std::string &) {
        return std::chrono::seconds(5);
      };
    hooks.make_request = [this](const std::string & skill_name) {
          auto req = std::make_shared<VisionSrv::Request>();
          // Prefer payload_yaml from calls
          if (TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
            return req;
          }
        // fallback to parameter
        const std::string param = "vision_object_recognition.camera_position";
        if (!this->has_parameter(param)) {
          this->declare_parameter<std::string>(param, std::string("left"));
        }
        req->camera_position = this->get_parameter(param).as_string();
        return req;
      };
    hooks.on_response = [this](
        const std::string & skill_name,
        const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
        const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
        std::string & detail) {
        if (!resp->success) {
          detail = resp->info.empty() ? std::string("Not success") : resp->info;
          RCLCPP_WARN(this->get_logger(), "[%s] VisionObjectRecognition failed: %s",
            skill_name.c_str(), detail.c_str());
          return false;
        }
        detail = resp->info.empty() ? std::string("success pose") : resp->info;
        const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
        RCLCPP_INFO(
          this->get_logger(),
          "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
          skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(),
          stamp_sec, resp->info.c_str());
        for (size_t i = 0; i < resp->objects.size(); ++i) {
          const auto & obj = resp->objects[i];

          //判断target_pose_的key是否等于obj.class_name，如果相等，将value存入target_pose_的PostStamped
          if (target_frame_ == obj.class_name) {
            target_pose_[target_frame_].header = resp->header;
            target_pose_[target_frame_].pose = obj.pose_list[0];
            RCLCPP_INFO(this->get_logger(), "target_frame_: %s, updated target_pose", target_frame_.c_str());
          }

          RCLCPP_INFO(
            this->get_logger(), "  [%zu] class='%s' id=%d poses=%zu", i,
            obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
          for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
            const auto & p = obj.pose_list[j];
            RCLCPP_INFO(
              this->get_logger(),
              "    pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
              p.position.x, p.position.y, p.position.z,
              p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
          }
          if (obj.pose_list.size() > 2) {
            RCLCPP_INFO(
              this->get_logger(), "    ... (%zu more poses omitted)", obj.pose_list.size() - 2);
          }
        }
        return true;
      };
    skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
  }
}

void CerebellumNode::ConfigureCameraTakePhotoHooks()
{
  // CameraTakePhoto
  SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::CameraTakePhoto::Goal goal{};
      (void)TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal);
      return goal;
    };
  hooks.on_result = [this](
      const std::string & skill_name,
      const ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>::WrappedResult & res) {
      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
      const std::string message = res.result ? res.result->message : std::string("无返回信息");
      if (success) {
        RCLCPP_INFO(this->get_logger(), "[%s] 成功: %s", skill_name.c_str(), message.c_str());
      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
        RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
      } else {
        RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
      }
    };
  hooks.on_goal_response = [this](
      const std::string & skill_name,
      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
      if (static_cast<bool>(handle)) {
        RCLCPP_INFO(this->get_logger(), "[%s] goal 已被服务器接收", skill_name.c_str());
      } else {
        RCLCPP_WARN(this->get_logger(), "[%s] goal 被服务器拒绝", skill_name.c_str());
      }
    };
  skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
}

void CerebellumNode::ConfigureMapLoadHooks()
{
  using MapLoadSrv = interfaces::srv::MapLoad;
  SkillManager::ServiceHookOptions<MapLoadSrv> hooks;
  hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
  hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
  hooks.make_request = [this](const std::string & skill_name) {
      auto req = std::make_shared<MapLoadSrv::Request>();
      if (TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
      // Fallback to parameter
      if (!this->has_parameter("map_load_url")) {
        this->declare_parameter<std::string>("map_load_url", std::string(""));
      }
      req->map_url = this->get_parameter("map_load_url").as_string();
      return req;
    };
  hooks.on_response = [this](
      const std::string & skill_name,
      const SkillManager::ServiceHookOptions<MapLoadSrv>::ClientPtr &,
      const SkillManager::ServiceHookOptions<MapLoadSrv>::ResponsePtr & resp,
      std::string & detail) {
      // MapLoad response schema may not include a message field; rely on success only
      detail = resp->success ? std::string("ok") : std::string("failed");
      if (!resp->success) {
        RCLCPP_WARN(this->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
      } else {
        RCLCPP_INFO(this->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
      }
      return resp->success;
    };
  skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(hooks));
}

void CerebellumNode::ConfigureMapSaveHooks()
{
  using MapSaveSrv = interfaces::srv::MapSave;
  SkillManager::ServiceHookOptions<MapSaveSrv> hooks;
  hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
  hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
  hooks.make_request = [this](const std::string & skill_name) {
      auto req = std::make_shared<MapSaveSrv::Request>();
      if (TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
      // Fallback to parameter group
      if (!this->has_parameter("map_save_url")) {
        this->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
      }
      req->map_url = this->get_parameter("map_save_url").as_string();
      return req;
    };
  hooks.on_response = [this](
      const std::string & skill_name,
      const SkillManager::ServiceHookOptions<MapSaveSrv>::ClientPtr &,
      const SkillManager::ServiceHookOptions<MapSaveSrv>::ResponsePtr & resp,
      std::string & detail) {
      // MapSave response schema may not include a message field; rely on success only
      detail = resp->success ? std::string("ok") : std::string("failed");
      if (!resp->success) {
        RCLCPP_WARN(this->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
      } else {
        RCLCPP_INFO(this->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
      }
      return resp->success;
    };
  skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(hooks));
}

/**
 * @brief Configure Service hooks for various movements.
 * 
 */
void CerebellumNode::ConfigureServiceHooks()
{
  if (!skill_manager_) {
    RCLCPP_WARN(this->get_logger(), "SkillManager 不可用，跳过服务钩子配置");
    return;
  }
  ConfigureVisionObjectRecognitionHooks();
  ConfigureMapLoadHooks();
  ConfigureMapSaveHooks();
}

/**
 * @brief Declare all configurable ROS parameters and load their initial values.
 *
 * Also parses the consolidated skill timeout override string parameter (skill_timeouts)
 * formatted as "SkillA=30,SkillB=10" into the internal map used by GetTimeoutForSkill.
 * Missing or malformed entries are skipped with a warning.
 */
void CerebellumNode::DeclareAndLoadParameters()
{
  // Core behavior parameters
  this->declare_parameter<std::string>("skills_file", "");
  this->declare_parameter<bool>("abort_on_failure", abort_on_failure_);
  this->declare_parameter<double>("default_action_timeout_sec", default_action_timeout_sec_);
  this->declare_parameter<double>(
    "vision_grasp_object_timeout_sec",
    vision_grasp_object_timeout_sec_);
  this->declare_parameter<std::string>("skill_timeouts", ""); // format: SkillA=30,SkillB=10
  this->declare_parameter<double>("stats_log_interval_sec", stats_log_interval_sec_);
  // this->declare_parameter<std::string>("skill_topic_prefix", "humanoid");
  this->declare_parameter<std::string>("map_save_url", map_save_url_);
  this->declare_parameter<std::string>("map_save_image_format", map_save_image_format_);
  this->declare_parameter<double>("map_save_free_thresh", map_save_free_thresh_);
  this->declare_parameter<double>("map_save_occupied_thresh", map_save_occupied_thresh_);
  this->declare_parameter<std::string>("map_load_url", map_load_url_);
  this->declare_parameter<double>("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
  this->declare_parameter<double>("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
  this->declare_parameter<bool>("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
  this->declare_parameter<std::string>("arm.target_pose_target_frame", arm_target_frame_);
  this->declare_parameter<double>("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
  // Read back
  this->get_parameter("abort_on_failure", abort_on_failure_);
  this->get_parameter("default_action_timeout_sec", default_action_timeout_sec_);
  this->get_parameter("vision_grasp_object_timeout_sec", vision_grasp_object_timeout_sec_);
  this->get_parameter("stats_log_interval_sec", stats_log_interval_sec_);
  this->get_parameter("map_save_url", map_save_url_);
  this->get_parameter("map_save_image_format", map_save_image_format_);
  this->get_parameter("map_save_free_thresh", map_save_free_thresh_);
  this->get_parameter("map_save_occupied_thresh", map_save_occupied_thresh_);
  this->get_parameter("map_load_url", map_load_url_);
  this->get_parameter("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
  this->get_parameter("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
  this->get_parameter("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
  this->get_parameter("arm.target_pose_target_frame", arm_target_frame_);
  this->get_parameter("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
  arm_transform_timeout_sec_ = std::max(0.01, arm_transform_timeout_sec_);
  map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);
  map_save_occupied_thresh_ = std::clamp(map_save_occupied_thresh_, 0.0, 1.0);
  nav_goal_distance_tolerance_ = std::max(0.0, nav_goal_distance_tolerance_);

  // Parse skill_timeouts param into map
  std::string raw;
  this->get_parameter("skill_timeouts", raw);
  if (!raw.empty()) {
    std::string token; token.reserve(raw.size());
    auto flush = [&]() {
        if (token.empty()) {return;}
        auto eq = token.find('=');
        if (eq != std::string::npos) {
          auto key = token.substr(0, eq);
          auto vstr = token.substr(eq + 1);
          try {
            skill_timeouts_[key] = std::stod(vstr);
          } catch (...) {
            RCLCPP_WARN(
              this->get_logger(), "Bad timeout value for %s: %s", key.c_str(),
              vstr.c_str());
          }
        }
        token.clear();
      };
    for (char c : raw) {
      if (c == ',' || c == ';' || c == ' ' || c == '\t') {
        flush();
      } else {
        token.push_back(c);
      }
    }
    flush();
    if (!skill_timeouts_.empty()) {
      RCLCPP_INFO(
        this->get_logger(), "[cerebellum_node] Loaded %zu skill-specific timeouts",
        skill_timeouts_.size());
    }
  }
  RCLCPP_INFO(
    this->get_logger(), "[cerebellum_node] abort_on_failure=%s default_action_timeout=%.1f vision_grasp_object_timeout=%.1f",
    abort_on_failure_ ? "true" : "false", default_action_timeout_sec_,
    vision_grasp_object_timeout_sec_);
}

/**
 * @brief Load the skills definition YAML file.
 *
 * If parameter 'skills_file' is empty, attempts to locate a default file within the
 * package share directory (config/robot_skills.yaml). Each successfully loaded skill
 * name is logged for visibility. Failure to load results in a warning but is non-fatal.
 */
void CerebellumNode::LoadSkillsFile()
{
  std::string skills_file;
  this->get_parameter("skills_file", skills_file);
  if (skills_file.empty()) {
    try {
      const auto share = ament_index_cpp::get_package_share_directory("brain");
      skills_file = share + std::string("/config/robot_skills.yaml");
      RCLCPP_INFO(
        this->get_logger(), "[cerebellum_node] Using default skills file: %s",
        skills_file.c_str());
    } catch (const std::exception & e) {
      RCLCPP_WARN(
        this->get_logger(), "[cerebellum_node] Could not locate default skills file: %s",
        e.what());
    }
  }
  if (skills_file.empty()) {return;}
  if (!skill_manager_->load_from_file(skills_file)) {
    RCLCPP_WARN(
      this->get_logger(), "[cerebellum_node] Failed to load skills from %s", skills_file.c_str());
    return;
  }
  for (const auto & kv : skill_manager_->skills()) {
    RCLCPP_INFO(this->get_logger(), "[cerebellum_node] Loaded skill '%s'", kv.first.c_str());
  }
}

/**
 * @brief Create the statistics publisher and periodic logging timer.
 *
 * The timer is only created when stats_log_interval_sec_ is strictly positive. Each tick
 * invokes LogStatsPeriodic to publish and log aggregate counts.
 */
void CerebellumNode::SetupStatsTimerAndPublisher()
{
  stats_pub_ = this->create_publisher<std_msgs::msg::String>("/brain/skill_stats", 10);
  if (stats_log_interval_sec_ > 0) {
    stats_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(stats_log_interval_sec_),
      [this]() {LogStatsPeriodic();});
  }
}

/**
 * @brief Register the ExecuteBtAction action server.
 *
 * Goal callback enforces monotonic epoch progression (rejecting stale requests) and posts
 * an SmACC2 EvGoalReceived event when available. Execution callback delegates to
 * RunExecuteBtGoal which performs the ordered skill execution logic.
 */
void CerebellumNode::SetupExecuteBtServer()
{
  using EBA = brain_interfaces::action::ExecuteBtAction;
  ActionServerRegistry::Handlers<EBA> h;
  // Track active goal handle for cancellation handling
  static std::weak_ptr<rclcpp_action::ServerGoalHandle<EBA>> active_goal_wptr;
  h.on_goal = [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const EBA::Goal> goal) {
      if (!goal) {return rclcpp_action::GoalResponse::REJECT;}
      if (goal->epoch < current_epoch_) {
        RCLCPP_WARN(
          this->get_logger(), "Reject stale ExecuteBtAction goal epoch=%u < current=%u", goal->epoch,
          current_epoch_.load());
        return rclcpp_action::GoalResponse::REJECT;
      }
      if (goal->epoch > current_epoch_) {
        current_epoch_ = goal->epoch;
        RCLCPP_INFO(this->get_logger(), "Switch to new epoch %u", current_epoch_.load());
      }
      if (sm_exec_ && sm_exec_->signalDetector) {
        sm_exec_->signalDetector->postEvent(new brain::EvGoalReceived());
      }
      // Log detailed goal contents to help debugging call propagation from Cerebrum
      RCLCPP_INFO(this->get_logger(), "ExecuteBtAction GoalReceived epoch=%u action_name=%s calls_count=%zu",
        goal->epoch, goal->action_name.c_str(), goal->calls.size());
      for (size_t i = 0; i < goal->calls.size(); ++i) {
        const auto & c = goal->calls[i];
        RCLCPP_INFO(this->get_logger(), "  call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
          i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
        if (!c.payload_yaml.empty()) {
          // Log payload content (trim if very long)
          const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml;
          RCLCPP_INFO(this->get_logger(), "    payload: %s", payload.c_str());
        }
      }
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    };
  // Accept cancel requests: set runtime preempt flag; the execute loop will notice and stop
  h.on_cancel = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<EBA>> gh) {
      auto & d = brain::runtime();
      d.preempt.store(true, std::memory_order_release);
      d.preempt_reason = "Cancelled";
      RCLCPP_WARN(this->get_logger(), "[ExecuteBtAction] cancellation requested (epoch=%u)", gh->get_goal()->epoch);
      return rclcpp_action::CancelResponse::ACCEPT;
    };
  h.execute = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<EBA>> goal_handle) {
      // Instead of executing inline, install execute_fn & complete_cb for state machine.
      active_goal_wptr = goal_handle;
      RunExecuteBtGoal(goal_handle);
    };
  action_registry_->register_action_server<EBA>(brain::kExecuteBtActionName, h);
}

bool CerebellumNode::TransformPoseToArmFrame(
  const geometry_msgs::msg::PoseStamped & source,
  geometry_msgs::msg::PoseStamped & transformed) const
{
  if (!tf_buffer_) {
    RCLCPP_ERROR(this->get_logger(), "TF buffer 未初始化，无法转换目标点位");
    return false;
  }
  if (source.header.frame_id.empty()) {
    RCLCPP_WARN(this->get_logger(), "目标姿态缺少来源坐标系，跳过转换");
    return false;
  }
  if (source.header.frame_id == arm_target_frame_) {
    RCLCPP_INFO(this->get_logger(), "目标姿态已位于 %s 坐标系，跳过转换", arm_target_frame_.c_str());
    transformed = source;
    return true;
  }
  try {
    transformed = tf_buffer_->transform(
      source, arm_target_frame_, tf2::durationFromSec(arm_transform_timeout_sec_));
    RCLCPP_INFO(
      this->get_logger(), "目标姿态从 %s 转换至 %s",
      source.header.frame_id.c_str(), arm_target_frame_.c_str());
    return true;
  } catch (const tf2::TransformException & ex) {
    RCLCPP_ERROR(
      this->get_logger(), "转换目标姿态失败: %s -> %s, 原因: %s",
      source.header.frame_id.c_str(), arm_target_frame_.c_str(), ex.what());
  }
  return false;
}

/**
 * @brief Parse a raw delimited skill list string into individual skill tokens.
 *
 * Accepted delimiters: comma, semicolon, space, tab. Removes a trailing '_H' suffix from
 * each token to normalize behavior-tree leaf decorated names back to canonical skill names.
 * @param raw Input raw skill list string.
 * @return Vector of parsed (possibly duplicate) canonical skill names.
 */
std::vector<std::string> CerebellumNode::ParseSkillSequence(
  const std::string & raw) const
{
  std::vector<std::string> skills;
  skills.reserve(8);
  std::string token;
  token.reserve(raw.size());
  for (char c : raw) {
    if (c == ',' || c == ';' || c == ' ' || c == '\t') {
      if (!token.empty()) {
        skills.push_back(token);
        token.clear();
      }
    } else {
      token.push_back(c);
    }
  }
  if (!token.empty()) {skills.push_back(token);}
  // Trim optional suffix _H.
  for (auto & s : skills) {
    if (s.size() > 2 && s.rfind("_H") == s.size() - 2) {
      s.erase(s.size() - 2);
    }
  }
  return skills;
}

/**
 * @brief Convert a CamelCase identifier to snake_case.
 *
 * Inserts underscores before interior uppercase letters and lowercases all characters.
 * @param s CamelCase input string.
 * @return snake_case transformed string.
 */
std::string CerebellumNode::ToSnake(const std::string & s) const
{
  std::string out;
  out.reserve(s.size() * 2);
  for (size_t i = 0; i < s.size(); ++i) {
    unsigned char uc = static_cast<unsigned char>(s[i]);
    if (::isupper(uc)) {
      if (i > 0) {out.push_back('_');}
      out.push_back(static_cast<char>(::tolower(uc)));
    } else {
      out.push_back(static_cast<char>(uc));
    }
  }
  return out;
}

/**
 * @brief Resolve timeout value for a specific skill.
 *
 * Precedence order: explicit per-skill override map > VisionGraspObject special timeout >
 * default configured timeout.
 * @param skill Skill name.
 * @return Timeout in seconds for the given skill.
 */
double CerebellumNode::GetTimeoutForSkill(const std::string & skill) const
{
  auto it = skill_timeouts_.find(skill);
  if (it != skill_timeouts_.end()) {return it->second;}
  if (skill == "VisionGraspObject") {return vision_grasp_object_timeout_sec_;}
  return default_action_timeout_sec_;
}

/**
 * @brief Publish one stage feedback message for ExecuteBtAction.
 *
 * Wraps construction and publishing of the feedback object so callers only
 * provide the semantic fields. Epoch is filled from current_epoch_.
 * @param gh Goal handle to publish feedback on.
 * @param stage Stage label (START, RUN, WAIT, END).
 * @param current_skill Skill name currently processed.
 * @param progress Overall fractional progress in current sequence [0,1].
 * @param detail Optional detail string (may encode status or diagnostics).
 */
void CerebellumNode::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
{
  auto fb = std::make_shared<brain_interfaces::action::ExecuteBtAction::Feedback>();
  fb->epoch = current_epoch_;
  fb->stage = stage;
  fb->current_skill = current_skill;
  fb->progress = progress;
  fb->detail = detail;
  gh->publish_feedback(fb);
}

/**
 * @brief Execute a single skill (action or service) with feedback and timeout handling.
 *
 * Delegates to ExecuteActionSkill or ExecuteServiceSkill depending on interface kind.
 * Unknown kinds reported as failure. Emits mandatory END feedback on failure paths.
 * @param skill Skill name.
 * @param chosen_topic Resolved action client topic key.
 * @param spec Skill specification metadata (interface list, etc.).
 * @param detail (out) Additional status information; may remain empty if OK.
 * @param index Zero-based index of this skill in the current sequence.
 * @param total_skills Total number of skills in the sequence.
 * @param goal_handle ExecuteBtAction goal handle for feedback.
 * @return true if execution succeeded, else false.
 */
bool CerebellumNode::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)
{
  if (spec.interfaces.empty()) {
    detail = "No interfaces";
    return false;
  }
  const std::string kind = GetInterfaceKind(spec);
  if (kind == "action") {
    return ExecuteActionSkill(
      skill, chosen_topic, spec, detail, index, total_skills, goal_handle);
  }
  if (kind == "service") {
    const float base_progress =
      (total_skills > 0) ? (static_cast<float>(index) / static_cast<float>(total_skills)) : 0.f;
    PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");
    return ExecuteServiceSkill(skill, spec, detail, base_progress, goal_handle);
  }
  detail = "Unknown interface kind";
  PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
  return false;
}

/**
 * @brief Extract lowercase interface kind string from first interface entry.
 *
 * Example interface tokens: package/Type.action -> kind = "action".
 * Returns empty string if specification has no interfaces.
 * @param spec Skill specification.
 * @return std::string Lowercase kind token (e.g., "action", "service").
 */
std::string CerebellumNode::GetInterfaceKind(const SkillSpec & spec) const
{
  if (spec.interfaces.empty()) {return {};}
  std::string first = spec.interfaces.front();
  auto pos = first.rfind('.');
  std::string kind = (pos == std::string::npos) ? std::string{} : first.substr(pos + 1);
  for (auto & c : kind) {
    c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
  }
  // Normalize common variants
  if (kind == "service") return "service";
  if (kind == "srv") return "service";
  return kind;
}

/**
 * @brief Retry-wait loop for action server availability with WAIT feedback.
 *
 * Attempts up to kMaxAttempts, each attempt waiting a short period. Publishes
 * WAIT feedback containing the attempt number. On success returns true.
 * @param topic Action client key/topic string.
 * @param skill Skill name (for feedback context).
 * @param base_progress Fractional progress before this skill runs.
 * @param goal_handle ExecuteBtAction goal handle for feedback.
 * @param detail (out) Set to error text on failure.
 * @return true if server became available; false otherwise.
 */
bool CerebellumNode::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
{
  constexpr int kMaxAttempts = 8;
  for (int attempt = 1; attempt <= kMaxAttempts; ++attempt) {
    if (action_clients_->is_server_available(topic, std::chrono::milliseconds(50))) {
      return true;
    }
    PublishFeedbackStage(
      goal_handle, "WAIT", skill, base_progress,
      std::string("attempt=") + std::to_string(attempt));
    std::this_thread::sleep_for(std::chrono::milliseconds(250));
  }
  detail = "Server unavailable after retries";
  return false;
}

/**
 * @brief Execute an action-based skill with periodic RUN feedback.
 *
 * Spawns a worker thread to perform the blocking dispatch_skill_action call.
 * Main thread emits RUN feedback until completion or timeout. Progress scales
 * within the skill's time slice based on elapsed/timeout.
 * @param skill Skill name.
 * @param topic Resolved action client topic.
 * @param spec Skill spec (currently unused beyond interface detection).
 * @param detail (out) Populated with result detail or error.
 * @param index Index of skill in sequence.
 * @param total_skills Total skills count.
 * @param goal_handle Goal handle for feedback.
 * @return true on success, false otherwise.
 */
bool CerebellumNode::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)
{
  (void)spec;  // Currently unused beyond interface kind selection.
  if (!action_clients_->has_client(topic)) {
    detail = "Client not registered";
    RCLCPP_ERROR(this->get_logger(), "[%s] %s, TOPIC: %s", skill.c_str(), detail.c_str(), topic.c_str());
    return false;
  }
  const float base_progress =
    (total_skills > 0) ? (static_cast<float>(index) / static_cast<float>(total_skills)) : 0.f;
  PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");
  if (!WaitForActionServer(topic, skill, base_progress, goal_handle, detail)) {
    return false;
  }
  auto timeout = std::chrono::duration_cast<std::chrono::nanoseconds>(
    std::chrono::duration<double>(GetTimeoutForSkill(skill)));
  if (skill == "Arm") {
    RCLCPP_INFO(this->get_logger(), "[Arm] 并发发送两个 goal: body_id=1 与 body_id=0");

    std::atomic<bool> finished1{false}, finished2{false};
    bool ok1 = false, ok2 = false;
    std::string d1, d2;

    auto worker = [this, &ok_out = ok1, &done_flag = finished1, &d_out = d1,
                   skill, topic, timeout](int body_id) {
      tls_arm_body_id = body_id;
      ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
        skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
      done_flag.store(true, std::memory_order_release);
    };

    std::thread t1(worker, 1); // RIGHT_ARM=1
    auto worker2 = [this, &ok_out = ok2, &done_flag = finished2, &d_out = d2,
                    skill, topic, timeout](int body_id) {
      tls_arm_body_id = body_id;
      ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
        skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
      done_flag.store(true, std::memory_order_release);
    };
    std::thread t2(worker2, 0); // LEFT_ARM=0

    const auto start_steady = std::chrono::steady_clock::now();
    const double timeout_sec = timeout.count() / 1e9;
    while (!(finished1.load(std::memory_order_acquire) && finished2.load(std::memory_order_acquire))) {
      auto now = std::chrono::steady_clock::now();
      double elapsed = std::chrono::duration<double>(now - start_steady).count();
      double phase = timeout_sec > 0.0 ? std::min(elapsed / timeout_sec, 0.99) : 0.0;
      double sub_done = (finished1.load(std::memory_order_relaxed) ? 1.0 : phase)
                      + (finished2.load(std::memory_order_relaxed) ? 1.0 : phase);
      double avg_phase = 0.5 * std::min(sub_done, 2.0);
      float seq_progress =
        (total_skills > 0) ? static_cast<float>((index + avg_phase) / total_skills) : 0.f;
      PublishFeedbackStage(goal_handle, "RUN", skill, seq_progress, "");
      std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }

    if (t1.joinable()) t1.join();
    if (t2.joinable()) t2.join();

    if (!ok1 || !ok2) {
      std::ostringstream oss;
      if (!ok1) { oss << "Arm(body_id=1) 失败"; if (!d1.empty()) oss << ": " << d1; }
      if (!ok2) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) 失败"; if (!d2.empty()) oss << ": " << d2; }
      detail = oss.str();
      return false;
    }
    detail.clear();
    return true;
  }
  // 其他动作按一次发送
  return RunActionSkillWithProgress(
    skill, topic, timeout, index, total_skills, goal_handle, detail);
}

/**
 * @brief Worker-thread based action execution with periodic RUN feedback.
 *
 * Launches a thread executing dispatch_skill_action while the main thread publishes
 * RUN stage feedback at fixed intervals to reflect intra-skill progress relative to the
 * skill's timeout budget.
 * @param skill Skill name.
 * @param topic Action topic/client key.
 * @param timeout_ns Absolute timeout for the action (nanoseconds).
 * @param index Skill's zero-based index within the sequence.
 * @param total_skills Total number of skills in the sequence.
 * @param goal_handle ExecuteBtAction goal handle for feedback publication.
 * @param detail [out] Detailed status or error information.
 * @return true if the action reports success; false otherwise.
 */
bool CerebellumNode::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)
{
  std::atomic<bool> finished{false};
  bool ok = false;
  auto logger = this->get_logger();
  std::string local_detail;
  std::thread worker([&]() {
      ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
        skill, topic, action_clients_.get(), logger, timeout_ns, local_detail);
      finished.store(true, std::memory_order_release);
    });
  const auto start_steady = std::chrono::steady_clock::now();
  const double timeout_sec = timeout_ns.count() / 1e9;
  while (!finished.load(std::memory_order_acquire)) {
    auto now = std::chrono::steady_clock::now();
    double elapsed = std::chrono::duration<double>(now - start_steady).count();
    double phase = timeout_sec > 0.0 ? std::min(elapsed / timeout_sec, 0.99) : 0.0;
    float seq_progress =
      (total_skills > 0) ? static_cast<float>((index + phase) / total_skills) : 0.f;
    PublishFeedbackStage(goal_handle, "RUN", skill, seq_progress, "");
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
  }
  if (worker.joinable()) {worker.join();}
  detail = local_detail;
  if (!ok && detail.empty()) {detail = "Unsupported action skill";}
  return ok;
}

/**
 * @brief Execute a supported service-based skill (currently VisionObjectRecognition).
 *
 * Performs synchronous service call with simple START/END feedback. Unsupported
 * service names or failures emit END with FAIL classification.
 * @param skill Skill name.
 * @param detail (out) Status / diagnostic text.
 * @param start_progress Sequence progress prior to invoking the service.
 * @param goal_handle ExecuteBtAction goal handle used for feedback.
 * @return true if service call succeeds with positive result; false otherwise.
 */
bool CerebellumNode::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)
{
  (void)goal_handle;
  (void)start_progress;
  auto to_lower_str = [](std::string s) {
      std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) {
        return static_cast<char>(std::tolower(c));
      });
      return s;
    };
  std::string service_base;
  for (const auto & iface : spec.interfaces) {
    const auto pos = iface.rfind('.');
    if (pos == std::string::npos || pos + 1 >= iface.size()) {continue;}
    if (to_lower_str(iface.substr(pos + 1)) == "service") {
      service_base = iface.substr(0, pos);
      break;
    }
  }

  if (service_base.empty()) {
    detail = "No service interface declared";
    return false;
  }

  bool ok = skill_manager_->invoke_service_hook(service_base, skill, detail);
  if (!ok && detail.empty()) {
    detail = "Service hook execution failed";
  }
  return ok;
}

/**
 * @brief Update success/failure counters for a single skill execution.
 *
 * Thread-safe via internal mutex protecting the statistics maps.
 * @param skill Skill name.
 * @param success True if the execution succeeded; false otherwise.
 */
void CerebellumNode::RecordSkillResult(const std::string & skill, bool success)
{
  std::lock_guard<std::mutex> lk(stats_mutex_);
  if (success) {
    ++skill_success_counts_[skill];
  } else {
    ++skill_failure_counts_[skill];
  }
}

/**
 * @brief Periodic statistics logger and publisher callback.
 *
 * Constructs a concise textual summary containing overall sequence counters and per-skill
 * success/failure tallies, publishes it on /brain/skill_stats, and logs the same.
 */
void CerebellumNode::LogStatsPeriodic()
{
  std::lock_guard<std::mutex> lk(stats_mutex_);
  RCLCPP_INFO(
    this->get_logger(), "[stats] sequences success=%zu failure=%zu",
    total_sequence_success_, total_sequence_failure_);
  for (const auto & kv : skill_success_counts_) {
    size_t fails = skill_failure_counts_[kv.first];
    RCLCPP_INFO(
      this->get_logger(), "  skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
      fails);
  }
  if (stats_pub_) {
    std_msgs::msg::String msg; std::ostringstream oss;
    oss << "seq_ok=" << total_sequence_success_ << ",seq_fail=" << total_sequence_failure_;
    for (const auto & kv : skill_success_counts_) {
      size_t fails = skill_failure_counts_[kv.first];
      oss << ";" << kv.first << "=" << kv.second << "/" << fails;
    }
    msg.data = oss.str();
    stats_pub_->publish(msg);
  }
}

/**
 * @brief Execute an accepted ExecuteBtAction goal end-to-end.
 *
 * Validates and parses the goal into an ordered skill list, then iteratively executes
 * each skill, publishing START/RUN/WAIT/END feedback, respecting the abort_on_failure
 * policy, updating per-skill and sequence statistics, and finally reporting the result.
 * @param goal_handle Goal handle provided by the action server layer.
 */
void CerebellumNode::RunExecuteBtGoal(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
{
  std::vector<std::string> skills;
  if (!ValidateAndParseGoal(goal_handle, skills)) {
    return; // goal aborted
  }
  // Cache calls for this goal (Plan A minimal support)
  last_calls_.clear();
  for (const auto & c : goal_handle->get_goal()->calls) { last_calls_.push_back(c); }
  // Log cached calls for verification
  if (!last_calls_.empty()) {
    std::ostringstream oss;
    oss << "Cached last_calls_ (count=" << last_calls_.size() << "): ";
    for (size_t i = 0; i < last_calls_.size(); ++i) {
      if (i) oss << ", ";
      oss << last_calls_[i].name;
    }
    RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str());
  }
  // Install execute_fn & complete_cb into runtime so state machine owns lifecycle.
  auto & d = brain::runtime();
  d.execute_fn = [this, goal_handle, skills](rclcpp::Logger logger) -> brain::CerebellumData::ExecResult {
      (void)logger; // we already have this->get_logger() inside helpers
      return ExecuteSequence(goal_handle, skills);
    };
  d.complete_cb = [this, goal_handle](bool /*success*/, const std::string & message) {
      // Build result message from runtime latched fields
      auto & d = brain::runtime();
      using EBA = brain_interfaces::action::ExecuteBtAction;
      auto res = std::make_shared<EBA::Result>();
      res->success = d.last_success.load();
      res->total_skills = d.last_total_skills;
      res->succeeded_skills = d.last_succeeded_skills;
      res->message = message.empty() ? std::string("(no message)") : message;
      if (res->success) {
        goal_handle->succeed(res);
        RCLCPP_INFO(this->get_logger(), "%s", res->message.c_str());
      } else {
        goal_handle->abort(res);
        RCLCPP_ERROR(this->get_logger(), "%s", res->message.c_str());
      }
    };
}

/**
 * @brief Execute the full ordered skill sequence inside the state-machine supervised context.
 *
 * This function is bound as execute_fn and invoked by StExecuting in a supervision thread.
 * It iterates over the provided skill list, honoring:
 *  - Preemption / cancellation (runtime().preempt flag)
 *  - Optional overall sequence timeout (parameter overall_sequence_timeout_sec)
 *  - Per-skill abort_on_failure policy
 *
 * For each skill it resolves the action/service topic, performs execution via ExecuteSingleSkill,
 * publishes END feedback (including final per-skill status), aggregates statistics, and builds a
 * textual summary string of the form: Seq{ActionName}: SkillA(OK), SkillB(FAIL)[detail], ...
 *
 * Sequence level success is true only if all executed skills succeeded or failures were allowed
 * to continue (abort_on_failure_ == false) and no cancellation or timeout occurred.
 *
 * @param goal_handle ExecuteBtAction goal handle used for publishing feedback / final result.
 * @param skills Ordered list of skill names parsed from the goal.
 * @return brain::CerebellumData::ExecResult Aggregated outcome for the state machine.
 */
brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
  const std::vector<std::string> & skills)
{
  brain::CerebellumData::ExecResult result;
  result.total_skills = static_cast<int32_t>(skills.size());
  if (skills.empty()) {
    result.success = false;
    result.message = "Empty skill list";
    return result;
  }
  // Optional overall sequence timeout parameter (seconds); default 0 = disabled
  double overall_timeout_sec = 0.0;
  (void)this->get_parameter("overall_sequence_timeout_sec", overall_timeout_sec); // ignore failure
  const bool timeout_enabled = overall_timeout_sec > 0.0;
  auto seq_start = std::chrono::steady_clock::now();

  std::ostringstream summary;
  summary << "Seq{" << goal_handle->get_goal()->action_name << "}: ";
  bool overall_ok = true;
  size_t loop_index = 0;
  for (size_t seq_index = 0; seq_index < skills.size(); ++seq_index) {
    // Preempt / cancel check
    if (brain::runtime().preempt.load(std::memory_order_acquire)) {
      overall_ok = false;
      summary << "(Cancelled)";
      break;
    }
    // Global timeout check
    if (timeout_enabled) {
      double elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - seq_start).count();
      if (elapsed > overall_timeout_sec) {
        overall_ok = false;
        brain::runtime().preempt.store(true, std::memory_order_release);
        brain::runtime().preempt_reason = "Sequence timeout";
        summary << "(Timeout)";
        break;
      }
    }
    const auto & skill = skills[seq_index];
    const std::string topic = ResolveTopicForSkill(skill);
    RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d", 
      skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
    bool ok = false; std::string detail;
    auto it = skill_manager_->skills().find(skill);
    if (it == skill_manager_->skills().end()) {
      detail = "Skill not defined";
      PublishFeedbackStage(
        goal_handle, "END", skill,
        static_cast<float>(seq_index + 1) / static_cast<float>(skills.size()),
        "FAIL:undefined");
    } else {
      ok = ExecuteSingleSkill(
        skill, topic, it->second, detail, static_cast<int>(seq_index), static_cast<int>(skills.size()), goal_handle);
      PublishFeedbackStage(
        goal_handle, "END", skill,
        static_cast<float>(seq_index + 1) / static_cast<float>(skills.size()),
        ok ? (detail.empty() ? "OK" : std::string("OK:") + detail)
           : (detail.empty() ? "FAIL" : std::string("FAIL:") + detail));
    }
    RecordSkillResult(skill, ok);
    if (ok) { ++result.succeeded_skills; }
    overall_ok = overall_ok && ok;
    summary << skill << '(' << (ok ? "OK" : "FAIL") << ')';
    if (!detail.empty()) summary << '[' << detail << ']';
    if (loop_index + 1 < skills.size()) summary << ", ";
    ++loop_index;
    if (!ok) {
      RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
        abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
      if (abort_on_failure_) break;
    }
  }
  // stats update
  {
    std::lock_guard<std::mutex> lk(stats_mutex_);
    if (overall_ok) ++total_sequence_success_; else ++total_sequence_failure_;
  }
  result.success = overall_ok;
  result.message = summary.str();
  return result;
}

/**
 * @brief Validate the presence of a goal and parse its action_name into skill tokens.
 *
 * Aborts the goal with an appropriate message if the goal pointer is null or the parsed
 * skill list is empty.
 * @param goal_handle Goal handle from action server.
 * @param skills_out [out] Parsed list of skills (unchanged on failure).
 * @return true if validation succeeds and skills_out is populated; false otherwise.
 */
bool CerebellumNode::ValidateAndParseGoal(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
  std::vector<std::string> & skills_out) const
{
  using EBA = brain_interfaces::action::ExecuteBtAction;
  auto logger = this->get_logger();
  const auto goal = goal_handle->get_goal();
  if (!goal) {
    auto res = std::make_shared<EBA::Result>();
    res->success = false;
    res->message = "Null goal";
    goal_handle->abort(res);
    return false;
  }
  // Prefer new Plan-A payload path when calls are provided; fallback to legacy action_name
  if (!goal->calls.empty()) {
    skills_out.clear(); skills_out.reserve(goal->calls.size());
    for (const auto & c : goal->calls) {
      // Minimal loop: only use name to drive existing machinery (MoveWaist/MoveLeg first)
      skills_out.push_back(c.name);
    }
    std::ostringstream oss; oss << "[ExecuteBtAction] Received goal calls=[";
    for (size_t i = 0; i < goal->calls.size(); ++i) {
      if (i) oss << ','; oss << goal->calls[i].name;
    }
    oss << "] (count=" << goal->calls.size() << ")";
    RCLCPP_INFO(logger, "%s", oss.str().c_str());
  } else {
    const std::string & raw = goal->action_name;
    RCLCPP_INFO(logger, "[ExecuteBtAction] Received goal action_name=%s", raw.c_str());
    skills_out = ParseSkillSequence(raw);
  }
  if (skills_out.empty()) {
    auto res = std::make_shared<EBA::Result>();
    res->success = false;
    res->message = "Empty skill sequence";
    goal_handle->abort(res);
    return false;
  }
  return true;
}

/**
 * @brief Resolve an action topic/client key for a skill.
 *
 * Prefers the snake_case variant if a registered client exists; otherwise tries the
 * original CamelCase skill name; finally falls back to snake_case even if unregistered
 * (allowing later availability checks to fail cleanly).
 * @param skill Skill name.
 * @return Chosen topic string.
 */
std::string CerebellumNode::ResolveTopicForSkill(const std::string & skill) const
{
  // Plan A minimal: if last_calls_ has a matching name with non-empty topic, use it
  for (const auto & c : last_calls_) {
    if (c.name == skill && !c.topic.empty()) {
      return c.topic;
    }
  }
  const std::string snake = ToSnake(skill);
  if (action_clients_->has_client(snake)) {return snake;}
  if (action_clients_->has_client(skill)) {return skill;}
  if (action_clients_->has_client(skill + "Action")) {return skill + "Action";}
  return snake;  // fallback
}

/**
 * @brief Finalize an ExecuteBtAction goal with the aggregated result summary.
 *
 * Emits either succeed() or abort() on the goal handle, logging the composed summary
 * which includes per-skill success/failure annotations.
 * @param goal_handle Goal handle.
 * @param overall_ok True if all (or allowed subset under abort policy) skills succeeded.
 * @param succeeded_skills Count of individually successful skills.
 * @param total_skills Total number of attempted skills.
 * @param summary_stream Stream containing the pre-built textual summary.
 */
void CerebellumNode::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)
{
  using EBA = brain_interfaces::action::ExecuteBtAction;
  auto res = std::make_shared<EBA::Result>();
  res->success = overall_ok;
  res->message = summary_stream.str();
  res->total_skills = total_skills;
  res->succeeded_skills = succeeded_skills;
  auto logger = this->get_logger();
  if (overall_ok) {
    goal_handle->succeed(res);
    RCLCPP_INFO(logger, "%s", res->message.c_str());
  } else {
    goal_handle->abort(res);
    RCLCPP_ERROR(logger, "%s", res->message.c_str());
  }
}

}  // namespace brain
