#include "pico/stdlib.h"
#include "pico/bootrom.h"
#include "pico/unique_id.h"
#include "tusb.h"
#include "wire.h"

#include "w6300.h"

struct tx_buf {
    uint8_t data[512];
    uint16_t head = 0;
    uint16_t tail = 0;

    uint16_t used() const { return tail - head; }
    uint16_t free() const { return sizeof(data) - used(); }
    bool empty() const { return head == tail; }

    void push(const uint8_t* src, uint16_t len) {
        if (len > free()) return;
        for (uint16_t i = 0; i < len; i++)
            data[(tail + i) % sizeof(data)] = src[i];
        tail += len;
    }

    void drain() {
        while (!empty()) {
            uint32_t avail = tud_cdc_write_available();
            if (avail == 0) break;
            uint16_t offset = head % sizeof(data);
            uint16_t contig = sizeof(data) - offset;
            uint16_t pending = used();
            uint16_t chunk = pending < contig ? pending : contig;
            if (chunk > avail) chunk = avail;
            tud_cdc_write(data + offset, chunk);
            head += chunk;
        }
        tud_cdc_write_flush();
        if (head >= sizeof(data)) {
            head -= sizeof(data);
            tail -= sizeof(data);
        }
    }
};

static tx_buf usb_tx;

static void send_bytes(const std::vector<uint8_t>& data) {
    usb_tx.push(data.data(), data.size());
    usb_tx.drain();
}

static bool w6300_init() {
    w6300::init_spi();
    w6300::init_critical_section();
    w6300::reset();
    w6300::init();
    if (!w6300::check()) return false;

    pico_unique_board_id_t uid;
    pico_get_unique_board_id(&uid);
    w6300::net_info info = {};
    info.mac[0] = (uid.id[0] & 0xFC) | 0x02;
    info.mac[1] = uid.id[1];
    info.mac[2] = uid.id[2];
    info.mac[3] = uid.id[3];
    info.mac[4] = uid.id[4];
    info.mac[5] = uid.id[5];
    w6300::init_net(info);

    return true;
}

int main() {
    tusb_init();

    w6300_init();

    static static_vector<uint8_t, 256> rx_buf;

    while (true) {
        tud_task();

        usb_tx.drain();

        while (tud_cdc_available()) {
            uint8_t byte;
            if (tud_cdc_read(&byte, 1) != 1) break;

            rx_buf.push_back(byte);

            auto msg = try_decode(rx_buf);
            if (!msg) {
                if (rx_buf.full()) rx_buf.clear();
                continue;
            }

            rx_buf.clear();

            switch (msg->type_id) {
            case RequestPICOBOOT::ext_id:
                send_bytes(encode_response(msg->message_id, ResponsePICOBOOT{}));
                sleep_ms(100);
                reset_usb_boot(0, 1);
                break;
            case RequestInfo::ext_id: {
                ResponseInfo resp;
                pico_unique_board_id_t uid;
                pico_get_unique_board_id(&uid);
                std::copy(uid.id, uid.id + 8, resp.board_id.begin());
                auto ninfo = w6300::get_net_info();
                resp.mac = ninfo.mac;
                send_bytes(encode_response(msg->message_id, resp));
                break;
            }
            }
        }

        __wfi();
    }
}