#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
	while(!(PINC & pin)){asm volatile("nop");}

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

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

	return (float(result) * DISTANCE_PER_VALUE);
}