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

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

#include <hardware/structs/iobank0.h>
#include <hardware/adc.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 "glitch.h"

#include "trigctl.pio.h"

volatile struct glitch_params glitch_param_cur = {0};
#define param_cur glitch_param_cur


static PIO trigctl_pio = NULL;
static uint trigctl_sm, trigctl_off;

static int trigctl_pio_can_init(void) {
	if (pio_can_add_program(pio0, &trigctl_program)) {
		int r = pio_claim_unused_sm(pio0, false);
		if (r >= 0) return r;
	}

	if (pio_can_add_program(pio1, &trigctl_program)) {
		int r = pio_claim_unused_sm(pio1, false);
		if (r >= 0) return r + 4;
	}

	return -1;
}
static void trigctl_pio_alloc_init(int piosm) {
	trigctl_pio = (piosm & 4) ? pio1 : pio0;
	trigctl_sm = piosm & 3;

	trigctl_off = pio_add_program(trigctl_pio, &trigctl_program);

	enum trigctl_source src = (param_cur.trigger_in_pin < 0)
		? trig_source_irq : trig_source_pin;
	trigctl_pio_init(trigctl_pio, trigctl_sm, trigctl_off,
		src, param_cur.glitch_out_pin, param_cur.trigger_in_pin,
		//param_cur.trigger_in_polarity, param_cur.glitch_out_polarity
		false // disabled by default; needs to be armed
	);

	param_cur.trigctl_pio = trigctl_pio;
	param_cur.trigctl_sm = trigctl_sm;
}
static void trigctl_pio_deinit(void) {
	if (trigctl_pio) {
		pio_sm_set_enabled(trigctl_pio, trigctl_sm, false);
		pio_sm_unclaim(trigctl_pio, trigctl_sm);
		pio_remove_program(trigctl_pio, &trigctl_program, trigctl_off);
	}

	trigctl_pio = NULL;
	trigctl_off = trigctl_sm = ~(uint32_t)0;
}

#define CORE1_PRE_CALC() \
	uint32_t len = param_cur.length_ns.getter(param_cur.length_ns.ud), \
	         off = param_cur.offset_ns.getter(param_cur.offset_ns.ud), \
	         iom = 1u << param_cur.glitch_out_pin; \


//#define CORE1_BUSYLOOP(v) do { \
//	uint32_t counter = (v)/*((v) >> 2) / 3*/; \
//	asm volatile( \
//		"1: sub %[counter], #12\n" \
//		"bgt 1b\n" \
//		:[counter]"+r"(counter) \
//		:: \
//	); \
//} while (0) \
//
//#undef CORE1_BUSYLOOP
#define CORE1_BUSYLOOP(v) busy_wait_us_32((v)/1000)

#define CORE1_DO_GLITCH() \
	do { \
		CORE1_BUSYLOOP(off); \
		param_cur.offset_ns.cur = off; \
		param_cur.length_ns.cur = len; \
		sio_hw->gpio_set = iom; \
		CORE1_BUSYLOOP(len); \
		sio_hw->gpio_clr = iom; \
	} while (0) \

__attribute__((__noreturn__))
static void CORE1_FUNC(glitch_core1_thread_core1_fifoirq)(void) {
	SCB->SCR &= ~SCB_SCR_SEVONPEND_Msk; // don't resume WFE on interrupt

	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) {
		CORE1_PRE_CALC();
		bool arm = *(volatile bool*)&param_cur.armed;

		__WFI();
		irq_set_enabled(irq, false);
		if (!arm) goto continue_;
		arm = *(volatile bool*)&param_cur.armed;
		if (!arm) goto continue_;

		CORE1_DO_GLITCH();
		multicore_fifo_drain();
		multicore_fifo_clear_irq();
		gpio_put(25, true);
	continue_:
		irq_set_enabled(irq, true);
	}
}
__attribute__((__noreturn__))
static void CORE1_FUNC(glitch_core1_thread_core1_gpio)(void) {
	// init gpio irq
	const int gpio = param_cur.trigger_in_pin;
	const int irq = IO_IRQ_BANK0;
	// always rising edge: already using input direction override in case
	// of negative trigger input polarity
	const int event = /*(param_cur.trigger_in_polarity == glitch_positive)
		?*/ GPIO_IRQ_EDGE_RISE /*: GPIO_IRQ_EDGE_FALL*/;
	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_enabled(irq, true);

	while (true) {
		CORE1_PRE_CALC();

		__WFI();
		iobank0_hw->intr[gpio>>3] = event << 4*(gpio&7); // acknowledge irq
		bool arm = *(volatile bool*)&param_cur.armed;
		if (!arm) continue;

		irq_set_enabled(irq, false);
		//CORE1_DO_GLITCH();
		CORE1_BUSYLOOP(off);
		if (!(sio_hw->gpio_in & (1u<<gpio))) goto cont;
		param_cur.offset_ns.cur = off;
		param_cur.length_ns.cur = len;
		sio_hw->gpio_set = iom;
		CORE1_BUSYLOOP(len);
		sio_hw->gpio_clr = iom;
	cont:
		irq_set_enabled(irq, true);
	}
}
__attribute__((__noreturn__))
static void CORE1_FUNC(glitch_core1_thread_pio)(void) {
	const int irq = (trigctl_pio == pio0) ? PIO0_IRQ_1 : PIO1_IRQ_1;
	irq_set_enabled(irq, false);
	trigctl_ack_glitch_irq(trigctl_pio, trigctl_sm);
	trigctl_set_glitch_irq_enabled(trigctl_pio, trigctl_sm, 1, true);
	irq_set_priority(irq, PICO_HIGHEST_IRQ_PRIORITY);
	irq_set_enabled(irq, true);

	while (true) {
		CORE1_PRE_CALC();
		(void)iom;
		param_cur.offset_ns.cur = off;
		param_cur.length_ns.cur = len;
		trigctl_push_off_len(trigctl_pio, trigctl_sm, off>>2, len>>2);

		trigctl_wait_glitch_irq(trigctl_pio, trigctl_sm, true);
		trigctl_ack_glitch_irq(trigctl_pio, trigctl_sm);
		//gpio_put(25, true);
		//sio_hw->gpio_togl = 1u<<25;
	}
}

__attribute__((__noreturn__))
static void CORE1_FUNC(glitch_core1_thread)(void) {
	__disable_irq();

	if (param_cur.impl == glitch_impl_pio) {
		glitch_core1_thread_pio();
	} else if (param_cur.impl == glitch_impl_core1) {
		if (param_cur.trigger_in_pin < 0) {
			glitch_core1_thread_core1_fifoirq();
		} else {
			glitch_core1_thread_core1_gpio();
		}
	}

}

#undef CORE1_DO_GLITCH
#undef CORE1_PRE_CALC

static void CORE0_FUNC(glitch_stop_no_clock_chg)(void) {
	multicore_reset_core1();

	if (param_cur.impl != glitch_impl__none) {
		if (param_cur.impl == glitch_impl_pio) {
			trigctl_pio_deinit();
		}

		// deinit GPIO
		if (param_cur.trigger_in_pin >= 0) {
			hw_write_masked(&padsbank0_hw->io[param_cur.trigger_in_pin]
				, (PADS_BANK0_GPIO0_IE_BITS)
				| (PADS_BANK0_GPIO0_SCHMITT_BITS)
				, (PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS)
				| (PADS_BANK0_GPIO0_SCHMITT_BITS)
			);
			hw_write_masked(&iobank0_hw->io[param_cur.trigger_in_pin].ctrl
				, ((uint)GPIO_FUNC_NULL << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
				| ((uint)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 << param_cur.glitch_out_pin;
		sio_hw->gpio_oe_clr = 1u << param_cur.glitch_out_pin;
		hw_write_masked(&padsbank0_hw->io[param_cur.glitch_out_pin]
			, (PADS_BANK0_GPIO0_IE_BITS)
			| ((uint)GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB)
			| (PADS_BANK0_GPIO0_DRIVE_VALUE_2MA << 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[param_cur.glitch_out_pin].ctrl
			, ((uint)GPIO_FUNC_NULL << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
			| ((uint)GPIO_OVERRIDE_NORMAL << IO_BANK0_GPIO0_CTRL_OUTOVER_LSB)
			, (IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS)
			| (IO_BANK0_GPIO0_CTRL_OUTOVER_BITS)
		);
	}

	memset(&param_cur, 0, sizeof param_cur);
}

bool CORE0_FUNC(glitch_ready)(const struct glitch_params* params) {
	/*printf("params: off = %p %p ; len = %p %p\n",
			params->offset_ns.getter, params->offset_ns.ud,
			params->length_ns.getter, params->length_ns.ud);
	printf("trigger in = %d pol %d ; glitch out = %d pol %d\n",
			params->trigger_in_pin, params->trigger_in_polarity,
			params->glitch_out_pin, params->glitch_out_polarity);
	printf("impl: %d\n", params->impl);*/
	// check params values
	if (!params->offset_ns.getter || !params->offset_ns.ud) return false;
	if (!params->length_ns.getter || !params->length_ns.ud) return false;
	if (params->trigger_in_pin < -1 || params->trigger_in_pin >= 28
			|| (params->trigger_in_pin >= 23 && params->trigger_in_pin <= 25))
		return false;
	if (params->glitch_out_pin < 0 || params->glitch_out_pin >= 28
			|| (params->glitch_out_pin >= 23 && params->glitch_out_pin <= 25))
		return false;
	if (params->trigger_in_polarity != glitch_positive
			&& params->trigger_in_polarity != glitch_negative) return false;
	if (params->glitch_out_polarity != glitch_positive
			&& params->glitch_out_polarity != glitch_negative) return false;
	if (params->impl != glitch_impl_core1 && params->impl != glitch_impl_pio)
		return false;

	int r = 0x99990;
	if (params->impl == glitch_impl_pio) {
		r = trigctl_pio_can_init();
		if (r < 0) {
			printf("no pio!\n");
			return false;
		}
	}

	glitch_stop();
	memcpy(&param_cur, params, sizeof param_cur); // apply new params

	param_cur.offset_ns.cur = 0;
	param_cur.length_ns.cur = 0;
	param_cur.armed = false;

	gpio_init(25);
	gpio_set_dir(25, GPIO_OUT);
	gpio_set_function(25, GPIO_FUNC_SIO);
	gpio_put(25, false);

	// let's not care about impl and always use core1 for now
	// TODO: if pio: check SM availability etc (& clear param_cur if fails)

	// overclock to 250 MHz
	vreg_set_voltage(VREG_VOLTAGE_1_15);
	set_sys_clock_khz(250*1000, true);

	uint func = GPIO_FUNC_SIO;
	if (params->impl == glitch_impl_pio) {
		trigctl_pio_alloc_init(r);
		func = (trigctl_pio == pio0) ? GPIO_FUNC_PIO0 : GPIO_FUNC_PIO1;
	}

	/*if (params->length_min_ns < 0) {
		// FIXME: use a more generic approach (using dragonzap adc)

		// ADC1: ADC mode
		adc_init();
		adc_gpio_init(27);
		adc_select_input(1);
		// ADC0 = GPIO26 = fixed high (as vref)
		sio_hw->gpio_set = 1u << 26;
		sio_hw->gpio_oe_set = 1u << 26;
		gpio_set_function(26, GPIO_FUNC_SIO);
	} else {
		gpio_set_function(26, GPIO_FUNC_NULL);
	}*/

	if (params->trigger_in_pin >= 0) {
		//gpio_set_input_hysteresis_enabled(params->trigger_in_pin, false);
		//gpio_set_dir(params->trigger_in_pin, GPIO_IN );
		//gpio_set_inover(params->trigger_in_pin,
		//	params->trigger_in_polarity ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL);
		//gpio_set_function(params->trigger_in_pin, func);

		// perform all of these at once to minimize the amount of spurious edges
		sio_hw->gpio_oe_clr = 1u << param_cur.trigger_in_pin;
		hw_write_masked(&padsbank0_hw->io[param_cur.trigger_in_pin]
			, (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[param_cur.trigger_in_pin].ctrl
			, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
			| ((uint)(param_cur.trigger_in_polarity ? GPIO_OVERRIDE_INVERT
				: GPIO_OVERRIDE_NORMAL) << IO_BANK0_GPIO0_CTRL_INOVER_LSB)
			, (IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS)
			| (IO_BANK0_GPIO0_CTRL_INOVER_BITS)
		);
	}

	//gpio_put(params->glitch_out_pin, false);
	//gpio_set_drive_strength(PIN_PULSE, GPIO_DRIVE_STRENGTH_12MA);
	//gpio_set_slew_rate(params->glitch_out_pin, GPIO_SLEW_RATE_FAST);
	//gpio_set_dir(params->glitch_out_pin, GPIO_OUT);
	//gpio_set_outover(params->glitch_out_pin,
	//	params->glitch_out_polarity ? GPIO_OVERRIDE_INVERT : GPIO_OVERRIDE_NORMAL);
	//gpio_set_function(params->glitch_out_pin, func);

	// perform all of these at once to minimize the amount of spurious glitches
	sio_hw->gpio_clr = 1u << param_cur.glitch_out_pin;
	sio_hw->gpio_oe_set = 1u << param_cur.glitch_out_pin;
	hw_write_masked(&padsbank0_hw->io[param_cur.glitch_out_pin]
		, 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[param_cur.glitch_out_pin].ctrl
		, (func << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
		| ((uint)(param_cur.glitch_out_polarity ? GPIO_OVERRIDE_INVERT
			: GPIO_OVERRIDE_NORMAL) << IO_BANK0_GPIO0_CTRL_OUTOVER_LSB)
		, IO_BANK0_GPIO0_CTRL_FUNCSEL_BITS | IO_BANK0_GPIO0_CTRL_OUTOVER_BITS
	);
	//printf("glitch out init: func %d on pin %d\n", func, param_cur.glitch_out_pin);

	multicore_launch_core1(glitch_core1_thread);

	return true;
}

void CORE0_FUNC(glitch_stop)(void) {
	glitch_stop_no_clock_chg();
	set_sys_clock_khz(125*1000, true);
	vreg_set_voltage(VREG_VOLTAGE_DEFAULT);
}