#include <libopencm3/stm32/gpio.h>

#include "lcd.h"

#define LCD_USE_4BIT_MODE 1

#define LCD_PORT    GPIOA
#define LCD_E       GPIO10
#define LCD_RW      GPIO11
#define LCD_RS      GPIO12
#define LCD_D4      GPIO4
#define LCD_D5      GPIO5
#define LCD_D6      GPIO6
#define LCD_D7      GPIO7

struct LCDCommand {
	uint8_t         data;
	enum LCDRegType reg_type;
};

#define LCD_QUEUE_LEN 64
struct LCDCommand lcd_cmd_buffer[LCD_QUEUE_LEN];
uint16_t lcd_buf_read_pos, lcd_buf_write_pos;

uint8_t lcd_init_sequence_idx = 0;

static inline void busy_wait(uint32_t cycles)
{
	for (uint32_t i = 0; i < cycles; i++) {
		__asm__("nop");
	}
}

#if LCD_USE_4BIT_MODE
/*
 * Send 4 bits to the LCD. Only use directly during initialization.
 *
 * The display samples data on the falling edge of E.
 *
 * \param data    lower nibble contains the data to send
 */
static void lcd_send_4bit(uint8_t data, enum LCDRegType reg_type)
{
	uint8_t gpios2set = 0, gpios2clear = 0;

	// set E high (rising edge has no effect)
	gpio_set(LCD_PORT, LCD_E);

	if(reg_type == LCD_REG_CONTROL) {
		gpio_clear(LCD_PORT, LCD_RS);
	} else { // LCD_REG_DATA
		gpio_set(LCD_PORT, LCD_RS);
	}

	busy_wait(100);

	// calculate target gpio states
	if(data & (1 << 0)) { gpios2set |= LCD_D4; } else { gpios2clear |= LCD_D4; }
	if(data & (1 << 1)) { gpios2set |= LCD_D5; } else { gpios2clear |= LCD_D5; }
	if(data & (1 << 2)) { gpios2set |= LCD_D6; } else { gpios2clear |= LCD_D6; }
	if(data & (1 << 3)) { gpios2set |= LCD_D7; } else { gpios2clear |= LCD_D7; }

	// apply data pins
	gpio_set(LCD_PORT, gpios2set);
	gpio_clear(LCD_PORT, gpios2clear);

	busy_wait(100);

	// set E low (display samples on falling edge)
	gpio_clear(LCD_PORT, LCD_E);

	busy_wait(100);
}
#else /* !LCD_USE_4BIT_MODE */
static void lcd_send_8bit(uint8_t data, enum LCDRegType reg_type)
{
	uint32_t gpios2set = 0, gpios2clear = 0;

	// set E high (rising edge has no effect)
	gpio_set(LCD_PORT, LCD_E);

	if(reg_type == LCD_REG_CONTROL) {
		gpio_clear(LCD_PORT, LCD_RS);
	} else { // LCD_REG_DATA
		gpio_set(LCD_PORT, LCD_RS);
	}

	busy_wait(100);

	// calculate target gpio states
	if(data & (1 << 0)) { gpios2set |= LCD_D0; } else { gpios2clear |= LCD_D0; }
	if(data & (1 << 1)) { gpios2set |= LCD_D1; } else { gpios2clear |= LCD_D1; }
	if(data & (1 << 2)) { gpios2set |= LCD_D2; } else { gpios2clear |= LCD_D2; }
	if(data & (1 << 3)) { gpios2set |= LCD_D3; } else { gpios2clear |= LCD_D3; }
	if(data & (1 << 4)) { gpios2set |= LCD_D4; } else { gpios2clear |= LCD_D4; }
	if(data & (1 << 5)) { gpios2set |= LCD_D5; } else { gpios2clear |= LCD_D5; }
	if(data & (1 << 6)) { gpios2set |= LCD_D6; } else { gpios2clear |= LCD_D6; }
	if(data & (1 << 7)) { gpios2set |= LCD_D7; } else { gpios2clear |= LCD_D7; }

	// apply data pins
	gpio_set(LCD_PORT, gpios2set);
	gpio_clear(LCD_PORT, gpios2clear);

	busy_wait(100);

	// set E low (display samples on falling edge)
	gpio_clear(LCD_PORT, LCD_E);

	busy_wait(100);
}
#endif /* LCD_USE_4BIT_MODE */

void lcd_send_init(uint8_t data, enum LCDRegType reg_type)
{
#if LCD_USE_4BIT_MODE
	lcd_send_4bit((data >> 4) & 0x0F, reg_type);
#else
	lcd_send_8bit(data, reg_type);
#endif
}

void lcd_init(void)
{
	lcd_buf_write_pos = 0;
	lcd_buf_read_pos = 0;

	lcd_init_sequence_idx = 0;

	// Set up GPIOs. RCC setup must be done externally!
	gpio_mode_setup(LCD_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,
			LCD_E | LCD_RW | LCD_RS | LCD_D4 | LCD_D5 | LCD_D6 | LCD_D7);
}

void lcd_send(uint8_t data, enum LCDRegType reg_type)
{
#if LCD_USE_4BIT_MODE
	lcd_send_4bit((data >> 4) & 0x0F, reg_type);
	lcd_send_4bit((data >> 0) & 0x0F, reg_type);
#else
	lcd_send_8bit(data, reg_type);
#endif
}

/*!
 * Send next command from the queue.
 */
int lcd_process(void)
{
	if(lcd_buf_read_pos != lcd_buf_write_pos) {
		lcd_send(lcd_cmd_buffer[lcd_buf_read_pos].data,
		         lcd_cmd_buffer[lcd_buf_read_pos].reg_type);

		lcd_buf_read_pos++;
		if(lcd_buf_read_pos >= LCD_QUEUE_LEN) {
			lcd_buf_read_pos = 0;
		}

		return 0; // success
	} else {
		return -1; // queue empty
	}
}

int lcd_enqueue(uint8_t data, enum LCDRegType reg_type)
{
	uint16_t tmp_pos = lcd_buf_write_pos + 1;
	if(tmp_pos >= LCD_QUEUE_LEN) {
		tmp_pos = 0;
	}

	if(tmp_pos != lcd_buf_read_pos) {
		lcd_cmd_buffer[tmp_pos].data = data;
		lcd_cmd_buffer[tmp_pos].reg_type = reg_type;

		lcd_buf_write_pos = tmp_pos;

		return 0; // success
	} else {
		return -1; // queue full
	}
}

void lcd_send_string(char *data)
{
	while(*data) {
		lcd_enqueue((uint8_t)(*data), LCD_REG_DATA);
		data++;
	}
}

void lcd_clear(void)
{
	lcd_enqueue(0x01, LCD_REG_CONTROL);
}

void lcd_set_cursor_pos(uint8_t line, uint8_t col)
{
	lcd_enqueue(0x80 | (line << 6) | col, LCD_REG_CONTROL);
}

/* called in a 1-ms cycle */
int lcd_setup(void)
{
	/* Display INIT:
	 * Power On
	 * sleep(15) minimum
	 * send(0x30, RW = RS = 0) -> command = 0x3X
	 * sleep(4.1) minimum
	 * send(0x30, RW = RS = 0) -> command = 0x33 -> 8-bit mode
	 * sleep(0.1) minimum
	 * send(0x20, RW = RS = 0) -> command = 0x2X -> 4-bit mode active
	 * now write upper 4 bits and lower 4 bits afterwards
	 */
	if(lcd_init_sequence_idx < 38) {
		// init sequence
		switch(lcd_init_sequence_idx) {
			case 19:
				lcd_send_init(0x30, LCD_REG_CONTROL);
				break;
			case 24:
				lcd_send_init(0x30, LCD_REG_CONTROL);
				break;
			case 25:
				lcd_send_init(0x30, LCD_REG_CONTROL);
				break;
#if LCD_USE_4BIT_MODE
			case 26:
				lcd_send_init(0x20, LCD_REG_CONTROL); // switch to 4-bit mode
				break;
#endif
			case 27:
#if LCD_USE_4BIT_MODE
				lcd_send(0x28, LCD_REG_CONTROL); // function set: 4-bit mode, 2 lines, 5x7 font
#else
				lcd_send(0x38, LCD_REG_CONTROL); // function set: 8-bit mode, 2 lines, 5x7 font
#endif
				break;
			case 28:
				lcd_send(0x08, LCD_REG_CONTROL); // display off
				break;
			case 30:
				lcd_send(0x01, LCD_REG_CONTROL); // display clear
				break;
			case 32:
				lcd_send(0x06, LCD_REG_CONTROL); // entry mode set: increment, no shift
				break;
			case 34:
				lcd_send(0x0C, LCD_REG_CONTROL); // display on, cursor off
				break;
			case 36:
				lcd_send(0x02, LCD_REG_CONTROL); // cursor home, needs 1.6 ms to execute
				break;
		}

		lcd_init_sequence_idx++;

		return 0;
	} else {
		return 1;
	}
}