#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 "hbm_img_msgs/msg/hbm_msg1080_p.hpp"

class MinimalHbmemSubscriber : public rclcpp::Node
{
public:
  MinimalHbmemSubscriber()
  : Node("qr_detection"), detect_qr_code_(true)  // 默认不检测二维码
  {
    // 创建 subscription_hbmem，topic 为 "hbmem_img"
    subscription_hbmem_ =
      this->create_subscription_hbmem<hbm_img_msgs::msg::HbmMsg1080P>(
        "hbmem_img",
        10,
        std::bind(&MinimalHbmemSubscriber::hbmem_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));

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

private:
  // 消息回调函数，处理 hbmem_img 话题
  void hbmem_callback(const hbm_img_msgs::msg::HbmMsg1080P::SharedPtr msg)
  {
    if (!detect_qr_code_)
    {
      RCLCPP_INFO(this->get_logger(), "QR detection is disabled");
      return; // 如果标志位为 false，则不进行二维码检测
    }

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

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

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

    // 打印延迟
    RCLCPP_INFO(this->get_logger(), "msg index: %d, time cost %dus", msg->index, delay_us);

    // 获取图像数据并将其转换为 OpenCV 格式
    cv::Mat image(msg->height, msg->width, CV_8UC1, const_cast<uint8_t*>(msg->data.data()));

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

    // 将 OpenCV 图像数据包装成 ZBar 图像
    zbar::Image zbar_image(image.cols, image.rows, "Y800", image.data, image.cols * 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());
      }
    } else {
      qr_results = "QR Code not detected";
      RCLCPP_INFO(this->get_logger(), "QR Code not detected");
    }

    // 发布 QR 结果
    auto message = std_msgs::msg::String();
    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");
    }
  }

  // hbmem_img 订阅器
  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr subscription_hbmem_;
  // sign4return 订阅器
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
  // QR code results 发布器
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

  // 二维码检测标志位
  bool detect_qr_code_;
};

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

/*
实现接收和发布
*/
//#include <memory>
//#include <iostream>
//#include <opencv2/opencv.hpp>
//#include <zbar.h>
//#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"
//#include "hbm_img_msgs/msg/hbm_msg1080_p.hpp"
//
//class MinimalHbmemSubscriber : public rclcpp::Node
//{
//public:
//  MinimalHbmemSubscriber()
//  : Node("qr_detection")
//  {
//    // 创建 subscription_hbmem，topic 为 "hbmem_img"
//    // 消息回调函数为 topic_callback
//    subscription_ =
//      this->create_subscription_hbmem<hbm_img_msgs::msg::HbmMsg1080P>(
//        "hbmem_img",
//        10,
//        std::bind(&MinimalHbmemSubscriber::topic_callback, this, std::placeholders::_1));
//
//    // 创建 publisher，topic 为 "qr_results"
//    publisher_ =
//      this->create_publisher<std_msgs::msg::String>("qr_results", 10);
//  }
//
//private:
//  // 消息回调函数
//  void topic_callback(const hbm_img_msgs::msg::HbmMsg1080P::SharedPtr msg) const
//  {
//    // 创建 Clock 对象并获取当前时间
//    auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
//    auto now = clock->now();
//
//    // 转换消息时间戳为 ROS 时间
//    auto msg_time = rclcpp::Time(msg->time_stamp.sec, msg->time_stamp.nanosec);
//
//    // 计算延时（单位：微秒）
//    auto duration = now - msg_time;
//    auto delay_us = duration.nanoseconds() / 1000; // 转换为微秒
//
//    // 打印延迟
//    RCLCPP_INFO(this->get_logger(), "msg index: %d, time cost %dus", msg->index, delay_us);
//
//    // 获取图像数据并将其转换为 OpenCV 格式
//    cv::Mat image(msg->height, msg->width, CV_8UC1, const_cast<uint8_t*>(msg->data.data()));
//
//    // 初始化 ZBar 扫描器
//    zbar::ImageScanner scanner;
//    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
//
//    // 将 OpenCV 图像数据包装成 ZBar 图像
//    zbar::Image zbar_image(image.cols, image.rows, "Y800", image.data, image.cols * 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());
//      }
//    } else {
//      qr_results = "QR Code not detected";
//      RCLCPP_INFO(this->get_logger(), "QR Code not detected");
//    }
//
//    // 发布 QR 结果
//    auto message = std_msgs::msg::String();
//    message.data = qr_results;
//    publisher_->publish(message);
//  }
//
//  // hbmem subscription
//  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr subscription_;
//  // QR code results publisher
//  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
//};
//
//int main(int argc, char * argv[])
//{
//  rclcpp::init(argc, argv);
//  rclcpp::spin(std::make_shared<MinimalHbmemSubscriber>());
//  rclcpp::shutdown();
//  return 0;
//}


/*
只实现接收消息，不进行发布
*/


//#include <memory>
//#include <iostream>
//#include <opencv2/opencv.hpp>
//#include <zbar.h>
//#include "rclcpp/rclcpp.hpp"
//#include "hbm_img_msgs/msg/hbm_msg1080_p.hpp"
//
//class MinimalHbmemSubscriber : public rclcpp::Node
//{
//public:
//  MinimalHbmemSubscriber()
//  : Node("qr_detection")
//  {
//    // 创建 subscription_hbmem，topic 为 "hbmem_img"
//    // 消息回调函数为 topic_callback
//    subscription_ =
//      this->create_subscription_hbmem<hbm_img_msgs::msg::HbmMsg1080P>(
//        "hbmem_img",
//        10,
//        std::bind(&MinimalHbmemSubscriber::topic_callback, this, std::placeholders::_1));
//  }
//private:
//  // 消息回调函数
//  void topic_callback(
//    const hbm_img_msgs::msg::HbmMsg1080P::SharedPtr msg) const
//  {
//    // 创建 Clock 对象并获取当前时间
//    auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
//    auto now = clock->now();
//
//    // 转换消息时间戳为 ROS 时间
//    auto msg_time = rclcpp::Time(msg->time_stamp.sec, msg->time_stamp.nanosec);
//
//    // 计算延时（单位：微秒）
//    auto duration = now - msg_time;
//    auto delay_us = duration.nanoseconds() / 1000; // 转换为微秒
//
//    // 打印延迟
//    RCLCPP_INFO(this->get_logger(), "msg index: %d, time cost %dus", msg->index, delay_us);
//
//    // 获取图像数据并将其转换为 OpenCV 格式
//    cv::Mat image(msg->height, msg->width, CV_8UC1, const_cast<uint8_t*>(msg->data.data()));
//
//    // 初始化 ZBar 扫描器
//    zbar::ImageScanner scanner;
//    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
//
//    // 将 OpenCV 图像数据包装成 ZBar 图像
//    zbar::Image zbar_image(image.cols, image.rows, "Y800", image.data, image.cols * image.rows);
//
//    // 扫描图像中的条形码和二维码
//    int n = scanner.scan(zbar_image);
//    if (n > 0) {
//      for (auto symbol = zbar_image.symbol_begin(); symbol != zbar_image.symbol_end(); ++symbol) {
//        RCLCPP_INFO(this->get_logger(), "Decoded %s symbol \"%s\"", symbol->get_type_name().c_str(), symbol->get_data().c_str());
//      }
//    } else {
//      RCLCPP_INFO(this->get_logger(), "QR Code not detected");
//    }
//  }
//
//  // hbmem subscription
//  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr subscription_;
//};
//
//int main(int argc, char * argv[])
//{
//  rclcpp::init(argc, argv);
//  rclcpp::spin(std::make_shared<MinimalHbmemSubscriber>());
//  rclcpp::shutdown();
//  return 0;
//}
