#define F_CPU 16000000
#define USART_BAUDRATE 115200
#define NUMBER_OF_DIGITS 16

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

//HC-SR04 sensor -> Arduino UNO
#define TRIGGER_PIN     PIND3           // arduino PIN 3.
#define ECHO_PIN        PINB0           // arduino PIN 8. (ICP1)
#define LED_13          PINB5           // arduino PIN 13 Led on board.
#define echo_On         PINB & 0x01

volatile uint16_t TCNT1_copy;
volatile uint8_t echoOff;

uint8_t string [50];                        // usart buff
uint16_t distance[10];
uint8_t nDelay;                             //
uint32_t average;

void uitoa(unsigned int value, char* string, int radix);
void ping(void);

ISR(TIMER0_OVF_vect)
{
        // send request Echo to HCSR04
        PORTD &= ~(1 << TRIGGER_PIN);       // Trigger Pin Low
        _delay_us(1);
        PORTD |= (1 << TRIGGER_PIN);        // Trigger Pin high
        _delay_us(10);                      // min 10us ( datasheet)
        PORTD &= ~(1 << TRIGGER_PIN);       // Trigger Pin low ( pulse) -> HCSR04 outputs(modulate) 8 cycles@40Khz ( 200us) (datasheet)
}
ISR(TIMER1_CAPT_vect)
{
    if (echo_On)                            //Test echo input pin ( Arduino)
      {
        TIMSK0 &= ~(1 << TOIE0);
        TCNT1 = 0;                          //clear counter
        TCCR1B &= ~(1 << ICES1);            //invert edge select ( falling)
      }
    else
      {
        TCNT1_copy = ICR1;                  //save steps
        TCCR1B |=  (1 << ICES1);            //invert edge select ( rising)
        TCNT0 = 0;
        TIMSK0 |= (1 << TOIE0);             // Start tmr0 to make 64ms delay between echo (datasheet)
      }
}
int main (void)
{
    DDRB = 0;                               // Clear
    DDRD = 0;                               //
    PORTB = 0;                              //
    TCCR1B = 0;                             //
    TIMSK1 = 0;                             //
    cli();                                  // Clear global interrupt

    DDRB |=  (1 << LED_13);                 // led_13 set as output.
    DDRB &= ~(1 << ECHO_PIN);               // ECHO_PIN = PINB0(ICP1) set as input.
    DDRD |=  (1 << TRIGGER_PIN);            // Sonar trigger PIND3 set as output.

    TCCR0A = 0;                             // Normal mode (counter) port operation.
    TCCR0B = 0;                             // Normal mode (counter) port operation.
    TCCR0B |= (1 << CS01) | (1 << CS00);    // TMR0 prescaler 1/64 -> overflow 64ms@16Mhz

    TCCR1A = 0;                             // Normal mode (counter) port operation.
    TCCR1B = 0;                             // Normal mode (counter) port operation.
    TCCR1B |= (0 << ICNC1) | (1 << ICES1) \
           | (1 << CS10) | (1 << CS11);     // Echo cancelling ( add 4cyc before to copy icr1 value to tcnt1), RISING EDGE,  Prescaler -> 16:64 tick = 4us
    TCCR1C = 0;                             // FOC1A/FOC1B inactive
    TIFR1 |= (1 << TOV1) | ( 1 << ICF1);    // clear any flag occurred

    USART_puts("Hello !");
    USART_puts("\n");
    sei();
    TIMSK0 &= ~(1 << TOIE0);                // overflow interrupt tmr0 OFF
    TIMSK1 |= (1 << ICIE1) | (1 << TOV1);   // Input capture ON
    TCNT1 = 0;                              // clear counter tmr1.

    ping();                                 // First ping

    for(;;)
     {
                do  //  8 sample
                {
                    average+= distance[nDelay = (nDelay + 1 == 9 ? 0: nDelay + 1)] = (TCNT1_copy << 2) /58;
                } while ( !(nDelay & (1 << 3)));

                average >>=3;               // sample/8
                uitoa(average,string,10);
                USART_puts(string);         // print average value
                USART_puts("\n");
    }
    return 0;
}
void ping (void)
{
    // send request Echo to HCSR04
    PORTD &= ~(1 << TRIGGER_PIN);           // Trigger Pin Low
    _delay_us(1);
    PORTD |= (1 << TRIGGER_PIN);            // Trigger Pin high
    _delay_us(10);                          // min 10us
    PORTD &= ~(1 << TRIGGER_PIN);           // Trigger Pin low ( pulse) -> HCSR04 outputs(modulate) 8 cycles@40Khz ( 200us)
}