/* KISS protocol implementation — C99 */
#include "kiss.h"

void kiss_decoder_init(kiss_decoder_t *dec) {
    dec->state = KISS_STATE_IDLE;
    dec->len = 0;
}

bool kiss_decode(kiss_decoder_t *dec, uint8_t byte, kiss_frame_t *frame) {
    switch (dec->state) {
    case KISS_STATE_IDLE:
        if (byte == KISS_FEND) {
            dec->len = 0;
            dec->state = KISS_STATE_IN_FRAME;
        }
        return false;

    case KISS_STATE_IN_FRAME:
        if (byte == KISS_FESC) {
            dec->state = KISS_STATE_ESCAPE;
            return false;
        }
        if (byte == KISS_FEND) {
            if (dec->len > 0) {
                /* Frame complete */
                frame->port = dec->buf[0] & 0x0Fu;
                frame->len = dec->len - 1;
                if (frame->len > 0) {
                    for (size_t i = 0; i < frame->len; i++) {
                        frame->data[i] = dec->buf[i + 1];
                    }
                }
                dec->state = KISS_STATE_IDLE;
                return true;
            }
            dec->state = KISS_STATE_IDLE;
            return false;
        }
        if (dec->len < KISS_MAX_FRAME + 1) {
            dec->buf[dec->len++] = byte;
        }
        return false;

    case KISS_STATE_ESCAPE:
        if (byte == KISS_TFEND) {
            byte = KISS_FEND;
        } else if (byte == KISS_TFESC) {
            byte = KISS_FESC;
        }
        if (dec->len < KISS_MAX_FRAME + 1) {
            dec->buf[dec->len++] = byte;
        }
        dec->state = KISS_STATE_IN_FRAME;
        return false;
    }
    return false;
}

size_t kiss_encode(uint8_t port, const uint8_t *data, size_t len,
                   uint8_t *dst, size_t dst_cap) {
    if (dst_cap < len + 3)
        return 0;   /* Need at least: FEND type data... FEND */

    size_t pos = 0;
    dst[pos++] = KISS_FEND;

    /* Type byte: port in upper nibble, cmd in lower nibble (0 for data) */
    uint8_t type = (port << 4) | 0x00;
    if (type == KISS_FEND) {
        dst[pos++] = KISS_FESC;
        dst[pos++] = KISS_TFEND;
    } else if (type == KISS_FESC) {
        dst[pos++] = KISS_FESC;
        dst[pos++] = KISS_TFESC;
    } else {
        dst[pos++] = type;
    }

    /* Payload with escaping */
    for (size_t i = 0; i < len; i++) {
        if (pos + 2 > dst_cap)
            return 0;   /* Overflow */
        if (data[i] == KISS_FEND) {
            dst[pos++] = KISS_FESC;
            dst[pos++] = KISS_TFEND;
        } else if (data[i] == KISS_FESC) {
            dst[pos++] = KISS_FESC;
            dst[pos++] = KISS_TFESC;
        } else {
            dst[pos++] = data[i];
        }
    }

    if (pos + 1 > dst_cap)
        return 0;
    dst[pos++] = KISS_FEND;
    return pos;
}

size_t kiss_encode_quality(int8_t snr, int16_t rssi, uint8_t *dst,
                           size_t dst_cap) {
    if (dst_cap < 5)
        return 0;   /* Need: FEND type snr rssi_hi rssi_lo FEND */

    uint8_t payload[3];
    payload[0] = (uint8_t)snr;
    payload[1] = (uint8_t)((rssi >> 8) & 0xFF);
    payload[2] = (uint8_t)(rssi & 0xFF);

    return kiss_encode(KISS_PORT_QUALITY, payload, 3, dst, dst_cap);
}