/**
 * qr_hbmem.cpp — RDKx5 零拷贝二维码识别
 *
 * 通过 hbmem 订阅 NV12 图像（零拷贝），ZBar 识别二维码，
 * 结果仅发布到 qr_results (std_msgs/String)。
 * sign4return 控制启停。
 */

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"
#include "hbm_img_msgs/msg/hbm_msg1080_p.hpp"

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


class QrHbmemNode : public rclcpp::Node
{
public:
  explicit QrHbmemNode(const rclcpp::NodeOptions &opts = rclcpp::NodeOptions())
    : Node("qr_hbmem", opts), detect_enabled_(true), last_found_("")
  {
    // hbmem 零拷贝订阅
    using HbmMsg = hbm_img_msgs::msg::HbmMsg1080P;
    hbmem_sub_ = this->create_subscription_hbmem<HbmMsg>(
      "hbmem_img", 10,
      std::bind(&QrHbmemNode::callback, this, std::placeholders::_1));

    // sign4return 启停控制
    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
    sign_sub_ = this->create_subscription<std_msgs::msg::Int32>(
      "sign4return", sign_qos,
      std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1));

    // 结果发布
    result_pub_ = this->create_publisher<std_msgs::msg::String>("qr_results", 10);

    RCLCPP_INFO(this->get_logger(), "QR Hbmem ready");
  }

private:
  void callback(const hbm_img_msgs::msg::HbmMsg1080P::SharedPtr msg)
  {
    if (!detect_enabled_) return;

    // NV12 → BGR → Gray
    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::Mat gray;
    cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY);

    // 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);

    std::string text;
    if (n > 0) {
      text = zimg.symbol_begin()->get_data();
      if (text != last_found_) {
        RCLCPP_INFO(this->get_logger(), "QR: %s", text.c_str());
        last_found_ = text;
      }
    }

    auto out = std_msgs::msg::String();
    out.data = text;
    result_pub_->publish(out);
  }

  void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
  {
    if (msg->data == 0) {
      detect_enabled_ = true;
      last_found_.clear();
      RCLCPP_INFO(this->get_logger(), "ON");
    } else if (msg->data == 5) {
      detect_enabled_ = false;
      RCLCPP_INFO(this->get_logger(), "OFF");
    }
  }

  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr hbmem_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr                      sign_sub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr                        result_pub_;

  bool        detect_enabled_;
  std::string last_found_;
};

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