/*
 * ATmega2560  (Arduino Mega)
 * HC-SR04 Ultrasonic Sensor + Timer5 Input Capture (Interrupt Based)
 * USART0 used for serial output
 *
 * Connections:
 *   HC-SR04  |  Arduino Mega Pin
 *   -----------------------------
 *    VCC     |  5V
 *    GND     |  GND
 *    TRIG    |  Digital Pin 5  (PD5)
 *    ECHO    |  Digital Pin 49 (PL0 / ICP5)
 */

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

#define F_CPU 16000000UL
#define BAUD 9600
#define MYUBRR (F_CPU/16/BAUD - 1)

#define TRIG_PIN PH4          // TRIG = digital 5 (PORTD bit 5)
#define ECHO_PIN_NUM PL1      // ECHO = digital 49 (PORTL bit 0 = ICP5)

// -------------------- USART SETUP --------------------
void USART_Init(unsigned int ubrr) {
    UBRR0H = (unsigned char)(ubrr >> 8);
    UBRR0L = (unsigned char)ubrr;
    UCSR0B = (1 << TXEN0);                 // Enable transmitter
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); // 8-bit frame, 1 stop bit
}

void USART_Transmit(unsigned char data) {
    while (!(UCSR0A & (1 << UDRE0))); // Wait for buffer ready
    UDR0 = data;
}

void USART_SendString(const char* str) {
    while (*str) USART_Transmit(*str++);
}

// -------------------- GLOBAL VARIABLES --------------------
volatile uint16_t rising_edge = 0;
volatile uint16_t falling_edge = 0;
volatile uint8_t capture_done = 0;

// -------------------- TIMER5 INPUT CAPTURE ISR --------------------
ISR(TIMER5_CAPT_vect) {
   PORTB=255;
    
    if ((TCCR5B & (1 << ICES5))) {
   
        // Rising edge detected
        rising_edge = ICR5;
        // Switch to falling edge detection
        TCCR5B &= ~(1 << ICES5);
    } else {
        // Falling edge detected
        falling_edge = ICR5;
        capture_done = 1;                 // Signal measurement complete
        // Stop Timer5 until next trigger
        TCCR5B &= ~((1 << CS52)|(1 << CS51)|(1 << CS50));
    }
}

// -------------------- ULTRASONIC SETUP --------------------
void init_ultrasonic() {
    DDRH |= (1 << TRIG_PIN);             // TRIG as output
    DDRL &= ~(1 << ECHO_PIN_NUM);        // ECHO (ICP5) as input

    // Configure Timer5
    TCCR5A = 0;
    TCCR5B = (1 << ICES5);               // Capture rising edge initially
    TIMSK5 = (1 << ICIE5);               // Enable Input Capture interrupt
    sei();                               // Enable global interrupts
}

// Trigger a single 10 µs ultrasonic pulse
void trigger_pulse() {
    PORTD &= ~(1 << TRIG_PIN);
    _delay_us(2);
    PORTD |= (1 << TRIG_PIN);
    _delay_us(10);
    PORTD &= ~(1 << TRIG_PIN);
}

// Measure echo duration using interrupt-driven capture
uint16_t get_pulse_width_us() {
    capture_done = 0;
    TCCR5B = (1 << ICES5) | (1 << CS51); // Rising edge, prescaler = 8
    TCNT5 = 0;                           // Reset timer counter

    trigger_pulse();                     // Send trigger

    //while (!capture_done);               // Wait until ISR signals done

    uint16_t pulse_ticks = falling_edge - rising_edge;
    // Each tick = 0.5 µs (since prescaler = 8)
    return (pulse_ticks / 2);
}

// Convert echo time to distance (cm)
uint16_t measure_distance_cm() {
    uint16_t time_us = get_pulse_width_us();
    return time_us / 58;                 // Standard formula
}

// -------------------- MAIN PROGRAM --------------------
int main(void) {
    char buffer[16];
    uint16_t distance;
    DDRB=0xff;
    

    USART_Init(MYUBRR);
    init_ultrasonic();

    USART_SendString("HC-SR04 Timer5 Input Capture (Interrupt-Based)\r\n");

    while (1) {
        distance = measure_distance_cm();
        //itoa(distance, buffer, 10);
        sprintf(buffer, "%d", distance);

        USART_SendString("Distance: ");
        USART_SendString(buffer);
        USART_SendString(" cm\r\n");

        _delay_ms(500);
    }
}