#include "uart.h"
#include <avr/io.h>
#include <util/delay.h>
#include <stdarg.h> // va_list, va_start, va_end
#include <stdlib.h> // itoa

// 9600 seems to be the highest the ATmega328P can handle in this config
#define BAUD 9600  
#include <util/setbaud.h>

void initUART() {
    // Set baud rate with precalculated values (see setbaud.h)
    UBRR0H = UBRRH_VALUE;
    UBRR0L = UBRRL_VALUE;
    // Enable receiver and transmitter
    UCSR0B |= (1 << RXEN0) | (1 << TXEN0);
    // Set frame format: 8 data, 1 stop bit
    UCSR0C |= (1 << UCSZ01) | (1 << UCSZ00);
}

void UART_transmit(uint8_t data) {
    // Wait for empty transmit buffer
    while (!(UCSR0A & (1 << UDRE0)));
    // Put data into buffer, sends the data
    UDR0 = data;
}

void UART_println(const char* format, ...) {
    va_list args;
    va_start(args, format);

    // Iterate through the variadic arguments
    while (*format != '\0') {
        if (*format == '%') {
            format++;
            if (*format == 'd') {
                int i = va_arg(args, int);
                char buffer[12]; // Increase buffer size to accommodate integers and null terminator
                itoa(i, buffer, 10);
                // Transmit the string representation of the integer
                for (int j = 0; buffer[j] != '\0'; j++) {
                    UART_transmit(buffer[j]);
                }
            } else if (*format == 's') {
                char *s = va_arg(args, char *);
                // Transmit each character of the string
                while (*s != '\0') {
                    UART_transmit(*s++);
                }
            } else if (*format == 'c') {
                int c = va_arg(args, int);
                // Transmit the character
                UART_transmit(c);
            }
        } else {
            // Transmit the character
            UART_transmit(*format);
        }
        // Move to the next format specifier or character
        format++;
    }

    // Transmit carriage return and line feed characters
    UART_transmit('\r');
    UART_transmit('\n');

    va_end(args);
}