#include <socks/console.h>
#include <arch/irq.h>
#include <socks/printk.h>
#include <socks/panic.h>
#include <arch/serial.h>
#include <arch/ports.h>

#define COM1 0x3F8
#define COM2 0x2F8
#define COM3 0x3E8
#define COM4 0x2E8

static int transmit_empty(int device)
{
	return inportb(device + 5) & 0x20;
}

static int serial_received(int device)
{
	return inportb(device + 5) & 0x1;
}

void serial_send_byte(int device, char out)
{
	volatile unsigned int _count = 0;
	while (!transmit_empty(device)) {
		_count++;
	}

	outportb(device, out);

	while (!transmit_empty(device)) {
		_count++;
	}
}

char serial_recv_byte(int device)
{
	volatile unsigned int _count = 0;
	while (!serial_received(device)) {
		_count++;
	}

	char c = inportb(device);

	outportb(device + 5, inportb(device + 5) & ~0x1);

	return c;
}

void serial_putchar(int port, char ch)
{
	if (ch == '\n') {
		serial_send_byte(port, '\r');
	}

	serial_send_byte(port, ch);
}

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

static int get_baud_divisor(int baud)
{
	int freq = 115200;
	int best_baud = -1;
	int best_div = -1;

	for (int i = 1; i < 254; i++) {
		int this_baud = freq / i;

		if (this_baud == baud) {
			return i;
		}

		if (best_baud == -1) {
			best_baud = this_baud;
			best_div = i;
			continue;
		}

		if (this_baud < baud && best_baud > baud) {
			/* TODO pick divisor that gives the closest baud rate */
			return best_div;
		}

		best_baud = this_baud;
		best_div = i;
	}

	return best_div;
}

static void init_serial_port(int port, int baud)
{
	int baud_div = get_baud_divisor(baud);
	outportb(port + 1, 0x00);     // Disable all interrupts
	outportb(port + 3, 0x80);     // Enable DLAB (set baud rate divisor)
	outportb(port + 0, baud_div); // Set divisor
	outportb(port + 1, 0x00);
	outportb(port + 3, 0x03);     // 8 bits, no parity, one stop bit
	outportb(port + 2, 0xC7);     // Enable FIFO, clear them, with 14-byte threshold
	outportb(port + 4, 0x0B);     // IRQs enabled, RTS/DSR set
	outportb(port + 4, 0x1E);     // Set in loopback mode, test the serial chip
	outportb(port + 0, 0xAE);     // Test serial chip (send byte 0xAE and check if serial returns same byte)

	volatile unsigned int q = 0;
	while (!serial_received(port)) {
		q++;
	}

	// Check if serial is faulty (i.e: not same byte as sent)
	if(inportb(port + 0) != 0xAE) {
		return;
	}

	// If serial is not faulty set it in normal operation mode
	// (not-loopback with IRQs enabled and OUT#1 and OUT#2 bits enabled)
	outportb(port + 1, 0x01);
	outportb(port + 4, 0x0F);
	printk("serial: port %x initialised", port);
}

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

static int serial_irq1(void)
{
	if (serial_received(COM1)) {
		unsigned char c = serial_recv_byte(COM1);
		printk("serial: COM1 received %c", c);
	}

	if (serial_received(COM3)) {
		unsigned char c = serial_recv_byte(COM3);
		printk("serial: COM3 received %c", c);
	}

	return 0;
}

static struct irq_hook irq1_hook = {
	.irq_callback = serial_irq1,
};

void serialcon_init(int baud)
{
	hook_irq(IRQ4, &irq1_hook);
	init_serial_port(COM1, baud);
	console_register(&serialcon);
}