#include "set_protocol.h"

#include <string.h>

uint16_t setp_get_u16(const uint8_t *data)
{
    return (uint16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
}

uint32_t setp_get_u32(const uint8_t *data)
{
    return (uint32_t)data[0]
        | ((uint32_t)data[1] << 8)
        | ((uint32_t)data[2] << 16)
        | ((uint32_t)data[3] << 24);
}

void setp_put_u16(uint8_t *data, uint16_t value)
{
    data[0] = (uint8_t)(value & 0xFFU);
    data[1] = (uint8_t)(value >> 8);
}

void setp_put_u32(uint8_t *data, uint32_t value)
{
    data[0] = (uint8_t)(value & 0xFFU);
    data[1] = (uint8_t)((value >> 8) & 0xFFU);
    data[2] = (uint8_t)((value >> 16) & 0xFFU);
    data[3] = (uint8_t)(value >> 24);
}

uint32_t setp_crc32(const uint8_t *data, size_t length)
{
    uint32_t crc = 0xFFFFFFFFUL;
    size_t index;
    uint8_t bit;

    if ((data == NULL) && (length != 0U)) {
        return 0U;
    }
    for (index = 0U; index < length; ++index) {
        crc ^= data[index];
        for (bit = 0U; bit < 8U; ++bit) {
            crc = (crc >> 1) ^ ((crc & 1U) ? 0xEDB88320UL : 0U);
        }
    }
    return crc ^ 0xFFFFFFFFUL;
}

size_t setp_frame_encode(const setp_frame_t *frame, uint8_t *out, size_t capacity)
{
    size_t total;
    uint32_t crc;

    if ((frame == NULL) || (out == NULL)) {
        return 0U;
    }
    if ((frame->flags & SETP_FLAG_RESERVED_MASK) != 0U) {
        return 0U;
    }
    if ((frame->payload_length > SETP_MAX_PAYLOAD)
            || ((frame->payload_length != 0U) && (frame->payload == NULL))) {
        return 0U;
    }
    total = (size_t)SETP_HEADER_SIZE + frame->payload_length + SETP_CRC_SIZE;
    if (capacity < total) {
        return 0U;
    }

    out[0] = SETP_SOF0;
    out[1] = SETP_SOF1;
    out[2] = SETP_VERSION;
    out[3] = frame->flags;
    setp_put_u16(&out[4], frame->message_type);
    setp_put_u16(&out[6], frame->source);
    setp_put_u16(&out[8], frame->destination);
    setp_put_u16(&out[10], frame->sequence);
    setp_put_u16(&out[12], frame->payload_length);
    if (frame->payload_length != 0U) {
        (void)memcpy(&out[SETP_HEADER_SIZE], frame->payload, frame->payload_length);
    }
    crc = setp_crc32(&out[2], (size_t)(SETP_HEADER_SIZE - 2U) + frame->payload_length);
    setp_put_u32(&out[SETP_HEADER_SIZE + frame->payload_length], crc);
    return total;
}

bool setp_frame_decode_datagram(const uint8_t *data, size_t length, setp_frame_t *out)
{
    uint16_t payload_length;
    size_t expected_length;
    uint32_t expected_crc;
    if ((data == NULL) || (out == NULL) || (length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
            || (data[0] != SETP_SOF0) || (data[1] != SETP_SOF1)
            || (data[2] != SETP_VERSION)
            || ((data[3] & SETP_FLAG_RESERVED_MASK) != 0U)) {
        return false;
    }
    payload_length = setp_get_u16(&data[12]);
    expected_length = (size_t)SETP_HEADER_SIZE + payload_length + SETP_CRC_SIZE;
    if ((payload_length > SETP_MAX_PAYLOAD) || (length != expected_length)) {
        return false;
    }
    expected_crc = setp_get_u32(&data[SETP_HEADER_SIZE + payload_length]);
    if (setp_crc32(&data[2], (size_t)(SETP_HEADER_SIZE - 2U) + payload_length)
            != expected_crc) {
        return false;
    }
    out->flags = data[3];
    out->message_type = setp_get_u16(&data[4]);
    out->source = setp_get_u16(&data[6]);
    out->destination = setp_get_u16(&data[8]);
    out->sequence = setp_get_u16(&data[10]);
    out->payload_length = payload_length;
    out->payload = &data[SETP_HEADER_SIZE];
    return true;
}

void setp_parser_init(setp_parser_t *parser)
{
    if (parser != NULL) {
        (void)memset(parser, 0, sizeof(*parser));
    }
}

void setp_parser_reset(setp_parser_t *parser)
{
    if (parser != NULL) {
        parser->length = 0U;
    }
}

static void setp_drop(setp_parser_t *parser, uint16_t count)
{
    if (count >= parser->length) {
        parser->length = 0U;
        return;
    }
    (void)memmove(parser->buffer, &parser->buffer[count], parser->length - count);
    parser->length = (uint16_t)(parser->length - count);
}

static uint16_t setp_find_sof(const setp_parser_t *parser)
{
    uint16_t index;
    for (index = 0U; index + 1U < parser->length; ++index) {
        if ((parser->buffer[index] == SETP_SOF0)
                && (parser->buffer[index + 1U] == SETP_SOF1)) {
            return index;
        }
    }
    return UINT16_MAX;
}

static size_t setp_parser_process(setp_parser_t *parser,
                                  setp_frame_fn callback, void *user)
{
    size_t delivered = 0U;

    for (;;) {
        uint16_t sof;
        uint16_t payload_length;
        uint16_t total;
        uint32_t expected_crc;
        uint32_t actual_crc;
        setp_frame_t frame;

        if (parser->length < 2U) {
            break;
        }
        sof = setp_find_sof(parser);
        if (sof == UINT16_MAX) {
            if (parser->buffer[parser->length - 1U] == SETP_SOF0) {
                parser->stats.stray_bytes += parser->length - 1U;
                parser->buffer[0] = SETP_SOF0;
                parser->length = 1U;
            } else {
                parser->stats.stray_bytes += parser->length;
                parser->length = 0U;
            }
            break;
        }
        if (sof != 0U) {
            parser->stats.stray_bytes += sof;
            setp_drop(parser, sof);
        }
        if (parser->length < SETP_HEADER_SIZE) {
            break;
        }
        if (parser->buffer[2] != SETP_VERSION) {
            ++parser->stats.version_errors;
            setp_drop(parser, 1U);
            continue;
        }
        if ((parser->buffer[3] & SETP_FLAG_RESERVED_MASK) != 0U) {
            ++parser->stats.flag_errors;
            setp_drop(parser, 1U);
            continue;
        }
        payload_length = setp_get_u16(&parser->buffer[12]);
        if (payload_length > SETP_MAX_PAYLOAD) {
            ++parser->stats.length_errors;
            setp_drop(parser, 1U);
            continue;
        }
        total = (uint16_t)(SETP_HEADER_SIZE + payload_length + SETP_CRC_SIZE);
        if (parser->length < total) {
            break;
        }
        expected_crc = setp_get_u32(&parser->buffer[SETP_HEADER_SIZE + payload_length]);
        actual_crc = setp_crc32(&parser->buffer[2],
                                (size_t)(SETP_HEADER_SIZE - 2U) + payload_length);
        if (expected_crc != actual_crc) {
            ++parser->stats.crc_errors;
            setp_drop(parser, 1U);
            continue;
        }

        frame.flags = parser->buffer[3];
        frame.message_type = setp_get_u16(&parser->buffer[4]);
        frame.source = setp_get_u16(&parser->buffer[6]);
        frame.destination = setp_get_u16(&parser->buffer[8]);
        frame.sequence = setp_get_u16(&parser->buffer[10]);
        frame.payload_length = payload_length;
        frame.payload = &parser->buffer[SETP_HEADER_SIZE];
        if (callback != NULL) {
            callback(&frame, user);
        }
        ++parser->stats.frames;
        ++delivered;
        setp_drop(parser, total);
    }
    return delivered;
}

size_t setp_parser_feed(setp_parser_t *parser, const uint8_t *data, size_t length,
                        setp_frame_fn callback, void *user)
{
    size_t index;
    size_t delivered = 0U;

    if ((parser == NULL) || ((data == NULL) && (length != 0U))) {
        return 0U;
    }
    for (index = 0U; index < length; ++index) {
        if (parser->length >= SETP_FRAME_MAX) {
            ++parser->stats.overflows;
            parser->length = 0U;
        }
        parser->buffer[parser->length++] = data[index];
        delivered += setp_parser_process(parser, callback, user);
    }
    return delivered;
}

size_t setp_status_encode(uint16_t status, uint8_t *out, size_t capacity)
{
    if ((out == NULL) || (capacity < 2U)) {
        return 0U;
    }
    setp_put_u16(out, status);
    return 2U;
}

bool setp_status_decode(const setp_frame_t *frame, uint16_t *status,
                        const uint8_t **body, uint16_t *body_length)
{
    if ((frame == NULL) || (status == NULL)
            || ((frame->flags & SETP_FLAG_RESPONSE) == 0U)
            || (frame->payload_length < 2U) || (frame->payload == NULL)) {
        return false;
    }
    *status = setp_get_u16(frame->payload);
    if (body != NULL) {
        *body = &frame->payload[2];
    }
    if (body_length != NULL) {
        *body_length = (uint16_t)(frame->payload_length - 2U);
    }
    return true;
}