#include "v2g_context.h"

/* defines for V2G SDP implementation */
#define SDP_SRV_PORT 15118

#define SDP_VERSION         0x01
#define SDP_INVERSE_VERSION 0xfe

#define SDP_HEADER_LEN           8
#define SDP_REQUEST_PAYLOAD_LEN  2
#define SDP_RESPONSE_PAYLOAD_LEN 20

#define SDP_REQUEST_TYPE  0x9000
#define SDP_RESPONSE_TYPE 0x9001

/* link-local multicast address ff02::1 aka ip6-allnodes */
#define IN6ADDR_ALLNODES  { 0xff, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x01 }

#define POLL_TIMEOUT 2000


typedef enum sdp_security {
    SDP_SECURITY_TLS = 0x00,
    SDP_SECURITY_NONE = 0x10,
}sdp_security_t;

typedef enum sdp_transport_protocol {
    SDP_TRANSPORT_PROTOCOL_TCP = 0x00,
    SDP_TRANSPORT_PROTOCOL_UDP = 0x10,
}sdp_transport_protocol_t;

/* bundles various aspects of a SDP query */
struct sdp_query {
    struct sockaddr_in6 remote_addr;
    sdp_security_t security_requested;
    sdp_transport_protocol_t proto_requested;
};


/*
 * Fills the SDP header into a given buffer
 */
static int sdp_write_header(uint8_t* buffer, uint16_t payload_type, uint32_t payload_len) 
{
    int offset = 0;

    buffer[offset++] = SDP_VERSION;
    buffer[offset++] = SDP_INVERSE_VERSION;

    /* payload is network byte order */
    buffer[offset++] = (payload_type >> 8) & 0xff;
    buffer[offset++] = payload_type & 0xff;

    /* payload_length is network byte order */
    buffer[offset++] = (payload_len >> 24) & 0xff;
    buffer[offset++] = (payload_len >> 16) & 0xff;
    buffer[offset++] = (payload_len >> 8) & 0xff;
    buffer[offset++] = payload_len & 0xff;

    return offset;
}

static int sdp_validate_header(uint8_t* buffer, uint16_t expected_payload_type, uint32_t expected_payload_len) 
{
    uint16_t payload_type;
    uint32_t payload_len;

    if (buffer[0] != SDP_VERSION) {
        mlog_e(LOG_TAG_V2G, "Invalid SDP version");
        return -1;
    }

    if (buffer[1] != SDP_INVERSE_VERSION) {
        mlog_e(LOG_TAG_V2G, "Invalid SDP inverse version");
        return -1;
    }

    payload_type = (buffer[2] << 8) + buffer[3];
    if (payload_type != expected_payload_type) {
        mlog_e(LOG_TAG_V2G, "Invalid payload type: expected %u, received %u", expected_payload_type, payload_type);
        return -1;
    }

    payload_len = (buffer[4] << 24) + (buffer[5] << 16) + (buffer[6] << 8) + buffer[7];
    if (payload_len != expected_payload_len) {
        mlog_e(LOG_TAG_V2G, "Invalid payload length: expected %u, received %u", expected_payload_len, payload_len);
        return -1;
    }

    return 0;
}

static int sdp_create_response(uint8_t* buffer, struct sockaddr_in6* addr, enum sdp_security security, enum sdp_transport_protocol proto)
{
    int offset = SDP_HEADER_LEN;

    /* fill in first the payload */

    /* address is already network byte order */
    memcpy(&buffer[offset], &addr->sin6_addr, sizeof(addr->sin6_addr));
    offset += sizeof(addr->sin6_addr);

    memcpy(&buffer[offset], &addr->sin6_port, sizeof(addr->sin6_port));
    offset += sizeof(addr->sin6_port);

    buffer[offset++] = security;
    buffer[offset++] = proto;

    /* now fill in the header with payload length */
    sdp_write_header(buffer, SDP_RESPONSE_TYPE, offset - SDP_HEADER_LEN);

    return offset;
}

/*
 * Sends a SDP response packet
 */
static int sdp_send_response(struct v2g_context* v2g, struct sdp_query* sdp_query) 
{
    uint8_t buffer[SDP_HEADER_LEN + SDP_RESPONSE_PAYLOAD_LEN];
    int rv = 0;

    /* at the moment we only understand TCP protocol */
    if (sdp_query->proto_requested != SDP_TRANSPORT_PROTOCOL_TCP) {
        mlog_e(LOG_TAG_V2G, "SDP requested unsupported protocol 0x%02x, announcing nothing", sdp_query->proto_requested);
        return 1;
    }

    switch (sdp_query->security_requested) {
    case SDP_SECURITY_TLS:
        mlog_d(LOG_TAG_V2G, "SDP requested TLS, announcing TLS");
        sdp_create_response(buffer, &v2g->local_tls_addr, SDP_SECURITY_TLS, SDP_TRANSPORT_PROTOCOL_TCP);
        break;

    case SDP_SECURITY_NONE:
        mlog_d(LOG_TAG_V2G, "SDP requested NO-TLS, announcing NO-TLS");
        sdp_create_response(buffer, &v2g->local_tcp_addr, SDP_SECURITY_NONE, SDP_TRANSPORT_PROTOCOL_TCP);
        break;

    default:
        mlog_e(LOG_TAG_V2G, "SDP requested unsupported security 0x%02x, announcing nothing", sdp_query->security_requested);
        return 1;
    }

    if (sendto(v2g->sdp_socket, buffer, sizeof(buffer), 0, (struct sockaddr*)&sdp_query->remote_addr, sizeof(struct sockaddr_in6)) != sizeof(buffer)) 
    {
        rv = -1;
    }
    {
        char addrbuf[INET6_ADDRSTRLEN] = {0};
        const char* addr;
        int saved_errno = errno;

        addr = inet_ntop(AF_INET6, &sdp_query->remote_addr.sin6_addr, addrbuf, sizeof(addrbuf));
        if (rv == 0) {
            mlog_d(LOG_TAG_V2G, "sendto([%s]:%u) succeeded", addr, ntohs(sdp_query->remote_addr.sin6_port));
        } else {
            mlog_e(LOG_TAG_V2G, "sendto([%s]:%u) failed: %s", addr, ntohs(sdp_query->remote_addr.sin6_port), strerror(saved_errno));
        }
    }

    return rv;
}

void* v2g_sdp_server_listen(void* args) 
{
    struct v2g_context* v2g = (struct v2g_context*)args;
    /* Init pollfd struct */
    struct pollfd pollfd = {v2g->sdp_socket, POLLIN, 0};
    uint8_t buffer[SDP_HEADER_LEN + SDP_REQUEST_PAYLOAD_LEN];
    char addrbuf[INET6_ADDRSTRLEN] = {0};
    const char* addr = addrbuf;
    struct sdp_query sdp_query = {};
    socklen_t addrlen = sizeof(sdp_query.remote_addr);

    while (v2g->sdp_server_quit == 0) 
    {

        /* Check if data was received on socket */
        pollfd.events = POLLIN;
        pollfd.fd = v2g->sdp_socket;
        pollfd.revents = 0;
        signed status = poll(&pollfd, 1, POLL_TIMEOUT);

        if (status == -1) {
            if (errno == EINTR) { // If the call did not succeed because it was interrupted
                continue;
            } else {
                mlog_d(LOG_TAG_V2G, "poll() failed: %s", strerror(errno));
                continue;
            }
        }
        // printd("step(%d), v2g->sdp_socket=%d\n", status, v2g->sdp_socket);
        /* If new data was received, handle sdp request */
        if (status > 0) {
            ssize_t len = recvfrom(v2g->sdp_socket, buffer, sizeof(buffer), 0, (struct sockaddr*)&sdp_query.remote_addr, &addrlen);
            if (len == -1) {
                if (errno != EINTR)
                    mlog_d(LOG_TAG_V2G, "recvfrom() failed: %s", strerror(errno));
                continue;
            }

            addr = inet_ntop(AF_INET6, &sdp_query.remote_addr.sin6_addr, addrbuf, sizeof(addrbuf));

            if (len != sizeof(buffer)) {
                mlog_w(LOG_TAG_V2G, "Discarded packet from [%s]:%u due to unexpected length %zd", addr, ntohs(sdp_query.remote_addr.sin6_port), len);
                continue;
            }

            printline(buffer, len, "recvfrom(%s:%d)", addr, ntohs(sdp_query.remote_addr.sin6_port));

            if (sdp_validate_header(buffer, SDP_REQUEST_TYPE, SDP_REQUEST_PAYLOAD_LEN)) {
                mlog_w(LOG_TAG_V2G, "Packet with invalid SDP header received from [%s]:%u" , addr, ntohs(sdp_query.remote_addr.sin6_port));
                continue;
            }

            sdp_query.security_requested = (sdp_security_t)buffer[SDP_HEADER_LEN + 0];
            sdp_query.proto_requested = (sdp_transport_protocol_t)buffer[SDP_HEADER_LEN + 1];

            mlog_d(LOG_TAG_V2G, "Received packet from [%s]:%u with security 0x%02x and protocol 0x%02x", addr, ntohs(sdp_query.remote_addr.sin6_port), sdp_query.security_requested, sdp_query.proto_requested);

            sdp_send_response(v2g, &sdp_query);
        }
    }
    
    return NULL;
}


int v2g_sdp_server_init(v2g_context_t* v2g)
{
    struct sockaddr_in6 sdp_addr = {AF_INET6, htons(SDP_SRV_PORT)};
    struct ipv6_mreq mreq = {{IN6ADDR_ALLNODES}, 0};
    int enable = 1;

    v2g->sdp_server_quit = 0;
    v2g->sdp_socket = -1;

    mreq.ipv6mr_interface = if_nametoindex(v2g->if_name);
    if (!mreq.ipv6mr_interface) {
        mlog_e(LOG_TAG_V2G, "No such interface: %s", v2g->if_name);
        return -1;
    }

    /* create receiving socket */
    v2g->sdp_socket = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
    if (v2g->sdp_socket == -1) {
        mlog_e(LOG_TAG_V2G, "socket() failed: %s", strerror(errno));
        return -1;
    }

    if (setsockopt(v2g->sdp_socket, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable)) == -1) {
        mlog_e(LOG_TAG_V2G, "setsockopt(SO_REUSEPORT) failed: %s", strerror(errno));
        close(v2g->sdp_socket);
        return -1;
    }

    sdp_addr.sin6_addr = in6addr_any;

    if (bind(v2g->sdp_socket, (struct sockaddr*)&sdp_addr, sizeof(sdp_addr)) == -1) {
        mlog_e(LOG_TAG_V2G, "bind() failed: %s", strerror(errno));
        close(v2g->sdp_socket);
        return -1;
    }

    mlog_i(LOG_TAG_V2G, "SDP socket setup succeeded");

    /* bind only to specified device */
    if (setsockopt(v2g->sdp_socket, SOL_SOCKET, SO_BINDTODEVICE, v2g->if_name, strlen(v2g->if_name)) ==
        -1) {
        mlog_e(LOG_TAG_V2G, "setsockopt(SO_BINDTODEVICE) failed: %s", strerror(errno));
        close(v2g->sdp_socket);
        return -1;
    }

    mlog_i(LOG_TAG_V2G, "bind only to specified device");
    
    /* join multicast group */
    if (setsockopt(v2g->sdp_socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) == -1) {
        mlog_e(LOG_TAG_V2G, "setsockopt(IPV6_JOIN_GROUP) failed: %s", strerror(errno));
        close(v2g->sdp_socket);
        return -1;
    }

    mlog_i(LOG_TAG_V2G, "joined multicast group");

    mthread_init(&v2g->sdp_thread, v2g_sdp_server_listen, v2g);

    return 0;
}

void v2g_sdp_server_exit(v2g_context_t* v2g)
{
    if (v2g)
    {
        printd("step\n");
        if (v2g->sdp_server_quit == 0)
        {
            v2g->sdp_server_quit = 1;
            mthread_exit(&v2g->sdp_thread);
        }
        if (v2g->sdp_socket > 0)
        {
            close(v2g->sdp_socket);
            v2g->sdp_socket = - 1;
        }
    }
}
