#define _GNU_SOURCE
#include <libopencm3/stm32/usart.h>
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/scb.h>

#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <stddef.h>
#include <limits.h>
#include <sys/types.h>

#include <console.h>

#define RECV_BUF_LEN 128

struct usart_console {
	char recv_buf[RECV_BUF_LEN];
	volatile size_t cur, next;
	uint32_t dev;
};

struct usart_console *usart1 = NULL;
struct usart_console user_console;

static void usart_isr(struct usart_console *console)
{
	uint32_t reg;
	size_t i;

	if (!console)
		return;

	do {
		reg = USART_ISR(console->dev);

		if (!(reg & USART_ISR_RXNE))
			continue;

		console->recv_buf[console->next] = usart_recv(console->dev);

		if (console->recv_buf[console->next] == '\003')
			scb_reset_system();

		i = (console->next + 1) % RECV_BUF_LEN;

		if (i != console->cur)
			console->next = i;
	} while (usart_get_flag(console->dev, USART_ISR_RXNE));
}

void usart1_isr(void)
{
	usart_isr(usart1);
}

static char usart_getc(struct usart_console *console, int block)
{
	char c = 0;

	while (block && (console->cur == console->next));

	if (console->cur != console->next) {
		c = console->recv_buf[console->cur];
		console->cur = (console->cur + 1) % RECV_BUF_LEN;
	}

	return c;
}

static ssize_t usart_read(void *cookie, char *buf, size_t n)
{
	struct usart_console *console = cookie;
	char *p = buf;
	char c;

	*buf = '\0';

	while ((c = usart_getc(console, 1)) != '\r') {
		switch (c) {
		case 10:
		case 127:
			if (buf < p) {
				usart_send_blocking(console->dev, '\010');
				usart_send_blocking(console->dev, ' ');
				usart_send_blocking(console->dev, '\010');
				--p;
			}

			break;
		default:
			*p = c;
			usart_send_blocking(console->dev, c);

			if (((size_t)p - (size_t)buf) < n) {
				++p;
			}
		}

		*p = '\0';
	}

	*p = '\n';

	if (((size_t)p - (size_t)buf) < n) {
		++p;
	}

	*p = '\0';

	return p - buf;
}

static ssize_t usart_write(void *cookie, const char *buf, size_t n)
{
	struct usart_console *console = cookie;
	size_t i;

	for (i = 0; i < n; ++i) {
		if (buf[i] == '\n')
			usart_send_blocking(console->dev, '\r');

		usart_send_blocking(console->dev, buf[i]);
	};

	return i;
}

static int usart_get_irq_no(unsigned *irq_no, unsigned dev)
{
	if (irq_no)
		return -1;

	switch (dev) {
	case USART1: *irq_no = NVIC_USART1_IRQ; break;
	case USART2: *irq_no = NVIC_USART2_IRQ; break;
	default: return -1;
	}

	return 0;
}

static int usart_init(struct usart_console *console)
{
	unsigned irq_no;

	if (usart_get_irq_no(&irq_no, console->dev) < 0)
		return -1;

	usart_set_baudrate(console->dev, 115200);
	usart_set_databits(console->dev, 8);
	usart_set_parity(console->dev, USART_PARITY_NONE);
	usart_set_stopbits(console->dev, USART_CR2_STOP_1_0BIT);
	usart_set_mode(console->dev, USART_MODE_TX_RX);
	usart_set_flow_control(console->dev, USART_FLOWCONTROL_NONE);

	nvic_enable_irq(irq_no);
	usart_enable(console->dev);
	usart_enable_rx_interrupt(console->dev);

	return 0;
}

void console_init(void)
{
	cookie_io_functions_t console_in = {
		.read = usart_read,
		.write = NULL,
		.seek = NULL,
		.close = NULL,
	};
	cookie_io_functions_t console_out = {
		.read = NULL,
		.write = usart_write,
		.seek = NULL,
		.close = NULL,
	};

	user_console.dev = USART1;
	user_console.cur = 0;
	user_console.next = 0;
	usart1 = &user_console;
	usart_init(&user_console);

	stdin = fopencookie(&user_console, "r", console_in);
	stdout = fopencookie(&user_console, "w", console_out);
	stderr = fopencookie(&user_console, "w", console_out);

	setbuf(stdout, NULL);
	//setlinebuf(stdout);
	setbuf(stderr, NULL);
}