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


//timeout of USonic after 38 ms
// TODO
// BUGS
// PD2 (reading FROM US detector) is always high (Voltage detected) 


void initTimer(void){
    TCCR1B |= (1 << CS11) | (1 << CS10); //init timer clock speed of 1MHx /64 >> 64 microseconds/tick
  }


int count(float tick){
  float distance;
  float microsec = tick/32; // how many microseconds passed while held high
  distance = (microsec * 3.44)*0.393701;  //  .344cm/ms = speed of sound .393701 inch per cm
  return distance;

}


int main(void)
{
  //initinterrupt();
  //main function initializations
  DDRB = 0xff; //Data Direction Register B set to output
  DDRD = 0x00; //Data Direction Register D set for input (Redundant/Not necessary initialization)

  //10 US minimum high time for trigger on input
  //The amound of time input is held high (from USonic rangefinder) is proportional to distance
  initUSART();
  initTimer();
  while (1)
  {
    
    _delay_ms(10);
    PORTB = (1 << PB0); // sends output signal to USonic trigger
    _delay_us(10); //mandatory >10 micro second wait (of trigger being high)
    PORTB = ~(1 << PB0); //turns USonic trigger off
    while ((PIND & (1 << PD2)) == 0)
    {
      // wait loop
    }
    if((PIND & (1 << PD2)) != 0)
    {

      float distance;
      int i = 0;
        for (i;(PIND & (1 << PD2)) != 0; i++) // counts how long PD2 isn't low
        {
          
        }
        distance = count(i);
        printString("\r\n");
        printByte(distance);
        printString("\r\n");
    }
    

    _delay_ms(100);
 

}
return 0;
}