#include "paracomm.h"

#include <stdlib.h>

#define kMosiIdleByte 0x00
#define kMosiStartByte 0x42
#define kMisoIdleNibble 0x0
#define kMisoStartNibble 0xa

static uint8_t mosi_workbuf[2][256];
static uint8_t mosi_workbuf_idx;
static uint8_t mosi_workbuf_size;

static uint8_t* mosi_sendbuf;
static uint8_t mosi_size;
static uint8_t mosi_sent;

static uint8_t mosi_state;

static uint8_t miso_recvbuf[256];
static uint8_t miso_size;
static uint8_t miso_received_nibbles;
static uint8_t miso_state;
static void (*miso_cb)(const uint8_t*, uint8_t);

static void swapbuffers() {
    mosi_sendbuf = mosi_workbuf[mosi_workbuf_idx];
    mosi_size = mosi_workbuf_size;

    mosi_workbuf_size = 0;
    mosi_workbuf_idx = (mosi_workbuf_idx + 1) % 2;
}

uint8_t paracomm_nextbyte() {
    switch (mosi_state) {
    case 0:
        if (mosi_size == 0) {
            swapbuffers();
            if (mosi_size == 0) {
                return kMosiIdleByte;
            }
        }
        mosi_state = 1;
        return kMosiStartByte;
    case 1:
        // assert(mosi_size > 0)
        mosi_sent = 0;
        mosi_state = 2;
        return mosi_size;
    case 2: {
        uint8_t b = mosi_sendbuf[mosi_sent];
        mosi_sent += 1;

        if (mosi_sent == mosi_size) {
            swapbuffers();
            mosi_state = 0;
        }

        return b;
    }
    }

    __builtin_unreachable();
}

void paracomm_feed(uint8_t n) {
    switch (miso_state) {
    case 0:
        if (n == kMisoStartNibble) {
            miso_state = 1;
        } else if (n == kMisoIdleNibble) {
        } else {
            // error: spurious nibble
        }
        break;
    case 1:
        miso_size = n;
        miso_state = 2;
        break;
    case 2:
        miso_size += n << 4;
        miso_received_nibbles = 0;
        miso_state = 3;
        break;
    case 3: {
        uint8_t idx = miso_received_nibbles / 2;
        if (miso_received_nibbles % 2 == 0) {
            miso_recvbuf[idx] = n;
        } else {
            miso_recvbuf[idx] += n << 4;
        }
        miso_received_nibbles += 1;

        if (miso_received_nibbles == 2 * miso_size) {
            if (miso_cb != 0) {
                miso_cb(miso_recvbuf, miso_size);
            }
            miso_state = 0;
        }
    } break;
    }
}

void paracomm_init(miso_cb_t cb) {
    mosi_size = 0;
    mosi_workbuf_idx = 0;
    mosi_workbuf_size = 0;
    mosi_state = 0;

    miso_state = 0;
    miso_cb = cb;
}

int paracomm_send(uint8_t b) {
    if (mosi_workbuf_size == 256) {
        return -1;
    }

    uint8_t* buff = mosi_workbuf[mosi_workbuf_idx];
    buff[mosi_workbuf_size] = b;
    mosi_workbuf_size += 1;

    return 0;
}