#include <stdint.h>
#include <stdio.h>

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/pio.h"

#include "core_comm.h"
#include "z80_bus.pio.h"

#define Z80_PIO          pio1
#define Z80_MONITOR_SM   0

#define Z80_OE_PIN       0
#define Z80_DIR_PIN      1
#define Z80_DATA_PIN     2
#define Z80_DATA_WIDTH   8
#define Z80_IOREQ_PIN    10
#define Z80_RD_PIN       11
#define Z80_CS_PIN       12
#define Z80_A0_PIN       13
#define Z80_A1_PIN       14

#define Z80_RD_MASK      (1u << Z80_RD_PIN)
#define Z80_CS_MASK      (1u << Z80_CS_PIN)
#define Z80_ADDR_MASK    0x3u
#define Z80_DATA_MASK    0xffu

// --------------------------------
// CORE 1
// --------------------------------

static void core1_puts(const char *s) {
    while(*s){
        core1_to_core0_write_blocking((uint32_t)*s);
        s++;
    }
}

static void z80_write_monitor_init(void) {
    uint offset = pio_add_program(Z80_PIO, &z80_write_monitor_program);

    // Keep the bus isolated while selecting B (Z80) -> A (Pico).
    gpio_init(Z80_OE_PIN);
    gpio_put(Z80_OE_PIN, 1);
    gpio_set_dir(Z80_OE_PIN, GPIO_OUT);
    gpio_init(Z80_DIR_PIN);
    gpio_put(Z80_DIR_PIN, 0);
    gpio_set_dir(Z80_DIR_PIN, GPIO_OUT);

    for (uint pin = Z80_DATA_PIN; pin <= Z80_A1_PIN; pin++) {
        pio_gpio_init(Z80_PIO, pin);
    }
    pio_sm_set_consecutive_pindirs(Z80_PIO, Z80_MONITOR_SM,
                                   Z80_DATA_PIN,
                                   Z80_A1_PIN - Z80_DATA_PIN + 1,
                                   false);

    pio_sm_config config = z80_write_monitor_program_get_default_config(offset);
    sm_config_set_in_pins(&config, Z80_OE_PIN);
    sm_config_set_in_shift(&config, false, false, 32);
    pio_sm_init(Z80_PIO, Z80_MONITOR_SM, offset, &config);
    pio_sm_set_enabled(Z80_PIO, Z80_MONITOR_SM, true);

    // Enable only after every Pico-side data pin has become an input.
    gpio_put(Z80_OE_PIN, 0);
}

void core1_entry() {
    z80_write_monitor_init();
    core1_puts("core 1: Z80 write monitor started\r\n");

    uint64_t write_count = 0;
    uint8_t last_addr = 0;
    uint8_t last_data = 0;
    absolute_time_t next_report = make_timeout_time_ms(100);

    while (true) {
        while (!pio_sm_is_rx_fifo_empty(Z80_PIO, Z80_MONITOR_SM)) {
            uint32_t bus_sample = pio_sm_get(Z80_PIO, Z80_MONITOR_SM);
            // Check if it is for us -> CS=1
            if ((bus_sample & Z80_CS_MASK) != 0) {
                uint8_t addr = (bus_sample >> Z80_A0_PIN) & Z80_ADDR_MASK;
                if ((bus_sample & Z80_RD_MASK) != 0) {
                    // WR
                    last_addr = addr;
                    last_data = (bus_sample >> Z80_DATA_PIN) & Z80_DATA_MASK;
                    write_count += 1;
                } else {
                    // RD
                }
            }
        }

        if (time_reached(next_report)) {
            char message[48];
            snprintf(message, sizeof(message), "Z80 writes: %llu\r\n", (unsigned long long)write_count);
            core1_puts(message);
            snprintf(message, sizeof(message), "Last address: %02x\r\n", last_addr);
            core1_puts(message);
            snprintf(message, sizeof(message), "Last data: %02x\r\n", last_data);
            core1_puts(message);
            next_report = make_timeout_time_ms(100);
        }

        tight_loop_contents();
    }
}