#include <ice_fpga.h>
#include <ice_led.h>
#include <ice_usb.h>

#include <tusb.h>
#include <bsp/board_api.h>

#include <pico/stdio.h>
#include <hardware/gpio.h>
#include <hardware/uart.h>

static void video_task();

int main()
{
	stdio_init_all();

	uart_init(uart0, 115200);
	gpio_set_function(0, GPIO_FUNC_UART);
	gpio_set_function(1, GPIO_FUNC_UART);

	ice_led_init();
	ice_usb_init();

	ice_fpga_init(12);
	ice_fpga_start();

	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 unsigned int frame_num = 0;
static bool tx_busy = false;

static uint8_t frame_buffer[FRAME_WIDTH * FRAME_HEIGHT * 16 / 8] = { };

static void fill_color_bar(uint8_t* buffer, const unsigned int start_position)
{
	static constexpr uint8_t bar_color[8][4] =
	{
		/*  Y,   U,   Y,   V */
		{ 235, 128, 235, 128 }, /* 100% White */
		{ 219,  16, 219, 138 }, /* Yellow */
		{ 188, 154, 188,  16 }, /* Cyan */
		{ 173,  42, 173,  26 }, /* Green */
		{  78, 214,  78, 230 }, /* Magenta */
		{  63, 102,  63, 240 }, /* Red */
		{  32, 240,  32, 118 }, /* Blue */
		{  16, 128,  16, 128 }, /* Black */
	};

	uint8_t* p;

	const uint8_t* end = &buffer[FRAME_WIDTH * 2];
	const unsigned int index = (FRAME_WIDTH / 2 - 1) - (start_position % (FRAME_WIDTH / 2));

	p = &buffer[index * 4];
	for (unsigned int i = 0; i < 8; i++)
	{
		for (int j = 0; j < FRAME_WIDTH / (2 * 8); j++)
		{
			memcpy(p, &bar_color[i], 4);
			p += 4;
			if (p >= end) { p = buffer; }
		}
	}

	p = &buffer[FRAME_WIDTH * 2];
	for (unsigned int i = 1; i < FRAME_HEIGHT; i++)
	{
		memcpy(p, buffer, FRAME_WIDTH * 2);
		p += FRAME_WIDTH * 2;
	}
}

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

	// don't send data if not streaming
	if (!tud_video_n_streaming(0, 0))
	{
		already_sent = false;
		frame_num = 0;
		return;
	}

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

		fill_color_bar(frame_buffer, frame_num);

		tud_video_n_frame_xfer(0, 0, frame_buffer, 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;

	fill_color_bar(frame_buffer, frame_num);
	tud_video_n_frame_xfer(0, 0, frame_buffer, 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;

	frame_num++;
}

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;
}