#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include <string>
#include <thread>
#include <chrono>

class CameraDriverNode : public rclcpp::Node
{
public:
  CameraDriverNode() : Node("camera_driver_node")
  {
    // 声明并获取参数
    this->declare_parameter<int>("camera_id", 0);
    this->declare_parameter<std::string>("frame_id", "camera_frame");
    this->declare_parameter<int>("width", 640);
    this->declare_parameter<int>("height", 480);
    this->declare_parameter<double>("fps", 30.0);
    
    camera_id_ = this->get_parameter("camera_id").as_int();
    frame_id_ = this->get_parameter("frame_id").as_string();
    width_ = this->get_parameter("width").as_int();
    height_ = this->get_parameter("height").as_int();
    fps_ = this->get_parameter("fps").as_double();
    
    // 创建图像消息发布者
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 10);
    
    // 初始化相机
    if (!initCamera()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize camera");
      return;
    }
    
    // 创建图像采集线程
    image_thread_ = std::thread(&CameraDriverNode::imageCaptureLoop, this);
    RCLCPP_INFO(this->get_logger(), "Camera driver node initialized");
  }
  
  ~CameraDriverNode()
  {
    // 停止图像采集线程
    running_ = false;
    if (image_thread_.joinable()) {
      image_thread_.join();
    }
    
    // 释放相机资源
    if (cap_.isOpened()) {
      cap_.release();
    }
  }

private:
  bool initCamera()
  {
    // 打开相机
    cap_.open(camera_id_);
    
    if (!cap_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(), "Unable to open camera %d", camera_id_);
      return false;
    }
    
    // 设置相机参数
    cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_);
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_);
    cap_.set(cv::CAP_PROP_FPS, fps_);
    
    // 检查参数设置是否成功
    int actual_width = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
    int actual_height = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
    double actual_fps = cap_.get(cv::CAP_PROP_FPS);
    
    RCLCPP_INFO(this->get_logger(), "Camera parameters: width=%d, height=%d, fps=%.1f", 
                actual_width, actual_height, actual_fps);
    
    // 捕获一帧测试
    cv::Mat test_frame;
    if (!cap_.read(test_frame)) {
      RCLCPP_ERROR(this->get_logger(), "Failed to capture test frame from camera");
      return false;
    }
    
    return true;
  }
  
  void imageCaptureLoop()
  {
    running_ = true;
    rclcpp::Rate rate(fps_);
    
    while (running_ && rclcpp::ok()) {
      // 捕获图像
      cv::Mat frame;
      if (cap_.read(frame)) {
        // 转换为ROS图像消息
        auto image_msg = cv_bridge::CvImage(
          std_msgs::msg::Header(),
          "bgr8",
          frame
        ).toImageMsg();
        
        // 设置时间戳和坐标系
        image_msg->header.stamp = this->now();
        image_msg->header.frame_id = frame_id_;
        
        // 发布图像消息
        image_pub_->publish(*image_msg);
      } else {
        RCLCPP_WARN(this->get_logger(), "Failed to capture frame from camera");
      }
      
      rate.sleep();
    }
  }
  
  // 相机对象
  cv::VideoCapture cap_;
  
  // 参数
  int camera_id_;
  std::string frame_id_;
  int width_;
  int height_;
  double fps_;
  
  // 发布者
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  
  // 图像采集线程
  std::thread image_thread_;
  bool running_{false};
};

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