//Ping sensor
//By Emil.fi
//adapted for m8 - plpg
#include <avr/io.h>
#include <avr/interrupt.h>

void uart_write(unsigned char c)
{
	while(!(UCSRA&(1<<UDRE))); //wait until uart is ready
	UDR=c; //send c out of uart
}

void uart_write_du16(unsigned int n) //Unsigned int as decimal printing
{
	unsigned char i=0, m=0;
	unsigned int k[5]={10000, 1000, 100, 10, 1};
	while(m<5)
	{
		i=0;
		while(n>(k[m]-1))
		{
			n-=k[m];
			i++;
		}
		uart_write('0'+i);
		m++;
	}
}

void gpio_setup(void)
{
	DDRD|=(1<<PD4); //PD7 as output
	PORTD&=~(1<<PD4); //PD7 to ground
	DDRD&=~(1<<PD2); //PB0 as input
	PORTD&=~(1<<PD2); //PB0 no pullup
}

void uart_setup(void)
{
  /*	UCSRA=(1<<U2X); //Double speed uart
	UCSRB=(1<<TXEN); //Enable tx circuitry.
	UCSRC=(1<<UCSZ1)|(1<<UCSZ0); //Select 8-bit data
	UBRRL=1; //Select 1MBaud speed  */
  UBRRH = 0;
  UBRRL = 12; //baudrate 9600 @ 1M clock

  /* enable 2x speed mode */
  UCSRA |= (1<<U2X);
  /* Enable receiver and transmitter */
  UCSRB = (1<<RXEN)|(1<<TXEN);
  /* Set frame format: 8data, 1stop bit */
  UCSRC = (1<<URSEL)|(3<<UCSZ0);
}

void timer_setup(void)
{
	TCNT1=0; //Clear TIMER1 value
	OCR1A=9; //5us when TIMER1 is divided by 8
	TIMSK=(1<<OCIE1A); //Enable compare match interrupt
	TCCR1B=(1<<ICES1)|(1<<CS11); //Trigger input capture on rising edge, divide TIMER1 by 8 and start TIMER1
}

ISR(TIMER1_COMPA_vect)
{
	if(PORTD&(1<<PD4)) //Test if query pulse on
	{
		PORTD&=~(1<<PD4); //End query pulse
		TIMSK=(1<<TICIE1); //Disable compare match interrupt and enable input capture interrupt
	}
	else //Query pulse not started
	{
		PORTD|=(1<<PD4); //Start query pulse
	}
	TCNT1=0; //Clear TIMER1 value
}

ISR(TIMER1_CAPT_vect)
{
	TCCR1B&=~(1<<CS11); //Stop TIMER1
	if(PINB&(1<<PB0)) //Test if response pulse started
	{
		TCCR1B&=~(1<<ICES1); //Next timer trigger on falling edge
	}
	else //Response pulse ended
	{
		uart_write_du16(ICR1); //Print ICR1 value
		uart_write('\n'); //Change line
		uart_write('\r');
		TIMSK=(1<<OCIE1A); //Disable input capture interrupt and enable compare match interrupt
		TCCR1B|=(1<<ICES1); //Next timer trigger on rising edge
	}
	TCNT1=0; //Clear TIMER1 value
	TCCR1B|=(1<<CS11); //Start TIMER1
}

int main(void)
{
	gpio_setup(); //Configure gpio
	uart_setup(); //Configure uart
	timer_setup(); //Configure timer
	sei(); //Enable interrupts
	while(1); //Wait forever
	return(0); //Magic
}