#include "VpnPeer.hpp"
#include "UdpVpn.hpp"

#include <cstdint>
#include <cstring>
#include <functional>

VpnPeer::VpnPeer(UdpVpn* vpn, const sockaddr_in6& ext_addr,
        const in6_addr& int_addr)
    : _vpn(vpn), _ext_addr(ext_addr), _int_addr(int_addr), _next_send_seqno(0)
{}

void VpnPeer::set_int_addr(const in6_addr& int_addr) {
    memcpy(&_int_addr, &int_addr, sizeof(_int_addr));
}

size_t VpnPeer::write(const char* data, size_t len) {
    ssize_t nsent;

    nsent = sendto(_vpn->get_socket_fd(), data, len, MSG_CONFIRM,
            (const struct sockaddr*) &_ext_addr, sizeof(_ext_addr));
    if(nsent < 0)
        throw NetError("Could not send UDP packet", errno, true);

    return (size_t) nsent;
}

size_t VpnPeer::write(const VpnPacket& packet) {
    return write(packet.get_data(), packet.get_data_size());
}

void VpnPeer::got_inbound_packet(const VpnPacket& packet) {
    _packet_loss.log_packet(packet.get_seqno());
}

PacketLossLogger::PacketLossLogger() : _cur_seqno(0) {}

void PacketLossLogger::log_packet(uint32_t seqno) {
    kdebugf(">> Logging %lu (loss %lf)\n", seqno, get_loss_rate());
    uint32_t m_seqno = seqno % PACKET_LOSS_HISTSIZE;
    int64_t diff = (int64_t)seqno - _cur_seqno;

    if(diff == 1) {
        _cur_seqno++;
        _packet_loss_hist.reset(m_seqno);
        while(_received_ahead.test((_cur_seqno + 1) % PACKET_LOST_AFTER)) {
            _cur_seqno++;
            _packet_loss_hist.reset(_cur_seqno % PACKET_LOSS_HISTSIZE);
            _received_ahead.reset(_cur_seqno % PACKET_LOST_AFTER);
        }
    } else if(LIKELY(diff > 1)) {
        if(diff < PACKET_LOST_AFTER)
            _received_ahead.set(seqno % PACKET_LOST_AFTER);
        else if(diff < PACKET_LOSS_HISTSIZE) {
            // Packet too much forwards -- consider _cur_seqno lost
            for(int offs=1; offs < PACKET_LOST_AFTER; ++offs) {
                _packet_loss_hist[(_cur_seqno + offs) % PACKET_LOSS_HISTSIZE] =
                    !_received_ahead[(_cur_seqno + offs) % PACKET_LOST_AFTER];
            }
            _received_ahead.reset();
            _cur_seqno = seqno;
            _packet_loss_hist.reset(m_seqno);
        } else
            reboot(); // This is a huge gap -- reboot
    } else {
        if(diff < - 2*PACKET_LOSS_HISTSIZE)
            reboot(); // this is too much backwards -- something's wrong, reboot
        // else: ignore, we've moved forward and counted the packet as lost
    }
}

void PacketLossLogger::reboot() {
    _packet_loss_hist.reset();
    _received_ahead.reset();
}