#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"

#include <opencv2/opencv.hpp>
#include <zbar.h>

#include <chrono>
#include <memory>
#include <string>

class QrDeteDepthNode : public rclcpp::Node
{
public:
  explicit QrDeteDepthNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("qr_dete_depth", options), enabled_(true)
  {
    image_topic_ = declare_parameter<std::string>("image_topic", "/aurora/rgb/image_raw");
    use_buffer_ = declare_parameter<bool>("use_buffer", true);
    tts_service_ = declare_parameter<std::string>("tts_service", "/tts/speak");

    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
      image_topic_, rclcpp::SensorDataQoS(),
      std::bind(&QrDeteDepthNode::image_callback, this, std::placeholders::_1));

    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
      "sign4return", sign_qos,
      std::bind(&QrDeteDepthNode::sign_callback, this, std::placeholders::_1));

    result_pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
    tts_client_ = create_client<origincar_msg::srv::Speak>(tts_service_);

    RCLCPP_INFO(
      get_logger(), "QR depth detect ready, topic=%s, use_buffer=%s, tts_service=%s",
      image_topic_.c_str(), use_buffer_ ? "true" : "false", tts_service_.c_str());
  }

private:
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    if (!enabled_) {
      return;
    }

    log_frame_delay(msg);

    cv::Mat gray_image;
    if (!image_to_gray(msg, gray_image)) {
      return;
    }

    scan_and_publish(gray_image);
  }

  void log_frame_delay(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    const auto now = get_clock()->now();
    const auto msg_time = rclcpp::Time(msg->header.stamp);
    const auto delay_us = (now - msg_time).nanoseconds() / 1000;

    RCLCPP_INFO(
      get_logger(), "frame_id: %s, time cost %ldus",
      msg->header.frame_id.c_str(), delay_us);
  }

  bool image_to_gray(const sensor_msgs::msg::Image::SharedPtr msg, cv::Mat & gray_image)
  {
    if (msg->encoding != "bgr8") {
      static int warn_count = 0;
      if (warn_count++ < 3) {
        RCLCPP_ERROR(get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
      }
      return false;
    }

    cv::Mat bgr_image(
      msg->height, msg->width, CV_8UC3,
      const_cast<uint8_t *>(msg->data.data()));
    cv::cvtColor(bgr_image, gray_image, cv::COLOR_BGR2GRAY);
    return true;
  }

  void scan_and_publish(cv::Mat & gray_image)
  {
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

    zbar::Image zbar_image(
      gray_image.cols, gray_image.rows, "Y800",
      gray_image.data, gray_image.cols * gray_image.rows);

    std::string qr_results;
    const int n = scanner.scan(zbar_image);
    if (n > 0) {
      for (auto symbol = zbar_image.symbol_begin();
        symbol != zbar_image.symbol_end(); ++symbol)
      {
        qr_results += symbol->get_data();
        RCLCPP_INFO(
          get_logger(), "Decoded %s symbol \"%s\"",
          symbol->get_type_name().c_str(), symbol->get_data().c_str());
      }
      qr_results_buff_ = qr_results;
    } else {
      qr_results = "QR Code not detected";
      RCLCPP_INFO(get_logger(), "QR Code not detected");
    }

    const std::string result_text = use_buffer_ ? qr_results_buff_ : qr_results;
    if (result_text.empty()) {
      return;
    }

    publish_result(result_text);
  }

  void publish_result(const std::string & qr_data)
  {
    auto out = std_msgs::msg::String();
    if (is_numeric(qr_data)) {
      out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针");
    } else {
      out.data = qr_data;
    }

    if (out.data != last_) {
      RCLCPP_INFO(get_logger(), "QR: %s", out.data.c_str());
      last_ = out.data;
    }

    result_pub_->publish(out);
    speak_once(out.data);
  }

  bool is_numeric(const std::string & input) const
  {
    return !input.empty() && input.find_first_not_of("0123456789") == std::string::npos;
  }

  void speak_once(const std::string & text)
  {
    if (text == last_spoken_) {
      return;
    }
    last_spoken_ = text;

    if (!tts_client_->service_is_ready()) {
      static int warn_count = 0;
      if (warn_count++ < 3) {
        RCLCPP_WARN(get_logger(), "TTS service %s not available", tts_service_.c_str());
      }
      return;
    }

    auto request = std::make_shared<origincar_msg::srv::Speak::Request>();
    request->text = text;
    tts_client_->async_send_request(
      request,
      [this](rclcpp::Client<origincar_msg::srv::Speak>::SharedFuture future) {
        try {
          const auto response = future.get();
          if (!response->success) {
            RCLCPP_WARN(get_logger(), "TTS failed: %s", response->message.c_str());
          }
        } catch (const std::exception & e) {
          RCLCPP_ERROR(get_logger(), "TTS call error: %s", e.what());
        }
      });
  }

  void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
  {
    if (msg->data == 0) {
      enabled_ = true;
      last_.clear();
      last_spoken_.clear();
      qr_results_buff_.clear();
      RCLCPP_INFO(get_logger(), "ON");
    } else if (msg->data == 5) {
      enabled_ = false;
      RCLCPP_INFO(get_logger(), "OFF");
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sign_sub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr result_pub_;
  rclcpp::Client<origincar_msg::srv::Speak>::SharedPtr tts_client_;

  bool enabled_;
  bool use_buffer_;
  std::string image_topic_;
  std::string tts_service_;
  std::string last_;
  std::string last_spoken_;
  std::string qr_results_buff_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<QrDeteDepthNode>());
  rclcpp::shutdown();
  return 0;
}
