#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <assert.h>

#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/multicore.h"
#include "pico/sync.h"

#include "ws2812.pio.h"

#include "bsp/board.h"
#include "tusb.h"

#include "tusb_desc.c"


#define COL_GRN 0x33000000
#define COL_RED 0x00330000
#define COL_BLU 0x00003300
#define COL_ORG 0x05330000

/*
 * Needed Pins:
 *  8 data        5v
 *  1 eeprom ce   5v
 *  1 eeprom oe   5v
 *  1 eeprom we   5v
 *  1 shift  oe   5v
 *  1 shift  clr  5v
 *  1 shift  sclk 5v
 *  1 shift  rclk 5v
 *  1 shift  in   5v
 *  1 FET power   3v3
 *  -----------------
 *  16 pins       5v
 *  1  pin        3v3
 *  -----------------
 *  17 pins
 */

#define PIN_RGB 16

enum
{
    SR_IN = 0,
    SR_nOE,
    SR_RCLK,
    SR_SCLK,
    SR_nCLR,

    SR_PIN_CNT
};

const static uint8_t sr_pins[SR_PIN_CNT] =
{
    [SR_IN]   =  9,
    [SR_nOE]  = 10,
    [SR_RCLK] = 11,
    [SR_SCLK] = 12,
    [SR_nCLR] = 13,
};

const static uint8_t sr_pins_def[SR_PIN_CNT] =
{
    [SR_IN]   = 0,
    [SR_nOE]  = 1,
    [SR_RCLK] = 0,
    [SR_SCLK] = 0,
    [SR_nCLR] = 1,
};


enum
{
    ROM_nWE = 0,
    ROM_nOE,
    ROM_nCE,

    ROM_PIN_CNT
};

const static uint8_t rom_pins[ROM_PIN_CNT] =
{
    [ROM_nWE] = 14,
    [ROM_nOE] = 15,
    [ROM_nCE] = 26,
};

const static uint8_t rom_pins_def[ROM_PIN_CNT] =
{
    [ROM_nWE] = 1,
    [ROM_nOE] = 1,
    [ROM_nCE] = 0,
};


#define PIN_IO_OE     29
#define PIN_IO_OE_DEF  1

const static uint8_t io_pins[8] =
{
    2,
    3,
    4,
    5,
    6,
    7,
    27,
    28
};


#define BLKS   64
#define BLK_SZ CFG_TUD_MSC_EP_BUFSIZE
#define EEPROM_BLK_SZ 64

static semaphore_t core1_sem;
static mutex_t  busy_lock;
static uint16_t pending = 0;
static uint16_t writeoffset = 0;
static uint8_t  writebuf[BLK_SZ] = {0};
static uint64_t access = 0;


// XXX: assumes stock clockspeed of 125MHz
#define sleep_ns(ns) \
    do{ for(int i = 0; i < ((ns) / 8); i++) __asm volatile ("nop\n"); }while(0)

static void readmode(void)
{
    gpio_put(PIN_IO_OE, 0);

    for(int i = 0; i < 8; i++)
    {
        gpio_init(io_pins[i]);
        gpio_set_dir(io_pins[i], GPIO_IN);
        gpio_pull_up(io_pins[i]);
    }

    gpio_put(PIN_IO_OE, 1);
}


static void writemode(void)
{
    gpio_put(PIN_IO_OE, 0);

    for(int i = 0; i < 8; i++)
    {
        gpio_init(io_pins[i]);
        gpio_set_dir(io_pins[i], GPIO_OUT);
        gpio_put(io_pins[i], 0);
    }

    gpio_put(PIN_IO_OE, 1);
}

static void set_addr(uint16_t addr)
{
    gpio_put(sr_pins[SR_nOE],  1);
    gpio_put(sr_pins[SR_RCLK], 0);

    gpio_put(sr_pins[SR_nCLR], 0);
    sleep_ns(24);

    gpio_put(sr_pins[SR_nCLR], 1);

    for(int i = 0; i < 16; i++)
    {
        gpio_put(sr_pins[SR_IN], addr & 1);
        addr = addr >> 1;

        gpio_put(sr_pins[SR_SCLK], 0);
        sleep_ns(17);
        gpio_put(sr_pins[SR_SCLK], 1);
        sleep_ns(17);
    }

    gpio_put(sr_pins[SR_RCLK], 1);
    gpio_put(sr_pins[SR_nOE], 0);
    sleep_ns(24);
}

void core1_main(void)
{
    while(1)
    {
        sem_acquire_blocking(&core1_sem);
        {
            mutex_enter_blocking(&busy_lock);
                writemode();
                gpio_put(rom_pins[ROM_nOE], 1);

                int pages = (pending / EEPROM_BLK_SZ) + ((pending % EEPROM_BLK_SZ) ? 1 : 0);
                for(int i = 0; i < pages; i++)
                {
                    for(int j = 0; j < EEPROM_BLK_SZ; j++)
                    {
                        int off = i * EEPROM_BLK_SZ + j;
                        if(off >= pending) break;

                        set_addr(writeoffset + off);

                        for(int k = 0; k < 8; k++)
                            gpio_put(io_pins[k], (writebuf[off] >> k) & 1);

                        sleep_ns(50);

                        gpio_put(rom_pins[ROM_nWE], 0);
                        sleep_ns(150);

                        gpio_put(rom_pins[ROM_nWE], 1);
                        sleep_ns(50);
                    }

                    busy_wait_ms(10);
                }
                printf("core1: wrote %d bytes to %04x\n", pending, writeoffset);
                pending = 0;
            mutex_exit(&busy_lock);
        }
    }
}

int main(void)
{
    board_init();
    tusb_init();
    stdio_init_all();

    gpio_init(PIN_RGB);
    gpio_set_dir(PIN_RGB, GPIO_OUT);
    gpio_put(PIN_RGB, 0);

    gpio_init(PIN_IO_OE);
    gpio_set_dir(PIN_IO_OE, GPIO_OUT);
    gpio_put(PIN_IO_OE, PIN_IO_OE_DEF);

    for(int i = 0; i < SR_PIN_CNT; i++)
    {
        gpio_init(sr_pins[i]);
        gpio_set_dir(sr_pins[i], GPIO_OUT);
        gpio_put(sr_pins[i], sr_pins_def[i]);
    }

    for(int i = 0; i < ROM_PIN_CNT; i++)
    {
        gpio_init(rom_pins[i]);
        gpio_set_dir(rom_pins[i], GPIO_OUT);
        gpio_put(rom_pins[i], rom_pins_def[i]);
    }

    readmode();

    int sm = 0; // XXX: ???
    uint32_t offset = pio_add_program(pio0, &ws2812_program);

    ws2812_program_init(pio0, sm, offset, PIN_RGB, 800000, false);
    pio_sm_put_blocking(pio0, 0, 0);

    sem_init(&core1_sem, 0, 1);
    mutex_init(&busy_lock);

    multicore_reset_core1();
    multicore_launch_core1(core1_main);

    sleep_ms(1000);


    printf("Hell, world...\n");

    uint32_t prev = access;
    uint32_t start_ms = 0;
    uint32_t interval = 60;
    int state = 1;
    int prevs = state;
    while(1)
    {
        tud_task();

        uint32_t diff = board_millis() - start_ms;
        if(diff >= interval)
        {
            start_ms += diff;

            if(prev != access)
            {
                prev  = access;

                prevs = state;
                state = !state;
                if(state) pio_sm_put_blocking(pio0, 0, COL_GRN);
                else      pio_sm_put_blocking(pio0, 0, 0);
            }
            else if(prevs != state)
            {
                state = 1;
                prevs = state;
                pio_sm_put_blocking(pio0, 0, COL_GRN);
            }
        }
    }

    return 0;
}

uint8_t tud_msc_get_maxlun_cb(void)
{
    return 1;
}

void tud_msc_inquiry_cb(uint8_t lun, uint8_t vendor_id[8], uint8_t product_id[16],
                        uint8_t product_rev[4])
{
    const char vid[] = "CAB_DIG";
    const char pid[] = "EEPROM Progrmmr";
    const char rev[] = "0dev";

    memcpy(vendor_id  , vid, strlen(vid));
    memcpy(product_id , pid, strlen(pid));
    memcpy(product_rev, rev, strlen(rev));

    printf("scsi: inq\n");
}

bool tud_msc_test_unit_ready_cb(uint8_t lun)
{
    printf("scsi: rdy\n");

    return true;
}

void tud_msc_capacity_cb(uint8_t lun, uint32_t* block_count, uint16_t* block_size)
{
    *block_count = BLKS;
    *block_size  = BLK_SZ;

    printf("scsi: cap\n");
}

bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject)
{
    printf("scsi: stop\n");

    if(load_eject)
    {
        if(start) {} // load disk storage
        else      {} // unload disk storage
    }

    return true;
}

int32_t tud_msc_read10_cb(uint8_t lun, uint32_t lba, uint32_t offset, void* buffer, uint32_t bufsize) {
    access++;

    if(lba >= BLKS)      return -1;
    if(bufsize > BLK_SZ) return -1;

    if(!mutex_try_enter(&busy_lock, NULL)) return 0;

    int addr = (lba * BLK_SZ) + offset;

    printf("scsi: read @ %04X\n", addr);

    readmode();

    uint8_t* bytes = buffer;
    for(int i = 0; i < bufsize; i++)
    {
        set_addr(addr + i);

        gpio_put(rom_pins[ROM_nOE], 0);
        sleep_ns(150);

        uint8_t byte = 0;
        for(int j = 0; j < 8; j++)
            byte |= gpio_get(io_pins[j]) << j;

        bytes[i] = byte;

        gpio_put(rom_pins[ROM_nOE], 1);
        sleep_ns(150);

    }

    mutex_exit(&busy_lock);
    return (int32_t)bufsize;
}

bool tud_msc_is_writable_cb(uint8_t lun)
{
    printf("scsi: rw?\n");
    return true;
}

int32_t tud_msc_write10_cb(uint8_t lun, uint32_t lba, uint32_t offset, uint8_t* buffer,
                           uint32_t bufsize)
{
    access++;

    if(lba >= BLKS)      return -1;
    if(bufsize > BLK_SZ) return -1;
    if(!mutex_try_enter(&busy_lock, NULL)) return 0;

    writeoffset = (lba * BLK_SZ) + offset;

    printf("scsi: write req @ %04x\n", writeoffset);

    memcpy(writebuf, buffer, bufsize);
    pending = bufsize;

    sem_release(&core1_sem);
    mutex_exit(&busy_lock);

    return (int32_t) bufsize;
}

int32_t tud_msc_scsi_cb(uint8_t lun, uint8_t const scsi_cmd[16], void* buffer, uint16_t bufsize)
{
  void const* response = NULL;
  int32_t resplen = 0;

  // most scsi handled is input
  bool in_xfer = true;

  switch (scsi_cmd[0]) {
    default:
      tud_msc_set_sense(lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
      return -1;
  }

  if (resplen > bufsize) resplen = bufsize;

  if (response && (resplen > 0)) {
    if (in_xfer) {
      memcpy(buffer, response, (size_t) resplen);
    } else {
      // SCSI output
    }
  }

  printf("scsi: generic\n");
  return resplen;
}

void tud_mount_cb(void)
{
    pio_sm_put_blocking(pio0, 0, COL_GRN);
}

void tud_umount_cb(void)
{
    pio_sm_put_blocking(pio0, 0, COL_RED);
}

void tud_suspend_cb(bool remote_wakeup_en)
{
    pio_sm_put_blocking(pio0, 0, COL_ORG);
}

void tud_resume_cb(void)
{
    pio_sm_put_blocking(pio0, 0, COL_BLU);
}