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

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

#define private public
#define main qr_detect_node_main
#include "../src/qr_detect.cpp"
#undef main
#undef private

namespace
{

bool has_subscription_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(QrDetectNode, subscribesToCompressedImageWhenCompressedPublisherExists)
{
  if (!rclcpp::ok()) {
    rclcpp::init(0, nullptr);
  }

  auto camera_node = std::make_shared<rclcpp::Node>("qr_detect_compressed_camera_test");
  auto camera_pub = camera_node->create_publisher<sensor_msgs::msg::CompressedImage>(
    "/qr_test_image", rclcpp::SensorDataQoS());

  rclcpp::NodeOptions options;
  options.append_parameter_override("image_topic", "/qr_test_image");
  auto qr_node = std::make_shared<QrDetectNode>(options);
  auto graph_node = std::make_shared<rclcpp::Node>("qr_detect_graph_test");
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(camera_node);
  executor.add_node(qr_node);
  executor.add_node(graph_node);

  std::vector<rclcpp::TopicEndpointInfo> endpoints;
  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
  while (std::chrono::steady_clock::now() < deadline) {
    sensor_msgs::msg::CompressedImage msg;
    msg.format = "jpeg";
    msg.data = {0xff, 0xd8, 0xff, 0xd9};
    camera_pub->publish(msg);
    executor.spin_some();
    endpoints = graph_node->get_subscriptions_info_by_topic("/qr_test_image");
    if (has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage")) {
      break;
    }
    rclcpp::sleep_for(std::chrono::milliseconds(100));
  }

  EXPECT_TRUE(has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage"));
}

TEST(QrDetectNode, publishesQrResultsOnly)
{
  if (!rclcpp::ok()) {
    rclcpp::init(0, nullptr);
  }

  auto qr_node = std::make_shared<QrDetectNode>();
  auto graph_node = std::make_shared<rclcpp::Node>("qr_detect_publication_graph_test");
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(qr_node);
  executor.add_node(graph_node);

  std::vector<rclcpp::TopicEndpointInfo> qr_results_publishers;
  std::vector<rclcpp::TopicEndpointInfo> vlm_result_publishers;
  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
  while (std::chrono::steady_clock::now() < deadline) {
    executor.spin_some();
    qr_results_publishers = graph_node->get_publishers_info_by_topic("/qr_results");
    vlm_result_publishers = graph_node->get_publishers_info_by_topic("/vlm_result");
    if (has_subscription_type(qr_results_publishers, "std_msgs/msg/String")) {
      break;
    }
    rclcpp::sleep_for(std::chrono::milliseconds(100));
  }

  EXPECT_TRUE(has_subscription_type(qr_results_publishers, "std_msgs/msg/String"));
  EXPECT_FALSE(has_subscription_type(vlm_result_publishers, "std_msgs/msg/String"));
}

TEST(QrDetectNode, createsTtsSpeakClient)
{
  if (!rclcpp::ok()) {
    rclcpp::init(0, nullptr);
  }

  auto qr_node = std::make_shared<QrDetectNode>();
  ASSERT_NE(qr_node->tts_client_, nullptr);
  EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
}
