/**
 * qr_usb_camera.cpp — USB 相机二维码识别节点
 *
 * 订阅普通 USB 相机的 sensor_msgs::Image → 识别二维码并发布结果
 * 相比旧版 qr_dete.cpp 的改进:
 *   1. 图像话题可配置 (参数化)
 *   2. 编码自适应 (bgr8 / rgb8 / mono8 / yuv422)
 *   3. 输出二维码位置信息 (用于导航逼近)
 *   4. 结果合法性过滤 (白名单可配)
 *   5. 缓冲区管理 (信号消失后清空)
 *   6. 减少日志刷屏
 */

#include <memory>
#include <iostream>
#include <unordered_set>
#include <vector>
#include <string>
#include <cmath>

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

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "qr_detection/msg/qr_detection.hpp"


class QrUsbCameraNode : public rclcpp::Node
{
public:
  QrUsbCameraNode()
  : Node("qr_usb_camera"),
    detect_enabled_(true),
    qr_found_(false),
    qr_lost_count_(0),
    prev_qr_text_("")
  {
    // ── 参数声明 ──
    image_topic_     = declare_parameter("image_topic", "/image_raw");
    result_legal_    = declare_parameter("result_legal", false);
    legal_strings_   = declare_parameter("legal_strings",
                        std::vector<std::string>{"1","2","顺时针","逆时针","顺","逆"});
    use_buffer_      = declare_parameter("use_buffer", true);
    buffer_timeout_  = declare_parameter("buffer_timeout", 3.0);   // 信号消失 N 秒后清缓存
    sign_on_detect_  = declare_parameter("sign_on_detect", true);  // 检测到后发 sign4return
    sign_value_      = declare_parameter("sign_value", 5);
    qos_depth_       = declare_parameter("qos_depth", 10);

    // ── 订阅 image ──
    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
        image_topic_, rclcpp::QoS(qos_depth_),
        std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1));

    // ── 订阅 sign4return (远程控制开关) ──
    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
        "sign4return", 10,
        std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1));

    // ── 发布者 ──
    qr_pub_      = create_publisher<qr_detection::msg::QrDetection>("qr_detection", 10);
    qr_text_pub_ = create_publisher<std_msgs::msg::String>("qr_text", 10);
    sign_pub_    = create_publisher<std_msgs::msg::Int32>("sign4return", 10);

    // ── 定时器: 检查缓冲区超时 ──
    buffer_timer_ = create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&QrUsbCameraNode::buffer_check, this));

    RCLCPP_INFO(get_logger(),
        "QR USB Camera node ready\n"
        "  image_topic: %s\n"
        "  result_legal: %s\n"
        "  use_buffer: %s (timeout=%.1fs)\n"
        "  sign_on_detect: %s (value=%ld)",
        image_topic_.c_str(),
        result_legal_ ? "true" : "false",
        use_buffer_ ? "true" : "false", buffer_timeout_,
        sign_on_detect_ ? "true" : "false", sign_value_);
  }

private:
  // ── 图像回调 ──
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    if (!detect_enabled_) return;

    // 延时
    auto now = get_clock()->now();
    auto msg_time = rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec);
    auto delay_ms = (now - msg_time).nanoseconds() / 1e6;

    // 解码 → OpenCV
    cv::Mat gray;
    if (!image_to_gray(msg, gray)) return;

    // ZBar 扫描
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
    zbar::Image zbar_img(gray.cols, gray.rows, "Y800",
                         gray.data, gray.cols * gray.rows);
    scanner.scan(zbar_img);

    // 构造消息
    auto result = qr_detection::msg::QrDetection();
    result.header = msg->header;
    result.detected = false;
    result.data = "";

    bool found_now = false;
    std::string all_texts;

    for (auto sym = zbar_img.symbol_begin();
         sym != zbar_img.symbol_end(); ++sym)
    {
      std::string text = sym->get_data();
      all_texts += (all_texts.empty() ? "" : "|") + text;

      // 合法性过滤（只取第一个合法的）
      if (!found_now && (!result_legal_ || is_legal(text))) {
        result.data     = text;
        result.detected = true;
        found_now       = true;

        // 提取位置
        int n = sym->get_location_size();
        if (n == 4) {
          for (int i = 0; i < 4; ++i) {
            result.corner_x[i] = sym->get_location_x(i) / (double)gray.cols;
            result.corner_y[i] = sym->get_location_y(i) / (double)gray.rows;
          }
          // 中心
          result.center_x = (result.corner_x[0] + result.corner_x[2]) * 0.5;
          result.center_y = (result.corner_y[0] + result.corner_y[2]) * 0.5;
        }
      }
    }

    // ── 状态转换处理 ──
    if (found_now) {
      qr_lost_count_ = 0;
      if (!qr_found_) {
        // 刚发现 → 发 sign4return
        qr_found_ = true;
        buffer_text_ = result.data;
        RCLCPP_INFO(get_logger(), "QR DETECTED: \"%s\" delay=%.0fms",
                    result.data.c_str(), delay_ms);

        if (sign_on_detect_) {
          auto sign = std_msgs::msg::Int32();
          sign.data = sign_value_;
          sign_pub_->publish(sign);
        }
      }
      // 持续检测: 只在新文本出现时打印
      if (result.data != prev_qr_text_) {
        RCLCPP_INFO(get_logger(), "QR changed: \"%s\" → \"%s\"",
                    prev_qr_text_.c_str(), result.data.c_str());
        prev_qr_text_ = result.data;
        buffer_text_  = result.data;
      }
    } else {
      qr_lost_count_++;
      if (qr_found_ && qr_lost_count_ > 4) {  // ~2s 无信号
        qr_found_ = false;
        RCLCPP_INFO(get_logger(), "QR LOST (was: \"%s\")",
                    prev_qr_text_.c_str());
      }
    }

    // ── 发布 ──
    qr_pub_->publish(result);  // 带位置的结构化消息

    // 文本消息 (带缓冲)
    auto text_msg = std_msgs::msg::String();
    text_msg.data = use_buffer_ ? buffer_text_
                                : (found_now ? result.data : "");
    qr_text_pub_->publish(text_msg);
  }

  // ── 缓冲区超时检查 ──
  void buffer_check()
  {
    if (!qr_found_ && !buffer_text_.empty()) {
      buffer_time_acc_ += 0.5;
      if (buffer_time_acc_ >= buffer_timeout_) {
        buffer_text_.clear();
        buffer_time_acc_ = 0.0;
        prev_qr_text_.clear();
        RCLCPP_INFO(get_logger(), "Buffer cleared (QR lost for %.1fs)",
                    buffer_timeout_);
      }
    } else {
      buffer_time_acc_ = 0.0;
    }
  }

  // ── sign4return 回调 ──
  void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
  {
    if (msg->data == 0) {
      detect_enabled_ = true;
      RCLCPP_INFO(get_logger(), "QR detection ENABLED");
    } else if (msg->data == 5) {
      detect_enabled_ = false;
      RCLCPP_INFO(get_logger(), "QR detection DISABLED");
    }
  }

  // ── Image → Grayscale (编码自适应) ──
  static bool 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();
      return true;
    }

    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);
      return true;
    }

    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);
      return true;
    }

    if (enc == "bgra8") {
      cv::Mat bgra(msg->height, msg->width, CV_8UC4,
                   const_cast<uint8_t*>(msg->data.data()));
      cv::cvtColor(bgra, gray, cv::COLOR_BGRA2GRAY);
      return true;
    }

    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);
      return true;
    }

    // 不支持的类型
    static std::set<std::string> warned;
    if (warned.find(enc) == warned.end()) {
      warned.insert(enc);
      RCLCPP_ERROR(rclcpp::get_logger("qr_usb_camera"),
          "Unsupported encoding: %s (h=%d, w=%d, step=%d)",
          enc.c_str(), msg->height, msg->width, msg->step);
    }
    return false;
  }

  // ── 合法性检查 ──
  bool is_legal(const std::string &text)
  {
    const auto &whitelist = get_parameter("legal_strings").as_string_array();
    for (const auto &s : whitelist) {
      if (text.find(s) != std::string::npos) return true;
    }
    return false;
  }

  // ── 成员变量 ──
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     sign_sub_;
  rclcpp::Publisher<qr_detection::msg::QrDetection>::SharedPtr  qr_pub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr           qr_text_pub_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr            sign_pub_;
  rclcpp::TimerBase::SharedPtr buffer_timer_;

  // 参数
  std::string image_topic_;
  bool        result_legal_;
  bool        use_buffer_;
  double      buffer_timeout_;
  bool        sign_on_detect_;
  int64_t     sign_value_;
  int         qos_depth_;
  std::vector<std::string> legal_strings_;

  // 状态
  bool        detect_enabled_;
  bool        qr_found_;
  int         qr_lost_count_;
  std::string prev_qr_text_;
  std::string buffer_text_;
  double      buffer_time_acc_ = 0.0;
};

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