#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 "usart.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';
			++i;
			break;
		}
	}

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

FILE *init_usart(uint32_t dev)
{
	cookie_io_functions_t stub = { usart_read, usart_write, NULL, NULL };
	FILE *fp;

	/* 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(dev, 115200);
	usart_set_databits(dev, 8);
	usart_set_parity(dev, USART_PARITY_NONE);
	usart_set_stopbits(dev, USART_CR2_STOP_1_0BIT);
	usart_set_mode(dev, USART_MODE_TX_RX);
	usart_set_flow_control(dev, USART_FLOWCONTROL_NONE);

	usart_enable(dev);

	fp = fopencookie((void *)dev, "rw+", stub);
	setvbuf(fp, NULL, _IONBF, 0);
	return fp;
}