#include <Servo.h>

double distEntrada;
double distSalida;
double plazasLibres;
Servo servo_5;
Servo servo_6;
unsigned long task_time_ms=0;

unsigned long task_time_ms2=0;

double fnc_ultrasonic_distance(int _t, int _e){
	unsigned long dur=0;
	digitalWrite(_t, LOW);
	delayMicroseconds(5);
	digitalWrite(_t, HIGH);
	delayMicroseconds(10);
	digitalWrite(_t, LOW);
	dur = pulseIn(_e, HIGH, 18000);
	if(dur==0)return 999.0;
	return (dur/57);
}

void setup()
{
  	servo_5.attach(5);
	servo_6.attach(6);
	pinMode(2, OUTPUT);
	pinMode(8, OUTPUT);
	pinMode(7, INPUT);
	pinMode(9, OUTPUT);
	pinMode(10, INPUT);
	pinMode(3, OUTPUT);

	Serial.begin(9600);
	Serial.flush();
	while(Serial.available()>0)Serial.read();

	servo_5.write(0);
	servo_6.write(0);
	plazasLibres = 5;
	digitalWrite(2, HIGH);
	digitalWrite(2, LOW);

}


void loop()
{

  	distEntrada = fnc_ultrasonic_distance(8,7);
  	distSalida = fnc_ultrasonic_distance(9,10);
  	if((millis()-task_time_ms)>=1000){
  		task_time_ms=millis();
  		Serial.println(String("Plazas libres: ")+String(plazasLibres));
  	}
  	if (((distEntrada < 20) && (plazasLibres > 0))) {
  		servo_5.write(0);
  		plazasLibres = (plazasLibres - 1);
  		if((millis()-task_time_ms2)>=60){
  			task_time_ms2=millis();
  			while(!(((fnc_ultrasonic_distance(8,7) > 20))));
  		}
  	}
  	else {
  		servo_5.write(90);
  	}

  	if ((distSalida < 20)) {
  		servo_6.write(0);
  		plazasLibres = (plazasLibres + 1);
  		while(!(((fnc_ultrasonic_distance(9,10) > 20))));
  		if ((plazasLibres > 5)) {
  			plazasLibres = 5;
  		}

  	}
  	else {
  		servo_6.write(90);
  	}

  	if ((plazasLibres == 0)) {
  		digitalWrite(2, LOW);
  		digitalWrite(3, HIGH);
  	}
  	else {
  		digitalWrite(2, HIGH);
  		digitalWrite(3, LOW);
  	}

  	delay(100);

}