#include <rclcpp/rclcpp.hpp>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <ament_index_cpp/get_package_share_directory.hpp>

// 引入刚才写的假节点
#include "bt_nodes/dummy_nodes.hpp" 

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    
    std::cout << "\n=========================================" << std::endl;
    std::cout << "🚀 AGV 标定车间中央大脑 [空转测试版] 启动！" << std::endl;
    std::cout << "=========================================\n" << std::endl;

    BT::BehaviorTreeFactory factory;

    // 1. 把 C++ 类注册到工厂，名字必须和 XML 里的一模一样！
    factory.registerNodeType<MockConnectAGV>("MockConnectAGV");
    factory.registerNodeType<MockCallChassisAlgo>("MockCallChassisAlgo");
    factory.registerNodeType<MockTuneControl>("MockTuneControl");
    factory.registerNodeType<MockCallControlAlgo>("MockCallControlAlgo");
    factory.registerNodeType<MockMoveAndCapture>("MockMoveAndCapture");
    factory.registerNodeType<MockCallSensorAlgo>("MockCallSensorAlgo");
    factory.registerNodeType<MockCommitAllParams>("MockCommitAllParams");

    try {
        // 2. 动态获取 XML 剧本的绝对路径 (防止你运行程序时路径不对找不到文件)
        std::string pkg_path = ament_index_cpp::get_package_share_directory("agv_calib_core");
        std::string xml_file = pkg_path + "/behavior_trees/main_pipeline.xml";
        
        auto tree = factory.createTreeFromFile(xml_file);
        std::cout << "📜 行为树剧本加载完毕，开始全自动流水线...\n" << std::endl;
        
        // 3. 开始执行总控流！
        tree.tickRoot();
        
    } catch (const std::exception& e) {
        std::cerr << "❌ 加载 XML 失败: " << e.what() << std::endl;
    }

    std::cout << "🎉 全流程执行完毕，完美收工！\n" << std::endl;
    rclcpp::shutdown();
    return 0;
}