#include <stdlib.h>
#include <string.h>

#include <skipstone/link.h>
#include <skipstone/map.h>
#include <skipstone/queue.h>
#include <skipstone/message.h>

struct part {
    uint8_t size;
};

struct _skipstone_message {
    skipstone_queue *parts;
    uint16_t size;
};

struct endpoint {
    skipstone_message_handler *handler;
    void *context;
    uint16_t id;
};

struct _skipstone_message_service {
    skipstone_map *endpoints;
    skipstone_queue *pending;
    void *buf;
};

skipstone_message *skipstone_message_new() {
    skipstone_message *message;

    if ((message = malloc(sizeof(*message))) == NULL) {
        goto error_malloc_message;
    }

    if ((message->parts = skipstone_queue_new()) == NULL) {
        goto error_queue_new_parts;
    }

    message->size = 0;

    return message;

error_queue_new_parts:
    free(message);

error_malloc_message:
    return NULL;
}

void skipstone_message_destroy(skipstone_message *message) {
    void *part;

    while (skipstone_queue_remove(message->parts, &part)) {
        free(part);
    }

    skipstone_queue_destroy(message->parts);

    free(message);

    return;
}

static int append_part(skipstone_message *message, void *buf, uint8_t size) {
    struct part *part;

    if ((part = malloc(sizeof(*part) + size)) == NULL) {
        goto error_malloc_part;
    }

    part->size = size;

    memcpy(part + 1, buf, size);

    if (skipstone_queue_add(message->parts, part) < 0) {
        goto error_queue_add_parts;
    }

    return 0;

error_queue_add_parts:
    free(part);

error_malloc_part:
    return -1;
}

int skipstone_message_append_string(skipstone_message *message,
        char *string, uint8_t size) {
    struct part *part;
    uint8_t bodysz = sizeof(size) + size;

    if ((part = malloc(sizeof(*part) + bodysz)) == NULL) {
        goto error_malloc_part;
    }

    part->size = bodysz;

    memcpy(part + 1,                               &size,  sizeof(size));
    memcpy(((uint8_t *)(part + 1)) + sizeof(size), string, size);

    if (skipstone_queue_add(message->parts, part) < 0) {
        goto error_queue_add_part;
    }

    message->size += bodysz;

    return 0;

error_queue_add_part:
    free(part);

error_malloc_part:
    return -1;
}

uint16_t skipstone_message_size(skipstone_message *message) {
    return message->size;
}

int skipstone_message_append_uint8(skipstone_message *message, uint8_t value) {
    if (append_part(message, &value, sizeof(value)) < 0) {
        return -1;
    }

    message->size += sizeof(value);

    return 0;
}

int skipstone_message_append_uint16(skipstone_message *message, uint16_t value) {
    if (append_part(message, &value, sizeof(value)) < 0) {
        return -1;
    }

    message->size += sizeof(value);

    return 0;
}

int skipstone_message_append_uint32(skipstone_message *message, uint32_t value) {
    if (append_part(message, &value, sizeof(value)) < 0) {
        return -1;
    }

    message->size += sizeof(value);

    return 0;
}

int skipstone_message_pack(skipstone_message *message, void *buf) {
    size_t offset = 0;

    struct part *part;

    if (message->size > SKIPSTONE_MESSAGE_MAX_PAYLOAD) {
        return -1;
    }

    while (skipstone_queue_remove(message->parts, (void **)&part)) {
        memcpy(((uint8_t *)buf) + offset, part + 1, part->size);

        offset += part->size;
    }

    return 0;
}

skipstone_message_service *skipstone_message_service_new() {
    skipstone_message_service *service;

    if ((service = malloc(sizeof(*service))) == NULL) {
        goto error_malloc_service;
    }

    if ((service->buf = malloc(SKIPSTONE_MESSAGE_MAX_PAYLOAD)) == NULL) {
        goto error_malloc_buf;
    }

    if ((service->endpoints = skipstone_map_new()) == NULL) {
        goto error_map_new_endpoints;
    }

    if ((service->pending = skipstone_queue_new()) == NULL) {
        goto error_queue_new_pending;
    }

    return service;

error_queue_new_pending:
    skipstone_map_destroy(service->endpoints, free);

error_map_new_endpoints:
    free(service->buf);

error_malloc_buf:
    free(service);

error_malloc_service:
    return NULL;
}

void skipstone_message_service_destroy(skipstone_message_service *service) {
    skipstone_message *message;

    skipstone_map_destroy(service->endpoints, free);

    while (skipstone_queue_remove(service->pending, (void **)&message)) {
        skipstone_message_destroy(message);
    }

    skipstone_queue_destroy(service->pending);

    free(service->buf);

    free(service);

    return;
}

int skipstone_message_service_register(skipstone_message_service *service,
        uint16_t id, skipstone_message_handler *handler, void *context) {
    struct endpoint *endpoint;

    if ((endpoint = malloc(sizeof(*endpoint))) == NULL) {
        goto error_malloc_endpoint;
    }

    endpoint->id      = id;
    endpoint->handler = handler;
    endpoint->context = context;

    return skipstone_map_set(service->endpoints, id, endpoint);

error_malloc_endpoint:
    return -1;
}

int skipstone_message_service_deregister(skipstone_message_service *service, uint16_t id) {
    return skipstone_map_set((skipstone_map *)service, id, NULL);
}

int skipstone_message_service_queue_packed(skipstone_message_service *service,
        void *buf, uint16_t size, uint16_t id) {
    skipstone_message_header *message;

    if ((message = malloc(sizeof(*message) + size)) == NULL) {
        goto error_malloc_message;
    }

    message->size = size;
    message->id   = id;

    memcpy(message + 1, buf, size);

    if (skipstone_queue_add(service->pending, message) < 0) {
        goto error_queue_add_pending;
    }

    return 0;

error_queue_add_pending:
    free(message);

error_malloc_message:
    return -1;
}

int skipstone_message_service_queue(skipstone_message_service *service,
        skipstone_message *message, uint16_t id) {
    skipstone_message_header *packed;

    if ((packed = malloc(sizeof(*packed) + message->size)) == NULL) {
        goto error_malloc_packed;
    }

    packed->size = message->size;
    packed->id   = id;

    if (skipstone_message_pack(message, packed + 1) < 0) {
        goto error_pack;
    }

    if (skipstone_queue_add(service->pending, packed) < 0) {
        goto error_queue_add_pending;
    }

    return 0;

error_queue_add_pending:
error_pack:
    free(message);

error_malloc_packed:
    return -1;
}

int skipstone_message_service_next_event(skipstone_message_service *service,
        skipstone_link *link) {
    skipstone_message_header *message;
    struct endpoint *endpoint;
    uint16_t size, id;

    if (skipstone_link_recv(link, service->buf, &size, &id) < 0) {
        goto error_io;
    }

    if ((endpoint = skipstone_map_get(service->endpoints, id)) != NULL) {
        if (endpoint->handler(service, service->buf, size, id, endpoint->context) < 0) {
            goto error_io;
        }
    }

    while (skipstone_queue_remove(service->pending, (void **)&message)) {
        if (skipstone_link_send(link, message + 1, message->size, message->id) < 0) {
            goto error_io;
        }
    }

    return 0;

error_io:
    return -1;
}

int skipstone_message_service_run(skipstone_message_service *service,
        skipstone_link *link) {
    while (1) {
        if (skipstone_message_service_next_event(service, link) < 0) {
            goto error_service_next_event;
        }
    }

    return 0;

error_service_next_event:
    return -1;
}