#include "agv_calib_core/brain_node.hpp"
#include "agv_calib_core/bt_ros2_nodes.hpp"

#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp_components/register_node_macro.hpp>

namespace agv_calib_core {

BrainNode::BrainNode(const rclcpp::NodeOptions & options) 
: Node("brain_node", options), is_running_(false) {
    
    RCLCPP_INFO(this->get_logger(), "👑 AGV 标定中央大脑 (Component 版) 正在挂载...");

    // 1. 从 YAML 配置文件读取动态参数！绝不硬编码！
    this->declare_parameter<std::string>("tree_xml_filename", "main_tree.xml");
    this->declare_parameter<int>("tick_rate_ms", 50);

    // 2. 启动专属的后台守护线程去运行行为树。将主线程交还给容器处理网络回调！
    is_running_ = true;
    bt_thread_ = std::thread(&BrainNode::execute_behavior_tree, this);
}

BrainNode::~BrainNode() {
    is_running_ = false;
    if (bt_thread_.joinable()) {
        bt_thread_.join();
    }
}

void BrainNode::execute_behavior_tree() {
    // 稍微延时 0.5 秒，确保节点完全被容器接管，再发起 Client 寻址
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    BT::BehaviorTreeFactory factory;

    // 注册业务积木，传递 this 裸指针给所有积木
    factory.registerBuilder<SetChassisModeNode>("SetChassisMode", 
        [this](const std::string& name, const BT::NodeConfiguration& config) {
            return std::make_unique<SetChassisModeNode>(name, config, this);
        });

    factory.registerBuilder<TriggerCaptureNode>("TriggerCapture", 
        [this](const std::string& name, const BT::NodeConfiguration& config) {
            return std::make_unique<TriggerCaptureNode>(name, config, this);
        });

    factory.registerBuilder<DownloadDataNode>("DownloadData", 
        [this](const std::string& name, const BT::NodeConfiguration& config) {
            return std::make_unique<DownloadDataNode>(name, config, this);
        });

    try {
        std::string xml_filename = this->get_parameter("tree_xml_filename").as_string();
        int tick_rate = this->get_parameter("tick_rate_ms").as_int();

        std::string pkg_path = ament_index_cpp::get_package_share_directory("agv_calib_core");
        std::string xml_file = pkg_path + "/behavior_trees/" + xml_filename;
        
        auto tree = factory.createTreeFromFile(xml_file);
        BT::StdCoutLogger logger_cout(tree);
        
        RCLCPP_INFO(this->get_logger(), "📜 XML 剧本 [%s] 加载完毕，开始全自动流水线...", xml_filename.c_str());
        
        // 按照 YAML 配置的频率持续 Tick
        BT::NodeStatus status = BT::NodeStatus::RUNNING;
        while (rclcpp::ok() && is_running_ && status == BT::NodeStatus::RUNNING) {
            status = tree.tickRoot();
            std::this_thread::sleep_for(std::chrono::milliseconds(tick_rate));
        }

        if (status == BT::NodeStatus::SUCCESS) {
            RCLCPP_INFO(this->get_logger(), "🎉 标定流水线全流程完美结束！");
        } else {
            RCLCPP_WARN(this->get_logger(), "⚠️ 流水线未成功完成 (可能被中止)。");
        }
        
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "❌ 行为树崩溃: %s", e.what());
    }
}

} // namespace agv_calib_core

// 🚨 终极一步：将该类注册为 ROS 2 Component (插件)！
RCLCPP_COMPONENTS_REGISTER_NODE(agv_calib_core::BrainNode)