#include "main.h"
#include "lcd.h"
#include "generic_macros.h"

static char display_framebuffer[16*2*DISPLAY_FRAMES_AVAILABLE];
size_t display_current_frame;
static size_t display_framebuffer_mode;

static struct Display_emu_state des;

void display_init(void)
{
	// prepare virtual framebuffer
	display_current_frame = 0;
	display_framebuffer_mode = 0;
	des.cursor_offset = 0;
	des.next = 0;
	memset(display_framebuffer, 0x20, 16*2*DISPLAY_FRAMES_AVAILABLE);

	// switch to 4-bit 2-line mode
	display_write_instruction_byte(0x28);

	// clear display
	display_write_instruction_byte(0x01);

	// enable display
	display_write_instruction_byte(0x0C);

	// move cursor to first line
	display_write_instruction_byte(0x80);
}

void display_to_framebuffer(void)
{
	display_framebuffer_mode = 1;
}

void display_to_direct(void)
{
	display_framebuffer_mode = 0;
}

static uint8_t display_read_status(void)
{
	// make sure GPIOE is in correct mode
	GPIOE->MODER = 0x00504000;

	if (GPIOE->MODER != 0x00504000)
		PANIC(0x4000);

	uint8_t status = 0;

	GPIOE->ODR = 0x0;
	GPIOE->BSRR = DISPLAY_RW;

	GPIOE->BSRR = DISPLAY_ENA;
	status |= (GPIOE->IDR & 0xF000) >> 8;
	GPIOE->BSRR = (DISPLAY_ENA << 16);

	GPIOE->BSRR = DISPLAY_ENA;
	status |= (GPIOE->IDR & 0xF000) >> 12;
	GPIOE->BSRR = (DISPLAY_ENA << 16);

	GPIOE->ODR = 0x0;

	return status;
}

static void display_write_instruction_byte_direct(uint8_t code)
{
	DISPLAY_POLL_UNTIL_READY;

	// make sure GPIOE is in correct mode
	GPIOE->MODER = 0x55504000;

	if (GPIOE->MODER != 0x55504000)
		PANIC(0x4000);

	GPIOE->ODR = (code & 0xF0) << 8;
	GPIOE->BSRR = DISPLAY_ENA;
	GPIOE->BSRR = (DISPLAY_ENA << 16);

	GPIOE->ODR = (code & 0x0F) << 12;
	GPIOE->BSRR = DISPLAY_ENA;
	GPIOE->BSRR = (DISPLAY_ENA << 16);
}

static void display_write_instruction_byte_framebuffer(uint8_t code)
{
	// emulate physical display behavior on receiving instructions
	if (code & 0x80) {
		// decode new cursor offset
		size_t offset = ((code & 0x40) >> 2) | (code & 0xF);
		des.cursor_offset = offset;
	} else if (code == 0x01) {
		// reset screen
		memset(&(display_framebuffer[16*2*display_current_frame]), 0x20, 16*2);
		des.cursor_offset = 0;
		des.next = 0;
	} else if (code == 0x06) {
		// set increment mode
		des.next = 0;
	} else if (code == 0x04) {
		// set decrement mode
		des.next = 1;
	}
}

void display_write_instruction_byte(uint8_t code)
{
	if (display_framebuffer_mode) {
		display_write_instruction_byte_framebuffer(code);
	} else {
		display_write_instruction_byte_direct(code);
	}
}

static void display_write_data_byte_direct(uint8_t code)
{
	DISPLAY_POLL_UNTIL_READY;

	// make sure GPIOE is in correct mode
	GPIOE->MODER = 0x55504000;

	if (GPIOE->MODER != 0x55504000)
		PANIC(0x4000);

	GPIOE->ODR = ((code & 0xF0) << 8) | (DISPLAY_RS);
	GPIOE->BSRR = DISPLAY_ENA;
	GPIOE->BSRR = DISPLAY_ENA << 16;

	GPIOE->ODR = ((code & 0x0F) << 12) | (DISPLAY_RS);
	GPIOE->BSRR = DISPLAY_ENA;
	GPIOE->BSRR = DISPLAY_ENA << 16;
}

static void display_write_data_byte_framebuffer(uint8_t code)
{
	display_framebuffer[16*2*display_current_frame + des.cursor_offset] = (char) code;
	des.cursor_offset += des.next ? -1 : 1;
}

void display_write_data_byte(uint8_t code)
{
	if (display_framebuffer_mode) {
		display_write_data_byte_framebuffer(code);
	} else {
		display_write_data_byte_direct(code);
	}
}

void display_write_data_seq(char *codes)
{
	for (size_t i = 0; i < 16; i++) {
		if (codes[i])
			display_write_data_byte(codes[i]);
		else
			break;
	}
}

void display_load(uint32_t frame_no)
{
	if (display_framebuffer_mode)
		return;

	DISPLAY_CLEAR;

	for (uint32_t i = 0; i < 16; i++) {
		display_write_data_byte_direct(display_framebuffer[16*2*display_current_frame + i]);
	}

	DISPLAY_SET_CURSOR(1, 0);

	for (uint32_t i = 0; i < 16; i++) {
		display_write_data_byte_direct(display_framebuffer[16*2*display_current_frame + i + 16]);
	}
}