#define _GNU_SOURCE
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/usart.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>

static ssize_t usart_read(void *cookie, char *buf, size_t n)
{
	uint32_t dev = (uint32_t)cookie;
	size_t i;

	for (i = 0; i < n; ++i) {
		buf[i] = (char)usart_recv_blocking(dev);

		if (buf[i] == '\r') {
			buf[i++] = '\n';
			putchar('\n');
			break;
		}

		putchar(buf[i]);
	}

	return i;
}

static ssize_t usart_write(void *cookie, const char *buf, size_t n)
{
	uint32_t dev = (uint32_t)cookie;
	size_t i;

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

		usart_send_blocking(dev, buf[i]);
	};

	return i;
}

static void usart_init(void)
{
	/* Set up clocks for USART 1 */
	rcc_periph_clock_enable(RCC_GPIOA);
	rcc_periph_clock_enable(RCC_USART1);

	/* Set up GPIOs for USART 1 */
	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9 | GPIO10);
	gpio_set_af(GPIOA, GPIO_AF1, GPIO9 | GPIO10);
	gpio_set_af(GPIOA, GPIO_AF1, GPIO10);

	usart_set_baudrate(USART1, 115200);
	usart_set_databits(USART1, 8);
	usart_set_parity(USART1, USART_PARITY_NONE);
	usart_set_stopbits(USART1, USART_CR2_STOP_1_0BIT);
	usart_set_mode(USART1, USART_MODE_TX_RX);
	usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE);

	usart_enable(USART1);
}

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

	usart_init();

	stdin = fopencookie((void *)USART1, "r", console_in);
	stdout = fopencookie((void *)USART1, "w", console_out);
	stderr = fopencookie((void *)USART1, "w", console_out);

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