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

#include <RP2040.h>
#include <system_RP2040.h>
#include <core_cm0plus.h>

#include <hardware/structs/iobank0.h>
#include <hardware/clocks.h>
#include <hardware/dma.h>
#include <hardware/gpio.h>
#include <hardware/irq.h>
#include <hardware/pio.h>
#include <hardware/pwm.h>
#include <pico/stdlib.h>
#include <pico/stdio_usb.h>

#include "trigctl.pio.h"
#include "uart_rx.pio.h"

#define PIN_TRIGGER 14
#define PIN_TRIGGER_POL glitch_positive

#define PIN_NMI 15
#define PIN_NMI_POL glitch_negative

#define PIN_ACK  16
#define PIN_UART 17

#define PIN_ACT  18
#define PIN_STAT 19

#define PIO_UNIT pio0
#define PIOSM_UART 0
#define PIOSM_TRIG 1

#define UART_BAUD 9600

/*static void act_irq(uint gpio, uint32_t ev) {
	if (gpio != PIN_ACT) return;

	//printf("ev: 0x%lx\n", ev);
	if (ev & GPIO_IRQ_EDGE_FALL) {
		if (ev & GPIO_IRQ_EDGE_RISE) {
			//printf("wut\n");
		} else {
			//printf("fall\n");
			//gpio_set_function(PIN_NMI, GPIO_FUNC_NULL);
			const uint func = GPIO_FUNC_NULL;
			hw_write_masked(&iobank0_hw->io[PIN_NMI].ctrl
				, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
				, IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS
			);
		}
	} else if (ev & GPIO_IRQ_EDGE_RISE) {
		//printf("rise\n");
		//gpio_set_function(PIN_NMI, GPIO_FUNC_PIO0);
		const uint func = GPIO_FUNC_PIO0;
		hw_write_masked(&iobank0_hw->io[PIN_NMI].ctrl
			, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
			, IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS
		);
	}
}*/

static void init_gpio(void) {
	const int func = GPIO_FUNC_PIO0;

	// init trigger pin
	sio_hw->gpio_oe_clr = 1u << PIN_TRIGGER;
	hw_write_masked(&padsbank0_hw->io[PIN_TRIGGER]
		, (PADS_BANK0_GPIO0_IE_BITS)
		, (PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS)
		| (PADS_BANK0_GPIO0_SCHMITT_BITS)
	);
	hw_write_masked(&iobank0_hw->io[PIN_TRIGGER].ctrl
		, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
		| ((uint)(PIN_TRIGGER_POL ? GPIO_OVERRIDE_INVERT
				: GPIO_OVERRIDE_NORMAL) << IO_BANK0_GPIO0_CTRL_INOVER_LSB)
		, (IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS)
		| (IO_BANK0_GPIO0_CTRL_INOVER_BITS)
	);

	sio_hw->gpio_clr = 1u << PIN_NMI;
	sio_hw->gpio_oe_set = 1u << PIN_NMI;
	hw_write_masked(&padsbank0_hw->io[PIN_NMI]
		, PADS_BANK0_GPIO0_IE_BITS
		| ((uint)GPIO_SLEW_RATE_FAST << PADS_BANK0_GPIO0_SLEWFAST_LSB)
		| (PADS_BANK0_GPIO0_DRIVE_VALUE_12MA << PADS_BANK0_GPIO0_DRIVE_LSB)
		, PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
		| PADS_BANK0_GPIO0_SLEWFAST_BITS | PADS_BANK0_GPIO0_DRIVE_BITS
	);
	hw_write_masked(&iobank0_hw->io[PIN_NMI].ctrl
		, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
		| ((uint)(/*PIN_NMI_POL ?*/ GPIO_OVERRIDE_NORMAL//INVERT
				/*: GPIO_OVERRIDE_NORMAL*/) << IO_BANK0_GPIO0_CTRL_OUTOVER_LSB)
		, IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS | IO_BANK0_GPIO0_CTRL_OUTOVER_BITS
	);
	gpio_set_outover(PIN_NMI, GPIO_OVERRIDE_INVERT);

	sio_hw->gpio_clr = 1u << PIN_ACK;
	sio_hw->gpio_oe_set = 1u << PIN_ACK;
	gpio_init(PIN_ACK);
	gpio_set_dir(PIN_ACK, GPIO_OUT);
	gpio_set_function(PIN_ACK, GPIO_FUNC_SIO);

	sio_hw->gpio_oe_clr = 1u << PIN_ACT;
	gpio_init(PIN_ACT);
	gpio_set_dir(PIN_ACT, GPIO_IN);
	gpio_set_function(PIN_ACT, GPIO_FUNC_SIO);

	sio_hw->gpio_set = 1u << PIN_STAT;
	sio_hw->gpio_oe_set = 1u << PIN_STAT;
	gpio_init(PIN_STAT);
	gpio_put(PIN_STAT, true);
	gpio_set_dir(PIN_STAT, GPIO_OUT);
	gpio_set_function(PIN_STAT, GPIO_FUNC_SIO);

	const int irq = PIO0_IRQ_1;
	irq_set_enabled(irq, false);
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	irq_set_enabled(irq, true);
}

static uint off_uart, off_trig;
static void init_pio(void) {
	off_uart = pio_add_program(PIO_UNIT, &uart_rx_program);
	uart_rx_program_init(PIO_UNIT, PIOSM_UART, off_uart, PIN_UART, UART_BAUD);

	off_trig = pio_add_program(PIO_UNIT, &trigctl_program);
	trigctl_pio_init(PIO_UNIT, PIOSM_TRIG, off_trig,
		trig_source_pin, PIN_NMI, PIN_TRIGGER,
		//PIN_TRIGGER_POL, PIN_NMI_POL,
		true, 5); // 25 MHz

	/*const int irq = PIO0_IRQ_1;
	irq_set_enabled(irq, false);
	trigctl_ack_glitch_irq(PIO_UNIT, PIOSM_TRIG);
	trigctl_set_glitch_irq_enabled(PIO_UNIT, PIOSM_TRIG, 1, true);
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	irq_set_enabled(irq, true);*/
}

static bool uart_poll_ch(uint8_t* ch) {
	if (pio_sm_is_rx_fifo_empty(PIO_UNIT, PIOSM_UART)) return false;

	// 8-bit read from the uppermost byte of the FIFO, as data is left-justified
	io_rw_8 *rxfifo_shift = (io_rw_8*)&PIO_UNIT->rxf[PIOSM_UART] + 3;
	*ch = *rxfifo_shift;
	return true;
}

static void flush_pio_fifo(void) {
	PIO pio = PIO_UNIT;
	const uint sm = PIOSM_TRIG;
	pio_sm_set_enabled(pio, sm, false);
	pio_sm_clear_fifos(pio, sm);
	pio_sm_restart(pio, sm);
	pio_sm_clkdiv_restart(pio, sm);
	pio_sm_exec(pio, sm, pio_encode_jmp(off_trig));
	pio_sm_set_enabled(pio, sm, true);
}

int main() {
	stdio_init_all();

	init_pio();
	init_gpio();

	/*gpio_set_irq_enabled_with_callback(PIN_ACT,
			GPIO_IRQ_EDGE_RISE|GPIO_IRQ_EDGE_FALL, true, act_irq);*/

	const uint LED_PIN = PICO_DEFAULT_LED_PIN;
	gpio_init(LED_PIN);
	gpio_set_dir(LED_PIN, GPIO_OUT);
	gpio_put(LED_PIN, true);

	//while (!stdio_usb_connected());

	uint32_t delay = 1;
	const uint32_t len = 16;

	while (true) {
		if (pio_sm_is_tx_fifo_empty(PIO_UNIT, PIOSM_TRIG)) {
			//printf("push offlen\n");
			trigctl_push_off_len(PIO_UNIT, PIOSM_TRIG, delay*25, len*25);
		}

		uint8_t ch;
		if (uart_poll_ch(&ch)) {
			//printf("got cmd: %c\n", ch);
			switch (ch) {
			case '0':
				delay = 1;
				flush_pio_fifo();
				break;
			case '+': ++delay; break;
			case '-': --delay; break;
			default: goto noack;
			}

			//printf("send ack\n");
			sio_hw->gpio_set = 1u << PIN_ACK;
			busy_wait_us_32(50);
			sio_hw->gpio_clr = 1u << PIN_ACK;
			//printf("sent ack\n");

		noack:;
		}

		if (PIO_UNIT->irq & (1u << PIOSM_TRIG)) {
			//printf("sent nmi\n");
			trigctl_ack_glitch_irq(PIO_UNIT, PIOSM_TRIG);
		}
	}
}