// Pi Pico V1
// This uses a new protocol and is able to lock on and maintain perfect sync across the entire block
// **without** additional clockbits meaning I get the full 8 or 16-bit bytes!
// It does this by calculating Paulas exact frequency from the first two words written which are
// just 0x0001 0x0001. First it times between the two 1's and divides by 16, thats the exact 
// bit time. Then it uses this sequence to properly start the sync for playback.
// The code uses the two cores, one running the SPI to the D2A and the other to read the drive.
// The floppy signal SIDE/HEAD is used to switch between 8-bit stereo and 16-bit mono.
// https://rev26.robsmith.dev
// https://robsmithdev.co.uk
// RobSmithDev


#include <stdio.h>
#include <SPI.h>
#include "hardware/timer.h"
#include "hardware/structs/systick.h"
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"

// Pin definitions
#define CS_PIN      17    // SPI CS
#define DATA_PIN    15    // Bitstream input
#define SPI_SCK     18
#define SPI_MOSI    19


#define PIN_SELECT               11
#define PIN_SIDE                 12          // Floppy disk side
#define PIN_WRITE_DATA           14          // Reads RAW floppy data on this pin.  
#define PIN_WRITE_GATE           15          // Enable RAW floppy data on this pin.  
//#define PIN_LED                  25

#define DAC_A           0
#define DAC_B           1
#define DAC_BOTH        15
#define CMD_WRITEUPDATE 48

PIO pio = pio0;
uint sm = 0;

// Core 1 - handles SPI output to LTC2602
void core1_entry() {
    // Setup SPI
    spi_init(spi0, 8000000);
    spi_set_format(spi0, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);
    gpio_set_function(SPI_SCK, GPIO_FUNC_SPI);
    gpio_set_function(SPI_MOSI, GPIO_FUNC_SPI);
    gpio_init(CS_PIN);
    gpio_set_dir(CS_PIN, GPIO_OUT);
    gpio_put(CS_PIN, 1);

    uint8_t buffer[3] = {CMD_WRITEUPDATE | DAC_BOTH, 0, 0};

    while (1) {
        uint32_t sample = multicore_fifo_pop_blocking();
        if (sample & 0x80000) {
            // 15-bit mono
            sample ^= 0x8000; // signed/unsigned conversion
            buffer[0] = CMD_WRITEUPDATE | DAC_BOTH;
            buffer[1] = sample >> 8;
            buffer[2] = sample & 0xFF;
        } else {
            buffer[0] = CMD_WRITEUPDATE | DAC_A;
            buffer[1] = ((sample >> 8) & 0xFF) ^ 0x80;      
            buffer[2] = 0;
            gpio_put(CS_PIN, 0);
            spi_write_blocking(spi0, buffer, 3);
            gpio_put(CS_PIN, 1);     
            buffer[0] = CMD_WRITEUPDATE | DAC_B;
            buffer[1] = (sample & 0xFF) ^ 0x80;
            buffer[2] = 0;
        }
        gpio_put(CS_PIN, 0);
        spi_write_blocking(spi0, buffer, 3);
        gpio_put(CS_PIN, 1);            
    }
}

void setup() {
    set_sys_clock_khz(133000, true);
    Serial.begin(115200);
    gpio_init(PIN_WRITE_GATE);      gpio_set_dir(PIN_WRITE_GATE, GPIO_IN);
    gpio_init(PIN_WRITE_DATA);      gpio_set_dir(PIN_WRITE_DATA, GPIO_IN);
    gpio_init(PIN_SIDE);            gpio_set_dir(PIN_SIDE, GPIO_IN);
    gpio_init(PIN_DIR);             gpio_set_dir(PIN_DIR, GPIO_IN);
    gpio_init(PIN_SELECT);          gpio_set_dir(PIN_SELECT, GPIO_IN);
    gpio_init(PIN_LED);             gpio_set_dir(PIN_LED, GPIO_OUT);
    delay(2000); 
}


// Capture and playback 
void capture(bool stereo) {   
    while (!pio_sm_is_rx_fifo_empty(pio, sm)) pio_sm_get(pio, sm);  // flush anything else

    // The Sync bits are the LSB of each block as they arrive first
    // Read initial SYNC - NOTE the value from pio_sm_get_blocking COUNTS DOWN 
    uint32_t lastTime = pio_sm_get_blocking(pio, sm); 
    uint32_t nextTime = pio_sm_get_blocking(pio, sm); 

    // Time between bits - should be around 131 ish
    uint32_t bitPeriod = (lastTime-nextTime) / 16; 
    if ((bitPeriod < 110) || (bitPeriod > 145)) {
        // This is wrong and means there was a spurious bit from somewhere or something
        lastTime = nextTime;
        nextTime = pio_sm_get_blocking(pio, sm); 
        bitPeriod = (lastTime-nextTime) / 16; 
    }
    lastTime = nextTime;

    uint32_t data = 0;
    const int32_t bias = bitPeriod / 2;
    uint32_t runningCounter = 0;
    uint32_t totalTime = 0;
    uint32_t stereoMarker = stereo ? 0x80000 : 0;
    uint32_t samplesPlayed = 0;

    while (runningCounter < 20) {
        if (!gpio_get(PIN_WRITE_GATE)) runningCounter = 0; else runningCounter++;

        // Bit detected
        if (!pio_sm_is_rx_fifo_empty(pio, sm)) {
            uint32_t curr = pio_sm_get_blocking(pio, sm);

            // Work out what bit this should be
            uint32_t diff = lastTime - curr;
            totalTime += diff;
            lastTime = curr;

            uint32_t bit = (totalTime - bias) / bitPeriod;

            if (bit >= 15) {
                if (samplesPlayed && samplesPlayed<16380) {
                    multicore_fifo_push_blocking(data | stereoMarker);
                }
                samplesPlayed++;
                // This will go wrong if theres a lot of silence
                if (bit > 15) {
                    bit &= 0x0F;
                    data = 1 << (15-bit);
                    totalTime = bitPeriod * (bit+1);
                } else {
                    totalTime = 0;
                    data = 0;
                }
            } else {
                data |= 1 << (15-bit);
            }
        }
    }
}

// PIO program - counts cycles between falling edges
// X decrements every 2 cycles, push value on falling edge
static const uint16_t capture_program_instructions[] = {
    (uint16_t)pio_encode_jmp_x_dec(1),   // decrement X, jump to instruction 1
    (uint16_t)pio_encode_jmp_pin(0),     // if pin HIGH, jump back to 0 (keep waiting)
    // got falling edge - pin is LOW and fell through
    (uint16_t)pio_encode_mov(pio_isr, pio_x),
    (uint16_t)pio_encode_push(false, false),
    // wait for rising edge
    (uint16_t)pio_encode_jmp_pin(0),     // pin HIGH, jump to top
    (uint16_t)pio_encode_jmp_x_dec(4),   // still LOW, decrement and loop
};

static const struct pio_program capture_program = { .instructions = capture_program_instructions, .length = 6, .origin = -1};

void setup_pio(uint pin) {
    uint offset = pio_add_program(pio, &capture_program);
    pio_sm_config c = pio_get_default_sm_config();
    
    sm_config_set_wrap(&c, offset, offset + capture_program.length - 1);
    sm_config_set_in_pins(&c, pin);
    sm_config_set_jmp_pin(&c, pin);
    
    // Set clock divider to 1 for maximum resolution
    sm_config_set_clkdiv(&c, 1.0f);
    
    // Shift ISR left, no autopush
    sm_config_set_in_shift(&c, false, false, 32);
    
    pio_gpio_init(pio, pin);
    gpio_set_dir(pin, GPIO_IN);
    
    pio_sm_init(pio, sm, offset, &c);
    // Initialise X to 0xFFFFFFFF
    pio_sm_exec(pio, sm, pio_encode_set(pio_x, 0));
    pio_sm_exec(pio, sm, pio_encode_mov_not(pio_x, pio_x));
    pio_sm_set_enabled(pio, sm, true);    
}


// Core 0 - handles bitstream input
void loop() {  
    multicore_launch_core1(core1_entry); 
    setup_pio(PIN_WRITE_DATA);
    gpio_put(PIN_LED, 1); 
    while (1) {        
        // Wait for sync pulse
        if (!gpio_get(PIN_SELECT) && !gpio_get(PIN_WRITE_GATE)) {
            gpio_put(PIN_LED, 0); 
            capture(gpio_get(PIN_SIDE));
            gpio_put(PIN_LED, 1); 
        }
    }
}
