/**
 * @file udp_sender_node.hpp
 * @brief SkyLink ROS 2 UDP 发送节点 - 头文件
 * 
 * 这个节点订阅相机和 GPS Topic，将图像编码为 JPEG 并通过 UDP 广播发送。
 * 
 * @author flowzl
 * @date 2026-01-19
 */

#pragma once

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <mavros_msgs/msg/global_position_target.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <thread>
#include <atomic>
#include <cstring>

// 协议定义
#include "protocol.h"

/**
 * @class UdpSenderNode
 * @brief ROS 2 UDP 发送节点
 * 
 * 负责：
 * 1. 订阅相机和 GPS 数据
 * 2. 编码图像为 JPEG
 * 3. 分片打包为 UDP 数据包
 * 4. 通过网络发送数据包
 */
class UdpSenderNode : public rclcpp::Node {
public:
    /**
     * @brief 构造函数
     */
    UdpSenderNode();
    
    /**
     * @brief 析构函数
     */
    ~UdpSenderNode();
    
private:
    // ========================================================================
    // ROS 2 订阅和发布
    // ========================================================================
    
    /** 相机图像订阅 */
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_subscriber_;
    
    /** GPS 全球位置订阅 */
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;
    
    // ========================================================================
    // 参数
    // ========================================================================
    
    std::string target_ip_;           ///< 目标 IP 地址
    uint16_t target_port_;            ///< 目标 UDP 端口
    std::string camera_topic_;        ///< 相机 Topic 名称
    std::string gps_topic_;           ///< GPS Topic 名称
    int jpeg_quality_;                ///< JPEG 压缩质量 (1-100)
    bool adaptive_quality_;           ///< 自适应质量标志
    int quality_min_, quality_max_;   ///< 质量范围
    bool enable_gps_;                 ///< 是否启用 GPS
    uint16_t max_packet_size_;        ///< 最大 UDP 包大小
    uint32_t frame_timeout_ms_;       ///< 帧超时时间
    bool enable_fps_stats_;           ///< FPS 统计
    bool enable_drop_stats_;          ///< 丢弃帧统计
    
    // ========================================================================
    // 套接字和网络
    // ========================================================================
    
    int udp_socket_;                  ///< UDP 套接字文件描述符
    uint32_t frame_counter_;          ///< 帧计数器
    std::atomic<uint32_t> bytes_sent_{0};  ///< 已发送字节数
    std::atomic<uint32_t> packets_sent_{0}; ///< 已发送包数
    std::atomic<uint32_t> frames_dropped_{0}; ///< 丢弃的帧数
    
    // ========================================================================
    // 异步发送队列和线程
    // ========================================================================
    
    /** UDP 数据包队列项 */
    struct PacketQueueItem {
        std::vector<uint8_t> data;
        std::chrono::system_clock::time_point enqueue_time;
    };
    
    std::queue<PacketQueueItem> send_queue_;      ///< 发送队列
    std::mutex send_queue_mutex_;                 ///< 队列互斥锁
    std::condition_variable send_queue_cv_;       ///< 队列条件变量
    std::thread sender_thread_;                   ///< 后台发送线程
    std::atomic<bool> sender_running_{true};      ///< 线程运行标志
    static const size_t MAX_QUEUE_SIZE = 5000;    ///< 队列最大大小
    static const size_t BATCH_SEND_SIZE = 128;    ///< 批量发送大小 (增加以减少锁竞争)
    
    /**
     * @brief 后台发送工作函数
     */
    void sender_worker();
    
    /**
     * @brief 将数据包加入发送队列
     * @param data 数据指针
     * @param len 数据长度
     */
    void enqueue_packet(const uint8_t* data, uint16_t len);

    /**
     * @brief 批量将数据包加入发送队列
     * @param packets 数据包列表 (将被移动)
     */
    void enqueue_packets(std::vector<std::vector<uint8_t>>& packets);

    // ========================================================================
    // GPS 数据缓存
    // ========================================================================
    
    struct {
        double latitude = 39.9042;    ///< 纬度
        double longitude = 116.4074;  ///< 经度
        double altitude = 100.0;      ///< 海拔
        std::mutex mutex;
    } current_gps_data_;
    
    // ========================================================================
    // 统计信息
    // ========================================================================
    
    struct {
        uint32_t total_frames = 0;
        uint32_t total_bytes = 0;
        uint32_t total_packets = 0;
        std::chrono::system_clock::time_point last_report_time;
    } stats_;
    
    std::mutex stats_mutex_;
    
    // ========================================================================
    // 回调函数
    // ========================================================================
    
    /**
     * @brief 相机图像回调
     * @param msg 图像消息
     */
    void camera_callback(const sensor_msgs::msg::Image::SharedPtr msg);
    
    /**
     * @brief GPS 全球位置回调
     * @param msg GPS 消息
     */
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    
    // ========================================================================
    // 网络函数
    // ========================================================================
    
    /**
     * @brief 初始化 UDP 套接字
     * @return 成功返回 true
     */
    bool init_udp_socket();
    
    /**
     * @brief 关闭 UDP 套接字
     */
    void close_udp_socket();
    
    /**
     * @brief 发送 UDP 数据包
     * @param data 数据指针
     * @param len 数据长度
     * @return 发送的字节数，失败返回 -1
     */
    int send_udp_packet(const uint8_t* data, uint16_t len);
    
    // ========================================================================
    // 图像处理函数
    // ========================================================================
    
    /**
     * @brief 将 OpenCV Mat 编码为 JPEG
     * @param frame OpenCV 图像
     * @param quality 压缩质量
     * @param output 输出 JPEG 数据
     * @return 成功返回 true
     */
    bool encode_to_jpeg(const cv::Mat& frame, int quality, std::vector<uint8_t>& output);
    
    /**
     * @brief 将 JPEG 数据分片为 UDP 包并发送
     * @param jpeg_data JPEG 数据指针
     * @param jpeg_len JPEG 数据长度
     * @param timestamp 时间戳
     * @param lat 纬度
     * @param lon 经度
     * @param alt 海拔
     * @return 发送的包数
     */
    uint16_t fragment_and_send(
        const uint8_t* jpeg_data,
        uint32_t jpeg_len,
        double timestamp,
        double lat,
        double lon,
        double alt
    );
    
    // ========================================================================
    // 统计和监控
    // ========================================================================
    
    /**
     * @brief 更新统计信息
     * @param bytes_count 新增字节数
     * @param packet_count 新增包数
     */
    void update_stats(uint32_t bytes_count, uint32_t packet_count);
    
    /**
     * @brief 定期报告统计信息
     */
    void report_stats();
};

