#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define TRIG_PIN PD2
#define ECHO_PIN PD3

/* Intialize the uart */
void uart_init(void)
{
    UBRR0H = 0;
    UBRR0L = 16;  // Baud rate 115200 at 16MHz
    UCSR0B = (1 << TXEN0);  // Enable TX
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);  // 8-bit data
}

/* Send character via uart */
void uart_tx(char c)
{
    while (!(UCSR0A & (1 << UDRE0)));  // Wait for buffer
    UDR0 = c; // Send character
}

/* Send string via uart */
void uart_print(const char* str)
{
    while (*str)
        uart_tx(*str++);
}

/* Measure distance with sensor */
float measure()
{
    PORTD |= (1 << TRIG_PIN);
    _delay_us(10);
    PORTD &= ~(1 << TRIG_PIN);

    TCNT1 = 0;  // Reset Timer1
    TCCR1B = (1 << CS11);  // Start Timer1 with prescaler 8

    while (!(PIND & (1 << ECHO_PIN)));  // Wait for Echo HIGH
    TCNT1 = 0;  // Reset Timer1 again
    while (PIND & (1 << ECHO_PIN));  // Wait for Echo LOW

    TCCR1B = 0;  // Stop Timer1
    return ((float)TCNT1 / 2.0) * 0.0343 / 2.0;;
}

int main()
{
    char buf[32]; // Buffer for the message to send
    uart_init(); // Initialize uart

    DDRD |= (1 << TRIG_PIN);  // Trig as output
    DDRD &= ~(1 << ECHO_PIN); // Echo as input

    // Loop forever
    while (1)
    {
        // Get measurement
        float cm = measure();

        // Convert to string
        sprintf(buf, "Dist: %d cm\r\n", (uint16_t)cm);
        // Send message via uart
        uart_print(buf);

        // Wait 500ms
        _delay_ms(500);
    }
}