#include <avr/interrupt.h>
#include <util/delay.h>
#include "sonar.h"

volatile uint32_t overflow_counter = 0;
volatile uint32_t trig_counter = 0;
volatile uint32_t no_of_cycles = 0;

void init_sonar()
{
    SET(TRIG_DDR, TRIG_BIT);
    CLR(ECHO_DDR, ECHO_BIT);
}

void trigger_sonar()
{
    CLR(TRIG_PORT, TRIG_BIT);
    _delay_us(1);
    SET(TRIG_PORT, TRIG_BIT);
    _delay_us(12);
    CLR(TRIG_PORT, TRIG_BIT);
    _delay_us(1);
}

ISR(TIMER1_OVF_vect)
{
    ++overflow_counter;
    TCNT1 = 0;
}

int read_sonar()
{
    int dist_in_cm = 0;
    init_sonar();
    trigger_sonar();

    // While echo pin is low
    while (!(CHK(ECHO_PIN, ECHO_BIT))) {
        if (++trig_counter > SONAR_TIMEOUT)
            return TRIG_ERROR; // Received no response from echo
    }

    TCNT1 = 0;
    SET(TCCR1B, CS10);
    SET(TIMSK1, TOIE1);
    overflow_counter = 0;
    sei();

    // While echo pin is high
    while (CHK(ECHO_PIN, ECHO_BIT)) {
        if (((overflow_counter * TIMER_MAX) + TCNT1) > SONAR_TIMEOUT)
            return ECHO_ERROR; // No echo within sonar range
    }

    TCCR1B = 0x00;
    cli();
    no_of_cycles = ((overflow_counter * TIMER_MAX) + TCNT1);
    dist_in_cm = (no_of_cycles / (US_PER_CM * CYCLES_PER_US));

    return dist_in_cm;
}