#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() {
    VpnPacket packet(_vpn_mtu);
    size_t nread = read_from_tun(packet);
    if(nread == 0)
        return;

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

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

void UdpVpnServer::receive_from_udp() {
    VpnPacket packet(_vpn_mtu);
    sockaddr_in6 peer_addr;
    size_t nread = read_from_udp(packet, peer_addr);
    if(nread == 0)
        return;

    // Recover VpnPeer -- or create if new
    const in6_addr& peer_inner_addr = packet.get_ipv6_header().source;
    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(peer_inner_addr);
    if(!peer) {
        peer = std::make_shared<VpnPeer>(this, peer_addr, peer_inner_addr);
        _peers.insert({peer_inner_addr, peer});

        debugf("Got new peer %s:%d -- %s\n",
                format_address(peer_addr.sin6_addr.s6_addr),
                htons(peer_addr.sin6_port),
                format_address(peer_inner_addr.s6_addr));
    }
    packet.set_peer(peer);

    // Reinject into tun
    kdebugf("Receiving packet #%u of size %d from %s\n",
            packet.get_seqno(),
            nread,
            format_address(packet.get_ipv6_header().source.s6_addr));
    _tun_dev.write(packet.get_payload(), packet.get_payload_size());
}