#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>

#include <RP2040.h>
#include <system_RP2040.h>
#include <core_cm0plus.h>
#include <hardware/structs/iobank0.h>
#include <hardware/clocks.h>
#include <hardware/gpio.h>
#include <hardware/irq.h>
#include <hardware/vreg.h>
#include <pico/multicore.h>
#include <pico/platform.h>
#include <pico/stdlib.h>

#include "delaytest.h"
#include "delay.pio.h"

enum test {
	test_gpio_busyloop,
	test_gpio_irq_handle,
	test_gpio_irq_wfi,

	test_pio_if_busyloop,
	test_pio_if_irq_handle,
	test_pio_if_irq_wfi,

	test_pio_fifo,
	test_pio_irq,

	test_core0_mem,
	test_core0_fifo_busyloop,
	test_core0_fifo_irq_handle,
	test_core0_fifo_irq_wfi,
	test_core0_wfe,

	test__num
};

#define PIN_HIGH       8
#define PIN_TRIG_IN    9
#define PIN_TRIG_OUT  10
#define PIN_CORE0_TRG 15

typedef void (*test_fn)(void);

static volatile uint32_t shvar = 0;

__attribute__((__section__(".scratch_y.core1_init")))
static void core1_init(void);

__attribute__((__section__(".scratch_y.core1_irq")))
static void core1_irq(void) {
	sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
	busy_wait_us_32(100);
	sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;

	const int irq = SIO_IRQ_PROC1;
	const int gpio = PIN_TRIG_IN;
	const int event = GPIO_IRQ_EDGE_RISE;

	// acknowledge all IRQs
	irq_set_enabled(irq, false);
	iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7);
	delayt1_ack_irqflag(pio0);
	multicore_fifo_drain();
	multicore_fifo_clear_irq();
	irq_set_enabled(irq, true);
}

__attribute__((__section__(".scratch_y.t_gpio_busyloop")))
static void t_gpio_busyloop(void) {
	core1_init();

	while (true) {
		// ldr; tst; beq.n loop
		while (!(sio_hw->gpio_in & (1<<PIN_TRIG_IN))) ;
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		busy_wait_us_32(100);
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_gpio_irq_handle")))
static void t_gpio_irq_handle(void) {
	core1_init();

	const int gpio = PIN_TRIG_IN;
	const int irq = IO_IRQ_BANK0;
	const int event = GPIO_IRQ_EDGE_RISE;

	irq_set_enabled(irq, false);
	iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	hw_set_bits(&iobank0_hw->proc1_irq_ctrl.inte[gpio>>3], event << 4*(gpio&7));
	irq_set_exclusive_handler(irq, core1_irq);
	irq_set_enabled(irq, true);

	while (true) ;
}

__attribute__((__section__(".scratch_y.t_gpio_irq_wfi")))
static void t_gpio_irq_wfi(void) {
	core1_init();

	const int gpio = PIN_TRIG_IN;
	const int irq = IO_IRQ_BANK0;
	const int event = GPIO_IRQ_EDGE_RISE;

	irq_set_enabled(irq, false);
	iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	hw_set_bits(&iobank0_hw->proc1_irq_ctrl.inte[gpio>>3], event << 4*(gpio&7));
	// only enable in ^, hope wfi works w/o doing a PendSV
	//irq_set_exclusive_handler(irq, core1_irq);
	__disable_irq();
	irq_set_enabled(irq, true);

	while (true) {
		__WFI();
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		irq_set_enabled(irq, false);
		iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
		busy_wait_us_32(100);
		irq_set_enabled(irq, true);
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_pio_if_busyloop")))
static void t_pio_if_busyloop(void) {
	uint off = pio_add_program(pio0, &delayt1_program);

	delayt1_program_init(pio0, 0, off, PIN_TRIG_IN, true);

	while (true) {
		delayt1_wait_irqflag(pio0);
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		delayt1_ack_irqflag(pio0);
		busy_wait_us_32(100);
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_pio_if_irq_handle")))
static void t_pio_if_irq_handle(void) {
	uint off = pio_add_program(pio0, &delayt1_program);

	delayt1_program_init(pio0, 0, off, PIN_TRIG_IN, true);

	const int irq = PIO0_IRQ_1;

	irq_set_enabled(irq, false);
	delayt1_ack_irqflag(pio0);
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	delayt1_irq_enable(pio0);
	irq_set_exclusive_handler(irq, core1_irq);
	irq_set_enabled(irq, true);

	while (true) ;
}

__attribute__((__section__(".scratch_y.t_pio_if_irq_wfi")))
static void t_pio_if_irq_wfi(void) {
	uint off = pio_add_program(pio0, &delayt1_program);

	delayt1_program_init(pio0, 0, off, PIN_TRIG_IN, true);

	const int irq = PIO0_IRQ_1;

	irq_set_enabled(irq, false);
	delayt1_ack_irqflag(pio0);
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	delayt1_irq_enable(pio0);
	//irq_set_exclusive_handler(irq, core1_irq);
	__disable_irq();
	irq_set_enabled(irq, true);

	while (true) {
		__WFI();
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		irq_set_enabled(irq, false);
		delayt1_ack_irqflag(pio0);
		busy_wait_us_32(100);
		irq_set_enabled(irq, true);
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_pio_fifo")))
static void t_pio_fifo(void) {
	uint off = pio_add_program(pio0, &delayt2_program);

	delayt2_program_init(pio0, 0, off, PIN_TRIG_OUT, true);

	const int gpio = PIN_TRIG_IN;
	const int irq = IO_IRQ_BANK0;
	const int event = GPIO_IRQ_EDGE_RISE;

	irq_set_enabled(irq, false);
	iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	hw_set_bits(&iobank0_hw->proc1_irq_ctrl.inte[gpio>>3], event << 4*(gpio&7));
	__disable_irq();
	irq_set_enabled(irq, true);

	while (true) {
		__WFI();
		delayt2_put(pio0, 0, 1);
		//sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		irq_set_enabled(irq, false);
		iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
		busy_wait_us_32(100);
		irq_set_enabled(irq, true);
		//sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_pio_irq")))
static void t_pio_irq(void) {
	uint off = pio_add_program(pio0, &delayt3_program);

	delayt3_program_init(pio0, 0, off, PIN_TRIG_OUT, true);

	const int gpio = PIN_TRIG_IN;
	const int irq = IO_IRQ_BANK0;
	const int event = GPIO_IRQ_EDGE_RISE;

	irq_set_enabled(irq, false);
	iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	hw_set_bits(&iobank0_hw->proc1_irq_ctrl.inte[gpio>>3], event << 4*(gpio&7));
	__disable_irq();
	irq_set_enabled(irq, true);

	while (true) {
		__WFI();
		delayt3_put(pio0);
		//sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		irq_set_enabled(irq, false);
		iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
		busy_wait_us_32(100);
		irq_set_enabled(irq, true);
		//sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_core0_mem")))
static void t_core0_mem(void) {
	while (true) {
		while (!shvar) ;
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		busy_wait_us_32(100);
		shvar = 0;
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_core0_wfe")))
static void t_core0_wfe(void) {
	while (true) {
		__WFE();
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		busy_wait_us_32(100);
		shvar = 0;
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_core0_fifo_busyloop")))
static void t_core0_fifo_busyloop(void) {
	while (true) {
		while (!multicore_fifo_rvalid()) ;
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		busy_wait_us_32(100);
		multicore_fifo_drain();
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}

__attribute__((__section__(".scratch_y.t_core0_fifo_irq_handle")))
static void t_core0_fifo_irq_handle(void) {
	const int irq = SIO_IRQ_PROC1;

	irq_set_enabled(irq, false);
	multicore_fifo_clear_irq();
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	delayt1_irq_enable(pio0);
	irq_set_exclusive_handler(irq, core1_irq);
	irq_set_enabled(irq, true);

	while (true) ;
}

__attribute__((__section__(".scratch_y.t_core0_fifo_irq_wfi")))
static void t_core0_fifo_irq_wfi(void) {
	const int irq = SIO_IRQ_PROC1;

	irq_set_enabled(irq, false);
	multicore_fifo_clear_irq();
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	delayt1_irq_enable(pio0);
	//irq_set_exclusive_handler(irq, core1_irq);
	__disable_irq();
	irq_set_enabled(irq, true);

	while (true) {
		__WFI();
		sio_hw->gpio_set = 1<<PIN_TRIG_OUT;
		irq_set_enabled(irq, false);
		multicore_fifo_drain();
		multicore_fifo_clear_irq();
		busy_wait_us_32(100);
		irq_set_enabled(irq, true);
		sio_hw->gpio_clr = 1<<PIN_TRIG_OUT;
	}
}
static test_fn tests[] = {
	// delays: (units are ns, 40ns=1cyc@25MHz, so 200ns=5cyc)

	// 300 400 400 320 260 380 320 300 360 300
	// 320 320 400 280 300 400 360 260 280 280
	// 360 340 360 300 300 380 380 280 300 340
	//(240 300 340 360 360 300 400 300 260 380
	// 360 320 260 260 340 380 260 360 340 340)
	[test_gpio_busyloop] = t_gpio_busyloop,
	// 1520 1520 1520 1500 1500 1500 1520 1520 1500 1500
	// 1500 1480 1500 1500 1520 1500 1500 1500 1500 1520
	[test_gpio_irq_handle] = t_gpio_irq_handle,
	// 360 380 380 380 380 380 380 380 380 360
	//(380 360 380 400 380 380 380 380 380 380
	// 420 400 400 400 360 380 400 380 400 380)
	[test_gpio_irq_wfi] = t_gpio_irq_wfi,

	// these depend on the PIO SM wake insn resuming and then setting the irq
	// flag, which has a 2 cycle delay in total (but basically no variance)

	// 660 500 580 540 280 540 680 640 660 540
	// 620 520 540 540 540 540 600 560 500 480
	// 520 600 600 520 620 480 600 640 600 620
	// 620 660 580 680 660 620 560 480 580 540
	[test_pio_if_busyloop] = t_pio_if_busyloop,
	// 1380 1460 1480 1480 1460 1360 1400 1460 1480 1480
	// 1400 1460 1440 1360 1460 1460 1460 1440 1380 1500
	[test_pio_if_irq_handle] = t_pio_if_irq_handle,
	// 560 540 560 560 520 580 540 540 540 560
	// 560   0 540 540 560 540 560 560 560 560 580
	//(540 560 220 560 540 560 560 540 540 540
	// 560 560 540 520 540 560 540 540 560 520)
	[test_pio_if_irq_wfi] = t_pio_if_irq_wfi,

	// 500 500 520 520 500 520 520 500 520 520
	// 520 520 500 500 520 520 480 480 500 500
	[test_pio_fifo] = t_pio_fifo,
	// 520 500 500 500 500 500 500 500 520 520
	// 500 500 520 500 500 500 500 500 500 500
	[test_pio_irq ] = t_pio_irq,

	// these depend on core0 using a GPIO trigger using WFI, so this is a bit
	// more complicated. N_obsv = N_gpio_irq_wfi + N_core0
	// => x_core0 = x_obsv - x_gpio_irq_wfi
	// => σ_core0 = sqrt(σ²_obsv - σ²_gpio_irq_wfi)

	// 740 660 680 640 580 640 740 660 700 640
	// 660 580 720 680 740 620 620 660 640 660
	[test_core0_mem] = t_core0_mem,
	// 520 540 540 540 560 560 540 520 560 540
	// 540 560 520 540 540 520 560 540 560 540
	[test_core0_wfe] = t_core0_wfe,
	// 740 640 700 620 600 720 660 660 600 720
	// 560 660 600 680 660 580 740 560 700 700
	[test_core0_fifo_busyloop] = t_core0_fifo_busyloop,
	// 1460 1480 1500 1480 1520 1500 1540 1520 1500 1500
	// 1540 1520 1520 1520 1520 1500 1540 1500 1500 1520
	[test_core0_fifo_irq_handle] = t_core0_fifo_irq_handle,
	// 780 760 800 800 780 800 780 780 780 800
	// 800 760 800 800 800 820 780 780 800 780
	[test_core0_fifo_irq_wfi] = t_core0_fifo_irq_wfi,

	[test__num] = NULL,
};

__attribute__((__section__(".scratch_y.core1_init")))
static void core1_init(void) {
	SCB->SCR &= ~SCB_SCR_SEVONPEND_Msk;
	// TODO: IRQ stuff?
	// TODO: * set core1 irq as used irqs
}

__attribute__((__section__(".scratch_x.delaytest")))
void delaytest(void) {
	vreg_set_voltage(VREG_VOLTAGE_1_15);
	set_sys_clock_khz(25*1000, true);

	gpio_set_function(PIN_HIGH     , GPIO_FUNC_SIO);
	gpio_set_function(PIN_TRIG_IN  , GPIO_FUNC_SIO);
	gpio_set_function(PIN_TRIG_OUT , GPIO_FUNC_SIO);
	gpio_set_function(PIN_CORE0_TRG, GPIO_FUNC_SIO);
	gpio_set_dir(PIN_HIGH     , GPIO_OUT);
	gpio_set_dir(PIN_TRIG_IN  , GPIO_IN );
	gpio_set_dir(PIN_TRIG_OUT , GPIO_OUT);
	gpio_set_dir(PIN_CORE0_TRG, GPIO_IN );
	gpio_put(PIN_HIGH, true);
	gpio_put(PIN_TRIG_OUT, false);

	SCB->SCR &= ~SCB_SCR_SEVONPEND_Msk;

	const enum test t = test_pio_fifo;

	if (t <= test_core0_mem || t >= test_core0_wfe)
		__disable_irq();
	multicore_launch_core1(tests[t]);

	if (t >= test_core0_mem && t <= test_core0_wfe) {
		const int gpio = PIN_TRIG_IN;
		const int irq = IO_IRQ_BANK0;
		const int event = GPIO_IRQ_EDGE_RISE;

		irq_set_enabled(irq, false);
		iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
		irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
		hw_set_bits(&iobank0_hw->proc0_irq_ctrl.inte[gpio>>3], event << 4*(gpio&7));
		__disable_irq();
		irq_set_enabled(irq, true);

		while (true) {
			__WFI();
			sio_hw->fifo_wr = 1;//__SEV();//shvar = 1;//
			irq_set_enabled(irq, false);
			iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
			busy_wait_us_32(100);
			irq_set_enabled(irq, true);
		}

	} else while (true) ;
}