#include <chrono>
#include <future>
#include <thread>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

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

  // Publish a string message on a latched ros topic
  const std::string topic_name = "/pub_topic";
  std_msgs::msg::String rosMsg;
  rosMsg.data = "hello world";

  auto sub_node = rclcpp::Node::make_shared("sub");
  auto pub_node = rclcpp::Node::make_shared("pub");
  rclcpp::QoS qos = rclcpp::QoS{rclcpp::KeepLast(1lu)};
  qos.transient_local();
  auto pub = pub_node->create_publisher<std_msgs::msg::String>(topic_name, qos);
  pub->publish(rosMsg);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(pub_node);
  executor.add_node(sub_node);

  auto callbackGroupCreatedBeforeSpin =
    sub_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

  std::thread spinnerThread([&executor]() {
    executor.spin();
  });

  std::this_thread::sleep_for(std::chrono::milliseconds(100));

  auto callbackGroupCreatedAfterSpin =
    sub_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

  // Subscribe to the topic and verify that we have received a message.
  rclcpp::SubscriptionOptions subscriptionOptions;
  subscriptionOptions.callback_group = callbackGroupCreatedAfterSpin;  // This doesn't work
  // subscriptionOptions.callback_group = callbackGroupCreatedBeforeSpin;  // This works

  std::promise<void> msgReceivedPromise;
  auto msgFuture = msgReceivedPromise.get_future();
  auto sub = sub_node->create_generic_subscription(
    topic_name, "std_msgs/msg/String", qos,
    [&msgReceivedPromise](std::shared_ptr<rclcpp::SerializedMessage>) {
      msgReceivedPromise.set_value();
    },
    subscriptionOptions);

  int ret = EXIT_SUCCESS;
  if (std::future_status::ready == msgFuture.wait_for(std::chrono::milliseconds(500))) {
    RCLCPP_INFO(sub_node->get_logger(), "Message received");
  } else {
    RCLCPP_ERROR(sub_node->get_logger(), "Message NOT received");
    ret = EXIT_FAILURE;
  }

  executor.cancel();
  spinnerThread.join();
  rclcpp::shutdown();
  return ret;
}

executor.add_callback_group(callbackGroupCreatedAfterSpin, sub_node->get_node_base_interface());