#include <ice_fpga.h>
#include <ice_led.h>
#include <ice_usb.h>
#include <tusb.h>
#include <boards/pico_ice.h>
#include <bsp/board_api.h>
#include <hardware/clocks.h>
#include <hardware/uart.h>
#include <pico/stdio.h>

#include "fpga.pio.h"

static PIO pio;
static uint sm;

#define ICE_FPGA_START_FRAME_PIN ICE_FPGA_14_PIN

static void video_task();
static void start_next_frame()
{
	if (dma_channel_is_busy(DMA_CHANNEL)) { return; }

	gpio_put(ICE_FPGA_START_FRAME_PIN, true);
	sleep_ms(10);
	printf("%d - ", gpio_get(ICE_FPGA_START_FRAME_PIN));
	gpio_put(ICE_FPGA_START_FRAME_PIN, false);
	sleep_ms(10);
	printf("%d - ", gpio_get(ICE_FPGA_START_FRAME_PIN));

	pio_sm_set_enabled(pio, sm, false);
	pio_sm_clear_fifos(pio, sm);
	pio_sm_restart(pio, sm);
	dma_channel_set_write_addr(DMA_CHANNEL, frame_buffer[current_frame] - !current_frame, true);
	current_frame = !current_frame;
	pio_sm_set_enabled(pio, sm, true);
}

int main()
{
	// set to 120 MHz as it's exactly 2.5x the FPGA clock
	set_sys_clock_khz(120000, true);

	tusb_init();
	stdio_init_all();

	ice_led_init();
	ice_usb_init();

	ice_fpga_init(48);
	ice_fpga_start();

	gpio_init_mask(0b111111111);
	gpio_set_dir(ICE_FPGA_27_PIN, GPIO_IN); // 0
	gpio_set_dir(ICE_FPGA_25_PIN, GPIO_IN); // 1
	gpio_set_dir(ICE_FPGA_21_PIN, GPIO_IN); // 2
	gpio_set_dir(ICE_FPGA_19_PIN, GPIO_IN); // 3
	gpio_set_dir(ICE_FPGA_26_PIN, GPIO_IN); // 4
	gpio_set_dir(ICE_FPGA_23_PIN, GPIO_IN); // 5
	gpio_set_dir(ICE_FPGA_20_PIN, GPIO_IN); // 6
	gpio_set_dir(ICE_FPGA_18_PIN, GPIO_IN); // 7

	gpio_set_dir(ICE_FPGA_START_FRAME_PIN, GPIO_OUT); // 8

	gpio_put(ICE_FPGA_START_FRAME_PIN, false);

	pio = pio0;
	const uint offset = pio_add_program(pio, &fpga_program);
	sm = pio_claim_unused_sm(pio, true);
	fpga_program_init(pio, sm, offset, 0, 1);

	tud_init(0);

	if (board_init_after_tusb)
	{
		board_init_after_tusb();
	}

	while (true)
	{
		tud_task();
		video_task();
	}
	return 0;
}

static unsigned int interval_ms = 1000 / FRAME_RATE;
static bool tx_busy = false;

void video_task()
{
	static bool already_sent = false;
	static unsigned int start_ms = 0;

	if (!tud_video_n_streaming(0, 0))
	{
		already_sent = false;
		return;
	}

	if (!already_sent)
	{
		already_sent = true;
		tx_busy = true;
		start_ms = board_millis();

		start_next_frame();
		tud_video_n_frame_xfer(0, 0, frame_buffer[current_frame], FRAME_WIDTH * FRAME_HEIGHT * 16 / 8);
	}

	const unsigned int cur = board_millis();
	if (cur - start_ms < interval_ms) { return; }
	if (tx_busy) { return; }
	start_ms += interval_ms;
	tx_busy = true;

	tud_video_n_frame_xfer(0, 0, frame_buffer[current_frame], FRAME_WIDTH * FRAME_HEIGHT * 16 / 8);
}

void tud_video_frame_xfer_complete_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx)
{
	tx_busy = false;

	start_next_frame();
}

int tud_video_commit_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, const video_probe_and_commit_control_t* parameters)
{
	interval_ms = parameters->dwFrameInterval / 10000;
	return VIDEO_ERROR_NONE;
}