#include <behaviortree_ros2/tree_execution_server.hpp>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>

// #include <rclcpp/logging.hpp>
// #include <std_msgs/msg/float32.hpp>

class RbsBtExecutor : public BT::TreeExecutionServer {
public:
  RbsBtExecutor(const rclcpp::NodeOptions &options)
      : TreeExecutionServer(options) {}

  void onTreeCreated(BT::Tree &tree) override {
    logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
  }

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

    // RCLCPP_INFO(logger_cout_, "Tree completed with code: %d", status );

    logger_cout_.reset();
    return std::nullopt;
  }

private:
  std::shared_ptr<BT::StdCoutLogger> logger_cout_;
  // rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  auto action_server = std::make_shared<RbsBtExecutor>(options);

  rclcpp::executors::MultiThreadedExecutor exec(
      rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
  exec.add_node(action_server->node());
  exec.spin();
  exec.remove_node(action_server->node());

  rclcpp::shutdown();
}