/**
 * @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>
#include <regex>
#include <sstream>

/**
 * @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>("gps_topic", "/mavros/global_position/global");
    this->declare_parameter<bool>("enable_gps", true);
    this->declare_parameter<int>("max_packet_size", 1400); // Default to safer MTU
    this->declare_parameter<int>("frame_timeout_ms", 5000);
    this->declare_parameter<bool>("enable_fps_stats", true);
    this->declare_parameter<bool>("enable_drop_stats", true);
    this->declare_parameter<std::string>("save_dir", "./camera_data");  // 监控目录
    this->declare_parameter<std::string>("log_file", "./sent_log.txt"); // 日志文件

    
    // 读取参数
    target_ip_ = this->get_parameter("target_ip").as_string();
    target_port_ = static_cast<uint16_t>(this->get_parameter("target_port").as_int());
    gps_topic_ = this->get_parameter("gps_topic").as_string();
    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();
    save_dir_ = this->get_parameter("save_dir").as_string();
    log_file_path_ = this->get_parameter("log_file").as_string();
    
    // ========================================================================
    // 打印初始化信息
    // ========================================================================
    
    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(), "监控目录: %s", save_dir_.c_str());
    RCLCPP_INFO(this->get_logger(), "日志文件: %s", log_file_path_.c_str());
    RCLCPP_INFO(this->get_logger(), "GPS Topic: %s", gps_topic_.c_str());
    RCLCPP_INFO(this->get_logger(), "最大包大小: %d 字节", max_packet_size_);

    // 创建目录
    std::filesystem::create_directories(save_dir_);
    
    // 打开日志文件
    log_file_stream_.open(log_file_path_, std::ios::out | std::ios::app);
    if (!log_file_stream_.is_open()) {
         RCLCPP_ERROR(this->get_logger(), "无法打开日志文件: %s", log_file_path_.c_str());
    } else {
        // 写入头部
        auto now = std::chrono::system_clock::now();
        std::time_t now_c = std::chrono::system_clock::to_time_t(now);
        log_file_stream_ << "--- Session Started: " << std::put_time(std::localtime(&now_c), "%F %T") << " ---" << std::endl;
    }
    
    // ========================================================================
    // 初始化 UDP 套接字
    // ========================================================================
    
    if (!init_udp_socket()) {
        RCLCPP_ERROR(this->get_logger(), "❌ UDP 套接字初始化失败，节点退出");
        rclcpp::shutdown();
        return;
    }
    
    RCLCPP_INFO(this->get_logger(), " UDP 套接字初始化成功");
    
    // ========================================================================
    // 创建订阅和定时器
    // ========================================================================
    
    // 创建文件扫描定时器 (1Hz, 即每秒检查一次)
    file_scan_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(200), // 200ms
        std::bind(&UdpSenderNode::scan_and_send_files, this));
    
    RCLCPP_INFO(this->get_logger(), " 文件监控定时器已启动 (200ms)");

    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 失败");
    }
    
    // 设置发送缓冲区
    // 增加缓冲区大小到 4MB (4 * 1024 * 1024) 以应对高码率 MJPEG 流
    int buffer_size = 4 * 1024 * 1024; 
    if (setsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)) < 0) {
        RCLCPP_WARN(this->get_logger(), "设置发送缓冲区失败 (尝试 4MB): %s", strerror(errno));
        // Fallback to smaller buffer if failed
        buffer_size = 512 * 1024;
        setsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size));
    } else {
        // 验证实际设置的大小 (Linux 内核通常会将其翻倍)
        int actual_buf_size = 0;
        socklen_t opt_len = sizeof(actual_buf_size);
        if (getsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &actual_buf_size, &opt_len) == 0) {
             RCLCPP_INFO(this->get_logger(), "UDP 发送缓冲区已设置为: %d bytes", actual_buf_size);
        }
    }
    
    // 设置非阻塞模式
    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 retries = 0;
    const int MAX_RETRIES = 5;
    ssize_t sent = -1;

    while (retries < MAX_RETRIES) {
        sent = sendto(udp_socket_, data, len, 0, (struct sockaddr*)&addr, sizeof(addr));
        
        if (sent > 0) {
            bytes_sent_ += sent;
            packets_sent_++;
            return sent;
        }

        if (errno == EAGAIN || errno == EWOULDBLOCK) {
            // 资源暂时不可用，等待并重试
            retries++;
            // 短暂休眠 1ms，给内核一点时间清空缓冲区
            std::this_thread::sleep_for(std::chrono::microseconds(1000));
            continue;
        } else {
            // 其他错误，直接退出
            break;
        }
    }
    
    // 如果重试后仍然失败
    if (sent <= 0) {
        // 简单的错误限流日志
        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();
}

void UdpSenderNode::scan_and_send_files() {
    if (!std::filesystem::exists(save_dir_)) return;

    std::vector<std::filesystem::directory_entry> new_files;

    try {
        for (const auto& entry : std::filesystem::directory_iterator(save_dir_)) {
            if (entry.is_regular_file() && entry.path().extension() == ".jpg") {
                if (sent_files_.find(entry.path().string()) == sent_files_.end()) {
                    new_files.push_back(entry);
                }
            }
        }
    } catch (const std::exception& e) {
         RCLCPP_ERROR(this->get_logger(), "扫描目录失败: %s", e.what());
         return;
    }

    // 按修改时间排序（旧的先发）
    std::sort(new_files.begin(), new_files.end(), 
        [](const std::filesystem::directory_entry& a, const std::filesystem::directory_entry& b) {
            return std::filesystem::last_write_time(a) < std::filesystem::last_write_time(b);
        });

    for (const auto& entry : new_files) {
        process_file(entry.path().string(), entry.path().filename().string());
        
        // 标记为已发送
        sent_files_.insert(entry.path().string());
        
        // 为了防止内存无限增长，如果太大了可以清理旧的 (可选)
        if (sent_files_.size() > 10000) {
             // 简单清理策略：实际生产中可能需要更复杂的逻辑
             // 这里暂时不做，假设磁盘空间有限，文件会被外部删除
        }
    }
}

void UdpSenderNode::process_file(const std::string& filepath, const std::string& filename) {
    // 读取文件内容
    std::ifstream file(filepath, std::ios::binary);
    if (!file) return;

    // 获取文件大小
    file.seekg(0, std::ios::end);
    size_t size = file.tellg();
    file.seekg(0, std::ios::beg);

    if (size == 0) return;

    std::vector<uint8_t> buffer(size);
    if (!file.read(reinterpret_cast<char*>(buffer.data()), size)) return;
    
    frame_counter_++;
    
    // 从文件名提取照片编号
    // 文件名格式: photo_YYYYMMDD_HHMMSS_XXX.jpg
    uint32_t photo_number = frame_counter_;  // 默认使用frame_counter_
    try {
        // 尝试从文件名中提取编号（最后一个下划线后的数字）
        std::regex pattern(R"(photo_\d{8}_\d{6}_(\d+)\.jpg)");
        std::smatch match;
        if (std::regex_match(filename, match, pattern) && match.size() > 1) {
            photo_number = std::stoul(match[1].str());
        } else {
            // 如果格式不匹配，尝试提取任何数字作为编号
            std::regex alt_pattern(R"(_(\d+)\.jpg$)");
            if (std::regex_search(filename, match, alt_pattern) && match.size() > 1) {
                photo_number = std::stoul(match[1].str());
            }
        }
    } catch (const std::exception& e) {
        RCLCPP_WARN(this->get_logger(), "无法从文件名提取照片编号，使用frame_counter: %s", e.what());
    }
    
    // 计算需要多少个包
    const size_t header_size = PacketHeader::size();
    const size_t payload_size = max_packet_size_ - header_size;
    const uint16_t total_packets = (size + payload_size - 1) / payload_size;
    
    std::vector<std::vector<uint8_t>> packets;
    packets.reserve(total_packets);

    // 获取 GPS 信息
    double lat = 0.0, lon = 0.0, alt = 0.0;
    {
        std::lock_guard<std::mutex> gps_lock(current_gps_data_.mutex);
        if (enable_gps_) {
             lat = current_gps_data_.latitude;
             lon = current_gps_data_.longitude;
             alt = current_gps_data_.altitude;
        }
    }
    
    double timestamp = std::chrono::duration<double>(
        std::chrono::system_clock::now().time_since_epoch()
    ).count();

    for (uint16_t i = 0; i < total_packets; ++i) {
        size_t offset = i * payload_size;
        size_t current_payload_len = std::min(payload_size, size - offset);
        
        // 准备包数据
        std::vector<uint8_t> packet_data(header_size + current_payload_len);
        
        // 填充头部
        PacketHeader* header = reinterpret_cast<PacketHeader*>(packet_data.data());
        header->magic = SKYLINK_MAGIC;
        header->frame_id = frame_counter_;
        header->total_chunks = total_packets;
        header->chunk_index = i;
        header->data_len = static_cast<uint16_t>(current_payload_len);
        header->timestamp = timestamp;
        
        // GPS
        header->lat = lat;
        header->lon = lon;
        header->alt = alt;

        // 其他字段
        header->crc16 = 0;
        header->reserve = photo_number;  // 存储照片编号

        // 填充负载
        std::memcpy(packet_data.data() + header_size, buffer.data() + offset, current_payload_len);
        
        packets.push_back(std::move(packet_data));
    }
    
    // 加入发送队列
    enqueue_packets(packets);
    
    // 记录日志
    log_sent_file(filename, size);
    
    RCLCPP_INFO(this->get_logger(), "已排队文件: %s (大小: %zu bytes, 包数: %d, 照片编号: %u)", 
                filename.c_str(), size, total_packets, photo_number);
}

void UdpSenderNode::log_sent_file(const std::string& filename, size_t size) {
    if (log_file_stream_.is_open()) {
        auto now = std::chrono::system_clock::now();
        std::time_t now_c = std::chrono::system_clock::to_time_t(now);
        log_file_stream_ << std::put_time(std::localtime(&now_c), "[%F %T]") 
                         << " Sent: " << filename 
                         << " Size: " << size 
                         << " Total Sent: " << sent_files_.size() + 1
                         << std::endl;
        log_file_stream_.flush(); // 确保写入磁盘
    }
}

/**
 * @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 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 报告统计信息
 */
void UdpSenderNode::report_stats() {
    // 简单的统计报告实现
    if (!enable_fps_stats_) return;
    
    auto now = std::chrono::system_clock::now();
    auto diff = std::chrono::duration_cast<std::chrono::seconds>(now - stats_.last_report_time).count();
    
    if (diff >= 5) { // 每5秒报告一次
        std::lock_guard<std::mutex> lock(stats_mutex_);
        // 这里只是为了演示，实际可以计算速率
        stats_.last_report_time = now;
        RCLCPP_INFO(this->get_logger(), "Stats: Sent %u packets, %u bytes. Dropped %u frames.", 
            packets_sent_.load(), bytes_sent_.load(), frames_dropped_.load());
    }
}

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