/**
 * @file camera_driver_node.cpp
 * @brief V4L2 相机驱动节点 - 实现文件
 * 
 * 使用 V4L2 直接读取 USB 摄像头 MJPEG 流，支持定时拍照和服务触发拍照
 * 
 * @author flowzl
 * @date 2026-01-19
 */

#include "skylink_bridge/camera_driver_node.h"
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <errno.h>
#include <cstring>
#include <sstream>
#include <iomanip>

/**
 * @brief 构造函数
 */
CameraDriverNode::CameraDriverNode() : rclcpp::Node("camera_driver") {
    // ====================================================================
    // 声明参数
    // ====================================================================
    
    this->declare_parameter<std::string>("video_device", "/dev/video1");
    this->declare_parameter<double>("framerate", 30.0);
    this->declare_parameter<int>("image_width", 640);
    this->declare_parameter<int>("image_height", 480);
    this->declare_parameter<bool>("enable_fps_stats", true);
    
    // 拍照功能参数
    this->declare_parameter<std::string>("save_dir", "./camera_data");
    this->declare_parameter<double>("interval", 5.0);  // 默认5秒
    this->declare_parameter<int>("photo_count", 10);   // 默认10张
    this->declare_parameter<bool>("capture_enabled", true);
    this->declare_parameter<std::string>("gps_topic", "/mavros/global_position/global");
    this->declare_parameter<int>("thumbnail_width", 640);
    this->declare_parameter<int>("thumbnail_height", 480);
    
    // HTTP 服务器参数
    this->declare_parameter<int>("http_port", 8080);
    this->declare_parameter<bool>("http_enabled", 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();
    enable_fps_stats_ = this->get_parameter("enable_fps_stats").as_bool();
    
    // 拍照参数
    save_dir_ = this->get_parameter("save_dir").as_string();
    capture_interval_ = this->get_parameter("interval").as_double();
    photo_count_per_batch_ = this->get_parameter("photo_count").as_int();
    capture_enabled_ = this->get_parameter("capture_enabled").as_bool();
    gps_topic_ = this->get_parameter("gps_topic").as_string();
    thumbnail_width_ = this->get_parameter("thumbnail_width").as_int();
    thumbnail_height_ = this->get_parameter("thumbnail_height").as_int();
    
    // HTTP 服务器参数
    http_port_ = this->get_parameter("http_port").as_int();
    http_enabled_ = this->get_parameter("http_enabled").as_bool();
    
    // 创建保存目录
    std::filesystem::create_directories(save_dir_);
    
    // ====================================================================
    // 初始化摄像头
    // ====================================================================
    
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "V4L2 Native 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(), "✓ 摄像头初始化成功");
    
    // ====================================================================
    // 订阅 GPS
    // ====================================================================
    
    gps_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        gps_topic_, 10,
        [this](sensor_msgs::msg::NavSatFix::SharedPtr msg) {
            std::lock_guard<std::mutex> lock(gps_mutex_);
            latest_gps_fix_ = *msg;
            has_gps_fix_ = true;
        });
        
    RCLCPP_INFO(this->get_logger(), "✓ GPS 订阅已创建: %s", gps_topic_.c_str());

    // ====================================================================
    // 创建服务接口
    // ====================================================================
    
    trigger_service_ = this->create_service<skylink_bridge::srv::TriggerCapture>(
        "trigger_capture",
        std::bind(&CameraDriverNode::service_callback, this,
                 std::placeholders::_1, std::placeholders::_2)
    );
    
    RCLCPP_INFO(this->get_logger(), "✓ 服务接口已创建: /camera_driver/trigger_capture");
    
    // ====================================================================
    // 启动 HTTP 服务器（如果启用）
    // ====================================================================
    
    if (http_enabled_) {
        start_http_server();
        RCLCPP_INFO(this->get_logger(), "✓ HTTP REST API 服务器已启动，监听端口: %d", http_port_);
    } else {
        RCLCPP_INFO(this->get_logger(), "HTTP REST API 服务器已禁用");
    }
    
    // ====================================================================
    // 启动捕获线程（用于获取帧数据）
    // ====================================================================
    
    capturing_ = true;
    capture_thread_ = std::thread(&CameraDriverNode::capture_worker, this);
    
    RCLCPP_INFO(this->get_logger(), "✓ 捕获线程已启动");
    
    // ====================================================================
    // 启动定时拍照（如果启用）
    // ====================================================================
    
    if (capture_enabled_ && capture_interval_ > 0) {
        // 计算拍照间隔：总时间 / 拍照数量
        double photo_interval = capture_interval_ / photo_count_per_batch_;
        if (photo_interval < 0.1) {
            photo_interval = 0.1;  // 最小间隔0.1秒
            RCLCPP_WARN(this->get_logger(), "拍照间隔过小，调整为 %.2f 秒", photo_interval);
        }
        
        RCLCPP_INFO(this->get_logger(), "定时拍照配置: 每 %.2f 秒拍 %d 张，间隔 %.2f 秒",
                   capture_interval_, photo_count_per_batch_, photo_interval);
        
        // 启动定时器（默认使用原图模式）
        start_batch_capture(capture_interval_, photo_count_per_batch_, photo_interval, TransmissionMode::ORIGINAL);
    } else {
        RCLCPP_INFO(this->get_logger(), "定时拍照已禁用，仅通过服务触发");
    }
    
    RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成");
    RCLCPP_INFO(this->get_logger(), "======================================");
}

/**
 * @brief 析构函数
 */
CameraDriverNode::~CameraDriverNode() {
    // 停止 HTTP 服务器
    stop_http_server();
    
    // 停止定时器
    if (capture_timer_) {
        capture_timer_->cancel();
    }
    
    capturing_ = false;
    if (capture_thread_.joinable()) {
        capture_thread_.join();
    }
    
    stop_capturing();
    uninit_device();
    close_device();
    
    RCLCPP_INFO(this->get_logger(), "Camera Driver Node 已关闭，共保存 %u 张照片", total_photos_saved_.load());
}

// xioctl helper
int CameraDriverNode::xioctl(int fh, int request, void *arg) {
    int r;
    do {
        r = ioctl(fh, request, arg);
    } while (-1 == r && EINTR == errno);
    return r;
}

/**
 * @brief 关闭设备
 */
void CameraDriverNode::close_device() {
    if (fd_ != -1) {
        close(fd_);
        fd_ = -1;
    }
}

/**
 * @brief 释放内存映射
 */
void CameraDriverNode::uninit_device() {
    for (const auto& buffer : buffers_) {
        if (-1 == munmap(buffer.start, buffer.length)) {
            perror("munmap");
        }
    }
    buffers_.clear();
}

/**
 * @brief 停止流
 */
void CameraDriverNode::stop_capturing() {
    if (fd_ != -1) {
        enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        xioctl(fd_, VIDIOC_STREAMOFF, &type);
    }
}

/**
 * @brief 初始化摄像头
 */
bool CameraDriverNode::init_camera() {
    struct stat st;
    if (-1 == stat(video_device_.c_str(), &st)) {
        RCLCPP_ERROR(this->get_logger(), "无法识别 '%s': %d, %s", video_device_.c_str(), errno, strerror(errno));
        return false;
    }
    if (!S_ISCHR(st.st_mode)) {
        RCLCPP_ERROR(this->get_logger(), "%s 不是字符设备", video_device_.c_str());
        return false;
    }

    fd_ = open(video_device_.c_str(), O_RDWR | O_NONBLOCK, 0);
    if (-1 == fd_) {
        RCLCPP_ERROR(this->get_logger(), "无法打开 '%s': %d, %s", video_device_.c_str(), errno, strerror(errno));
        return false;
    }

    struct v4l2_capability cap;
    if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
         RCLCPP_ERROR(this->get_logger(), "不是 V4L2 设备");
         return false;
    }
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        RCLCPP_ERROR(this->get_logger(), "不是视频捕获设备");
        return false;
    }
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        RCLCPP_ERROR(this->get_logger(), "不支持流 I/O");
        return false;
    }
    device_name_ = reinterpret_cast<const char*>(cap.card);

    // 设置格式
    struct v4l2_format fmt;
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = image_width_;
    fmt.fmt.pix.height = image_height_;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; // 强制 MJPEG
    fmt.fmt.pix.field = V4L2_FIELD_ANY;

    if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) {
        RCLCPP_ERROR(this->get_logger(), "设置像素格式失败");
        return false;
    }
    
    // 检查实际设置的格式
    if (-1 == xioctl(fd_, VIDIOC_G_FMT, &fmt)) {
        RCLCPP_WARN(this->get_logger(), "无法获取实际格式");
    } else {
        char fourcc[5] = {0};
        fourcc[0] = (fmt.fmt.pix.pixelformat) & 0xFF;
        fourcc[1] = (fmt.fmt.pix.pixelformat >> 8) & 0xFF;
        fourcc[2] = (fmt.fmt.pix.pixelformat >> 16) & 0xFF;
        fourcc[3] = (fmt.fmt.pix.pixelformat >> 24) & 0xFF;
        RCLCPP_INFO(this->get_logger(), "实际设置的格式: %s (0x%08X), 分辨率: %dx%d",
                   fourcc, fmt.fmt.pix.pixelformat, 
                   fmt.fmt.pix.width, fmt.fmt.pix.height);
        
        if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG) {
            RCLCPP_WARN(this->get_logger(), 
                "⚠️ 设备不支持MJPEG格式！实际格式: %s，可能需要使用其他格式或软件编码",
                fourcc);
        }

        pixel_format_fourcc_ = fmt.fmt.pix.pixelformat;
    }

    // 设置帧率
    struct v4l2_streamparm streamparm;
    memset(&streamparm, 0, sizeof(streamparm));
    streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    streamparm.parm.capture.timeperframe.numerator = 1;
    streamparm.parm.capture.timeperframe.denominator = (unsigned int)target_framerate_;
    
    if (xioctl(fd_, VIDIOC_S_PARM, &streamparm) != -1) {
        double actual_fps = (double)streamparm.parm.capture.timeperframe.denominator / 
                            streamparm.parm.capture.timeperframe.numerator;
         RCLCPP_INFO(this->get_logger(), "尝试设置FPS: %.1f, 实际FPS: %.1f", target_framerate_, actual_fps);
    }

    // 请求 buffers
    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
        RCLCPP_ERROR(this->get_logger(), "VIDIOC_REQBUFS 失败");
        return false;
    }
    if (req.count < 2) {
        RCLCPP_WARN(this->get_logger(), "Buffer 数量过少: %d", req.count);
    }

    buffers_.resize(req.count);

    for (size_t i = 0; i < req.count; ++i) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) {
             RCLCPP_ERROR(this->get_logger(), "VIDIOC_QUERYBUF 失败");
             return false;
        }

        buffers_[i].length = buf.length;
        buffers_[i].start = mmap(NULL, buf.length,
                                 PROT_READ | PROT_WRITE, MAP_SHARED,
                                 fd_, buf.m.offset);

        if (MAP_FAILED == buffers_[i].start) {
             RCLCPP_ERROR(this->get_logger(), "mmap 失败");
             return false;
        }
    }
    
    RCLCPP_INFO(this->get_logger(), "实际设置 - 分辨率: %dx%d, MJPEG",
        fmt.fmt.pix.width, fmt.fmt.pix.height);
    
    // 开始流
    for (size_t i = 0; i < buffers_.size(); ++i) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
             RCLCPP_ERROR(this->get_logger(), "VIDIOC_QBUF 失败");
             return false;
        }
    }
    
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
         RCLCPP_ERROR(this->get_logger(), "VIDIOC_STREAMON 失败");
         return false;
    }

    return true;
}

/**
 * @brief 捕获工作线程（持续获取帧数据并缓存）
 */
void CameraDriverNode::capture_worker() {
    RCLCPP_INFO(this->get_logger(), "📷 捕获线程开始工作 (V4L2)");
    
    last_stats_time_ = std::chrono::system_clock::now();
    uint32_t last_frame_counter = 0;
    
    while (capturing_ && rclcpp::ok()) {
        fd_set fds;
        struct timeval tv;
        int r;

        FD_ZERO(&fds);
        FD_SET(fd_, &fds);

        tv.tv_sec = 2; // Timeout
        tv.tv_usec = 0;

        r = select(fd_ + 1, &fds, NULL, NULL, &tv);

        if (-1 == r) {
             if (EINTR == errno) continue;
             RCLCPP_ERROR(this->get_logger(), "select error");
             break;
        }
        if (0 == r) {
             // Timeout - 继续等待
             continue;
        }

        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;

        if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
            if (errno == EAGAIN) continue;
             RCLCPP_ERROR(this->get_logger(), "VIDIOC_DQBUF error: %d, %s", errno, strerror(errno));
             continue; // try next
        }
        
        // 数据在 buffers_[buf.index].start，长度 buf.bytesused
        try {
            // 缓存最新帧数据（用于拍照）
            {
                std::lock_guard<std::mutex> lock(frame_data_mutex_);
                latest_frame_data_.resize(buf.bytesused);
                memcpy(latest_frame_data_.data(), buffers_[buf.index].start, buf.bytesused);
                has_frame_data_ = true;
            }
            
            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) {
                    uint32_t current_frames = frame_counter_ - last_frame_counter;
                    uint32_t fps = current_frames / elapsed_seconds;
                    current_fps_ = static_cast<float>(fps);
                    
                    RCLCPP_INFO(this->get_logger(),
                        "相机统计 - 实时 FPS: %u, 总帧: %u, 已保存照片: %u",
                        fps, frame_counter_, total_photos_saved_.load()
                    );
                    
                    last_stats_time_ = now;
                    last_frame_counter = frame_counter_;
                }
            }
            
        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "处理帧失败: %s", e.what());
        }

        // 放回队列
        if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
             RCLCPP_ERROR(this->get_logger(), "VIDIOC_QBUF error");
        }
    }
    
    RCLCPP_INFO(this->get_logger(), "📷 捕获线程已停止");
}

/**
 * @brief 生成缩略图
 */
std::vector<uint8_t> CameraDriverNode::generate_thumbnail(const std::vector<uint8_t>& original_data) {
    try {
        // 解码JPEG
        cv::Mat img = cv::imdecode(original_data, cv::IMREAD_COLOR);
        if (img.empty()) {
            RCLCPP_WARN(this->get_logger(), "无法解码JPEG图像");
            return original_data;  // 返回原图
        }
        
        // 计算缩放比例
        double scale_w = static_cast<double>(thumbnail_width_) / img.cols;
        double scale_h = static_cast<double>(thumbnail_height_) / img.rows;
        double scale = std::min(scale_w, scale_h);
        
        int new_width = static_cast<int>(img.cols * scale);
        int new_height = static_cast<int>(img.rows * scale);
        
        // 缩放图像
        cv::Mat thumbnail;
        cv::resize(img, thumbnail, cv::Size(new_width, new_height), 0, 0, cv::INTER_AREA);
        
        // 编码为JPEG
        std::vector<int> compression_params;
        compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
        compression_params.push_back(85);  // 缩略图质量
        
        std::vector<uint8_t> thumbnail_data;
        cv::imencode(".jpg", thumbnail, thumbnail_data, compression_params);
        
        RCLCPP_DEBUG(this->get_logger(), "生成缩略图: %dx%d -> %dx%d (大小: %zu -> %zu bytes)",
                    img.cols, img.rows, new_width, new_height, original_data.size(), thumbnail_data.size());
        
        return thumbnail_data;
    } catch (const cv::Exception& e) {
        RCLCPP_ERROR(this->get_logger(), "生成缩略图失败: %s", e.what());
        return original_data;  // 返回原图
    }
}

/**
 * @brief 拍照并保存
 */
void CameraDriverNode::capture_photo(TransmissionMode mode) {
    std::lock_guard<std::mutex> lock(capture_mutex_);
    
    if (!has_frame_data_) {
        RCLCPP_WARN(this->get_logger(), "没有可用的帧数据，跳过拍照");
        return;
    }
    
    // 获取当前时间戳
    auto now = std::chrono::system_clock::now();
    auto time_t = std::chrono::system_clock::to_time_t(now);
            
    // 获取 GPS 信息
    double lat = 0.0, lon = 0.0, alt = 0.0;
    {
        std::lock_guard<std::mutex> gps_lock(gps_mutex_);
        if (has_gps_fix_) {
            lat = latest_gps_fix_.latitude;
            lon = latest_gps_fix_.longitude;
            alt = latest_gps_fix_.altitude;
        }
    }
    
    // 复制帧数据
    std::vector<uint8_t> frame_data;
    {
        std::lock_guard<std::mutex> frame_lock(frame_data_mutex_);
        frame_data = latest_frame_data_;
    }
    
    // 根据传输模式处理图像
    std::string mode_suffix = (mode == TransmissionMode::THUMBNAIL) ? "_thumb" : "";
    if (mode == TransmissionMode::THUMBNAIL) {
        frame_data = generate_thumbnail(frame_data);
    }
    
    // 生成文件名：photo_{id}_{time}_{gps}{mode}.jpg
    std::stringstream filename;
    filename << save_dir_ << "/photo_"
             << std::setfill('0') << std::setw(3) << total_photos_saved_.load() << "_"
             << std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S")
             << "_GPS_lat" << std::fixed << std::setprecision(6) << lat 
             << "_lon" << std::fixed << std::setprecision(6) << lon 
             << "_alt" << std::fixed << std::setprecision(1) << alt
             << mode_suffix
             << ".jpg";
    
    // 保存文件
    std::ofstream file(filename.str(), std::ios::binary);
    if (!file.is_open()) {
        RCLCPP_ERROR(this->get_logger(), "无法创建文件: %s", filename.str().c_str());
        return;
    }
    
    file.write(reinterpret_cast<const char*>(frame_data.data()), frame_data.size());
    file.close();
    
    total_photos_saved_++;
    
    const char* mode_str = (mode == TransmissionMode::THUMBNAIL) ? "缩略图" : "原图";
    RCLCPP_INFO(this->get_logger(), "✓ 照片已保存 (%s): %s (总照片数: %u, 大小: %zu bytes)",
               mode_str, filename.str().c_str(), total_photos_saved_.load(), frame_data.size());
}

/**
 * @brief 启动批量拍照
 */
void CameraDriverNode::start_batch_capture(double duration, int count, double interval, TransmissionMode mode) {
    if (is_capturing_) {
        RCLCPP_WARN(this->get_logger(), "拍照任务正在进行中，忽略新请求");
        return;
    }
    
    is_capturing_ = true;
    current_photo_index_ = 0;
    
    // 计算实际参数
    double actual_duration = (duration > 0) ? duration : capture_interval_;
    int actual_count = (count > 0) ? count : photo_count_per_batch_;
    double actual_interval = (interval > 0) ? interval : (actual_duration / actual_count);
    
    if (actual_interval < 0.1) {
        actual_interval = 0.1;
    }
    
    const char* mode_str = (mode == TransmissionMode::THUMBNAIL) ? "缩略图" : "原图";
    RCLCPP_INFO(this->get_logger(), "开始批量拍照 (%s): 总时间=%.2f秒, 数量=%d, 间隔=%.2f秒",
               mode_str, actual_duration, actual_count, actual_interval);
    
    // 创建定时器
    auto timer_duration = std::chrono::milliseconds(static_cast<int>(actual_interval * 1000));
    
    capture_timer_ = this->create_wall_timer(
        timer_duration,
        [this, actual_count, mode]() {
            if (current_photo_index_ < actual_count) {
                capture_photo(mode);
                current_photo_index_++;
            } else {
                // 完成拍照
                capture_timer_->cancel();
                is_capturing_ = false;
                RCLCPP_INFO(this->get_logger(), "批量拍照完成，共保存 %d 张照片", current_photo_index_);
            }
        }
    );
}

/**
 * @brief 服务回调函数
 */
void CameraDriverNode::service_callback(
    const std::shared_ptr<skylink_bridge::srv::TriggerCapture::Request> request,
    std::shared_ptr<skylink_bridge::srv::TriggerCapture::Response> response)
{
    // 解析传输模式
    TransmissionMode mode = (request->transmission_mode == 1) ? TransmissionMode::THUMBNAIL : TransmissionMode::ORIGINAL;
    const char* mode_str = (mode == TransmissionMode::THUMBNAIL) ? "缩略图" : "原图";
    
    RCLCPP_INFO(this->get_logger(), "收到拍照请求 (%s): duration=%.2f, count=%d, interval=%.2f",
               mode_str, request->duration, request->count, request->interval);
    
    // 如果正在拍照，先停止当前任务
    if (is_capturing_ && capture_timer_) {
        capture_timer_->cancel();
        is_capturing_ = false;
    }
    
    // 计算参数
    double duration = (request->duration > 0) ? request->duration : capture_interval_;
    int count = (request->count > 0) ? request->count : photo_count_per_batch_;
    double interval = (request->interval > 0) ? request->interval : (duration / count);
    
    if (interval < 0.1) {
        interval = 0.1;
    }
    
    // 启动拍照
    start_batch_capture(duration, count, interval, mode);
    
    response->success = true;
    response->message = std::string("拍照任务已启动 (") + mode_str + ")";
    response->total_photos = total_photos_saved_;
    
    RCLCPP_INFO(this->get_logger(), "服务响应: success=%d, total_photos=%d",
               response->success, response->total_photos);
}

/**
 * @brief 报告统计信息 (Legacy)
 */
void CameraDriverNode::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",
            fps, frame_counter_
        );
    }
}

// ========================================================================
// HTTP 服务器实现
// ========================================================================

/**
 * @brief 启动 HTTP 服务器
 */
void CameraDriverNode::start_http_server() {
    if (http_server_running_) {
        RCLCPP_WARN(this->get_logger(), "HTTP 服务器已在运行");
        return;
    }
    
    http_server_running_ = true;
    http_server_thread_ = std::thread(&CameraDriverNode::http_server_worker, this);
}

/**
 * @brief 停止 HTTP 服务器
 */
void CameraDriverNode::stop_http_server() {
    if (!http_server_running_) {
        return;
    }
    
    http_server_running_ = false;
    
    // 发送一个请求来唤醒服务器，使其能够退出
    // httplib 服务器在 stop() 调用后会退出
    if (http_server_thread_.joinable()) {
        http_server_thread_.join();
    }
    
    RCLCPP_INFO(this->get_logger(), "HTTP 服务器已停止");
}

/**
 * @brief HTTP 服务器工作线程
 */
void CameraDriverNode::http_server_worker() {
    httplib::Server svr;
    
    // 设置 CORS 头（允许跨域请求）
    svr.set_default_headers({
        {"Access-Control-Allow-Origin", "*"},
        {"Access-Control-Allow-Methods", "GET, POST, OPTIONS"},
        {"Access-Control-Allow-Headers", "Content-Type"}
    });
    
    // 处理 OPTIONS 请求（CORS 预检）
    svr.Options(".*", [](const httplib::Request&, httplib::Response& res) {
        res.status = 200;
    });
    
    // POST /api/v1/trigger_capture - 触发拍照
    svr.Post("/api/v1/trigger_capture", [this](const httplib::Request& req, httplib::Response& res) {
        this->handle_trigger_capture_api(req, res);
    });
    
    // GET /api/v1/status - 获取节点状态
    svr.Get("/api/v1/status", [this](const httplib::Request& req, httplib::Response& res) {
        this->handle_status_api(req, res);
    });
    
    // 健康检查端点
    svr.Get("/health", [](const httplib::Request&, httplib::Response& res) {
        res.set_content("{\"status\":\"ok\"}", "application/json");
    });
    
    RCLCPP_INFO(this->get_logger(), "HTTP 服务器线程启动，监听端口 %d", http_port_);
    
    // 启动服务器（阻塞调用）
    if (!svr.listen("0.0.0.0", http_port_)) {
        RCLCPP_ERROR(this->get_logger(), "HTTP 服务器启动失败，端口 %d 可能已被占用", http_port_);
        http_server_running_ = false;
    }
}

/**
 * @brief 处理状态查询 API 请求
 */
void CameraDriverNode::handle_status_api(const httplib::Request& /* req */, httplib::Response& res) {
    try {
        // 构建 JSON 响应
        std::ostringstream json;
        json << "{"
             << "\"device_name\":\"" << device_name_ << "\","
             << "\"image_width\":" << image_width_ << ","
             << "\"image_height\":" << image_height_ << ","
             << "\"current_fps\":" << std::fixed << std::setprecision(1) << current_fps_ << ","
             << "\"total_frames\":" << frame_counter_ << ","
             << "\"total_photos\":" << total_photos_saved_.load() << ","
             << "\"is_capturing\":" << (is_capturing_ ? "true" : "false")
             << "}";
        
        res.status = 200;
        res.set_content(json.str(), "application/json");
        
    } catch (const std::exception& e) {
        std::string error_json = R"({"error":"服务器错误: )" + std::string(e.what()) + "\"}";
        res.status = 500;
        res.set_content(error_json, "application/json");
        RCLCPP_ERROR(this->get_logger(), "HTTP API 状态查询错误: %s", e.what());
    }
}

/**
 * @brief JSON 解析辅助函数 - double 特化
 */
double CameraDriverNode::parse_json_value_double(const std::string& json_str, const std::string& key, double default_value) {
    std::string search_key = "\"" + key + "\":";
    size_t pos = json_str.find(search_key);
    if (pos == std::string::npos) {
        return default_value;
    }
    
    pos += search_key.length();
    // 跳过空格
    while (pos < json_str.length() && (json_str[pos] == ' ' || json_str[pos] == '\t')) {
        pos++;
    }
    
    // 提取数字
    size_t end_pos = pos;
    while (end_pos < json_str.length() && 
           (std::isdigit(json_str[end_pos]) || json_str[end_pos] == '.' || 
            json_str[end_pos] == '-' || json_str[end_pos] == '+' || json_str[end_pos] == 'e' || json_str[end_pos] == 'E')) {
        end_pos++;
    }
    
    if (end_pos > pos) {
        try {
            return std::stod(json_str.substr(pos, end_pos - pos));
        } catch (...) {
            return default_value;
        }
    }
    
    return default_value;
}

/**
 * @brief JSON 解析辅助函数 - int 特化
 */
int CameraDriverNode::parse_json_value_int(const std::string& json_str, const std::string& key, int default_value) {
    std::string search_key = "\"" + key + "\":";
    size_t pos = json_str.find(search_key);
    if (pos == std::string::npos) {
        return default_value;
    }
    
    pos += search_key.length();
    // 跳过空格
    while (pos < json_str.length() && (json_str[pos] == ' ' || json_str[pos] == '\t')) {
        pos++;
    }
    
    // 提取整数
    size_t end_pos = pos;
    while (end_pos < json_str.length() && 
           (std::isdigit(json_str[end_pos]) || json_str[end_pos] == '-' || json_str[end_pos] == '+')) {
        end_pos++;
    }
    
    if (end_pos > pos) {
        try {
            return std::stoi(json_str.substr(pos, end_pos - pos));
        } catch (...) {
            return default_value;
        }
    }
    
    return default_value;
}

/**
 * @brief 处理触发拍照 API 请求
 */
void CameraDriverNode::handle_trigger_capture_api(const httplib::Request& req, httplib::Response& res) {
    try {
        // 解析 JSON 请求体
        std::string json_body = req.body;
        
        // 提取参数（简单的 JSON 解析）
        double duration = parse_json_value_double(json_body, "duration", 0.0);
        int count = parse_json_value_int(json_body, "count", 0);
        double interval = parse_json_value_double(json_body, "interval", 0.0);
        int transmission_mode = parse_json_value_int(json_body, "transmission_mode", 0);
        
        // 参数验证
        if (duration < 0 || count < 0 || interval < 0) {
            std::string error_json = R"({"success":false,"message":"参数错误: 参数不能为负数","total_photos":)" +
                                    std::to_string(total_photos_saved_.load()) + "}";
            res.status = 400;
            res.set_content(error_json, "application/json");
            return;
        }
        
        if (transmission_mode != 0 && transmission_mode != 1) {
            std::string error_json = R"({"success":false,"message":"参数错误: transmission_mode 必须为 0 或 1","total_photos":)" +
                                    std::to_string(total_photos_saved_.load()) + "}";
            res.status = 400;
            res.set_content(error_json, "application/json");
            return;
        }
        
        // 解析传输模式
        TransmissionMode mode = (transmission_mode == 1) ? TransmissionMode::THUMBNAIL : TransmissionMode::ORIGINAL;
        const char* mode_str = (mode == TransmissionMode::THUMBNAIL) ? "缩略图" : "原图";
        
        RCLCPP_INFO(this->get_logger(), "HTTP API: 收到拍照请求 (%s): duration=%.2f, count=%d, interval=%.2f",
                   mode_str, duration, count, interval);
        
        // 如果正在拍照，先停止当前任务
        if (is_capturing_ && capture_timer_) {
            capture_timer_->cancel();
            is_capturing_ = false;
        }
        
        // 计算参数
        double actual_duration = (duration > 0) ? duration : capture_interval_;
        int actual_count = (count > 0) ? count : photo_count_per_batch_;
        double actual_interval = (interval > 0) ? interval : (actual_duration / actual_count);
        
        if (actual_interval < 0.1) {
            actual_interval = 0.1;
        }
        
        // 启动拍照
        start_batch_capture(actual_duration, actual_count, actual_interval, mode);
        
        // 构建 JSON 响应
        std::ostringstream json_stream;
        json_stream << "{\"success\":true,\"message\":\"拍照任务已启动 (" << mode_str << ")\",\"total_photos\":" 
                    << total_photos_saved_.load() << "}";
        std::string response_json = json_stream.str();
        
        res.status = 200;
        res.set_content(response_json, "application/json");
        
        RCLCPP_INFO(this->get_logger(), "HTTP API: 响应成功，total_photos=%u", total_photos_saved_.load());
        
    } catch (const std::exception& e) {
        std::string error_json = R"({"success":false,"message":"服务器错误: )" + 
                                std::string(e.what()) + R"(","total_photos":)" +
                                std::to_string(total_photos_saved_.load()) + "}";
        res.status = 500;
        res.set_content(error_json, "application/json");
        RCLCPP_ERROR(this->get_logger(), "HTTP API 处理错误: %s", e.what());
    }
}

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