#include <memory>
#include <iostream>
#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"

class MinimalHbmemSubscriber : public rclcpp::Node
{
public:
  MinimalHbmemSubscriber()
  : Node("qr_detection"), detect_qr_code_(true)  // 默认检测二维码
  {
    use_buffer = this->declare_parameter("use_buffer", true); // 声明参数，默认使用缓存
    // 订阅 /aurora/rgb/image_raw（sensor_msgs::msg::Image 格式）
    subscription_image_ =
      this->create_subscription<sensor_msgs::msg::Image>(
        "/aurora/rgb/image_raw",
        10,
        std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));

    // 创建订阅器，订阅 'sign4return' 话题
    // subscription_sign_ =
    //   this->create_subscription<std_msgs::msg::Int32>(
    //     "sign4return",
    //     10,
    //     std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
    // 创建发布器，发布 'sign4return' 话题
    sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);

    // 创建 publisher，topic 为 "qr_results"
    publisher_ =
      this->create_publisher<std_msgs::msg::String>("qr_results", 10);
  }

private:
  // 图像回调函数，处理 /aurora/rgb/image_raw 话题
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    if (!detect_qr_code_)
    {
      RCLCPP_INFO(this->get_logger(), "QR detection is disabled");
      return;
    }

    // 创建 Clock 对象并获取当前时间
    auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
    auto now = clock->now();

    // 转换消息时间戳为 ROS 时间
    auto msg_time = rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec);

    // 计算延时（单位：微秒）
    auto duration = now - msg_time;
    auto delay_us = duration.nanoseconds() / 1000; // 转换为微秒

    // 打印延迟
    RCLCPP_INFO(this->get_logger(), "frame_id: %s, time cost %ldus",
                msg->header.frame_id.c_str(), delay_us);

    // 将 sensor_msgs::Image 转换为 OpenCV 格式（Aurora RGB 为 bgr8 编码，需转为灰度给 ZBar）
    if (msg->encoding != "bgr8") {
      RCLCPP_ERROR(this->get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
      return;
    }
    cv::Mat rgb_image(msg->height, msg->width, CV_8UC3,
                      const_cast<uint8_t*>(msg->data.data()));
    cv::Mat gray_image;
    cv::cvtColor(rgb_image, gray_image, cv::COLOR_BGR2GRAY);

    // 初始化 ZBar 扫描器
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

    // 将 OpenCV 图像数据包装成 ZBar 图像
    zbar::Image zbar_image(gray_image.cols, gray_image.rows, "Y800",
                           gray_image.data, gray_image.cols * gray_image.rows);

    // 扫描图像中的条形码和二维码
    std::string qr_results;
    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(this->get_logger(), "Decoded %s symbol \"%s\"",
                    symbol->get_type_name().c_str(), symbol->get_data().c_str());
      }
      qr_results_buff_ = qr_results; // 更新缓存

      // 检测字符合法性
      // if (ResultLegal(qr_results)){
      //   // 发布sign4return话题
      //   auto message = std_msgs::msg::Int32();
      //   message.data = 5; // 5表示检测到二维码
      //   sign_publisher_->publish(message);
      //   RCLCPP_INFO(this->get_logger(), "QR Results Legal");
      // }
      // else{
      //   RCLCPP_INFO(this->get_logger(), "\033[31m QR Results Illegal \033[00m");
      // }

      // 发布sign4return话题
      auto message = std_msgs::msg::Int32();
      message.data = 5; // 5表示检测到二维码
      sign_publisher_->publish(message);
    } else {
      qr_results = "QR Code not detected";
      RCLCPP_INFO(this->get_logger(), "QR Code not detected");
    }

    // 发布 QR 结果
    auto message = std_msgs::msg::String();
    if (use_buffer){
      message.data = qr_results_buff_;
    } 
    else{
      message.data = qr_results;
    }
    publisher_->publish(message);
  }

  // 消息回调函数，处理 sign4return 话题
  // void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
  // {
  //   if (msg->data == 0)
  //   {
  //     detect_qr_code_ = true; // 启动二维码检测
  //     RCLCPP_INFO(this->get_logger(), "QR detection started");
  //   }
  //   else if (msg->data == 5)
  //   {
  //     detect_qr_code_ = false; // 停止二维码检测
  //     RCLCPP_INFO(this->get_logger(), "QR detection stopped");
  //   }
  // }

  // 识别结果合法性检测
  bool ResultLegal(std::string& input){
    // 合法字符串集合
    static const std::unordered_set<std::string> kValidStrings = {
        "1", "2", "顺时针", "逆时针", "顺", "逆"
    };
    return kValidStrings.find(input) != kValidStrings.end();
  }


  // /aurora/rgb/image_raw 订阅器
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
  // sign4return 订阅器
  // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
  // sign4return 发布器
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
  // QR code results 发布器
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

  // 参数
  bool use_buffer;
  // 二维码检测标志位
  bool detect_qr_code_;
  // 二维码检测结果缓存
  std::string qr_results_buff_;
};

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