#include "skylink_bridge/device_info_sender_node.h"

#include <arpa/inet.h>
#include <chrono>
#include <cstring>
#include <fcntl.h>
#include <sys/socket.h>
#include <unistd.h>

using namespace std::chrono_literals;

namespace {
uint16_t read_uint16(const std::string& s) {
    try {
        return static_cast<uint16_t>(std::stoi(s));
    } catch (...) {
        return 0;
    }
}
}  // namespace

DeviceInfoSenderNode::DeviceInfoSenderNode()
    : rclcpp::Node("device_info_sender_node") {
    load_parameters();

    // 创建 UDP 套接字
    udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (udp_socket_ < 0) {
        RCLCPP_FATAL(this->get_logger(), "创建 UDP 套接字失败: %s", strerror(errno));
        throw std::runtime_error("socket create failed");
    }

    // 允许广播
    int opt = 1;
    setsockopt(udp_socket_, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt));

    // 绑定监听端口
    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port = htons(info_port_);
    addr.sin_addr.s_addr = INADDR_ANY;
    if (bind(udp_socket_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
        RCLCPP_FATAL(this->get_logger(), "绑定端口 %u 失败: %s", info_port_, strerror(errno));
        throw std::runtime_error("bind failed");
    }

    // 定期广播
    if (enable_broadcast_) {
        broadcast_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(broadcast_interval_ms_),
            std::bind(&DeviceInfoSenderNode::broadcast_timer_callback, this));
    }

    // 请求响应线程
    if (enable_request_response_) {
        running_.store(true);
        listener_thread_ = std::thread(&DeviceInfoSenderNode::handle_request_loop, this);
    }

    RCLCPP_INFO(this->get_logger(),
                "设备信息发送节点启动，端口: %u, 广播: %s, 请求响应: %s",
                info_port_, enable_broadcast_ ? "开启" : "关闭",
                enable_request_response_ ? "开启" : "关闭");
}

DeviceInfoSenderNode::~DeviceInfoSenderNode() {
    running_.store(false);
    if (listener_thread_.joinable()) {
        listener_thread_.join();
    }
    if (udp_socket_ >= 0) {
        close(udp_socket_);
    }
}

void DeviceInfoSenderNode::load_parameters() {
    this->declare_parameter<int>("info_port", 10000);
    this->declare_parameter<std::string>("target_ip", "255.255.255.255");
    this->declare_parameter<bool>("enable_broadcast", true);
    this->declare_parameter<bool>("enable_request_response", true);
    this->declare_parameter<int>("broadcast_interval_ms", 1000);

    this->declare_parameter<std::string>("device_path", "/dev/video0");
    this->declare_parameter<std::string>("device_name", "Unknown");
    this->declare_parameter<int>("width", 640);
    this->declare_parameter<int>("height", 480);
    this->declare_parameter<double>("framerate", 30.0);
    this->declare_parameter<std::string>("pixel_format", "YUYV");

    // 读取
    info_port_ = static_cast<uint16_t>(this->get_parameter("info_port").as_int());
    target_ip_ = this->get_parameter("target_ip").as_string();
    enable_broadcast_ = this->get_parameter("enable_broadcast").as_bool();
    enable_request_response_ = this->get_parameter("enable_request_response").as_bool();
    broadcast_interval_ms_ = this->get_parameter("broadcast_interval_ms").as_int();

    device_path_ = this->get_parameter("device_path").as_string();
    device_name_ = this->get_parameter("device_name").as_string();
    width_ = static_cast<uint16_t>(this->get_parameter("width").as_int());
    height_ = static_cast<uint16_t>(this->get_parameter("height").as_int());
    framerate_ = static_cast<float>(this->get_parameter("framerate").as_double());
    pixel_format_ = fourcc_from_string(this->get_parameter("pixel_format").as_string());

    current_fps_ = 0.0f;
    total_frames_ = 0;
}

void DeviceInfoSenderNode::broadcast_timer_callback() {
    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port = htons(info_port_);
    addr.sin_addr.s_addr = inet_addr(target_ip_.c_str());
    send_device_info(addr);
}

void DeviceInfoSenderNode::send_device_info(const sockaddr_in& addr) {
    DeviceInfoPacket packet = build_packet();

    // 手动序列化为网络字节序
    std::vector<uint8_t> buffer(sizeof(DeviceInfoPacket), 0);
    uint8_t* p = buffer.data();

    auto write_u16 = [](uint8_t*& ptr, uint16_t v) {
        uint16_t n = htons(v);
        std::memcpy(ptr, &n, sizeof(n));
        ptr += sizeof(n);
    };
    auto write_u32 = [](uint8_t*& ptr, uint32_t v) {
        uint32_t n = htonl(v);
        std::memcpy(ptr, &n, sizeof(n));
        ptr += sizeof(n);
    };
    auto write_float = [](uint8_t*& ptr, float f) {
        uint32_t bits;
        std::memcpy(&bits, &f, sizeof(bits));
        uint32_t n = htonl(bits);
        std::memcpy(ptr, &n, sizeof(n));
        ptr += sizeof(n);
    };
    auto write_u64 = [](uint8_t*& ptr, uint64_t v) {
        uint64_t n = DeviceInfoSenderNode::htonll(v);
        std::memcpy(ptr, &n, sizeof(n));
        ptr += sizeof(n);
    };

    write_u16(p, packet.magic);
    write_u32(p, packet.sequence);
    write_u16(p, packet.width);
    write_u16(p, packet.height);
    write_float(p, packet.framerate);
    write_u32(p, packet.pixel_format);

    std::memcpy(p, packet.device_path, sizeof(packet.device_path));
    p += sizeof(packet.device_path);
    std::memcpy(p, packet.device_name, sizeof(packet.device_name));
    p += sizeof(packet.device_name);

    write_float(p, packet.current_fps);
    write_u32(p, packet.total_frames);
    write_u64(p, packet.timestamp);
    write_u16(p, packet.crc16);

    ssize_t sent = sendto(udp_socket_, buffer.data(), buffer.size(), 0,
                          reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
    if (sent < 0) {
        RCLCPP_WARN(this->get_logger(), "发送设备信息失败: %s", strerror(errno));
    }
}

void DeviceInfoSenderNode::handle_request_loop() {
    // 设置非阻塞
    int flags = fcntl(udp_socket_, F_GETFL, 0);
    fcntl(udp_socket_, F_SETFL, flags | O_NONBLOCK);

    std::vector<uint8_t> buf(512);
    while (running_.load()) {
        sockaddr_in sender{};
        socklen_t sender_len = sizeof(sender);
        ssize_t n = recvfrom(udp_socket_, buf.data(), buf.size(), 0,
                             reinterpret_cast<sockaddr*>(&sender), &sender_len);
        if (n <= 0) {
            std::this_thread::sleep_for(50ms);
            continue;
        }

        if (n < 2) {
            continue;
        }

        uint16_t magic = ntohs(*reinterpret_cast<uint16_t*>(buf.data()));
        if (magic == DEVICE_INFO_REQUEST) {
            send_device_info(sender);
        }
    }
}

DeviceInfoPacket DeviceInfoSenderNode::build_packet() {
    DeviceInfoPacket packet{};
    packet.magic = DEVICE_INFO_MAGIC;
    packet.sequence = sequence_++;
    packet.width = width_;
    packet.height = height_;
    packet.framerate = framerate_;
    packet.pixel_format = pixel_format_;

    std::memset(packet.device_path, 0, sizeof(packet.device_path));
    std::strncpy(packet.device_path, device_path_.c_str(), sizeof(packet.device_path) - 1);

    std::memset(packet.device_name, 0, sizeof(packet.device_name));
    std::strncpy(packet.device_name, device_name_.c_str(), sizeof(packet.device_name) - 1);

    packet.current_fps = current_fps_;
    packet.total_frames = total_frames_;

    auto now = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch());
    packet.timestamp = static_cast<uint64_t>(now.count());
    packet.crc16 = 0;
    return packet;
}

uint64_t DeviceInfoSenderNode::htonll(uint64_t value) {
    static const int num = 42;
    if (*reinterpret_cast<const char*>(&num) == num) {
        // little-endian
        uint32_t high_part = htonl(static_cast<uint32_t>(value >> 32));
        uint32_t low_part = htonl(static_cast<uint32_t>(value & 0xFFFFFFFFULL));
        return (static_cast<uint64_t>(low_part) << 32) | high_part;
    } else {
        return value;
    }
}

uint32_t DeviceInfoSenderNode::fourcc_from_string(const std::string& fmt) {
    if (fmt.size() >= 4) {
        return (static_cast<uint32_t>(fmt[0]) & 0xFF) |
               ((static_cast<uint32_t>(fmt[1]) & 0xFF) << 8) |
               ((static_cast<uint32_t>(fmt[2]) & 0xFF) << 16) |
               ((static_cast<uint32_t>(fmt[3]) & 0xFF) << 24);
    }
    // 默认 YUYV
    return ('Y') | ('U' << 8) | ('Y' << 16) | ('V' << 24);
}

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DeviceInfoSenderNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
