/**
 * qr_detect.cpp — RDKx5 二维码识别 (sensor_msgs::Image / CompressedImage)
 *
 * 同时支持:
 *   - USB 相机: 直接订阅 /image_raw (rgb8/bgr8/mono8) 或 JPEG 压缩 /image
 *   - MIPI 相机: hobot_codec 解码后订阅 /image (nv12→bgr8)
 *
 * 结果发布到 qr_results。sign4return=0 开 / =5 关。
 */

#include "origincar_msg/srv/speak.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "sensor_msgs/msg/image.hpp"

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

#include <chrono>

using namespace std::chrono_literals;


class QrDetectNode : public rclcpp::Node
{
public:
  explicit QrDetectNode(const rclcpp::NodeOptions &opts = rclcpp::NodeOptions())
    : Node("qr_detect", opts), enabled_(true)
  {
    image_topic_ = declare_parameter("image_topic", "/image");
    image_type_ = declare_parameter("image_type", "auto");

    if (image_type_ == "raw") {
      subscribe_raw_image();
    } else if (image_type_ == "compressed") {
      subscribe_compressed_image();
    } else if (image_type_ == "auto") {
      discovery_timer_ = create_wall_timer(
          500ms, std::bind(&QrDetectNode::auto_select_image_subscription, this));
      auto_select_image_subscription();
    } else {
      RCLCPP_WARN(
          get_logger(), "Unknown image_type=%s, falling back to auto discovery", image_type_.c_str());
      image_type_ = "auto";
      discovery_timer_ = create_wall_timer(
          500ms, std::bind(&QrDetectNode::auto_select_image_subscription, this));
      auto_select_image_subscription();
    }

    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
        "sign4return", 10,
        std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1));

    pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
    tts_client_ = create_client<origincar_msg::srv::Speak>("/tts/speak");

    RCLCPP_INFO(
        get_logger(),
        "QR Detect ready, topic=%s, image_type=%s",
        image_topic_.c_str(), image_type_.c_str());
  }

private:
  void subscribe_raw_image()
  {
    if (image_sub_ || compressed_image_sub_) {
      return;
    }

    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
        image_topic_, rclcpp::SensorDataQoS(),
        std::bind(&QrDetectNode::image_cb, this, std::placeholders::_1));
    stop_discovery_timer();
    RCLCPP_INFO(get_logger(), "QR image input selected: sensor_msgs/Image on %s", image_topic_.c_str());
  }

  void subscribe_compressed_image()
  {
    if (image_sub_ || compressed_image_sub_) {
      return;
    }

    compressed_image_sub_ = create_subscription<sensor_msgs::msg::CompressedImage>(
        image_topic_, rclcpp::SensorDataQoS(),
        std::bind(&QrDetectNode::compressed_image_cb, this, std::placeholders::_1));
    stop_discovery_timer();
    RCLCPP_INFO(
        get_logger(), "QR image input selected: sensor_msgs/CompressedImage on %s",
        image_topic_.c_str());
  }

  void stop_discovery_timer()
  {
    if (discovery_timer_) {
      discovery_timer_->cancel();
    }
  }

  void auto_select_image_subscription()
  {
    if (image_sub_ || compressed_image_sub_) {
      stop_discovery_timer();
      return;
    }

    const auto endpoints = get_publishers_info_by_topic(image_topic_);
    for (const auto &endpoint : endpoints) {
      if (endpoint.topic_type() == "sensor_msgs/msg/CompressedImage") {
        subscribe_compressed_image();
        return;
      }
    }

    for (const auto &endpoint : endpoints) {
      if (endpoint.topic_type() == "sensor_msgs/msg/Image") {
        subscribe_raw_image();
        return;
      }
    }
  }

  void image_cb(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    if (!enabled_) return;

    cv::Mat gray;
    if (!raw_image_to_gray(msg, gray)) {
      return;
    }

    scan_and_publish(gray);
  }

  void compressed_image_cb(const sensor_msgs::msg::CompressedImage::SharedPtr msg)
  {
    if (!enabled_) return;

    cv::Mat gray = cv::imdecode(msg->data, cv::IMREAD_GRAYSCALE);
    if (gray.empty()) {
      static int warn_count = 0;
      if (warn_count++ < 3) {
        RCLCPP_WARN(get_logger(), "Failed to decode compressed image: format=%s", msg->format.c_str());
      }
      return;
    }

    scan_and_publish(gray);
  }

  bool raw_image_to_gray(const sensor_msgs::msg::Image::SharedPtr msg, cv::Mat &gray)
  {
    const std::string &enc = msg->encoding;

    // 编码自适应
    if (enc == "mono8" || enc == "8UC1") {
      gray = cv::Mat(msg->height, msg->width, CV_8UC1,
                     const_cast<uint8_t*>(msg->data.data())).clone();
    } else if (enc == "rgb8") {
      cv::Mat rgb(msg->height, msg->width, CV_8UC3,
                  const_cast<uint8_t*>(msg->data.data()));
      cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
    } else if (enc == "bgr8") {
      cv::Mat bgr(msg->height, msg->width, CV_8UC3,
                  const_cast<uint8_t*>(msg->data.data()));
      cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
    } else if (enc == "nv12" || enc == "NV12") {
      cv::Mat nv12(msg->height * 3 / 2, msg->width, CV_8UC1,
                   const_cast<uint8_t*>(msg->data.data()));
      cv::Mat bgr;
      cv::cvtColor(nv12, bgr, cv::COLOR_YUV2BGR_NV12);
      cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);
    } else if (enc == "yuv422_yuy2" || enc == "yuyv") {
      cv::Mat yuv(msg->height, msg->width, CV_8UC2,
                  const_cast<uint8_t*>(msg->data.data()));
      cv::cvtColor(yuv, gray, cv::COLOR_YUV2GRAY_YUY2);
    } else {
      static int warn_count = 0;
      if (warn_count++ < 3)
        RCLCPP_WARN(get_logger(), "Unsupported encoding: %s", enc.c_str());
      return false;
    }

    return true;
  }

  void scan_and_publish(cv::Mat &gray)
  {
    // ZBar
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
    zbar::Image zimg(gray.cols, gray.rows, "Y800",
                     gray.data, gray.cols * gray.rows);
    int n = scanner.scan(zimg);

    auto out = std_msgs::msg::String();
    if (n > 0) {
      const std::string qr_data = zimg.symbol_begin()->get_data();
      if (qr_data != last_) {
        RCLCPP_INFO(get_logger(), "QR: %s", qr_data.c_str());
        last_ = qr_data;
      }

      if (!qr_data.empty() && qr_data.find_first_not_of("0123456789") == std::string::npos) {
        out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针");
        pub_->publish(out);
        speak_once(out.data);
      }
    }
  }

  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 /tts/speak not available");
      }
      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_cb(const std_msgs::msg::Int32::SharedPtr msg)
  {
    if (msg->data == 0)      { enabled_ = true;  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<sensor_msgs::msg::CompressedImage>::SharedPtr   compressed_image_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr                sign_sub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr                  pub_;
  rclcpp::Client<origincar_msg::srv::Speak>::SharedPtr                 tts_client_;
  rclcpp::TimerBase::SharedPtr                                         discovery_timer_;

  bool        enabled_;
  std::string image_topic_;
  std::string image_type_;
  std::string last_;
  std::string last_spoken_;
};

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