#include <cstring>

#include "UdpVpnServer.hpp"
#include "ip_header.hpp"

UdpVpnServer::UdpVpnServer(in_port_t port) : UdpVpn() {
    memset(&_bind_addr, 0, sizeof(_bind_addr));
    bind(in6addr_any, port);
}

UdpVpnServer::UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port)
    : UdpVpn()
{
    memset(&_bind_addr, 0, sizeof(_bind_addr));
    bind(bind_addr6, port);
}

void UdpVpnServer::bind(const struct in6_addr& bind_addr6, in_port_t port) {
    int rc;

    _bind_addr.sin6_family = AF_INET6;
    _bind_addr.sin6_port = htons(port);
    _bind_addr.sin6_addr = bind_addr6;

    rc = ::bind(
            _socket, (const struct sockaddr*)&_bind_addr, sizeof(_bind_addr));
    if(rc < 0) {
        throw UdpVpn::InitializationError("Cannot bind socket", errno, true);
    }

    debugf("> Listening on port %d\n", port);
}


std::shared_ptr<VpnPeer> UdpVpnServer::get_peer_for_ip(
        const in6_addr& peer_addr)
{
    auto peer_iter = _peers.find(peer_addr);
    if(peer_iter == _peers.end()) // Unknown peer
        return nullptr;
    return peer_iter->second;
}

void UdpVpnServer::receive_from_tun() {
    char buffer[1500];
    size_t nread = read_from_tun(buffer, 1500);
    if(nread == 0)
        return;

    // Parse inner packet header -- assume IPv6 for now [FIXME]
    IPv6Header inner_header;
    if(!parse_ipv6_header(buffer, nread, inner_header)) {
        // Not a valid header -- ignore the packet
        debugf("Ignoring outgoing packet with invalid header\n");
        return;
    }

    // Recover VpnPeer -- or drop if new
    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.dest);
    if(!peer) {
        debugf("Dropping packet for destination %s -- unknown peer.\n",
                format_address(inner_header.dest.s6_addr));
        return;
    }

    kdebugf("Transmitting %s -> %s, size %d\n",
            format_address(inner_header.source.s6_addr),
            format_address(inner_header.dest.s6_addr),
            nread);
    peer->write(buffer, nread);
}

void UdpVpnServer::receive_from_udp() {
    char buffer[1500];
    sockaddr_in6 peer_addr;
    size_t nread = read_from_udp(buffer, 1500, peer_addr);

    // Parse inner packet header -- assume IPv6 for now [FIXME]
    IPv6Header inner_header;
    if(!parse_ipv6_header(buffer, nread, inner_header)) {
        // Not a valid header -- ignore the packet
        debugf("Ignoring packet with invalid header\n");
        return;
    }

    // Recover VpnPeer -- or create if new
    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.source);
    if(!peer) {
        peer = std::make_shared<VpnPeer>(this, peer_addr, inner_header.source);
        auto insert_result = _peers.insert({inner_header.source, peer});

        debugf("Got new peer %s:%d -- %s [really inserted=%d]\n",
                format_address(peer_addr.sin6_addr.s6_addr),
                htons(peer_addr.sin6_port),
                format_address(inner_header.source.s6_addr),
                insert_result.second);
    }
    // VpnPeer* peer = (peer_iter->second);
    // TODO -- pass the packet to `peer` for a cleaner flow

    // Reinject into tun
    kdebugf("Receiving packet of size %d from peer\n", nread);
    _tun_dev.write(buffer, nread);
}