#include "distance_sensor.h"

//-----------------------------------------------------------------------------
float Distance_Sensor::GetDistance()
{
	uint32 result = 0;

	//Generate pulse
	*hardwareDDR |= pin;//Set pin output
	*hardwarePort |= pin;//Activate port
	usleep(10);//Wait for 10µs
	*hardwarePort &= ~pin;//Deactivate port
	*hardwareDDR &= ~pin;//Set pin input

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);

	//Wait for response
	for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
	
	//Calculate duration of response
	while((*hardwarePin & pin)&&(result < 300000)) 
	{
		result++;
		asm volatile("nop");
	}

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);

	return (float(result) * DISTANCE_PER_VALUE);
}