#include "pico/stdlib.h"
#include "hardware/spi.h"

#define DISPLAY_SPI spi0
#define PIN_TFT_BACKLIGHT 1
#define PIN_TFT_SCK 2
#define PIN_TFT_MOSI 3
// #define PIN_TFT_CS 5
#define PIN_TFT_DC 6
#define PIN_TFT_RESET 7
#include "display.h"
#include "data.h"

#define HEIGHT ILI9341_TFTHEIGHT
#define WIDTH ILI9341_TFTWIDTH

// todo pack as bits instead
#define AREA (HEIGHT * WIDTH)
u8 frame[AREA];

u32 reader;
bool error = false;
u8 last_frame_type;
u32 last_frame_start;

void decode_next_frame();
void refresh_screen();
void set_pixel(u16 x, u16 y, u8 color);

void draw_digit(u16 x, u16 y, u8 digit) {
	const u16 digit_font[10] = {
	0b111101101101111, // 0
	0b010110010010111, // 1
	0b111001111100111, // 2
	0b111001111001111, // 3
	0b101101111001001, // 4
	0b111100111001111, // 5
	0b111100111101111, // 6
	0b111001001001001, // 7
	0b111101111101111, // 8
	0b111101111001111, // 9
};
	for (int xx = 0; xx < 5; xx++)
	for (int yy = 0; yy < 7; yy++){
		set_pixel(x+xx-1, y+yy-1, 0);
	}

	u16 pixels = digit_font[digit];
	set_pixel(x + 0, y + 0, (pixels >> 14) & 1);
	set_pixel(x + 1, y + 0, (pixels >> 13) & 1);
	set_pixel(x + 2, y + 0, (pixels >> 12) & 1);
	set_pixel(x + 0, y + 1, (pixels >> 11) & 1);
	set_pixel(x + 1, y + 1, (pixels >> 10) & 1);
	set_pixel(x + 2, y + 1, (pixels >> 9) & 1);
	set_pixel(x + 0, y + 2, (pixels >> 8) & 1);
	set_pixel(x + 1, y + 2, (pixels >> 7) & 1);
	set_pixel(x + 2, y + 2, (pixels >> 6) & 1);
	set_pixel(x + 0, y + 3, (pixels >> 5) & 1);
	set_pixel(x + 1, y + 3, (pixels >> 4) & 1);
	set_pixel(x + 2, y + 3, (pixels >> 3) & 1);
	set_pixel(x + 0, y + 4, (pixels >> 2) & 1);
	set_pixel(x + 1, y + 4, (pixels >> 1) & 1);
	set_pixel(x + 2, y + 4, (pixels >> 0) & 1);
}

void draw_num(u16 x, u16 y, u32 num) {
	while (num) {
		u8 digit = num%10;
		num /= 10;
		draw_digit(x, y, digit);
		x -= 4;
	}
}

int main() {
	gpio_init(PICO_DEFAULT_LED_PIN);
	gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
	tft_init_display(100 * 1000 * 1000); // max is 62.5MHz

	while (!error) {
		decode_next_frame();
		refresh_screen();
		// sleep_ms(100);
		if (reader >= sizeof(video))
			error = true;
			// reader = 0;
	}
	gpio_put(PICO_DEFAULT_LED_PIN, 1);
	draw_num(30, 2, reader-1);
	draw_num(15, 8, video[reader-1]);
	draw_num(30, 16, last_frame_start);
	draw_num(15, 22, last_frame_type);
	refresh_screen();
	while(1){};
}


void refresh_screen() {
	tft_send_command(ILI9341_CASET, (u8[]){0, 0, HEIGHT >> 8, (u8)HEIGHT}, 4);
	tft_send_command(ILI9341_PASET, (u8[]){0, 0, WIDTH >> 8, (u8)WIDTH}, 4);
	DC_C;
	spi_get_hw(DISPLAY_SPI)->dr = ILI9341_RAMWR;
	WAIT_FOR_BUSY;
	DC_D;

	u32 pixel_count = AREA;
	const u8 pixels[2] = {0, 255};

	while (pixel_count--) {
		u8 p = pixels[frame[pixel_count]];
		tft_Write_8N(p);
		tft_Write_8N(p);
	}
	WAIT_FOR_BUSY;
}


u8 next_byte() {
	u8 byte = video[reader];
	reader += 1;
	return byte;
}

void bad_memset(u8* addr, u8 value, u32 len) {
	while (len--) {
		*(addr++) = value;
	}
}

void fill_frame(u8 color) {
	bad_memset(frame, color, sizeof(frame));
}

void set_pixel(u16 x, u16 y, u8 color)
{
	// if (x >= WIDTH || y >= HEIGHT){
	// 	while (1){
	// 		gpio_put(PICO_DEFAULT_LED_PIN, 1);
	// 		sleep_ms(50);
	// 		gpio_put(PICO_DEFAULT_LED_PIN, 0);
	// 		sleep_ms(50);
	// 	}
	// }
	x = (WIDTH-1) - x;
	const u32 index = x * HEIGHT + y;
	frame[index] = color;
}

void rle_horizontal();
void rle_vertical();
void rle_vertical_ext();
void bg_strips_h();
void cell_diff_4vv();

void decode_next_frame()
{
	Encoding_t encoding = next_byte();
	u32 frame_start = reader;
	switch (encoding)
	{
#ifdef USE_FillBlack
	case Encoding_FillBlack:
		fill_frame(0);
		break;
#endif
#ifdef USE_FillWhite
	case Encoding_FillWhite:
		fill_frame(1);
		break;
#endif
#ifdef USE_RLEHorizontal
	case Encoding_RLEHorizontal:
		rle_horizontal();
		break;
#endif
#ifdef USE_RLEVertical
	case Encoding_RLEVertical:
		rle_vertical();
		break;
#endif
#ifdef USE_RLEVerticalExt
	case Encoding_RLEVerticalExt:
		rle_vertical_ext();
		break;
#endif
#ifdef USE_BGStripsH24
	case Encoding_BGStripsH24:
		bg_strips_h();
		break;
#endif
#ifdef USE_CellDiff4VV
	case Encoding_CellDiff4VV:
		cell_diff_4vv();
		break;
#endif
	default:
		error = true;
		return;
	}
	last_frame_type = encoding;
	last_frame_start = frame_start;
}

#ifdef USE_RLEHorizontal
void rle_horizontal()
{
	u16 x = 0;
	u16 y = 0;
	u8 color = 0;
	while (y < HEIGHT) {
		u8 run = next_byte();
		for (int i = 0; i < run; i++) {
			set_pixel(x, y, color);
			x += 1;
			if (x == WIDTH) {
				x = 0;
				y += 1;
			}
		}
		color = !color;
	}
}
#endif

#ifdef USE_RLEVertical
void rle_vertical() {
	u16 x = 0;
	u16 y = 0;
	u8 color = 0;
	while (x < WIDTH)
	{
		u16 run = next_byte();
		for (u16 i = 0; i < run; i++)
		{
			set_pixel(x, y, color);
			y += 1;
			if (y == HEIGHT) {
				y = 0;
				x += 1;
			}
		}
		color = !color;
	}
}
#endif


#ifdef USE_RLEVerticalExt
void rle_vertical_ext() {
	u16 x = 0;
	u16 y = 0;
	u8 color = 0;
	while (x < WIDTH)  {
		u8 byte = next_byte();
		u16 run = byte;
		if (byte == 0 && video[reader] == 0) {
			// 16 bit run length
			u16 upper = video[reader + 1] << 8;
			if (upper) {
				u16 lower = video[reader + 2];
				run = upper | lower;
				reader += 3;
			}
		}
		for (u16 i = 0; i < run; i++) {
			set_pixel(x, y, color);
			y += 1;
			if (y == HEIGHT) {
				y = 0;
				x += 1;
			}
		}
		color = !color;
	}
}
#endif

#ifdef USE_BGStripsH24
void bg_strips_h() {
	u8 head = next_byte();
	u8 bg = head >> 7;
	u8 fg = 1 - bg;
	fill_frame(bg);
	u16 strip_count = ((u16)(head & 127) << 8) | next_byte();
	for (u16 i = 0; i < strip_count; i++) {
		u16 y = next_byte();
		u16 x_lower = next_byte();
		u16 x_width = next_byte();
		u16 x_start = x_lower | ((x_width & 0x80) << 1);
		u8 width = x_width & 0x7f;
		for (int x = x_start; x < (x_start + width); x++) {
			set_pixel(x, y, fg);
		}
	}
}
#endif

#ifdef USE_CellDiff4VV
void cell_diff_4vv() {
	const u32 cells_x = WIDTH/4;
	const u32 cells_y = HEIGHT/4;
	bool modified_cells[cells_x*cells_y];


	for (u16 x = 0; x < WIDTH; x++) {
		for (u16 y = 0; y < HEIGHT; y++) {
		}
	}
}
#endif
