#include <algorithm>
#include <chrono>
#include <string>

#include <gtest/gtest.h>
#include <origincar_msg/srv/speak.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>

#define private public
#define main qr_dete_depth_node_main
#include "../src/qr_dete_depth.cpp"
#undef main
#undef private

namespace
{

bool has_endpoint_type(
  const std::vector<rclcpp::TopicEndpointInfo> & endpoints,
  const std::string & topic_type)
{
  return std::any_of(
    endpoints.begin(), endpoints.end(),
    [&](const rclcpp::TopicEndpointInfo & endpoint) {
      return endpoint.topic_type() == topic_type;
    });
}

}  // namespace

TEST(QrDeteDepthNode, exposesQrResultAndControlEndpoints)
{
  if (!rclcpp::ok()) {
    rclcpp::init(0, nullptr);
  }

  rclcpp::NodeOptions options;
  options.append_parameter_override("image_topic", "/depth_qr_test_image");
  auto qr_node = std::make_shared<QrDeteDepthNode>(options);
  auto graph_node = std::make_shared<rclcpp::Node>("qr_dete_depth_graph_test");
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(qr_node);
  executor.add_node(graph_node);

  std::vector<rclcpp::TopicEndpointInfo> image_subscribers;
  std::vector<rclcpp::TopicEndpointInfo> sign_subscribers;
  std::vector<rclcpp::TopicEndpointInfo> result_publishers;
  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
  while (std::chrono::steady_clock::now() < deadline) {
    executor.spin_some();
    image_subscribers = graph_node->get_subscriptions_info_by_topic("/depth_qr_test_image");
    sign_subscribers = graph_node->get_subscriptions_info_by_topic("/sign4return");
    result_publishers = graph_node->get_publishers_info_by_topic("/qr_results");
    if (has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image") &&
      has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32") &&
      has_endpoint_type(result_publishers, "std_msgs/msg/String"))
    {
      break;
    }
    rclcpp::sleep_for(std::chrono::milliseconds(100));
  }

  EXPECT_TRUE(has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image"));
  EXPECT_TRUE(has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32"));
  EXPECT_TRUE(has_endpoint_type(result_publishers, "std_msgs/msg/String"));
  ASSERT_NE(qr_node->tts_client_, nullptr);
  EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
}
