/**
 * @file camera_driver_node.cpp
 * @brief OpenCV 相机驱动节点 - 发布 ROS 2 Image 消息
 * 
 * 使用 OpenCV 直接读取 USB 摄像头，发布为 ROS 2 sensor_msgs/Image
 * 
 * @author flowzl
 * @date 2026-01-19
 */

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <thread>
#include <atomic>
#include <chrono>

/**
 * @class CameraDriverNode
 * @brief OpenCV 相机驱动 ROS 2 节点
 */
class CameraDriverNode : public rclcpp::Node {
public:
    /**
     * @brief 构造函数
     */
    CameraDriverNode() : rclcpp::Node("camera_driver") {
        // ====================================================================
        // 声明参数
        // ====================================================================
        
        this->declare_parameter<std::string>("video_device", "/dev/video4");
        this->declare_parameter<double>("framerate", 30.0);
        this->declare_parameter<int>("image_width", 640);
        this->declare_parameter<int>("image_height", 480);
        this->declare_parameter<std::string>("camera_topic", "/camera/image_raw");
        this->declare_parameter<std::string>("camera_frame", "camera");
        this->declare_parameter<bool>("enable_fps_stats", true);
        
        // 读取参数
        video_device_ = this->get_parameter("video_device").as_string();
        target_framerate_ = this->get_parameter("framerate").as_double();
        image_width_ = this->get_parameter("image_width").as_int();
        image_height_ = this->get_parameter("image_height").as_int();
        camera_topic_ = this->get_parameter("camera_topic").as_string();
        camera_frame_ = this->get_parameter("camera_frame").as_string();
        enable_fps_stats_ = this->get_parameter("enable_fps_stats").as_bool();
        
        // ====================================================================
        // 初始化摄像头
        // ====================================================================
        
        RCLCPP_INFO(this->get_logger(), "======================================");
        RCLCPP_INFO(this->get_logger(), "OpenCV Camera Driver Node 启动");
        RCLCPP_INFO(this->get_logger(), "======================================");
        RCLCPP_INFO(this->get_logger(), "设备: %s", video_device_.c_str());
        RCLCPP_INFO(this->get_logger(), "分辨率: %dx%d", image_width_, image_height_);
        RCLCPP_INFO(this->get_logger(), "帧率: %.1f", target_framerate_);
        
        if (!init_camera()) {
            RCLCPP_ERROR(this->get_logger(), "❌ 摄像头初始化失败");
            rclcpp::shutdown();
            return;
        }
        
        RCLCPP_INFO(this->get_logger(), "✓ 摄像头初始化成功");
        
        // ====================================================================
        // 创建发布者
        // ====================================================================
        
        // 修改为默认 QoS (Reliable, KeepLast=10) 以匹配 Python 行为和 ros2 topic hz 工具
        // 之前使用 SensorDataQoS (BestEffort) 导致 ros2 topic hz (默认 Reliable) 无法正常接收
        rclcpp::QoS qos_profile(10); 
        
        image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(
            camera_topic_,
            qos_profile
        );
        
        RCLCPP_INFO(this->get_logger(), "✓ Image 发布器已创建: %s (Reliable)", camera_topic_.c_str());
        
        // ====================================================================
        // 启动捕获线程
        // ====================================================================
        
        capturing_ = true;
        capture_thread_ = std::thread(&CameraDriverNode::capture_worker, this);
        
        RCLCPP_INFO(this->get_logger(), "✓ 捕获线程已启动");
        RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成");
        RCLCPP_INFO(this->get_logger(), "======================================");
    }
    
    /**
     * @brief 析构函数
     */
    ~CameraDriverNode() {
        capturing_ = false;
        if (capture_thread_.joinable()) {
            capture_thread_.join();
        }
        
        if (camera_.isOpened()) {
            camera_.release();
        }
        
        RCLCPP_INFO(this->get_logger(), "Camera Driver Node 已关闭");
    }
    
private:
    // ========================================================================
    // 成员变量
    // ========================================================================
    
    /** OpenCV 视频捕获对象 */
    cv::VideoCapture camera_;
    
    /** 摄像头参数 */
    std::string video_device_;
    double target_framerate_;
    int image_width_;
    int image_height_;
    std::string camera_topic_;
    std::string camera_frame_;
    bool enable_fps_stats_;
    
    /** 发布者 */
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
    
    /** 捕获线程 */
    std::thread capture_thread_;
    std::atomic<bool> capturing_{false};
    
    /** 统计信息 */
    uint32_t frame_counter_ = 0;
    std::chrono::system_clock::time_point last_stats_time_;
    
    // ========================================================================
    // 私有方法
    // ========================================================================
    
    /**
     * @brief 初始化摄像头
     */
    bool init_camera() {
        // 从设备路径提取设备号（如 /dev/video0 -> 0）
        int device_id = 0;
        try {
            std::string device_num_str = video_device_.substr(video_device_.find_last_not_of("0123456789") + 1);
            device_id = std::stoi(device_num_str);
        } catch (...) {
            RCLCPP_WARN(this->get_logger(), "无法从 %s 解析设备号，使用默认值 0", video_device_.c_str());
        }
        
        RCLCPP_INFO(this->get_logger(), "打开设备: %s (ID: %d)", video_device_.c_str(), device_id);
        
        // 打开摄像头
        camera_.open(device_id);
        if (!camera_.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "无法打开摄像头: %s", video_device_.c_str());
            return false;
        }

        // 强制使用 MJPEG 格式 (关键: 降低 USB 带宽占用，解决卡顿)
        camera_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
        
        // 设置分辨率
        camera_.set(cv::CAP_PROP_FRAME_WIDTH, image_width_);
        camera_.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_);
        
        // 设置帧率
        camera_.set(cv::CAP_PROP_FPS, target_framerate_);
        
        // 设置缓冲区大小为 1，以获得最新的帧
        camera_.set(cv::CAP_PROP_BUFFERSIZE, 1);
        
        // 尝试启用硬件加速 (在 Linux 上通常无效或不需要，但在某些 Jetson 上可能有帮助)
        // camera_.set(cv::CAP_PROP_HW_ACCELERATION, cv::VIDEO_ACCELERATION_ANY);
        
        // 验证设置
        double actual_width = camera_.get(cv::CAP_PROP_FRAME_WIDTH);
        double actual_height = camera_.get(cv::CAP_PROP_FRAME_HEIGHT);
        double actual_fps = camera_.get(cv::CAP_PROP_FPS);
        double padding = camera_.get(cv::CAP_PROP_FOURCC);
        char fourcc[5];
        // 简单的 FourCC 解码
        int fourcc_int = static_cast<int>(padding);
        fourcc[0] = fourcc_int & 0XFF;
        fourcc[1] = (fourcc_int & 0XFF00) >> 8;
        fourcc[2] = (fourcc_int & 0XFF0000) >> 16;
        fourcc[3] = (fourcc_int & 0XFF000000) >> 24;
        fourcc[4] = '\0';
        
        RCLCPP_INFO(this->get_logger(), "实际设置 - 分辨率: %.0fx%.0f, FPS: %.1f, Format: %s",
            actual_width, actual_height, actual_fps, fourcc);
        
        return true;
    }
    
    /**
     * @brief 捕获工作线程
     */
    void capture_worker() {
        RCLCPP_INFO(this->get_logger(), "📷 捕获线程开始工作");
        
        last_stats_time_ = std::chrono::system_clock::now();
        uint32_t last_frame_counter = 0;
        
        while (capturing_ && rclcpp::ok()) {
            cv::Mat frame;
            
            // 读取帧 - 注意: read() 通常是阻塞的，会等待硬件产生下一帧
            if (!camera_.read(frame)) {
                RCLCPP_WARN(this->get_logger(), "⚠️ 读取帧失败");
                // 失败后简单休眠，避免死循环占用 CPU
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                continue;
            }
            
            if (frame.empty()) {
                continue;
            }
            
            // 转换为 ROS 2 Image 消息
            try {
                // 使用 cv_bridge 转换
                auto cv_image = std::make_shared<cv_bridge::CvImage>();
                cv_image->header.stamp = this->now();
                cv_image->header.frame_id = camera_frame_;
                cv_image->encoding = sensor_msgs::image_encodings::BGR8;
                cv_image->image = frame;
                
                // 发布消息
                image_publisher_->publish(*cv_image->toImageMsg());
                
                frame_counter_++;
                
                // 定期报告统计 (每 300 帧，约 10 秒)
                if (enable_fps_stats_ && (frame_counter_ % 300 == 0)) {
                    auto now = std::chrono::system_clock::now();
                    auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
                        now - last_stats_time_
                    ).count();
                    
                    if (elapsed_seconds > 0) {
                        // 计算当前区间的 FPS
                        uint32_t current_frames = frame_counter_ - last_frame_counter;
                        uint32_t fps = current_frames / elapsed_seconds;
                        
                        RCLCPP_INFO(this->get_logger(),
                            "📊 Camera - 实时 FPS: %u, 总帧: %u, 每帧: %zu B",
                            fps, frame_counter_, frame.total() * frame.elemSize()
                        );
                        
                        last_stats_time_ = now;
                        last_frame_counter = frame_counter_;
                    }
                }
                
            } catch (const std::exception& e) {
                RCLCPP_ERROR(this->get_logger(), "转换失败: %s", e.what());
            }
            
        }
        
        RCLCPP_INFO(this->get_logger(), "📷 捕获线程已停止");
    }
    
    /**
     * @brief 报告统计信息 (Legacy)
     */
    void report_stats() {
        auto now = std::chrono::system_clock::now();
        auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
            now - last_stats_time_
        ).count();
        
        if (elapsed_seconds > 0) {
            uint32_t fps = frame_counter_ / elapsed_seconds;
            RCLCPP_INFO(this->get_logger(),
                "相机统计 - FPS: %u, 总帧: %u ,每帧大小为 %u B",
                fps, frame_counter_, image_width_ * image_height_ * 3
            );
        }
    }
};

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