#include <arch/serial.h>
#include <socks/libc/string.h>
#include <socks/libc/ctype.h>
#include <arch/ports.h>
#include <socks/console.h>
#include <socks/vm.h>
#include <socks/printk.h>
#include <stdint.h>

#define VGA_PORT_CMD		0x3D4
#define VGA_PORT_DATA		0x3D5

#define VGA_CHAR(ch, attrib)	((uint16_t)(ch) | ((uint16_t)(attrib) << 8))
#define DEFAULT_ATTRIB		0x07

static uint16_t *g_console_fb = (uint16_t *)(VM_KERNEL_VOFFSET + 0xb8000);
static const unsigned int k_console_width = 80;
static const unsigned int k_console_height = 25;
static unsigned int g_console_cursor_xpos = 0;
static unsigned int g_console_cursor_ypos = 0;

static void init_vga_cursor(void)
{
	unsigned int start = 0, end = 15;
	outportb(VGA_PORT_CMD, 0x0A);
	outportb(VGA_PORT_DATA, (inportb(VGA_PORT_DATA) & 0xC0) | start);

	outportb(VGA_PORT_CMD, 0x0B);
	outportb(VGA_PORT_DATA, (inportb(VGA_PORT_DATA) & 0xE0) | end);
}

static void move_vga_cursor(unsigned int x, unsigned int y)
{
	unsigned int offset = y * k_console_width + x;

	outportb(VGA_PORT_CMD, 0x0F);
	outportb(VGA_PORT_DATA, (uint8_t)(offset & 0xFF));

	outportb(VGA_PORT_CMD, 0x0E);
	outportb(VGA_PORT_DATA, (uint8_t)((offset >> 8) & 0xFF));
}

static void scroll_display()
{
	uint16_t *src = g_console_fb + k_console_width;
	uint16_t *dst = g_console_fb;
	size_t n = k_console_width * (k_console_height - 1) * 2;

	memmove(dst, src, n);

	dst = g_console_fb + ((k_console_height - 1) * k_console_width);

	for (int i = 0; i < k_console_width; i++) {
		dst[i] = VGA_CHAR(0, DEFAULT_ATTRIB);
	}
}

static void handle_ctrl(int c)
{
	switch (c) {
	case '\n':
		g_console_cursor_xpos = 0;
		g_console_cursor_ypos++;

		if (g_console_cursor_ypos >= k_console_height) {
			scroll_display();
			g_console_cursor_ypos = k_console_height - 1;
		}
		break;
	default:
		break;
	}

	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);
}

static void vgacon_putchar(int c)
{
	//serial_putchar(SERIAL_PORT_A, c);

	if (iscntrl(c)) {
		handle_ctrl(c);
		return;
	}

	g_console_fb[(g_console_cursor_ypos * k_console_width) + g_console_cursor_xpos] = VGA_CHAR(c, DEFAULT_ATTRIB);

	g_console_cursor_xpos++;

	if (g_console_cursor_xpos >= k_console_width) {
		g_console_cursor_xpos = 0;
		g_console_cursor_ypos++;
	}

	if (g_console_cursor_ypos >= k_console_height) {
		scroll_display();
		g_console_cursor_ypos = k_console_height - 1;
		g_console_cursor_xpos = 0;
	}

	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);
}

static void vgacon_write(struct console *con, const char *s, unsigned int len)
{
	for (unsigned int i = 0; i < len; i++) {
		vgacon_putchar(s[i]);
	}
}

static struct console vgacon = {
	.c_name = "vgacon",
	.c_flags = CON_BOOT,
	.c_write = vgacon_write,
	.c_lock = SPIN_LOCK_INIT,
};

void vgacon_init(void)
{
	g_console_cursor_xpos = 0;
	g_console_cursor_ypos = 0;

	for (int i = 0; i < k_console_width * k_console_height; i++) {
		g_console_fb[i] = DEFAULT_ATTRIB << 8;
	}

	init_vga_cursor();
	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);

	console_register(&vgacon);
	early_printk_init(&vgacon);
}