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

#include <ctype.h>
#include <stdarg.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>
#include <macros.h>

#define RECV_BUF_LEN 128

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

static struct usart_console usart[2];

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

	if (!console)
		return;

	while (usart_get_flag(console->dev, USART_SR_RXNE)) {
		console->recv_buf[console->next] = usart_recv(console->dev);

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

		if (i != console->cur)
			console->next = i;
	}
}

void usart1_isr(void)
{
	usart_isr(usart + 0);
}

void usart2_isr(void)
{
	usart_isr(usart + 1);
}

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;
}

int console_peek(char *c, struct console *console_, int block)
{
	struct usart_console *console = container_of(console_,
		struct usart_console, console);

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

	if (console->cur != console->next) {
		*c = console->recv_buf[console->cur];
		return 0;
	}

	return -EAGAIN;
}

int console_getc(char *c, struct console *console_)
{
	struct usart_console *console = container_of(console_,
		struct usart_console, console);

	if (console->cur == console->next)
		return -1;

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

	return 0;
}

int console_getline(struct console *console_, char *buf, size_t n)
{
	struct usart_console *console = container_of(console_,
		struct usart_console, console);
	char *p = buf + strlen(buf);
	char c;

	*p = '\0';

	while (console_getc(&c, console_) == 0) {
		switch (c) {
		case '\r':
			usart_send_blocking(console->dev, '\r');
			usart_send_blocking(console->dev, '\n');
			return 0;
		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 = '\0';

	return -EAGAIN;
}

ssize_t console_read(struct console *console_, char *buf, size_t n)
{
	struct usart_console *console = container_of(console_,
		struct usart_console, console);
	char *p = buf;
	char c;

	*buf = '\0';

	while (console_peek(&c, console_, 1) == 0) {
		switch (c) {
		case '\004':
			if (buf < p)
				goto out;

			console_getc(&c, console_);
			return 0;
		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;
			}

			console_getc(&c, console_);

			break;
		case '\r':
			if (((size_t)p - (size_t)buf) >= n)
				goto out;

			*p++ = '\n';
			usart_send_blocking(console->dev, '\r');
			console_getc(&c, console_);
			goto out;
		default:
			if (((size_t)p - (size_t)buf) >= n)
				goto out;

			*p++ = c;
			usart_send_blocking(console->dev, c);
			console_getc(&c, console_);
			break;
		}
	}

out:
	*p = '\0';

	return p - buf;
}

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)
{
	if (usart_get_irq_no(&console->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_STOPBITS_1);
	usart_set_mode(console->dev, USART_MODE_TX_RX);
	usart_set_flow_control(console->dev, USART_FLOWCONTROL_NONE);

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

	return 0;
}

struct console *console_init(unsigned dev_id)
{
	cookie_io_functions_t fops = {
		.read = usart_read,
		.write = usart_write,
		.seek = NULL,
		.close = NULL,
	};
	struct usart_console *console;

	console = usart + dev_id;

	switch (dev_id) {
	case 0: console->dev = USART1; break;
	case 1: console->dev = USART2; break;
	default: return NULL;
	}

	console->cur = 0;
	console->next = 0;

	usart_init(console);

	console->console.fp = fopencookie(console, "r+w", fops);
	setvbuf(console->console.fp, NULL, _IONBF, 0);

	return &console->console;
}