#include "tft.h"

static struct tft_config tft_cfg;
static bool tft_data_mode = false;

void tft_cmd(uint8_t cmd, const uint8_t* data, size_t len)
{
	spi_set_format(tft_cfg.spi, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);
	tft_data_mode = false;

	gpio_put(tft_cfg.gpio_cs, 0);
	gpio_put(tft_cfg.gpio_dc, 0);
		
	spi_write_blocking(tft_cfg.spi, &cmd, sizeof(cmd));
	
	if (len) {
		gpio_put(tft_cfg.gpio_dc, 1);
		spi_write_blocking(tft_cfg.spi, data, len);
	}
	gpio_put(tft_cfg.gpio_cs, 1);
	gpio_put(tft_cfg.gpio_dc, 1);
}
void tft_caset(uint16_t xs, uint16_t xe)
{
	uint8_t data[] = {
		xs >> 8,
		xs & 0xff,
		xe >> 8,
		xe & 0xff,
	};

	// CASET (2Ah): Column Address Set
	tft_cmd(0x2a, data, sizeof(data));
}

void tft_raset(uint16_t ys, uint16_t ye)
{
	uint8_t data[] = {
		ys >> 8,
		ys & 0xff,
		ye >> 8,
		ye & 0xff,
	};

	// RASET (2Bh): Row Address Set
	tft_cmd(0x2b, data, sizeof(data));
}
void tft_init(const struct tft_config* config)
{
	memcpy(&tft_cfg, config, sizeof(tft_cfg));

	set_sys_clock_khz(250 * 1000, true);

	volatile uint ach = spi_init(tft_cfg.spi, 67 * 1000 * 1000); // original 125 * 1000 * 1000
	spi_set_format(tft_cfg.spi, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);

	gpio_set_function(tft_cfg.gpio_din, GPIO_FUNC_SPI);
	gpio_set_function(tft_cfg.gpio_clk, GPIO_FUNC_SPI);

	gpio_init(tft_cfg.gpio_cs);
	gpio_init(tft_cfg.gpio_dc);
	gpio_init(tft_cfg.gpio_rst);
	gpio_init(tft_cfg.gpio_bl);

	gpio_set_dir(tft_cfg.gpio_cs, GPIO_OUT);
	gpio_set_dir(tft_cfg.gpio_dc, GPIO_OUT);
	gpio_set_dir(tft_cfg.gpio_rst, GPIO_OUT);
	gpio_set_dir(tft_cfg.gpio_bl, GPIO_OUT);

	gpio_put(tft_cfg.gpio_cs, 1);
	gpio_put(tft_cfg.gpio_dc, 1);
	gpio_put(tft_cfg.gpio_rst, 1);
	sleep_ms(100);

	// SWRESET (01h): Software Reset
	tft_cmd(0x01, NULL, 0);
	sleep_ms(150);

	// SLPOUT (11h): Sleep Out
	tft_cmd(0x11, NULL, 0);
	sleep_ms(50);

	// COLMOD (3Ah): Interface Pixel Format
	// - RGB interface color format     = 65K of RGB interface
	// - Control interface color format = 16bit/pixel
	tft_cmd(0x3a, (uint8_t[]) { 0x55 }, 1);
	sleep_ms(10);

	// MADCTL (36h): Memory Data Access Control
	// - Page Address Order            = Top to Bottom
	// - Column Address Order          = Left to Right
	// - Page/Column Order             = Normal Mode
	// - Line Address Order            = LCD Refresh Top to Bottom
	// - RGB/BGR Order                 = RGB
	// - Display Data Latch Data Order = LCD Refresh Left to Right
	tft_cmd(0x36, (uint8_t[]) { 0x00 }, 1);

	tft_caset(0, tft_cfg.width);
	tft_raset(0, tft_cfg.height);

	// INVON (21h): Display Inversion On
	// tft_cmd(0x21, NULL, 0);
	// sleep_ms(10);

	// NORON (13h): Normal Display Mode On
	tft_cmd(0x13, NULL, 0);
	sleep_ms(10);

	// DISPON (29h): Display On
	tft_cmd(0x29, NULL, 0);
	sleep_ms(10);
	
	gpio_put(tft_cfg.gpio_bl, 1);
}
void tft_ramwr()
{
	gpio_put(tft_cfg.gpio_cs, 0);
	gpio_put(tft_cfg.gpio_dc, 0);

	// RAMWR (2Ch): Memory Write
	uint8_t cmd = 0x2c;
	spi_write_blocking(tft_cfg.spi, &cmd, sizeof(cmd));

	gpio_put(tft_cfg.gpio_cs, 0);
	gpio_put(tft_cfg.gpio_dc, 1);
}
void tft_write(const void* data, size_t len)
{
	spi_write16_blocking(tft_cfg.spi, data, len / 2);
}

void tft_put(uint16_t pixel)
{
	tft_write(&pixel, sizeof(pixel));
}

void tft_fill(uint16_t pixel)
{
	int num_pixels = tft_cfg.width * tft_cfg.height;

	tft_set_cursor(0, 0);
if (!tft_data_mode) {
			tft_ramwr();

			spi_set_format(tft_cfg.spi, 16, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);

			tft_data_mode = true;
		}

	printf("test");
	for (int i = 0; i < num_pixels; i++) {
		
		tft_put(pixel);
	}
	printf("test");
}

void tft_set_cursor(uint16_t x, uint16_t y)
{
	tft_caset(x, tft_cfg.width);
	tft_raset(y, tft_cfg.height);
}

void tft_vertical_scroll(uint16_t row)
{
	uint8_t data[] = {
		(row >> 8) & 0xff,
		row & 0x00ff
	};

	// VSCSAD (37h): Vertical Scroll Start Address of RAM
	tft_cmd(0x37, data, sizeof(data));
}