#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "brain/action_registry.hpp"
#include "interfaces/action/move_to_position.hpp"

using MoveToPosition = interfaces::action::MoveToPosition;

TEST(ActionRegistry, ServerRegistersAndAccepts)
{
  rclcpp::init(0, nullptr);
  auto node = std::make_shared<rclcpp::Node>("test_action_registry_node");
  ActionServerRegistry registry(node.get());

  bool goal_received = false;
  ActionServerRegistry::Handlers<MoveToPosition> h;
  h.on_goal =
    [&](const rclcpp_action::GoalUUID &, std::shared_ptr<const MoveToPosition::Goal> goal) {
      (void)goal;
      goal_received = true;
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    };
  h.on_cancel = [&](const std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveToPosition>>) {
      return rclcpp_action::CancelResponse::ACCEPT;
    };
  registry.register_action_server<MoveToPosition>("move_to_position", h);

  // Creation should not crash; we can't easily send a goal without spinning a client here.
  // Just spin some time to ensure server construction path runs
  rclcpp::executors::SingleThreadedExecutor exec;
  exec.add_node(node);
  exec.spin_some();

  SUCCEED();
  rclcpp::shutdown();
}
