//
// hello.HC-SR04.c
//
// HC-SR04 sonar hello-world
//    9600 baud FTDI interface
//
// Neil Gershenfeld 11/15/15
// (c) Massachusetts Institute of Technology 2015
//
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose. Copyright is
// retained and must be preserved. The work is provided
// as is; no warranty is provided, and users accept all 
// liability.
//

#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
#define bit_delay_time 102 // bit delay for 9600 with overhead
#define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay
#define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay
#define char_delay() _delay_ms(10) // char delay

#define trigger_port PORTB
#define trigger_direction DDRB
#define trigger_pin (1 << PB5)
#define echo_pins PINB
#define echo_direction DDRB
#define echo_pin (1 << PB6)

#define timeout 255

int time_count=0;


void setup() {
  
     
   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize output pins
   //
   Serial.begin(9600);
   
   clear(trigger_port,trigger_pin);
   output(trigger_direction,trigger_pin);
   //
   // start counter
   //
   TCCR0B |= (1 << CS00); // prescale /1

}

void loop() {
  
      static unsigned char high,low;
      // trigger pulse
      //
      set(trigger_port,trigger_pin);
      _delay_us(10);
      clear(trigger_port,trigger_pin);  
      //
      // wait for echo rising edge
      //
      high = 0;
      TCNT0 = 0;
      TIFR0 |= (1 << TOV0);
      while (1) {
         if ((echo_pins & echo_pin) != 0) // check for rising edge
            break;
         if ((TIFR0 & (1 << TOV0)) != 0) { // check for counter overflow
            high += 1;
            if (high == timeout)
               break;
            TIFR0 |= (1 << TOV0);
            }
         }
      //
      // rising edge found, wait for falling edge
      //
      high = 0;
      TCNT0 = 0;
      TIFR0 |= (1 << TOV0);
      while (1) {
         if ((echo_pins & echo_pin) == 0) { // check for falling edge
            low = TCNT0;
            break;
            }
         if ((TIFR0 & (1 << TOV0)) != 0) { // check for counter overflow
            high += 1;
            if (high == timeout)
               break;
            TIFR0 |= (1 << TOV0);
            }
         }
      //
      // send count with framing
      //
      time_count++;
      Serial.print("Time: ");
      Serial.println(time_count);
      
      Serial.print("LOW: ");
      Serial.println(low);
      
      Serial.print("HIGH: ");
      Serial.println(high);
      Serial.println(" ");
      
      //
      // delay before next cycle
      //
      _delay_ms(1000);

}