#define F_CPU 16000000UL

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

#ifdef __GNUC__
#pragma GCC poison putchar vfprintf
//#pragma GCC optimize("O3,inline")           // "inline" won't happen without it
#pragma GCC optimize("O3,fast-math,inline")   // "fast-math" helps auto-vectorize loops
//#pragma GCC optimize("Ofast,inline")        // "Ofast" = "O3,fast-math,allow-store-data-races,no-protect-parens"
#endif

volatile uint16_t pulse_width = 0;
volatile uint8_t new_measurement_available = 0;

// ISR para a interrupção externa INT0
ISR(INT0_vect)
{
    static uint16_t rising_edge_time = 0;

    if (PIND & (1<<PIND2)) // Verifica se é uma borda de subida
    {
        TCNT1 = 0; // Zera o contador
        rising_edge_time = TCNT1; // Armazena o tempo da borda de subida
    }
    else // Caso contrário, é uma borda de descida
    {
        uint16_t falling_edge_time = TCNT1; // Armazena o tempo da borda de descida
        pulse_width = falling_edge_time - rising_edge_time; // Calcula a largura do pulso
        new_measurement_available = 1; // Seta a flag indicando que uma nova medição está disponível
    }
}

void servo_init() {
    DDRB |= (1 << DDB1); // PB1 como saída (OC1A)
    TCCR1A |= (1 << COM1A1) | (1 << WGM11); // Modo Fast PWM, não inverso
    TCCR1B |= (1 << WGM12) | (1 << WGM13) | (1 << CS11); // Modo Fast PWM, prescaler de 8
    ICR1 = 20000; // TOP, para uma frequência de 50Hz
}

void servo_write(uint16_t angle) {
    if (angle > 180) angle = 180; // Limita o ângulo a 180 graus

    // Calcula o valor de OCR1A baseado no ângulo
    // Mapeia 0-180 graus para 1000-2000 (largura do pulso em µs)
    OCR1A = (angle * 10) + 2000; 
}

void init_ports(void)
{
    DDRD &= ~(1<<PIND2); // Configura o pino do echo (PD2/INT0) como entrada
    DDRD |= (1<<PIND3); // Configura o pino do trigger (PD3) como saída
}

void init_interrupts(void)
{
    EIMSK |= (1<<INT0); // Habilita a interrupção externa INT0
    EICRA |= (1<<ISC00); // Configura a interrupção para ser ativada em qualquer mudança lógica
    sei(); // Habilita interrupções globais
}

void trigger_sensor(void)
{
    PORTD |= (1<<PIND3); // Seta o pino do trigger como HIGH
    _delay_us(10); // Espera 10 microssegundos
    PORTD &= ~(1<<PIND3); // Reseta o pino do trigger para LOW
}

int main(void)
{
    init_ports();
    init_interrupts();

    while (1)
    {
        trigger_sensor(); // Inicia uma medição
        _delay_ms(60); // Espera um tempo antes da próxima medição
        
        if (new_measurement_available) // Se uma nova medição estiver disponível
        {
            uint16_t distance = pulse_width / 58; // Calcula a distância em cm
            printf("Distancia: %d cm\n", distance);
            new_measurement_available = 0; // Reseta a flag
        }
    }
}