#include "pcan_frame.h"
#include "pcan_crc.h"

size_t pcan_frame_encode(const pcan_frame_t *frame, uint8_t *out, size_t out_size)
{
    if ((frame == NULL) || (out == NULL) || (out_size < PCAN_FRAME_MAX)) {
        return 0U;
    }

    uint8_t dlc = (frame->dlc > PCAN_DATA_MAX) ? (uint8_t)PCAN_DATA_MAX : frame->dlc;
    size_t n = 0U;

    out[n++] = PCAN_SOF0;
    out[n++] = PCAN_SOF1;

    size_t body = n;
    out[n++] = (uint8_t)(PCAN_LEN_MIN + dlc);
    out[n++] = frame->seq;
    out[n++] = frame->flags;
    out[n++] = (uint8_t)(frame->id & 0xFFU);
    out[n++] = (uint8_t)((frame->id >> 8) & 0xFFU);
    out[n++] = (uint8_t)((frame->id >> 16) & 0xFFU);
    out[n++] = (uint8_t)((frame->id >> 24) & 0x1FU);
    for (uint8_t i = 0U; i < dlc; i++) {
        out[n++] = frame->data[i];
    }

    uint16_t crc = pcan_crc16(&out[body], n - body);
    out[n++] = (uint8_t)(crc & 0xFFU);
    out[n++] = (uint8_t)(crc >> 8);
    return n;
}

void pcan_parser_init(pcan_parser_t *p)
{
    p->state = PCAN_ST_SOF0;
    p->len = 0U;
    p->idx = 0U;
    p->crc_rx = 0U;
    p->stats.frames = 0U;
    p->stats.crc_errors = 0U;
    p->stats.bad_len = 0U;
    p->stats.stray_bytes = 0U;
}

bool pcan_parser_push(pcan_parser_t *p, uint8_t byte, pcan_frame_t *out)
{
    switch (p->state) {
    case PCAN_ST_SOF0:
        if (byte == PCAN_SOF0) {
            p->state = PCAN_ST_SOF1;
        } else {
            p->stats.stray_bytes++;
        }
        break;

    case PCAN_ST_SOF1:
        if (byte == PCAN_SOF1) {
            p->state = PCAN_ST_LEN;
        } else if (byte == PCAN_SOF0) {
            /* 0xAA 0xAA 0x55 - тоже валидное начало, остаёмся здесь. */
            p->stats.stray_bytes++;
        } else {
            p->stats.stray_bytes += 2U;
            p->state = PCAN_ST_SOF0;
        }
        break;

    case PCAN_ST_LEN:
        if ((byte < PCAN_LEN_MIN) || (byte > PCAN_LEN_MAX)) {
            p->stats.bad_len++;
            /* Байт мог оказаться началом настоящего кадра. */
            p->state = (byte == PCAN_SOF0) ? PCAN_ST_SOF1 : PCAN_ST_SOF0;
            break;
        }
        p->len = byte;
        p->body[0] = byte;
        p->idx = 1U;
        p->state = PCAN_ST_BODY;
        break;

    case PCAN_ST_BODY:
        p->body[p->idx++] = byte;
        if (p->idx >= (uint8_t)(p->len + 1U)) {
            p->state = PCAN_ST_CRC_L;
        }
        break;

    case PCAN_ST_CRC_L:
        p->crc_rx = byte;
        p->state = PCAN_ST_CRC_H;
        break;

    case PCAN_ST_CRC_H: {
        p->crc_rx |= (uint16_t)((uint16_t)byte << 8);
        p->state = PCAN_ST_SOF0;
        if (p->crc_rx != pcan_crc16(p->body, (size_t)p->len + 1U)) {
            p->stats.crc_errors++;
            return false;
        }
        out->seq = p->body[1];
        out->flags = p->body[2];
        out->id = ((uint32_t)p->body[3])
                | ((uint32_t)p->body[4] << 8)
                | ((uint32_t)p->body[5] << 16)
                | ((uint32_t)p->body[6] << 24);
        out->id &= 0x1FFFFFFFUL;
        out->dlc = (uint8_t)(p->len - PCAN_LEN_MIN);
        for (uint8_t i = 0U; i < out->dlc; i++) {
            out->data[i] = p->body[7U + i];
        }
        for (uint8_t i = out->dlc; i < PCAN_DATA_MAX; i++) {
            out->data[i] = 0U;
        }
        p->stats.frames++;
        return true;
    }

    default:
        p->state = PCAN_ST_SOF0;
        break;
    }
    return false;
}

size_t pcan_parser_feed(pcan_parser_t *p, const uint8_t *data, size_t len,
                        pcan_frame_cb_t cb, void *user)
{
    pcan_frame_t frame;
    size_t count = 0U;

    for (size_t i = 0U; i < len; i++) {
        if (pcan_parser_push(p, data[i], &frame)) {
            count++;
            if (cb != NULL) {
                cb(&frame, user);
            }
        }
    }
    return count;
}