/**
 * @file main.c
 * @author giomba@glgprograms.it
 * @brief
 *
 * @copyright Copyright Retrofficina GLG Programs (c) 2022
 *
 */

#include "hardware/clocks.h"
#include "hardware/dma.h"
#include "hardware/pio.h"
#include "hardware/pll.h"
#include "hardware/structs/pio.h"
#include "pico/stdlib.h"

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include "vga.pio.h"

#define HPIXEL 640 // number of pixels displayed horizontally
#define VPIXEL 480 // number of pixels displayed vertically
#define BPP    1   // bits per pixel

#define VSPULSE 3  // number of lines during the vertical sync pulse
#define VBBLANK 16 // number of lines of the initial back porch blanking period
#define VFBLANK 1  // number of lines of the final front porch blanking period

// Frame buffer.
// Due to limitations in program space of PIO interface,
// additional blank lines are needed and sent to the display,
// to fill the vertical blanking interval at top and bottom
// of the display
// (at 1BPP, this wastes about 1k)
uint8_t frame[HPIXEL * (VPIXEL + VBBLANK + VFBLANK) * BPP / 8] = {0};

static void vga_pixel_program_init(PIO pio, uint sm, uint offset)
{
	// magic definition of the called function?
	pio_sm_config config = vga_free_run_program_get_default_config(offset);
	// destination pins for OUT instructions
	sm_config_set_out_pins(&config, 22, 1);
	sm_config_set_out_shift(&config, true, true, 0);
	pio_gpio_init(pio, 22);
	pio_sm_set_consecutive_pindirs(pio, sm, 22, 1, true);
	sm_config_set_wrap(&config, offset + vga_free_run_wrap_target, offset + vga_free_run_wrap);

	pio_sm_init(pio, sm, offset, &config);

	pio_sm_set_clkdiv_int_frac(pio, sm, 1, 0);

	pio_sm_set_enabled(pio, sm, true);
}

static void vga_hsync_program_init(PIO pio, uint sm, uint offset)
{
	pio_sm_config config = vga_hsync_program_get_default_config(offset);
	// destination pins for SET instructions
	sm_config_set_set_pins(&config, 20, 1);
	pio_gpio_init(pio, 20);
	pio_sm_set_consecutive_pindirs(pio, sm, 20, 1, true);
	sm_config_set_wrap(&config, offset + vga_hsync_wrap_target, offset + vga_hsync_wrap);

	pio_sm_init(pio, sm, offset, &config);

	pio_sm_set_clkdiv_int_frac(pio, sm, 1, 0);

	pio->sm->shiftctrl = (1 << PIO_SM0_SHIFTCTRL_FJOIN_TX_LSB);

	pio_sm_set_enabled(pio, sm, true);
}

static void vga_vsync_program_init(PIO pio, uint sm, uint offset)
{
	pio_sm_config config = vga_vsync_program_get_default_config(offset);
	// destination pins for SET instructions
	sm_config_set_set_pins(&config, 21, 1);
	pio_gpio_init(pio, 21);
	pio_sm_set_consecutive_pindirs(pio, sm, 21, 1, true);
	sm_config_set_wrap(&config, offset + vga_vsync_wrap_target, offset + vga_vsync_wrap);

	pio_sm_init(pio, sm, offset, &config);

	pio_sm_set_clkdiv_int_frac(pio, sm, 1, 0);

	pio->sm->shiftctrl = (1 << PIO_SM0_SHIFTCTRL_FJOIN_TX_LSB);

	pio_sm_set_enabled(pio, sm, true);
}

void setup_clocks(void)
{
	// disable resuscitation clock
	clocks_hw->resus.ctrl = 0;

	// before changing PLL, switch sys and ref cleanly away from their aux sources
	hw_clear_bits(&clocks_hw->clk[clk_sys].ctrl, CLOCKS_CLK_SYS_CTRL_SRC_BITS);
	while (clocks_hw->clk[clk_sys].selected != 0x1)
		tight_loop_contents();
	hw_clear_bits(&clocks_hw->clk[clk_ref].ctrl, CLOCKS_CLK_REF_CTRL_SRC_BITS);
	while (clocks_hw->clk[clk_ref].selected != 0x1)
		tight_loop_contents();

	// set PLL at 126 MHz, and wait for it to stabilize
	pll_init(pll_sys, 1, 1512 * MHZ, 6, 2);

	// re-configure sys_clk to use PLL
	clock_configure(clk_sys, CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
	                CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS, 126 * MHZ, 126 * MHZ);

	// re-configure CLK PERI = clk_sys
	clock_configure(clk_peri, 0, CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS, 126 * MHZ, 126 * MHZ);
}

static bool dma_ready = false;
static int dma_channel;

/**
 * @brief Represents a program running on the SM of a PIO.
 *
 */
typedef struct PIORun
{
	PIO pio;     //< executing PIO
	uint offset; //< PIO memory offset for program
	uint sm;     //< executing SM
} PIORun;

static PIORun *vga_pixel_ptr = NULL;

static unsigned long int frame_counter = 0;

void dma_handler(void)
{
	// acknoweledge DMA
	dma_hw->ints0 = 1 << dma_channel;
}

bool valid_coord(uint16_t x, uint16_t y)
{
	if (x < 0 || x >= HPIXEL)
		return false;
	if (y < 0 || y >= VPIXEL)
		return false;

	return true;
}

void set_pixel(uint16_t x, uint16_t y)
{
	// ignore out of frame pixels
	if (!valid_coord(x, y))
		return;

	// Since there is not much space in PIO program memory,
	// it is not possible to set the last pixel of a line,
	// because there is not enough program space to unset it
	// in the hardware, and, if left set, will mess with
	// the sync signals.
	if (x == 639)
		return;

	// skip wasted blanking lines
	// (see frame struct declaration)
	y += VBBLANK + VFBLANK;

	// set bit
	frame[y * 80 + x / 8] |= (1 << x % 8);
}

void clear_pixel(uint16_t x, uint16_t y)
{
	if (!valid_coord(x, y))
		return;

	y += VBBLANK + VFBLANK;
	frame[y * 80 + x / 8] &= ~(1 << x % 8);
}

void draw_simple_line(uint16_t x, uint16_t y, uint16_t len, bool horizontal)
{
	for (uint16_t i = 0; i < len; ++i)
	{
		if (horizontal)
			set_pixel(x + i, y);
		else
			set_pixel(x, y + i);
	}
}

void new_frame_handler(void)
{
	if (vga_pixel_ptr && dma_ready)
	{
		// Explicitly put the first uint32_t word in the FIFO,
		// because, when PIO pulls data for the first time,
		// and its FIFO is empty, the DMA controller takes "a while"
		// to put the first word and keep up with the transfer,
		// making the whole frame a bit offset to the right.
		// Since front porch is very short, even the smallest delay,
		// may disrupt the timing.
		// Also note: despite the "blocking put", when this interrupt
		// is serviced, PIO FIFO is always empty, so in practice
		// this will never block.
		pio_sm_put_blocking(vga_pixel_ptr->pio, vga_pixel_ptr->sm, 0);

		// Start DMA (but skip the initial word, which has been put manually)
		dma_channel_set_read_addr(dma_channel, &frame[4], true);
	}

	frame_counter++;

	pio_interrupt_clear(pio0_hw, 1);
}

int main()
{
	setup_clocks();
	stdio_init_all();
	sleep_ms(1000);

	// clear frame buffer
	memset(&frame[0], 0, sizeof(frame));
	// draw a cross
	// horizontal lines
	for (uint16_t x = 0; x < 640; ++x)
	{
		if ((x / 32) % 2 == 0)
			set_pixel(x, 0);
		if ((x / 32) % 2 == 1)
			set_pixel(x, 2);
		set_pixel(x, 239);
		set_pixel(x, 479);
	}
	// vertical lines
	for (uint16_t y = 0; y < 480; ++y)
	{
		set_pixel(0 + (y / 10) * 4, y);
		set_pixel(0, y);
		set_pixel(319, y);
		// rightmost pixels can't be set, see description
		set_pixel(638, y);
	}
	// try to find strangeness using stairs
	for (uint16_t x = 0; x < 640; ++x)
	{
		set_pixel(x, (x / 32) * 4 + 120);
	}
	// draw "E" - nice because it's not symmetrical along X
	draw_simple_line(100, 100, 8, true);
	draw_simple_line(100, 100, 8, false);
	draw_simple_line(100, 104, 8, true);
	draw_simple_line(100, 108, 8, true);

	// Running programs on PIO
	PIORun vga_hsync, vga_vsync, vga_pixel;
	vga_hsync.pio = vga_vsync.pio = vga_pixel.pio = pio0;

	// Prepare frame interrupt
	// Frame interrupt is asserted on PIO0 interrupt 1 by SM1
	pio_set_irq1_source_enabled(vga_vsync.pio, pis_interrupt1,
	                            true); // allow SM1 of PIO to fire the interrupt
	irq_set_exclusive_handler(PIO0_IRQ_1, new_frame_handler); // set handler for IRQ1 of PIO
	irq_set_enabled(PIO0_IRQ_1, true);                        // enable interrupt

	// VGA HSYNC program
	printf("Starting VGA hsync machine... ");
	if (!pio_can_add_program(vga_hsync.pio, &vga_hsync_program))
		panic("cannot add program");
	vga_hsync.offset = pio_add_program(vga_hsync.pio, &vga_hsync_program);
	vga_hsync.sm = pio_claim_unused_sm(vga_hsync.pio, true);
	vga_hsync_program_init(vga_hsync.pio, vga_hsync.sm, vga_hsync.offset);
	// low pulse is X pixels long, and SM runs at 4x of pixel clock
	pio_sm_put_blocking(vga_hsync.pio, vga_hsync.sm, (64 - 2));
	pio_sm_put_blocking(vga_hsync.pio, vga_hsync.sm, (640 + 16 - 1)); // 16 is front porch
	printf("OK\n");

	// VGA VSYNC program
	printf("Starting VGA vsync machine... ");
	if (!pio_can_add_program(vga_vsync.pio, &vga_vsync_program))
		panic("cannot add program");
	vga_vsync.offset = pio_add_program(vga_vsync.pio, &vga_vsync_program);
	vga_vsync.sm = pio_claim_unused_sm(vga_vsync.pio, true);
	vga_vsync_program_init(vga_vsync.pio, vga_vsync.sm, vga_vsync.offset);
	// low pulse assert lasts for 3 horizontal lines, then adjust by one
	pio_sm_put_blocking(vga_vsync.pio, vga_vsync.sm, 3 - 1);
	pio_sm_put_blocking(vga_vsync.pio, vga_vsync.sm, 500 - 3 - 1);
	printf("OK\n");

	// VGA pixel program
	vga_pixel_ptr = &vga_pixel;
	printf("Starting VGA pixel machine... ");
	if (!pio_can_add_program(vga_pixel.pio, &vga_free_run_program))
		panic("cannot add program");
	vga_pixel.offset = pio_add_program(vga_pixel.pio, &vga_free_run_program);
	vga_pixel.sm = pio_claim_unused_sm(vga_pixel.pio, false);
	vga_pixel_program_init(vga_pixel.pio, vga_pixel.sm, vga_pixel.offset);
	pio_sm_put_blocking(vga_pixel.pio, vga_pixel.sm, 640 - 1);
	printf("OK\n");

	printf("Setting up DMA... ");
	dma_channel = dma_claim_unused_channel(true);
	dma_channel_config dma_config = dma_channel_get_default_config(dma_channel);
	channel_config_set_transfer_data_size(&dma_config, DMA_SIZE_32);
	channel_config_set_read_increment(&dma_config, true);
	channel_config_set_write_increment(&dma_config, false);
	channel_config_set_dreq(&dma_config, DREQ_PIO0_TX2);

	dma_channel_set_irq0_enabled(dma_channel, true);

	irq_set_exclusive_handler(DMA_IRQ_0, dma_handler);
	irq_set_enabled(DMA_IRQ_0, true);

	// -1 because first word is manually injected
	// before the first DMA request, in order to
	// avoid any possible delay
	dma_channel_configure(dma_channel, &dma_config, &pio0_hw->txf[2], &frame[0],
	                      HPIXEL * (VPIXEL + VBBLANK + VFBLANK) / 32 - 1, true);

	dma_ready = true;
	printf("OK\n");

	printf("Now running.\n");

	for (;;)
	{
		// fancy drawings
		const uint16_t fc = frame_counter;
		const uint16_t x = fc % 640;
		const uint16_t y = 239 + 100 * (abs(320 - x) / 640.0) * sin((fc % 640) / 12.738);
		if ((fc / 640) % 2)
			set_pixel(x, y);
		else
			clear_pixel(x, y);
		sleep_ms(5);
	}
}