/**
 * @file udp_sender_node.cpp
 * @brief SkyLink ROS 2 UDP 发送节点 - 实现文件
 * 
 * 完整实现相机和 GPS 数据采集、JPEG 编码、UDP 分片发送的功能。
 * 
 * @author flowzl
 * @date 2026-01-19
 */

#include "skylink_bridge/udp_sender_node.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#include <cerrno>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <thread>

/**
 * @brief 构造函数
 */
UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1), frame_counter_(0) {
    // ========================================================================
    // 声明和读取参数
    // ========================================================================
    
    this->declare_parameter<std::string>("target_ip", "255.255.255.255");
    this->declare_parameter<int>("target_port", 9999);
    this->declare_parameter<std::string>("camera_topic", "/camera/image_raw");
    this->declare_parameter<std::string>("gps_topic", "/mavros/global_position/global");
    this->declare_parameter<int>("jpeg_quality", 70);
    this->declare_parameter<bool>("adaptive_quality", true);
    this->declare_parameter<int>("quality_min", 30);
    this->declare_parameter<int>("quality_max", 95);
    this->declare_parameter<bool>("enable_gps", true);
    this->declare_parameter<int>("max_packet_size", 500);
    this->declare_parameter<int>("frame_timeout_ms", 5000);
    this->declare_parameter<bool>("enable_fps_stats", true);
    this->declare_parameter<bool>("enable_drop_stats", true);
    
    // 读取参数
    target_ip_ = this->get_parameter("target_ip").as_string();
    target_port_ = static_cast<uint16_t>(this->get_parameter("target_port").as_int());
    camera_topic_ = this->get_parameter("camera_topic").as_string();
    gps_topic_ = this->get_parameter("gps_topic").as_string();
    jpeg_quality_ = this->get_parameter("jpeg_quality").as_int();
    adaptive_quality_ = this->get_parameter("adaptive_quality").as_bool();
    quality_min_ = this->get_parameter("quality_min").as_int();
    quality_max_ = this->get_parameter("quality_max").as_int();
    enable_gps_ = this->get_parameter("enable_gps").as_bool();
    max_packet_size_ = static_cast<uint16_t>(this->get_parameter("max_packet_size").as_int());
    frame_timeout_ms_ = static_cast<uint32_t>(this->get_parameter("frame_timeout_ms").as_int());
    enable_fps_stats_ = this->get_parameter("enable_fps_stats").as_bool();
    enable_drop_stats_ = this->get_parameter("enable_drop_stats").as_bool();
    
    // ========================================================================
    // 打印初始化信息
    // ========================================================================
    
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "SkyLink UDP Sender Node 启动");
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "目标 IP: %s:%d", target_ip_.c_str(), target_port_);
    RCLCPP_INFO(this->get_logger(), "相机 Topic: %s", camera_topic_.c_str());
    RCLCPP_INFO(this->get_logger(), "GPS Topic: %s", gps_topic_.c_str());
    RCLCPP_INFO(this->get_logger(), "JPEG 质量: %d (自适应: %s)", jpeg_quality_, adaptive_quality_ ? "true" : "false");
    RCLCPP_INFO(this->get_logger(), "最大包大小: %d 字节", max_packet_size_);
    
    // ========================================================================
    // 初始化 UDP 套接字
    // ========================================================================
    
    if (!init_udp_socket()) {
        RCLCPP_ERROR(this->get_logger(), "❌ UDP 套接字初始化失败，节点退出");
        rclcpp::shutdown();
        return;
    }
    
    RCLCPP_INFO(this->get_logger(), "✓ UDP 套接字初始化成功");
    
    // ========================================================================
    // 创建订阅
    // ========================================================================
    
    // 使用更激进的 QoS 设置，保留更多消息
    rclcpp::QoS qos_profile = rclcpp::SensorDataQoS();
    qos_profile.keep_last(10);  // 保留最后 10 条消息
    
    camera_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
        camera_topic_,
        qos_profile,
        [this](const sensor_msgs::msg::Image::SharedPtr msg) {
            this->camera_callback(msg);
        }
    );
    
    RCLCPP_INFO(this->get_logger(), "✓ 订阅相机 Topic: %s", camera_topic_.c_str());
    
    if (enable_gps_) {
        gps_subscriber_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            gps_topic_,
            rclcpp::SensorDataQoS(),
            [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
                this->gps_callback(msg);
            }
        );
        
        RCLCPP_INFO(this->get_logger(), "✓ 订阅 GPS Topic: %s", gps_topic_.c_str());
    }
    
    // ========================================================================
    // 启动后台发送线程
    // ========================================================================
    
    sender_running_ = true;
    sender_thread_ = std::thread(&UdpSenderNode::sender_worker, this);
    RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已启动");
    
    // ========================================================================
    // 初始化统计
    // ========================================================================
    
    stats_.last_report_time = std::chrono::system_clock::now();
    
    RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成，等待数据...");
    RCLCPP_INFO(this->get_logger(), "======================================");
}

/**
 * @brief 析构函数
 */
UdpSenderNode::~UdpSenderNode() {
    // 停止后台发送线程
    sender_running_ = false;
    send_queue_cv_.notify_one();  // 唤醒等待的线程
    if (sender_thread_.joinable()) {
        sender_thread_.join();
        RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已停止");
    }
    
    close_udp_socket();
    RCLCPP_INFO(this->get_logger(), "UDP Sender Node 已关闭");
}

/**
 * @brief 初始化 UDP 套接字
 */
bool UdpSenderNode::init_udp_socket() {
    // 创建 UDP 套接字
    udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (udp_socket_ < 0) {
        RCLCPP_ERROR(this->get_logger(), "套接字创建失败: %s", strerror(errno));
        return false;
    }
    // 开启广播权限
    int broadcast_permission = 1;
    if (setsockopt(udp_socket_, SOL_SOCKET, SO_BROADCAST, &broadcast_permission, sizeof(broadcast_permission)) < 0) {
        RCLCPP_WARN(this->get_logger(), "设置 SO_BROADCAST 失败");
    }

    // 设置套接字可重用
    int opt = 1;
    if (setsockopt(udp_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
        RCLCPP_WARN(this->get_logger(), "设置 SO_REUSEADDR 失败");
    }
    
    // 设置发送缓冲区
    int buffer_size = 65536;
    if (setsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)) < 0) {
        RCLCPP_WARN(this->get_logger(), "设置发送缓冲区失败");
    }
    
    // 设置非阻塞模式
    int flags = fcntl(udp_socket_, F_GETFL, 0);
    fcntl(udp_socket_, F_SETFL, flags | O_NONBLOCK);
    
    return true;
}

/**
 * @brief 关闭 UDP 套接字
 */
void UdpSenderNode::close_udp_socket() {
    if (udp_socket_ >= 0) {
        close(udp_socket_);
        udp_socket_ = -1;
    }
}

/**
 * @brief 发送 UDP 数据包
 */
int UdpSenderNode::send_udp_packet(const uint8_t* data, uint16_t len) {
    if (udp_socket_ < 0 || !data || len == 0) {
        return -1;
    }
    
    struct sockaddr_in addr;
    std::memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(target_port_);
    addr.sin_addr.s_addr = inet_addr(target_ip_.c_str());
    
    int sent = sendto(udp_socket_, data, len, 0, (struct sockaddr*)&addr, sizeof(addr));
    
    if (sent > 0) {
        bytes_sent_ += sent;
        packets_sent_++;
    } else {
        // 简单的错误限流日志
        static auto last_error_time = std::chrono::steady_clock::now();
        auto now = std::chrono::steady_clock::now();
        if (std::chrono::duration_cast<std::chrono::seconds>(now - last_error_time).count() > 5) {
            RCLCPP_WARN(this->get_logger(), "UDP 发送失败: %s (errno=%d)", strerror(errno), errno);
            last_error_time = now;
        }
    }
    
    return sent;
}

/**
 * @brief 批量将数据包加入发送队列
 */
void UdpSenderNode::enqueue_packets(std::vector<std::vector<uint8_t>>& packets) {
    {
        std::lock_guard<std::mutex> lock(send_queue_mutex_);
        
        if (send_queue_.size() + packets.size() > MAX_QUEUE_SIZE) {
            frames_dropped_++;
            // 队列满时，丢弃整个 batch，避免部分帧发送
            return;
        }
        
        for (auto& packet_data : packets) {
            PacketQueueItem item;
            // 使用 move 避免拷贝
            item.data = std::move(packet_data);
            item.enqueue_time = std::chrono::system_clock::now();
            send_queue_.push(std::move(item));
        }
    }
    
    // 通知发送线程有新数据
    send_queue_cv_.notify_one();
}

/**
 * @brief 将数据包加入发送队列
 */
void UdpSenderNode::enqueue_packet(const uint8_t* data, uint16_t len) {
    {
        std::lock_guard<std::mutex> lock(send_queue_mutex_);
        
        if (send_queue_.size() >= MAX_QUEUE_SIZE) {
            frames_dropped_++;
            return;
        }
        
        PacketQueueItem item;
        item.data.assign(data, data + len);
        item.enqueue_time = std::chrono::system_clock::now();
        send_queue_.push(item);
    }
    
    // 通知发送线程有新数据
    send_queue_cv_.notify_one();
}

/**
 * @brief 后台发送工作线程
 */
void UdpSenderNode::sender_worker() {
    RCLCPP_INFO(this->get_logger(), "发送线程开始运行 PID: %d", (int)getpid());
    
    // 预分配 batch 内存以减少循环中的分配
    std::vector<PacketQueueItem> batch;
    batch.reserve(BATCH_SEND_SIZE);

    while (sender_running_) {
        try {
            batch.clear();
            
            {
                std::unique_lock<std::mutex> lock(send_queue_mutex_);
                
                // 如果队列为空，等待条件变量唤醒（等待最多 100ms）
                if (send_queue_.empty()) {
                    send_queue_cv_.wait_for(lock, std::chrono::milliseconds(100),
                        [this] { return !send_queue_.empty() || !sender_running_; });
                }
                
                // 批量获取数据包（一次最多 BATCH_SEND_SIZE 个）
                while (!send_queue_.empty() && batch.size() < BATCH_SEND_SIZE) {
                    // 使用 move 避免通过拷贝构造函数复制数据，大幅提高性能
                    batch.push_back(std::move(send_queue_.front()));
                    send_queue_.pop();
                }
            }
            
            // 发送批量数据包（不在锁内进行）
            for (const auto& item : batch) {
                send_udp_packet(item.data.data(), item.data.size());
            }

        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "发送线程发生异常: %s", e.what());
            // 发生异常后稍微休眠一下避免死循环疯狂报错
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        } catch (...) {
            RCLCPP_ERROR(this->get_logger(), "发送线程发生未知异常");
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    RCLCPP_INFO(this->get_logger(), "发送线程已退出");
}

/**
 * @brief 相机图像回调
 */
void UdpSenderNode::camera_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
    try {
        // 记录收到帧的基本信息，便于排查
        // RCLCPP_INFO(this->get_logger(), "收到帧信息: width=%u height=%u encoding=%s step=%u",
        //     msg->width, msg->height, msg->encoding.c_str(), msg->step);

        // 转换为 OpenCV 图像
        auto cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
        cv::Mat frame = cv_ptr->image;
        
        if (frame.empty()) {
            RCLCPP_WARN(this->get_logger(), "收到空帧");
            return;
        }
        
        // 编码为 JPEG
        std::vector<uint8_t> jpeg_data;
        if (!encode_to_jpeg(frame, jpeg_quality_, jpeg_data)) {
            RCLCPP_WARN(this->get_logger(), "JPEG 编码失败");
            frames_dropped_++;
            return;
        }
        
        // 获取当前 GPS 数据
        double lat, lon, alt;
        {
            std::lock_guard<std::mutex> lock(current_gps_data_.mutex);
            lat = current_gps_data_.latitude;
            lon = current_gps_data_.longitude;
            alt = current_gps_data_.altitude;
        }
        
        // 分片并发送
        double timestamp = rclcpp::Clock().now().seconds();
        uint16_t chunks_sent_local = fragment_and_send(
            jpeg_data.data(),
            jpeg_data.size(),
            timestamp,
            lat,
            lon,
            alt
        );
        (void)chunks_sent_local;

        // 记录本帧发送统计，便于调试（每帧输出可能较多，建议仅在调试时启用）
        // RCLCPP_INFO(this->get_logger(), "帧信息: jpeg_size=%zu bytes, chunks_sent=%u", jpeg_data.size(), chunks_sent_local);

        frame_counter_++;
        stats_.total_frames++;
        stats_.total_bytes += jpeg_data.size();
        
        // 定期打印统计
        if (enable_fps_stats_ && (frame_counter_ % 300 == 0)) {
            report_stats();
        }
        
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "相机回调异常: %s", e.what());
        frames_dropped_++;
    }
}

/**
 * @brief GPS 回调
 */
void UdpSenderNode::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
    std::lock_guard<std::mutex> lock(current_gps_data_.mutex);
    current_gps_data_.latitude = msg->latitude;
    current_gps_data_.longitude = msg->longitude;
    current_gps_data_.altitude = msg->altitude;
}

/**
 * @brief 将 Mat 编码为 JPEG
 */
bool UdpSenderNode::encode_to_jpeg(const cv::Mat& frame, int quality, std::vector<uint8_t>& output) {
    try {
        std::vector<int> params{cv::IMWRITE_JPEG_QUALITY, quality};
        cv::imencode(".jpg", frame, output, params);
        return !output.empty();
    } catch (const cv::Exception& e) {
        RCLCPP_ERROR(this->get_logger(), "OpenCV 编码异常: %s", e.what());
        return false;
    }
}

/**
 * @brief 分片并发送
 */
uint16_t UdpSenderNode::fragment_and_send(
    const uint8_t* jpeg_data,
    uint32_t jpeg_len,
    double timestamp,
    double lat,
    double lon,
    double alt) {
    
    // 计算需要的分片数
    uint16_t payload_size = max_packet_size_ - sizeof(PacketHeader);
    uint16_t total_chunks = (jpeg_len + payload_size - 1) / payload_size;
    
    if (total_chunks > MAX_CHUNKS) {
        RCLCPP_WARN(this->get_logger(), "帧太大，超过最大分片数");
        return 0;
    }

    // 预分配所有分片，进行批量入队
    std::vector<std::vector<uint8_t>> frame_packets;
    frame_packets.reserve(total_chunks);
    
    uint16_t chunks_sent = 0;
    
    // 创建每个分片
    for (uint16_t i = 0; i < total_chunks; i++) {
        uint16_t current_payload_size = std::min(
            (uint32_t)payload_size,
            jpeg_len - (uint32_t)i * payload_size
        );
        
        // 构造包头
        PacketHeader header;
        header.magic = SKYLINK_MAGIC;
        header.frame_id = frame_counter_;
        header.total_chunks = total_chunks;
        header.chunk_index = i;
        header.data_len = current_payload_size;
        header.timestamp = timestamp;
        header.lat = lat;
        header.lon = lon;
        header.alt = alt;
        header.crc16 = 0;  // TODO: 计算 CRC
        header.reserve = 0;
        
        // 创建完整数据包
        std::vector<uint8_t> packet(sizeof(PacketHeader) + current_payload_size);
        std::memcpy(packet.data(), &header, sizeof(PacketHeader));
        std::memcpy(
            packet.data() + sizeof(PacketHeader),
            jpeg_data + i * payload_size,
            current_payload_size
        );
        
        frame_packets.push_back(std::move(packet)); // Move 到 vector 中
        chunks_sent++;
    }

    // 批量入队，减少锁的竞争
    enqueue_packets(frame_packets);
    
    return chunks_sent;
}

/**
 * @brief 报告统计信息
 */
void UdpSenderNode::report_stats() {
    auto now = std::chrono::system_clock::now();
    auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
        now - stats_.last_report_time
    ).count();
    
    // 获取当前队列深度
    size_t current_queue_depth = 0;
    {
        std::lock_guard<std::mutex> lock(send_queue_mutex_);
        current_queue_depth = send_queue_.size();
    }
    
    if (elapsed_seconds > 0) {
        // 计算本周期的帧数和字节数（不是累计值）
        uint32_t frames_this_period = stats_.total_frames;  
        uint32_t bytes_this_period = stats_.total_bytes;
        
        // 计算实时 FPS 和吞吐量
        uint32_t fps = frames_this_period / elapsed_seconds;
        uint32_t kbps = (bytes_this_period / elapsed_seconds) / 1024;
        
        // 计算队列使用率（百分比）
        float queue_utilization = (current_queue_depth * 100.0f) / MAX_QUEUE_SIZE;
        
        RCLCPP_INFO(this->get_logger(),
            "发送统计 - FPS: %u, 速率: %u KB/s, 总帧: %u, 总包: %u, 丢弃: %u, 队列: %zu/%zu (%.1f%%)",
                        fps,      kbps,    frame_counter_, packets_sent_.load(), frames_dropped_.load(),
            current_queue_depth, MAX_QUEUE_SIZE, queue_utilization
        );
        
        // 如果队列接近满，发出警告
        if (queue_utilization > 80.0f) {
            RCLCPP_WARN(this->get_logger(),
                "发送队列接近饱和 (%.1f%%)，建议检查网络或增加发送线程",
                queue_utilization
            );
        }
        
        // 重置周期统计
        stats_.total_frames = 0;
        stats_.total_bytes = 0;
        stats_.last_report_time = now;
    }
}

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