#include "lcd.h"
#include "log.h"
#include <fcntl.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>

#define MIN(a, b) ((a) < (b) ? (a) : (b))

static int lcd_open(struct lcd_t *lcd, const bool reopen) {
	applog(LOG_INF, "Opening serial port at %s", lcd->port);
	if (lcd->fd <= 0 || reopen)
		lcd->fd = open(lcd->port, O_WRONLY | O_NOCTTY);
	if (lcd->fd <= 0) {
		applog(LOG_ERR, "Error opening serial port %s", lcd->port); // err
		return -1;
	}
	struct termios term = {};
	term.c_cflag = lcd->baud | CS8 | CLOCAL | CREAD;
	tcflush(lcd->fd, TCIFLUSH);
	tcsetattr(lcd->fd, TCSANOW, &term);
	return 0;
}

void lcd_close(struct lcd_t *lcd) {
	if (lcd->fd >= 0) {
		close(lcd->fd);
		applog(LOG_INF, "LCD port closed");
	}
	lcd->fd = -1;
}

static void lcd_write(struct lcd_t *lcd, const uint8_t *cmdbuf, const uint16_t len) {
	if (write(lcd->fd, cmdbuf, len) <= 0) {
		applog(LOG_ERR, "failed to execute write(), lost access to port?");
		lcd_close(lcd);
		lcd_open(lcd, true);
	}
}

void lcd_reinit(struct lcd_t *lcd) {
    constexpr uint8_t cmdbuf[] = {0xFE, 63};
    lcd_write(lcd, cmdbuf, sizeof(cmdbuf));
}

void lcd_splash(struct lcd_t *lcd) {
    constexpr uint8_t cmdbuf[] = {0xFE, 65};
    lcd_write(lcd, cmdbuf, sizeof(cmdbuf));
}

void lcd_clear(struct lcd_t *lcd) {
	constexpr uint8_t cmdbuf[] = {0xFE, 88};
	lcd_write(lcd, cmdbuf, sizeof(cmdbuf));
}

void lcd_set_contrast(struct lcd_t *lcd, const uint8_t contrast) {
	applog(LOG_INF, "Set contrast to %d", contrast);
	const uint8_t cmdbuf[] = {0xFE, 80, contrast};
	lcd_write(lcd, cmdbuf, sizeof(cmdbuf));
}

void lcd_set_backlight(struct lcd_t *lcd, const bool enabled) {
	applog(LOG_INF, "Set backlight to %s", enabled ? "true" : "false");
	const uint8_t cmdbuf[] = {0xFE, (enabled ? 66 : 70), 255};
	lcd_write(lcd, cmdbuf, enabled ? 3 : 2);
}

void lcd_set_brightness(struct lcd_t *lcd, const uint8_t brightness) {
    applog(LOG_INF, "Set backlight brightness to %d", brightness);
    const uint8_t cmdbuf[] = {0xFE, 152, brightness};
    lcd_write(lcd, cmdbuf, 3);
}

void lcd_set_cursor(struct lcd_t *lcd, const uint8_t row, const uint8_t col) {
	const uint8_t cmdbuf[] = {0xFE, 71, col + 1, row + 1};
	lcd_write(lcd, cmdbuf, sizeof(cmdbuf));
}

void lcd_set_custom_char(struct lcd_t *lcd, const uint8_t index, const lcd_glyph_t *glyph) {
	memcpy(lcd->writebuf.cchar[index] + 3, glyph, sizeof(lcd_glyph_t)); // we prepare the command in init, so we can just copy the glyph after the command
}

/* writes string to write buffer at calculated pos, we trust it to be null terminated */
void lcd_write_buf(struct lcd_t *lcd, const uint8_t row, const uint8_t col, const char *str) {
	size_t len = 0;
	while (str[len] != '\0')
		len++;

	len = MIN(len, sizeof(lcd->writebuf) - col);
	int pos = 4 + col + (LCD_COLS * (row % 2)); // skip 4 bytes to leave room for set cursor command
	if (row >= 2)
		pos += 2 * LCD_COLS + 4; // if we're writing to disp2, skip another 4 bytes for set cursor command and 2*40 bytes of disp 1
	memcpy(lcd->writebuf.text + pos, str, len);
}

/* hyperoptimized function to write a formatted string to lcd buffer. */
/* we cant just snprintf to the buffer directly bc it null terminates */
void lcd_writef(struct lcd_t *lcd, const uint8_t x, const uint8_t y, const char *fmt, ...) {
	char tmp[LCD_COLS + 1];
	va_list ap;
	va_start(ap, fmt);
	int len = vsnprintf(tmp, sizeof(tmp), fmt, ap); // print fmt and ap to a temporary buffer
	va_end(ap);
	len = MIN(len, LCD_COLS - x);
	int pos = 4 + x + (LCD_COLS * (y % 2)); // skip 4 bytes to leave room for set cursor command
	if (y >= 2)
		pos += 2 * LCD_COLS + 4;       // if we're writing to disp2, skip another 4 bytes for set cursor command and 2*40 bytes of disp 1
	memcpy(lcd->writebuf.text + pos, tmp, (size_t)len); // copy to lcd writebuffer address + whatever pos is
}

inline void lcd_clear_buf(struct lcd_t *lcd) {
	memset(lcd->writebuf.text, ' ', sizeof(lcd->writebuf.text));
	// prepare commands for custom chars
	for (uint8_t i = 0; i < 8; i++) {
		const uint8_t charbuf[] = {0xFE, 78, i};
		memcpy(lcd->writebuf.cchar[i], charbuf, sizeof(charbuf));
	}
}


/* flushes writebuffer to lcd, hyperoptmized, flushes buffer and commands in a single write operation */
void lcd_flush_buf(struct lcd_t *lcd) {
	constexpr uint8_t set_disp[] = {0xFE, 71, 1, 1}; // set cursor to char 1, line 1
	memcpy(&lcd->writebuf.text[0], set_disp, 4);
	memcpy(&lcd->writebuf.text[2 * LCD_COLS + 4], set_disp, 4);
	lcd->writebuf.text[2 * LCD_COLS + 7] = 3; // overwrite set cursor c1 l1 to c1 l3
	lcd_write(lcd, (const uint8_t *)&lcd->writebuf, sizeof(lcd->writebuf));
}

// i have a whole flushbuf func to avoid too many writes, but im writing on every bar, need to rethink this.
// we could have a separate pre-char buffer and put all the setCustomChar commands to that?
inline char lcd_bar(struct lcd_t *lcd, const uint8_t char_id, const double value) {
	uint8_t b[8];
	memset(b, 0b01110, sizeof(b));
	for (uint8_t i = 0; i < (1.0 - value) * 8; i++)
		b[i] = 0;
	lcd_set_custom_char(lcd, char_id, &b);
	return (char)char_id;
}

int lcd_init(struct lcd_t *lcd) {
	const int ret = lcd_open(lcd, false);
	if (ret < 0) {
		lcd_close(lcd);
		return -1;
	}

	lcd_clear_buf(lcd);
	lcd_reinit(lcd);
	return 0;
}