#include <Servo.h>

#define TRIGGER 12
#define ECHO 11
#define LED1 2
#define SERVO 9

Servo servo;
unsigned long ultimoIntervalo = millis();
const long intervalo = 15;

bool disparaFlagIntervalo(){
  unsigned long intervaloAtual = millis();
  unsigned long deltaTime = intervaloAtual - ultimoIntervalo;
  if(deltaTime >= intervalo ){
    ultimoIntervalo = intervaloAtual;
    return true;
  }
  return false;
}

float medeDistancia(){
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(3);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  int pulso = pulseIn(ECHO, HIGH);
  return pulso/58;
}

void piscaLed(int frequencia) {
  /*Acende e apaga em ciclos, se frequencia = 1, 1 segundo aceso e 1 segundo apagado*/
  uint64_t tempo = millis();
  int resto = frequencia * tempo%2000;
  if(resto <= 1000) digitalWrite(LED1, LOW);
  else digitalWrite(LED1,HIGH);
}

void viraVolante(int posicaoFinal, int velocidade){
  if(disparaFlagIntervalo()){
    int posicaoAtual = servo.read();
      if (posicaoAtual < posicaoFinal){
        servo.write(posicaoAtual + velocidade); 
      }
      else if(posicaoAtual > posicaoFinal){
        servo.write(posicaoAtual - velocidade);
      }
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(TRIGGER, OUTPUT);
  pinMode(ECHO, INPUT);
  servo.attach(SERVO);
  servo.write(0);
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(1);
  float distancia = medeDistancia();
  // Serial.println(servo.read());
  // Serial.println("Distância: " + (String) distancia);
  if(160 <= distancia  && distancia < 200) {
    piscaLed(1);
    viraVolante(36, 1);
  }
  else if (120 <= distancia && distancia < 160){
    piscaLed(2);
    viraVolante(72, 2);
  } 
  else if (80 <= distancia && distancia < 120){
    piscaLed(4);
    viraVolante(108, 3);
  } 
  else if (40 <= distancia && distancia < 80) {
    piscaLed(8);
    viraVolante(144, 4);
  } 
  else if (distancia < 40){
    digitalWrite(LED1, HIGH);
    viraVolante(180, 5);
  } 
  else {
    digitalWrite(LED1, LOW);
    viraVolante(0, 3);
  }
}