const int trigPin = 2;  
const int echoPin = 3; 
const int buttonPin = 4;
const int redLedPin = 5;
const int ToleratedDistanceInterval = 5;
const int yellowLedPin = 6;
const int greenLedPin = 7;
const int soundPin = 8;
float duration, distance; 
float configuredDistance = 0;
boolean currentButtonState = digitalRead(buttonPin);
int timePressed = 0,i;
void setup() {
  //seta variaveis
  	pinMode(trigPin, OUTPUT);  
    pinMode(redLedPin, OUTPUT);  
    pinMode(greenLedPin, OUTPUT);  
    pinMode(yellowLedPin, OUTPUT);
    pinMode(soundPin, OUTPUT);
   	pinMode(echoPin, INPUT);  
    pinMode(buttonPin, INPUT_PULLUP);
    Serial.begin(9600);   
}

void loop() {

      if(digitalRead(buttonPin)==0){
        timePressed++;
        blinkAll();
        Serial.print(timePressed);
        if(timePressed >= 10){
          Serial.print("Config Mode");
          configMode();
        }
      }

      if(configuredDistance == 0 && digitalRead(buttonPin)!=0){
      blinkAll();
      }
      else {
        if(configuredDistance > 0){
          check();
        }
      }

      
      

}     

void configMode() {
   digitalWrite(redLedPin, LOW);
   digitalWrite(yellowLedPin, LOW);
   digitalWrite(greenLedPin, LOW);
  for(i=0;i<10;i++){

    digitalWrite(redLedPin, !digitalRead(redLedPin));

    delay(600);
  }

  for(i=0;i<4;i++){

    digitalWrite(yellowLedPin, !digitalRead(yellowLedPin));

    delay(600);
  }

  for(i=0;i<2;i++){

    digitalWrite(greenLedPin, !digitalRead(greenLedPin));

    delay(600);
  }

  digitalWrite(greenLedPin, HIGH);
  configuredDistance = getDistance();
  delay(2000);
  digitalWrite(greenLedPin, LOW);
}

float getDistance(){
  
    	digitalWrite(trigPin, LOW);  
	    delayMicroseconds(2);  
	    digitalWrite(trigPin, HIGH);  
	    delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      /* Espera o  pino ficar HIGH denovo e começa a contar o tempo e para de contar quando fica low denovo, com isso temos a duration
      Este intervalo é o tempo entre a o som ser disparado e voltar para o sensor.
      */
      duration = pulseIn(echoPin, HIGH);
      // Considerando que estamos usando ultrasom podemos usar a velocidade do som como constante
      // Sabendo a velocidade e a duração podemos dividir por 2 (ida e volta) e teremos a distância.           
      delay(100);
      return (duration*.0343)/2;
}

void check(){
  distance = getDistance();
  if(configuredDistance - distance > ToleratedDistanceInterval ){
    blinkAllIntermitent();
    digitalWrite(soundPin, HIGH);  
    digitalWrite(soundPin, LOW); 
  }
  Serial.println(configuredDistance - distance);
}

void blinkAll(){
       digitalWrite(redLedPin, LOW);
       digitalWrite(yellowLedPin, LOW);
       digitalWrite(greenLedPin, LOW);  
       delay(200);
       digitalWrite(redLedPin, !digitalRead(redLedPin));
       digitalWrite(yellowLedPin, !digitalRead(yellowLedPin));
       digitalWrite(greenLedPin, !digitalRead(greenLedPin));
       delay(200);
       digitalWrite(redLedPin, LOW);
       digitalWrite(yellowLedPin, LOW);
       digitalWrite(greenLedPin, LOW);
}

void blinkAllIntermitent(){
       digitalWrite(redLedPin, LOW);
       digitalWrite(redLedPin, !digitalRead(redLedPin));
       delay(100);
       digitalWrite(redLedPin, LOW);
       digitalWrite(yellowLedPin, LOW);
       digitalWrite(yellowLedPin, !digitalRead(yellowLedPin));
       delay(100);
       digitalWrite(yellowLedPin, LOW);
       digitalWrite(greenLedPin, LOW);  
       digitalWrite(greenLedPin, !digitalRead(greenLedPin));
       delay(100);
       digitalWrite(greenLedPin, LOW);  
}