#define NEAR 10
#define IZQ 1
#define DCH 2
#define THRESHOLD 40 
#define MIC SENSOR_4

int parar;
int paro_total;
int motorA=1;
int time_out=0;
int mic=0;

task avanzar(){
	while(true){
		OnFwd(OUT_BC,75);
		until(parar||paro_total);
		Off(OUT_BC);
		until((!parar)&&(!paro_total));		
	}
}

inline int medir(){
	int i,d;
	// Acquire(motorA);
	until(motorA==1);motorA=0;
	RotateMotor(OUT_A,75,90);
	d=SensorUS(IN_1);
	RotateMotor(OUT_A,-75,180);
	i=SensorUS(IN_1);
	RotateMotor(OUT_A,75,90);
	// Release(motorA);
	motorA=1;
	if(i<d) return DCH;
	return IZQ;
}

inline void girar(int dir){
	if(dir==DCH){
		RotateMotor(OUT_B,-75,180);
		RotateMotor(OUT_C,75,180);
	}
	if(dir==IZQ){
		RotateMotor(OUT_C,-75,180);
		RotateMotor(OUT_B,75,180);
	}
}

task esperar(){
	time_out=0;
	Wait(1000);
	time_out=1;
}

task wait_for_mic(){
	mic=0;
	until((MIC > THRESHOLD)||(time_out));
	if(!time_out)
		mic=1;
}

task check_sonido(){
	paro_total=0;
	SetSensorSound(IN_4);
	while(true){ 
		until(MIC > THRESHOLD);
		paro_total=1;
		
		Wait(300);
		start esperar;
		start wait_for_mic;
		until((time_out)||(mic));
		if(mic){
			Wait(500);
			start wait_for_mic;
			start esperar;
			until((time_out)||(mic));
			if(mic){
				//tres palmadas, a la dcha, y sigo
				girar(DCH);
				paro_total=0;
			}
			else{
				//Dos palmadas, giro a la izquierda, y sigo
				girar(IZQ);
				paro_total=0;
			}
		}
		else{
			//una palmada, no hago nada, espero a otra palmada
			Wait(300); 
			until(MIC > THRESHOLD); 
			paro_total=0;
			Wait(300);
		}
	}
}



task main(){ 
	parar=0;
	SetSensorLowspeed(IN_1); 
	start avanzar;
	start check_sonido;
	// start girar_cabeza;
	while(true){ 
		if((SensorUS(IN_1)<NEAR)&&(!paro_total)){
			parar=1;
			girar(medir());
		}
		else{
			parar=0;
		}
   } 
}