#define F_CPU 16000000UL
#include <avr/delay.h>

#define TRIG_PIN PB4
#define ECHO_PIN PB5

uint32_t start_time = 0;
volatile uint32_t overflow_count = 0;

int main(){
  pinMode(12, OUTPUT);
  Serial.begin(9600);
  TCCR2A = 0;
  TCCR2B = 0;

  // PRESCALER 128
  TCCR2B = ((1 << CS22) | (1 << CS20));
  TIMSK2 = (1 << TOIE2);
  sei();

  while(1){

    //timerStart();
    //_delay_ms(100);
    //uint32_t duration = timerStop();
    //Serial.println(duration);
    //timerStart();

    ULTRASONIC_SendPulse();
    //ULTRASONIC_WaitForPulseReturn(calculateRuntimeMicros());
    //Serial.println(timerStop());

    if((PIND & (1 << PD3))){
      delayMicroseconds(10);
      timerStart();
      Serial.println("PRESSED");
      while (PIND & (1 << PD3)){;}
      Serial.print(timerStop());
    };
  }
  return 0;
}

uint32_t calculateRuntimeMicros() {
	uint8_t timer2_counter;
	
	cli();
	timer2_counter = TCNT2;
	sei();
	
	uint16_t micros_per_overflow = 2048;
	uint8_t micros_per_tick = 8;

	return (overflow_count * micros_per_overflow) + (timer2_counter * micros_per_tick);
  }

void timerStart(){
    overflow_count = 0;
    TCNT2 = 0;

    start_time = calculateRuntimeMicros();
}

uint32_t timerStop(){
    uint32_t stop_time = calculateRuntimeMicros();
    uint32_t duration = (stop_time - start_time);

    return duration;
}

ISR(TIMER2_OVF_vect){
	overflow_count++;
}

void ULTRASONIC_SendPulse(void){
    PORTB &= ~(1 << TRIG_PIN);
    _delay_us(5);
    PORTB |= (1 << TRIG_PIN);
    _delay_us(10);
    PORTB &= ~(1 << TRIG_PIN);

}

void ULTRASONIC_WaitForPulseReturn(long start){
    long current_time, duration = 0;
    bool ECHO_PIN_LOW = !(PINB & (1 << ECHO_PIN));

    while(ECHO_PIN_LOW){
        current_time = micros();
        duration = current_time - start;

        // TIMEOUT, NO ECHO RECEIVED
        if(duration > 100){
            return -1;
        }
    };
}