#include <catch2/catch_all.hpp>
#include <std_msgs/msg/string.hpp>
#include <wato_test/wato_test.hpp>

#include "test_nodes/publisher_test_node.hpp"
#include "test_nodes/subscriber_test_node.hpp"

TEST_CASE_METHOD(wato::test::TestExecutorFixture, "My ROS Test", "[ros]") {
  auto pub = std::make_shared<wato::test::PublisherTestNode<std_msgs::msg::String>>("/topic", "pub");
  auto sub = std::make_shared<wato::test::SubscriberTestNode<std_msgs::msg::String>>("/topic", "sub");
  add_node(pub);
  add_node(sub);
  start_spinning();

  // Wait for DDS discovery (replaces hardcoded sleeps)
  REQUIRE(pub->wait_for_subscribers(1));
  REQUIRE(sub->wait_for_publishers(1));

  SECTION("Publish and receive") {
    auto future = sub->expect_next_message();

    std_msgs::msg::String msg;
    msg.data = "hello";
    pub->publish(msg);

    REQUIRE(future.get().data == "hello");
  }
}