/**
 * @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/compressed_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 <fstream>
#include <filesystem>
#include <set>

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

/**
 * @class UdpSenderNode
 * @brief ROS 2 UDP 发送节点
 * 
 * 负责：
 * 1. 监控指定目录下的 JPEG 文件
 * 2. 读取文件并分片打包为 UDP 数据包
 * 3. 通过网络发送数据包
 * 4. 记录发送日志
 */
class UdpSenderNode : public rclcpp::Node {
public:
    /**
     * @brief 构造函数
     */
    UdpSenderNode();
    
    /**
     * @brief 析构函数
     */
    ~UdpSenderNode();
    
private:
    // ========================================================================
    // ROS 2 订阅和发布
    // ========================================================================
    
    /** GPS 订阅 */
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;

    /** 文件扫描定时器 */
    rclcpp::TimerBase::SharedPtr file_scan_timer_;
    
    // ========================================================================
    // 成员变量
    // ========================================================================
    
    // 网络相关
    std::string target_ip_;
    uint16_t target_port_;
    int udp_socket_;
    
    // GPS 数据
    std::string gps_topic_;
    bool enable_gps_;
    
    // 配置参数
    uint16_t max_packet_size_;
    uint32_t frame_timeout_ms_;
    bool enable_fps_stats_;
    bool enable_drop_stats_;
    
    // 图像源配置
    std::string save_dir_;      // 图片保存目录
    std::string log_file_path_; // 日志文件路径
    std::set<std::string> sent_files_; // 已发送文件记录
    std::filesystem::file_time_type last_check_time_; // 上次检查时间
    
    // 日志流
    std::ofstream log_file_stream_;

    // 统计相关
    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;    ///< 批量发送大小
    
    // ========================================================================
    // 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_;
    
    // ========================================================================
    // 方法声明
    // ========================================================================
    
    // ---- 初始化与销毁 ----
    bool init_udp_socket();
    void close_udp_socket();
    
    // ---- 核心工作流程 ----
    void sender_worker();
    void scan_and_send_files();
    void process_file(const std::string& filepath, const std::string& filename);
    
    // ---- 队列管理 ----
    void enqueue_packets(std::vector<std::vector<uint8_t>>& packets);
    int send_udp_packet(const uint8_t* data, uint16_t len);
    
    // ---- 辅助功能 ----
    void log_sent_file(const std::string& filename, size_t size);
    
    // ---- 回调函数 ----
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    
    // ---- 统计报告 ----
    void report_stats();

};

