#include <cstring>
#include <initializer_list>
#include "pico/stdlib.h"
#include "pico/error.h"
#include "hardware/dma.h"
#include "hardware/clocks.h"
#include "w6300.h"
#include "qspi.pio.h"

namespace w6300 {
namespace {

#define PIO_PROGRAM_NAME qspi
#define PIO_PROGRAM_FUNC __CONCAT(PIO_PROGRAM_NAME, _program)
#define PIO_PROGRAM_GET_DEFAULT_CONFIG_FUNC __CONCAT(PIO_PROGRAM_NAME, _program_get_default_config)
#define PIO_OFFSET_WRITE_BITS __CONCAT(PIO_PROGRAM_NAME, _offset_write_bits)
#define PIO_OFFSET_WRITE_BITS_END __CONCAT(PIO_PROGRAM_NAME, _offset_write_bits_end)
#define PIO_OFFSET_READ_BITS_END __CONCAT(PIO_PROGRAM_NAME, _offset_read_bits_end)

constexpr uint8_t PIN_INT              = 15;
constexpr uint8_t PIN_CS               = 16;
constexpr uint8_t PIO_SPI_SCK_PIN      = 17;
constexpr uint8_t PIO_SPI_DATA_IO0_PIN = 18;
constexpr uint8_t PIO_SPI_DATA_IO1_PIN = 19;
constexpr uint8_t PIO_SPI_DATA_IO2_PIN = 20;
constexpr uint8_t PIO_SPI_DATA_IO3_PIN = 21;
constexpr uint8_t PIN_RST              = 22;

constexpr uint16_t SPI_CLKDIV_MAJOR = 2;
constexpr uint8_t  SPI_CLKDIV_MINOR = 0;

constexpr uint32_t PADS_DRIVE = PADS_BANK0_GPIO0_DRIVE_VALUE_12MA;
constexpr uint32_t IRQ_DELAY_NS = 100;
constexpr uint32_t QSPI_LOOP_CNT = 2;

struct {
    pio_hw_t *pio;
    uint8_t pio_func_sel;
    int8_t pio_offset;
    int8_t pio_sm;
    int8_t dma_out;
    int8_t dma_in;
} state;

uint16_t mk_cmd_buf(uint8_t *pdst, uint8_t opcode, uint16_t addr) {
    pdst[0] = ((opcode >> 7 & 0x01) << 4) | ((opcode >> 6 & 0x01) << 0);
    pdst[1] = ((opcode >> 5 & 0x01) << 4) | ((opcode >> 4 & 0x01) << 0);
    pdst[2] = ((opcode >> 3 & 0x01) << 4) | ((opcode >> 2 & 0x01) << 0);
    pdst[3] = ((opcode >> 1 & 0x01) << 4) | ((opcode >> 0 & 0x01) << 0);
    pdst[4] = (uint8_t)(addr >> 8);
    pdst[5] = (uint8_t)(addr);
    pdst[6] = 0;
    return 7;
}

uint32_t data_pin_mask() {
    return (1u << PIO_SPI_DATA_IO0_PIN) | (1u << PIO_SPI_DATA_IO1_PIN) |
           (1u << PIO_SPI_DATA_IO2_PIN) | (1u << PIO_SPI_DATA_IO3_PIN);
}

__noinline void ns_delay(uint32_t ns) {
    uint32_t cycles = ns * (clock_get_hz(clk_sys) >> 16u) / (1000000000u >> 16u);
    busy_wait_at_least_cycles(cycles);
}

void pio_init() {
    for (auto pin : {PIO_SPI_DATA_IO0_PIN, PIO_SPI_DATA_IO1_PIN, PIO_SPI_DATA_IO2_PIN, PIO_SPI_DATA_IO3_PIN}) {
        gpio_init(pin);
        gpio_set_dir(pin, GPIO_OUT);
        gpio_put(pin, false);
    }
    gpio_init(PIN_CS);
    gpio_set_dir(PIN_CS, GPIO_OUT);
    gpio_put(PIN_CS, true);
    gpio_init(PIN_INT);
    gpio_set_dir(PIN_INT, GPIO_IN);
    gpio_pull_up(PIN_INT);
    gpio_set_irq_enabled_with_callback(PIN_INT, GPIO_IRQ_EDGE_FALL, true,
        [](uint, uint32_t){});

    pio_hw_t *pios[2] = {pio0, pio1};
    uint pio_index = 1;
    if (!pio_can_add_program(pios[pio_index], &PIO_PROGRAM_FUNC)) {
        pio_index ^= 1;
        assert(pio_can_add_program(pios[pio_index], &PIO_PROGRAM_FUNC));
    }

    state.pio = pios[pio_index];
    state.dma_in = -1;
    state.dma_out = -1;

    static_assert(GPIO_FUNC_PIO1 == GPIO_FUNC_PIO0 + 1);
    state.pio_func_sel = GPIO_FUNC_PIO0 + pio_index;
    state.pio_sm = (int8_t)pio_claim_unused_sm(state.pio, true);
    state.pio_offset = pio_add_program(state.pio, &PIO_PROGRAM_FUNC);

    pio_sm_config sm_config = PIO_PROGRAM_GET_DEFAULT_CONFIG_FUNC(state.pio_offset);
    sm_config_set_clkdiv_int_frac(&sm_config, SPI_CLKDIV_MAJOR, SPI_CLKDIV_MINOR);

    hw_write_masked(&pads_bank0_hw->io[PIO_SPI_SCK_PIN],
                    (uint)PADS_DRIVE << PADS_BANK0_GPIO0_DRIVE_LSB,
                    PADS_BANK0_GPIO0_DRIVE_BITS);
    hw_write_masked(&pads_bank0_hw->io[PIO_SPI_SCK_PIN],
                    1u << PADS_BANK0_GPIO0_SLEWFAST_LSB,
                    PADS_BANK0_GPIO0_SLEWFAST_BITS);

    sm_config_set_out_pins(&sm_config, PIO_SPI_DATA_IO0_PIN, 4);
    sm_config_set_in_pins(&sm_config, PIO_SPI_DATA_IO0_PIN);
    sm_config_set_set_pins(&sm_config, PIO_SPI_DATA_IO0_PIN, 4);
    sm_config_set_sideset(&sm_config, 1, false, false);
    sm_config_set_sideset_pins(&sm_config, PIO_SPI_SCK_PIN);
    sm_config_set_in_shift(&sm_config, false, true, 8);
    sm_config_set_out_shift(&sm_config, false, true, 8);
    hw_set_bits(&state.pio->input_sync_bypass, data_pin_mask());
    pio_sm_set_config(state.pio, state.pio_sm, &sm_config);
    pio_sm_set_consecutive_pindirs(state.pio, state.pio_sm, PIO_SPI_SCK_PIN, 1, true);

    for (auto pin : {PIO_SPI_DATA_IO0_PIN, PIO_SPI_DATA_IO1_PIN, PIO_SPI_DATA_IO2_PIN, PIO_SPI_DATA_IO3_PIN}) {
        gpio_set_function(pin, (gpio_function_t)state.pio_func_sel);
        gpio_set_pulls(pin, false, true);
        gpio_set_input_hysteresis_enabled(pin, true);
    }

    pio_sm_exec(state.pio, state.pio_sm, pio_encode_set(pio_pins, 1));

    state.dma_out = (int8_t)dma_claim_unused_channel(true);
    state.dma_in = (int8_t)dma_claim_unused_channel(true);
}


void pio_frame_start() {
    for (auto pin : {PIO_SPI_DATA_IO0_PIN, PIO_SPI_DATA_IO1_PIN, PIO_SPI_DATA_IO2_PIN, PIO_SPI_DATA_IO3_PIN})
        gpio_set_function(pin, (gpio_function_t)state.pio_func_sel);
    gpio_set_function(PIO_SPI_SCK_PIN, (gpio_function_t)state.pio_func_sel);
    gpio_pull_down(PIO_SPI_SCK_PIN);
    gpio_put(PIN_CS, false);
}

void pio_frame_end() {
    gpio_put(PIN_CS, true);
    ns_delay(IRQ_DELAY_NS);
}

void pio_read(uint8_t opcode, uint16_t addr, uint8_t* buf, uint16_t len) {
    uint8_t cmd[8] = {};
    uint16_t cmd_len = mk_cmd_buf(cmd, opcode, addr);

    pio_sm_set_enabled(state.pio, state.pio_sm, false);
    pio_sm_set_wrap(state.pio, state.pio_sm, state.pio_offset, state.pio_offset + PIO_OFFSET_READ_BITS_END - 1);
    pio_sm_clear_fifos(state.pio, state.pio_sm);
    pio_sm_set_pindirs_with_mask(state.pio, state.pio_sm, data_pin_mask(), data_pin_mask());
    pio_sm_restart(state.pio, state.pio_sm);
    pio_sm_clkdiv_restart(state.pio, state.pio_sm);

    pio_sm_put(state.pio, state.pio_sm, cmd_len * QSPI_LOOP_CNT - 1);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_out(pio_x, 32));
    pio_sm_put(state.pio, state.pio_sm, len - 1);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_out(pio_y, 32));
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_jmp(state.pio_offset));

    dma_channel_abort(state.dma_out);
    dma_channel_abort(state.dma_in);

    dma_channel_config out_cfg = dma_channel_get_default_config(state.dma_out);
    channel_config_set_transfer_data_size(&out_cfg, DMA_SIZE_8);
    channel_config_set_bswap(&out_cfg, true);
    channel_config_set_dreq(&out_cfg, pio_get_dreq(state.pio, state.pio_sm, true));
    dma_channel_configure(state.dma_out, &out_cfg, &state.pio->txf[state.pio_sm], cmd, cmd_len, true);

    dma_channel_config in_cfg = dma_channel_get_default_config(state.dma_in);
    channel_config_set_transfer_data_size(&in_cfg, DMA_SIZE_8);
    channel_config_set_bswap(&in_cfg, true);
    channel_config_set_dreq(&in_cfg, pio_get_dreq(state.pio, state.pio_sm, false));
    channel_config_set_write_increment(&in_cfg, true);
    channel_config_set_read_increment(&in_cfg, false);
    dma_channel_configure(state.dma_in, &in_cfg, buf, &state.pio->rxf[state.pio_sm], len, true);

    pio_sm_set_enabled(state.pio, state.pio_sm, true);
    __compiler_memory_barrier();
    dma_channel_wait_for_finish_blocking(state.dma_out);
    dma_channel_wait_for_finish_blocking(state.dma_in);
    __compiler_memory_barrier();
    pio_sm_set_enabled(state.pio, state.pio_sm, false);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_mov(pio_pins, pio_null));
}

void pio_write(uint8_t opcode, uint16_t addr, uint8_t* buf, uint16_t len) {
    uint8_t cmd[8] = {};
    uint16_t cmd_len = mk_cmd_buf(cmd, opcode, addr);
    uint16_t total = len + cmd_len;

    pio_sm_set_enabled(state.pio, state.pio_sm, false);
    pio_sm_set_wrap(state.pio, state.pio_sm, state.pio_offset, state.pio_offset + PIO_OFFSET_WRITE_BITS_END - 1);
    pio_sm_clear_fifos(state.pio, state.pio_sm);
    pio_sm_set_pindirs_with_mask(state.pio, state.pio_sm, data_pin_mask(), data_pin_mask());
    pio_sm_restart(state.pio, state.pio_sm);
    pio_sm_clkdiv_restart(state.pio, state.pio_sm);

    pio_sm_put(state.pio, state.pio_sm, total * QSPI_LOOP_CNT - 1);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_out(pio_x, 32));
    pio_sm_put(state.pio, state.pio_sm, 0);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_out(pio_y, 32));
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_jmp(state.pio_offset));

    dma_channel_abort(state.dma_out);

    dma_channel_config out_cfg = dma_channel_get_default_config(state.dma_out);
    channel_config_set_transfer_data_size(&out_cfg, DMA_SIZE_8);
    channel_config_set_bswap(&out_cfg, true);
    channel_config_set_dreq(&out_cfg, pio_get_dreq(state.pio, state.pio_sm, true));

    pio_sm_set_enabled(state.pio, state.pio_sm, true);
    dma_channel_configure(state.dma_out, &out_cfg, &state.pio->txf[state.pio_sm], cmd, cmd_len, true);
    dma_channel_wait_for_finish_blocking(state.dma_out);
    dma_channel_configure(state.dma_out, &out_cfg, &state.pio->txf[state.pio_sm], buf, len, true);
    dma_channel_wait_for_finish_blocking(state.dma_out);

    const uint32_t stall = 1u << (PIO_FDEBUG_TXSTALL_LSB + state.pio_sm);
    state.pio->fdebug = stall;
    while (!(state.pio->fdebug & stall)) tight_loop_contents();

    __compiler_memory_barrier();
    pio_sm_set_consecutive_pindirs(state.pio, state.pio_sm, PIO_SPI_DATA_IO0_PIN, 4, false);
    pio_sm_exec(state.pio, state.pio_sm, pio_encode_mov(pio_pins, pio_null));
    pio_sm_set_enabled(state.pio, state.pio_sm, false);
}

using iodata_t = uint8_t;
using datasize_t = int16_t;


constexpr uint8_t QSPI_MODE = 0x02 << 6;


constexpr uint8_t PACK_NONE      = 0x00;
constexpr uint8_t PACK_FIRST     = 1 << 1;
constexpr uint8_t PACK_REMAINED  = 1 << 2;
constexpr uint8_t PACK_COMPLETED = 1 << 3;

constexpr uint8_t SPI_READ  = (0x00 << 5);
constexpr uint8_t SPI_WRITE = (0x01 << 5);

constexpr uint32_t CREG_BLOCK           = 0x00;
constexpr uint32_t SREG_BLOCK(uint8_t n) { return 1 + 4 * n; }
constexpr uint32_t TXBUF_BLOCK(uint8_t n) { return 2 + 4 * n; }
constexpr uint32_t RXBUF_BLOCK(uint8_t n) { return 3 + 4 * n; }

constexpr uint32_t offset_inc(uint32_t addr, uint32_t n) { return addr + (n << 8); }

constexpr uint32_t REG_CIDR      = (0x0000 << 8) + CREG_BLOCK;
constexpr uint32_t REG_RTL       = (0x0004 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SYSR      = (0x2000 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SYCR0     = (0x2004 << 8) + CREG_BLOCK;
constexpr uint32_t REG_IMR       = (0x2104 << 8) + CREG_BLOCK;
constexpr uint32_t REG_IRCLR     = (0x2108 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SIMR      = (0x2114 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SLIMR     = (0x2124 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SLIRCLR   = (0x2128 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SHAR      = (0x4120 << 8) + CREG_BLOCK;
constexpr uint32_t REG_GAR       = (0x4130 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SUBR      = (0x4134 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SIPR      = (0x4138 << 8) + CREG_BLOCK;
constexpr uint32_t REG_LLAR      = (0x4140 << 8) + CREG_BLOCK;
constexpr uint32_t REG_GUAR      = (0x4150 << 8) + CREG_BLOCK;
constexpr uint32_t REG_SUB6R     = (0x4160 << 8) + CREG_BLOCK;
constexpr uint32_t REG_GA6R      = (0x4170 << 8) + CREG_BLOCK;
constexpr uint32_t REG_CHPLCKR   = (0x41F4 << 8) + CREG_BLOCK;
constexpr uint32_t REG_NETLCKR   = (0x41F5 << 8) + CREG_BLOCK;

constexpr uint32_t REG_SN_MR(uint8_t n)      { return (0x0000 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_CR(uint8_t n)      { return (0x0010 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_IR(uint8_t n)      { return (0x0020 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_IRCLR(uint8_t n)   { return (0x0028 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_SR(uint8_t n)      { return (0x0030 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_MR2(uint8_t n)     { return (0x0144 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_TX_BSR(uint8_t n)  { return (0x0200 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_TX_FSR(uint8_t n)  { return (0x0204 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_TX_WR(uint8_t n)   { return (0x020C << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_RX_BSR(uint8_t n)  { return (0x0220 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_RX_RSR(uint8_t n)  { return (0x0224 << 8) + SREG_BLOCK(n); }
constexpr uint32_t REG_SN_RX_RD(uint8_t n)   { return (0x0228 << 8) + SREG_BLOCK(n); }

constexpr uint8_t SYSR_CHPL            = 1 << 7;
constexpr uint8_t SYSR_NETL            = 1 << 6;
constexpr uint8_t SYCR0_RST            = 0x00;
constexpr uint8_t SN_MR_MACRAW         = 0x07;
constexpr uint8_t SN_CR_OPEN           = 0x01;
constexpr uint8_t SN_CR_CLOSE          = 0x10;
constexpr uint8_t SN_CR_SEND           = 0x20;
constexpr uint8_t SN_CR_RECV           = 0x40;
constexpr uint8_t SN_IR_SENDOK         = 0x10;
constexpr uint8_t SN_IR_TIMEOUT        = 0x08;
constexpr uint8_t SOCK_CLOSED          = 0x00;

uint8_t reg_read(uint32_t addr_sel);
void reg_write(uint32_t addr_sel, uint8_t wb);
void reg_read_buf(uint32_t addr_sel, uint8_t* buf, datasize_t len);
void reg_write_buf(uint32_t addr_sel, uint8_t* buf, datasize_t len);
uint16_t get_sn_tx_fsr(uint8_t sn);
uint16_t get_sn_rx_rsr(uint8_t sn);

uint16_t get_cidr() { return (((uint16_t)reg_read(REG_CIDR) | (((reg_read(REG_RTL)) & 0x0F) << 1)) << 8) + reg_read(offset_inc(REG_CIDR, 1)); }
uint8_t get_sysr() { return reg_read(REG_SYSR); }
uint8_t get_sycr0() { return reg_read(REG_SYCR0); }
void set_sycr0(uint8_t v) { reg_write(REG_SYCR0, v); }
void set_imr(uint8_t v) { reg_write(REG_IMR, v); }
void set_irclr(uint8_t v) { reg_write(REG_IRCLR, v); }
void set_simr(uint8_t v) { reg_write(REG_SIMR, v); }
void set_slimr(uint8_t v) { reg_write(REG_SLIMR, v); }
void set_slirclr(uint8_t v) { reg_write(REG_SLIRCLR, v); }
void set_shar(uint8_t* v) { reg_write_buf(REG_SHAR, v, 6); }
void get_shar(uint8_t* v) { reg_read_buf(REG_SHAR, v, 6); }
void set_gar(uint8_t* v) { reg_write_buf(REG_GAR, v, 4); }
void get_gar(uint8_t* v) { reg_read_buf(REG_GAR, v, 4); }
void set_subr(uint8_t* v) { reg_write_buf(REG_SUBR, v, 4); }
void get_subr(uint8_t* v) { reg_read_buf(REG_SUBR, v, 4); }
void set_sipr(uint8_t* v) { reg_write_buf(REG_SIPR, v, 4); }
void get_sipr(uint8_t* v) { reg_read_buf(REG_SIPR, v, 4); }
void set_llar(uint8_t* v) { reg_write_buf(REG_LLAR, v, 16); }
void get_llar(uint8_t* v) { reg_read_buf(REG_LLAR, v, 16); }
void set_guar(uint8_t* v) { reg_write_buf(REG_GUAR, v, 16); }
void get_guar(uint8_t* v) { reg_read_buf(REG_GUAR, v, 16); }
void set_sub6r(uint8_t* v) { reg_write_buf(REG_SUB6R, v, 16); }
void get_sub6r(uint8_t* v) { reg_read_buf(REG_SUB6R, v, 16); }
void set_ga6r(uint8_t* v) { reg_write_buf(REG_GA6R, v, 16); }
void get_ga6r(uint8_t* v) { reg_read_buf(REG_GA6R, v, 16); }
void set_chplckr(uint8_t v) { reg_write(REG_CHPLCKR, v); }
void chip_lock() { set_chplckr(0xFF); }
void chip_unlock() { set_chplckr(0xCE); }
void set_netlckr(uint8_t v) { reg_write(REG_NETLCKR, v); }
void net_lock() { set_netlckr(0xC5); }
void net_unlock() { set_netlckr(0x3A); }

void set_sn_mr(uint8_t sn, uint8_t v) { reg_write(REG_SN_MR(sn), v); }
void set_sn_cr(uint8_t sn, uint8_t v) { reg_write(REG_SN_CR(sn), v); }
uint8_t get_sn_cr(uint8_t sn) { return reg_read(REG_SN_CR(sn)); }
uint8_t get_sn_ir(uint8_t sn) { return reg_read(REG_SN_IR(sn)); }
void set_sn_irclr(uint8_t sn, uint8_t v) { reg_write(REG_SN_IRCLR(sn), v); }
void set_sn_ir(uint8_t sn, uint8_t v) { set_sn_irclr(sn, v); }
uint8_t get_sn_sr(uint8_t sn) { return reg_read(REG_SN_SR(sn)); }
void set_sn_mr2(uint8_t sn, uint8_t v) { reg_write(REG_SN_MR2(sn), v); }
void set_sn_tx_bsr(uint8_t sn, uint8_t v) { reg_write(REG_SN_TX_BSR(sn), v); }
void set_sn_txbuf_size(uint8_t sn, uint8_t v) { set_sn_tx_bsr(sn, v); }
uint8_t get_sn_tx_bsr(uint8_t sn) { return reg_read(REG_SN_TX_BSR(sn)); }
uint16_t get_sn_tx_max(uint8_t sn) { return get_sn_tx_bsr(sn) << 10; }
uint16_t get_sn_tx_wr(uint8_t sn) { return ((uint16_t)reg_read(REG_SN_TX_WR(sn)) << 8) + reg_read(offset_inc(REG_SN_TX_WR(sn), 1)); }
void set_sn_tx_wr(uint8_t sn, uint16_t v) {
    reg_write(REG_SN_TX_WR(sn), (uint8_t)(v >> 8));
    reg_write(offset_inc(REG_SN_TX_WR(sn), 1), (uint8_t)v);
}
void set_sn_rx_bsr(uint8_t sn, uint8_t v) { reg_write(REG_SN_RX_BSR(sn), v); }
void set_sn_rxbuf_size(uint8_t sn, uint8_t v) { set_sn_rx_bsr(sn, v); }
void set_sn_rx_rd(uint8_t sn, uint16_t v) {
    reg_write(REG_SN_RX_RD(sn), (uint8_t)(v >> 8));
    reg_write(offset_inc(REG_SN_RX_RD(sn), 1), (uint8_t)v);
}
uint16_t get_sn_rx_rd(uint8_t sn) { return ((uint16_t)reg_read(REG_SN_RX_RD(sn)) << 8) + reg_read(offset_inc(REG_SN_RX_RD(sn), 1)); }
static uint8_t make_opcode(uint32_t addr, uint8_t rw) {
    return static_cast<uint8_t>((addr & 0xFF) | rw | QSPI_MODE);
}

static uint16_t make_addr(uint32_t addr) {
    return static_cast<uint16_t>((addr & 0x00FFFF00) >> 8);
}

void reg_write(uint32_t addr_sel, uint8_t wb) {
    pio_frame_start();
    pio_write(make_opcode(addr_sel, SPI_WRITE), make_addr(addr_sel), &wb, 1);
    pio_frame_end();
}

uint8_t reg_read(uint32_t addr_sel) {
    uint8_t ret[2] = {0};
    pio_frame_start();
    pio_read(make_opcode(addr_sel, SPI_READ), make_addr(addr_sel), ret, 1);
    pio_frame_end();
    return ret[0];
}

void reg_write_buf(uint32_t addr_sel, uint8_t* buf, datasize_t len) {
    pio_frame_start();
    pio_write(make_opcode(addr_sel, SPI_WRITE), make_addr(addr_sel), buf, len);
    pio_frame_end();
}

void reg_read_buf(uint32_t addr_sel, uint8_t* buf, datasize_t len) {
    pio_frame_start();
    pio_read(make_opcode(addr_sel, SPI_READ), make_addr(addr_sel), buf, len);
    pio_frame_end();
}

uint16_t get_sn_tx_fsr(uint8_t sn) {
    uint16_t prev_val = -1, val = 0;
    do {
        prev_val = val;
        val = reg_read(REG_SN_TX_FSR(sn));
        val = (val << 8) + reg_read(offset_inc(REG_SN_TX_FSR(sn), 1));
    } while (val != prev_val);
    return val;
}

uint16_t get_sn_rx_rsr(uint8_t sn) {
    uint16_t prev_val = -1, val = 0;
    do {
        prev_val = val;
        val = reg_read(REG_SN_RX_RSR(sn));
        val = (val << 8) + reg_read(offset_inc(REG_SN_RX_RSR(sn), 1));
    } while (val != prev_val);
    return val;
}

void send_data(uint8_t sn, uint8_t *data, uint16_t len) {
    uint16_t ptr = get_sn_tx_wr(sn);
    uint32_t addrsel = ((uint32_t)ptr << 8) + TXBUF_BLOCK(sn);
    reg_write_buf(addrsel, data, len);
    ptr += len;
    set_sn_tx_wr(sn, ptr);
}

void recv_data(uint8_t sn, uint8_t *data, uint16_t len) {
    if (len == 0) return;
    uint16_t ptr = get_sn_rx_rd(sn);
    uint32_t addrsel = ((uint32_t)ptr << 8) + RXBUF_BLOCK(sn);
    reg_read_buf(addrsel, data, len);
    ptr += len;
    set_sn_rx_rd(sn, ptr);
}

void soft_reset() {
    uint8_t gw[4], sn[4], sip[4], mac[6];
    uint8_t gw6[16], sn6[16], lla[16], gua[16];
    uint8_t islock = get_sysr();

    chip_unlock();
    get_shar(mac); get_gar(gw); get_subr(sn); get_sipr(sip);
    get_ga6r(gw6); get_sub6r(sn6); get_llar(lla); get_guar(gua);
    set_sycr0(SYCR0_RST);
    get_sycr0();

    net_unlock();
    set_shar(mac); set_gar(gw); set_subr(sn); set_sipr(sip);
    set_ga6r(gw6); set_sub6r(sn6); set_llar(lla); set_guar(gua);

    if (islock & SYSR_CHPL) chip_lock();
    if (islock & SYSR_NETL) net_lock();
}

int8_t init_buffers(std::span<const uint8_t> txsize, std::span<const uint8_t> rxsize) {
    soft_reset();
    if (!txsize.empty()) {
        int8_t tmp = 0;
        for (int i = 0; i < sock_count; i++) {
            tmp += txsize[i];
            if (tmp > 32) return -1;
        }
        for (int i = 0; i < sock_count; i++) set_sn_txbuf_size(i, txsize[i]);
    }
    if (!rxsize.empty()) {
        int8_t tmp = 0;
        for (int i = 0; i < sock_count; i++) {
            tmp += rxsize[i];
            if (tmp > 32) return -1;
        }
        for (int i = 0; i < sock_count; i++) set_sn_rxbuf_size(i, rxsize[i]);
    }
    return 0;
}

uint16_t sock_is_sending = 0;
uint16_t sock_remained_size[sock_count] = {0,};
uint8_t sock_pack_info[sock_count] = {0,};

#define FAIL(e) return std::unexpected(sock_error::e)
#define CHECK_SOCKNUM()   do { if(sn >= sock_count) FAIL(sock_num); } while(0)
#define CHECK_SOCKDATA()  do { if(len == 0) FAIL(data_len); } while(0)

std::expected<void, sock_error> close(socket_id sid) {
    uint8_t sn = static_cast<uint8_t>(sid);
    CHECK_SOCKNUM();
    set_sn_cr(sn, SN_CR_CLOSE);
    while (get_sn_cr(sn));
    set_sn_ir(sn, 0xFF);
    sock_is_sending &= ~(1 << sn);
    sock_remained_size[sn] = 0;
    sock_pack_info[sn] = PACK_NONE;
    while (get_sn_sr(sn) != SOCK_CLOSED);
    return {};
}

} // namespace

void clear_interrupt(intr_kind intr) {
    set_irclr((uint8_t)intr);
    uint8_t sir = (uint8_t)((uint16_t)intr >> 8);
    for (int i = 0; i < sock_count; i++)
        if (sir & (1 << i)) set_sn_irclr(i, 0xFF);
    set_slirclr((uint8_t)((uint32_t)intr >> 16));
}

void set_interrupt_mask(intr_kind intr) {
    set_imr((uint8_t)intr);
    set_simr((uint8_t)((uint16_t)intr >> 8));
    set_slimr((uint8_t)((uint32_t)intr >> 16));
}


std::expected<socket_id, sock_error> open_socket(socket_id sid, protocol proto, sock_flag flag) {
    uint8_t sn = static_cast<uint8_t>(sid);
    uint8_t pr = static_cast<uint8_t>(proto);
    uint8_t fl = static_cast<uint8_t>(flag);
    CHECK_SOCKNUM();
    if ((pr & 0x0F) != SN_MR_MACRAW) FAIL(sock_mode);
    close(sid);
    set_sn_mr(sn, (pr | (fl & 0xF0)));
    set_sn_mr2(sn, fl & 0x03);
    set_sn_cr(sn, SN_CR_OPEN);
    while (get_sn_cr(sn));
    sock_is_sending &= ~(1 << sn);
    sock_remained_size[sn] = 0;
    sock_pack_info[sn] = PACK_COMPLETED;
    while (get_sn_sr(sn) == SOCK_CLOSED);
    return sid;
}

std::expected<uint16_t, sock_error> send(socket_id sid, std::span<const uint8_t> buf) {
    uint8_t sn = static_cast<uint8_t>(sid);
    uint16_t len = buf.size();
    uint8_t tmp = 0;
    uint16_t freesize = 0;

    CHECK_SOCKNUM();

    freesize = get_sn_tx_max(sn);
    if (len > freesize) len = freesize;
    while (1) {
        freesize = get_sn_tx_fsr(sn);
        if (get_sn_sr(sn) == SOCK_CLOSED) FAIL(sock_closed);
        if (len <= freesize) break;
    };
    send_data(sn, const_cast<uint8_t*>(buf.data()), len);
    set_sn_cr(sn, SN_CR_SEND);
    while (get_sn_cr(sn));
    while (1) {
        tmp = get_sn_ir(sn);
        if (tmp & SN_IR_SENDOK) {
            set_sn_ir(sn, SN_IR_SENDOK);
            break;
        } else if (tmp & SN_IR_TIMEOUT) {
            set_sn_ir(sn, SN_IR_TIMEOUT);
            FAIL(timeout);
        }
    }
    return len;
}

std::expected<uint16_t, sock_error> recv(socket_id sid, std::span<uint8_t> buf) {
    uint8_t sn = static_cast<uint8_t>(sid);
    uint16_t len = buf.size();
    uint8_t head[2];
    uint16_t pack_len = 0;

    CHECK_SOCKNUM();
    CHECK_SOCKDATA();

    if (sock_remained_size[sn] == 0) {
        while (1) {
            pack_len = get_sn_rx_rsr(sn);
            if (get_sn_sr(sn) == SOCK_CLOSED) FAIL(sock_closed);
            if (pack_len != 0) {
                sock_pack_info[sn] = PACK_NONE;
                break;
            }
        };
    }

    recv_data(sn, head, 2);
    set_sn_cr(sn, SN_CR_RECV);
    while (get_sn_cr(sn));

    if (sock_remained_size[sn] == 0) {
        sock_remained_size[sn] = head[0];
        sock_remained_size[sn] = (sock_remained_size[sn] << 8) + head[1] - 2;
        if (sock_remained_size[sn] > 1514) {
            close(sid);
            FAIL(fatal_packlen);
        }
        sock_pack_info[sn] = PACK_FIRST;
    }
    if (len < sock_remained_size[sn]) pack_len = len;
    else pack_len = sock_remained_size[sn];
    recv_data(sn, buf.data(), pack_len);

    sock_remained_size[sn] = pack_len;
    sock_pack_info[sn] |= PACK_FIRST;

    if (len < sock_remained_size[sn]) pack_len = len;
    else pack_len = sock_remained_size[sn];
    recv_data(sn, buf.data(), pack_len);
    set_sn_cr(sn, SN_CR_RECV);
    while (get_sn_cr(sn));

    sock_remained_size[sn] -= pack_len;
    if (sock_remained_size[sn] != 0) sock_pack_info[sn] |= PACK_REMAINED;
    else sock_pack_info[sn] |= PACK_COMPLETED;

    return pack_len;
}


void reset() {
    gpio_init(PIN_RST);
    gpio_set_dir(PIN_RST, GPIO_OUT);
    gpio_put(PIN_RST, 0);
    sleep_ms(100);
    gpio_put(PIN_RST, 1);
    sleep_ms(100);
}

void init_spi() {
    pio_init();
}

void init() {
    pio_frame_end();
    std::array<uint8_t, 8> txsize = {4, 4, 4, 4, 4, 4, 4, 4};
    std::array<uint8_t, 8> rxsize = {4, 4, 4, 4, 4, 4, 4, 4};
    init_buffers(txsize, rxsize);
}

bool check() {
    return get_cidr() == 0x6300;
}



uint16_t get_socket_recv_buf(socket_id sid) {
    return get_sn_rx_rsr(static_cast<uint8_t>(sid));
}

} // namespace w6300