#pragma once
#include "rclcpp/rclcpp.hpp"

#include "behaviortree_ros2/tree_execution_server.hpp"
#include "behaviortree_cpp/loggers/bt_cout_logger.h"

#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"

namespace toid {
    class TreeExecutor : public BT::TreeExecutionServer {
      public:
        TreeExecutor(const rclcpp::NodeOptions opts);
        void onTreeCreated(BT::Tree& tree) override;
        
        void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;

        std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;

        void position(geometry_msgs::msg::PoseStamped &pose);

        std::string describeCustomNodes();

      private:
        std::shared_ptr<BT::StdCoutLogger>          logger_cout_;
        tf2_ros::Buffer::SharedPtr                  tf_buffer_;
        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

        std::string base_frame_;
        std::string global_frame_;
    };
}