#include "UltrasonicSR04.h"
//
void CUltrasonicSR04::Init(int pinout, int pinecho[], int pincount)
{
	this->PinOut = pinout;
	this->PinEcho = pinecho;
	this->PinCount = pincount;
	pinMode(this->PinOut, OUTPUT);
	for (int i = 0; i < this->PinCount; i++)
		pinMode(this->PinEcho[i], INPUT);
}
CUltrasonicSR04::CUltrasonicSR04(int pinout, int pinecho) 
{ 
	this->Init(pinout, new int[1]{pinecho}, 1); 
}
CUltrasonicSR04::CUltrasonicSR04(int pinout, int pinecho[], int pincount) 
{ 
	this->Init(pinout, pinecho, pincount); 
}
CUltrasonicSR04::~CUltrasonicSR04()
{
	~this->PinOut;
	delete[] this->PinEcho;
	~this->PinCount;
}
//
void CUltrasonicSR04::Open(void)
{
	// nothing
}
void CUltrasonicSR04::Close(void)
{
	// nothing
}
//
float CUltrasonicSR04::GetDistance(int n) const
{
	digitalWrite(this->PinOut, LOW);
	delayMicroseconds(2);
	digitalWrite(this->PinOut, HIGH);
	delayMicroseconds(10);
	digitalWrite(this->PinOut, LOW);
	noInterrupts();
	float d = pulseIn(this->PinEcho[n], HIGH, 23529.4); // max sensor dist ~4m
	interrupts();
	return d / 58.8235;
}
float CUltrasonicSR04::GetDistance() const 
{ 
	return this->GetDistance(0); 
}
