#pragma once

#include <stdint.h>

struct ParaComms {
    using MessageCallback = void (*)(uint8_t *, uint8_t);
    enum State {
        kIdle,
        kLength,
        kLength2, // for miso
        kData,
        // kCrc,  // ?
    };
    static constexpr uint8_t kMosiStartByte = 0x42;
    static constexpr uint8_t kIdleByte = 0x00;
    static constexpr uint8_t kMisoStartNibble = 0xa;
    static constexpr uint8_t kMisoIdleNibble = 0x0;

    MessageCallback mosi_cb = nullptr;
    uint8_t *miso_buffer = nullptr;
    uint8_t miso_size = 0;
    uint8_t miso_nibbles_sent = 0;
    State miso_state = kIdle;

    // double work buffer. one being sent, one being written to
    uint8_t miso_workbuf[2][256];
    uint8_t miso_workbuf_idx = 0;
    uint8_t miso_workbuf_size = 0;

    uint8_t mosi_buffer[256];
    uint8_t mosi_size = 0;
    uint8_t mosi_received = 0;
    State mosi_state = kIdle;

    void SwapBuffers() {
        miso_buffer = miso_workbuf[miso_workbuf_idx];
        miso_size = miso_workbuf_size;

        miso_workbuf_idx = (miso_workbuf_idx + 1) % 2;
        miso_workbuf_size = 0;
    }

    int SendByte(uint8_t b) {
        if (miso_workbuf_size == 256) {
            return -1; // sorry we're full
        }

        uint8_t *buff = miso_workbuf[miso_workbuf_idx];
        buff[miso_workbuf_size] = b;
        miso_workbuf_size += 1;

        return 0;
    }

    void FeedMosiData(uint8_t b) {
        switch (mosi_state) {
        case kIdle:
            // check start byte
            if (b == kMosiStartByte) {
                mosi_state = kLength;
            } else if (b == kIdleByte) {
                // just twiddling our thumb drives
            } else {
                // Serial.printf("sp: %02x\n", b);
            }
            break;
        case kLength:
            // assert(b > 0)
            mosi_size = b;
            mosi_received = 0;
            mosi_state = kData;
            break;
        case kData:
            mosi_buffer[mosi_received] = b;
            mosi_received += 1;

            if (mosi_received == mosi_size) {
                if (mosi_cb != nullptr) {
                    mosi_cb(mosi_buffer, mosi_received);
                }
                mosi_state = kIdle;
            }
        case kLength2: // error
            return;
        }
    }

    uint8_t NextMisoNibble() {
        switch (miso_state) {
        case kIdle:
            if (miso_size == 0) {
                SwapBuffers();
                if (miso_size == 0) {
                    return kMisoIdleNibble;
                }
            }
            miso_state = kLength;
            return kMisoStartNibble;
        case kLength:
            // assert(miso_size > 0);
            miso_state = kLength2;
            return miso_size & 0xf;
        case kLength2:
            miso_nibbles_sent = 0;
            miso_state = kData;
            return miso_size >> 4;
        case kData: {
            uint8_t b = miso_buffer[miso_nibbles_sent / 2];
            if (miso_nibbles_sent % 2 == 0) {
                b &= 0xf;
            } else {
                b >>= 4;
            }
            miso_nibbles_sent += 1;
            if (miso_nibbles_sent == miso_size * 2) {
                SwapBuffers();
                miso_state = kIdle;
            }
            return b;
        }
        }
        __builtin_unreachable();
    }
};