#include "port/udp.h"

#include <cstring>
#include <poll.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>

#include "crsf.h"


drivers::UdpDriver::UdpDriver(uint16_t port) {
    fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0) {
        throw std::runtime_error(std::string("UdpDriver open error: ") + std::strerror(errno));
    }

    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(fd, (sockaddr*)&addr, sizeof(addr)) < 0) if (fd < 0) {
        close(fd);
        throw std::runtime_error(std::string("UdpDriver bind error: ") + std::strerror(errno));
    }

    events = POLLIN;
}
bool drivers::UdpDriver::sendTo(std::span<const uint8_t> data, const std::string& addr, uint16_t port) {
    if (fd < 0) return false;

    sockaddr_in dst{};
    dst.sin_family = AF_INET;
    dst.sin_port = htons(port);
    inet_aton(addr.c_str(), &dst.sin_addr);

    auto w = sendto(fd, data.data(), data.size(), 0, (sockaddr*)&dst, sizeof(dst));
    return w == data.size();
}
bool drivers::UdpDriver::recvPacket(std::vector<uint8_t>& out) {
    out.resize(crsf::CRSF_MAX_FRAME_SIZE);
    ssize_t received = recvfrom(fd, out.data(), out.size(), MSG_DONTWAIT, nullptr, nullptr);

    if (received > 0) {
        out.resize(received);
        return true;
    }
    out.clear();

    return false;
}
drivers::UdpDriver::~UdpDriver() {
    close(fd);
}