#include <LiquidCrystal.h>
#include <Arduino_FreeRTOS.h>


LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

int sonarTrigger = 9; //configurando trigger no pino 9
int sonarEcho = 10; //configurando echo no pino 10

float distancia = 0.0;
int tempo = 0;

//variaveis para o uso do millis
unsigned long previousTime1 = 0;
unsigned long previousTime2 = 0;
unsigned long previousTime3 = 0;
unsigned long previousTimeBuzzer = 0;

void setup() {
  // put your setup code here, to run once:
  pinMode(sonarTrigger, OUTPUT);
  pinMode(sonarEcho, INPUT);
  pinMode(27, OUTPUT);
  pinMode(29, OUTPUT);
  pinMode(31, OUTPUT);
  pinMode(33, OUTPUT);
  pinMode(35, OUTPUT);
  pinMode(37, OUTPUT);
  pinMode(39, OUTPUT);
  pinMode(8, OUTPUT);

  lcd.begin(16, 2);
  lcd.setCursor(0, 0);

  xTaskCreate(AtivaSonar, "ativarSonar", 128, NULL, 1, NULL);
  xTaskCreate(MainTask, "mainTask", 128, NULL, 1, NULL);
  xTaskCreate(MostrarDistancia, "mostrarDistancia", 128, NULL, 1, NULL);
}

void AtivaSonar(void *pvParameters) { //Tarefa para 

  while (1) {
    digitalWrite(sonarTrigger, LOW);
    delayMicroseconds(2);
    digitalWrite(sonarTrigger, HIGH); //envia um pulso de ativação
    delayMicroseconds(10);
    digitalWrite(sonarTrigger, LOW); //para o pulso para começar
    tempo = pulseIn(sonarEcho, HIGH); //função que recebe o pulso
    distancia = tempo / 58.2; //conversão para distânci
  }

}

void MostrarDistancia(void *pvParameters) { //tarefa que mostra a distância no LCD

  while (1) {
    lcd.setCursor(0, 0);
    lcd.print(distancia);
  }

}

void MainTask (void *pvParameters) { //tarefa principal do código
  while (1) {
    if (distancia < 30) { //acende o led vermelho
      //apaga os leds verdes
      apagaVerde();
      //apaga os leds amarelos
      apagaAmarelo();
      //liga o led vermelho
      tarefaLedRed();
      tone(8, 700); //liga a buzina constante
    } else if (distancia >= 30 && distancia < 100) { //acende o led amarelo
      //apaga os led verdes
      apagaVerde();
      //apaga os leds vermelhos
      apagaVermelho;
      //liga os leds amarelos
      tarefaLedYellow();
      tarefaBuzina();

    } else if (distancia >= 100) { //acende o led verde
      //apaga os leds vermelhos
      apagaVermelho();
      //apaga os leds amarelos
      apagaAmarelo();
      //acende os leds verdes
      acendeVerde();
      noTone(8);//desliga a buzina
    }
  }

}


void loop() {


}

//************Seção de Funções auxiliares no código********************
void acendeVerde() {
  digitalWrite(27, HIGH);
  digitalWrite(29, HIGH);
}

void apagaVerde() {
  digitalWrite(27, LOW);
  digitalWrite(29, LOW);

}

void apagaAmarelo() {
  digitalWrite(31, LOW);
  digitalWrite(33, LOW);
  digitalWrite(35, LOW);
}

void apagaVermelho() {
  digitalWrite(37, LOW);
  digitalWrite(39, LOW);

}

void tarefaLedYellow() { //função para acender o LED Amarelo a cada 1 segundo
  unsigned long current = millis() - previousTime2;

  if ((millis() - previousTime2) < 1000) {
    digitalWrite(31, HIGH);
    digitalWrite(33, HIGH);
    digitalWrite(35, HIGH);
  } else {
    digitalWrite(31, LOW);
    digitalWrite(33, LOW);
    digitalWrite(35, LOW);
  }
  if ((millis() - previousTime2) > 2000) {
    previousTime2 = millis();
  }

}

void tarefaLedRed() { //função para acender o LED RED a cada meio segundo
  unsigned long current = millis() - previousTime3;

  if ((millis() - previousTime3) < 500) {
    digitalWrite(37, HIGH);
    digitalWrite(39, HIGH);
  } else {
    digitalWrite(37, LOW);
    digitalWrite(39, LOW);
  }
  if ((millis() - previousTime3) > 1000) {
    previousTime3 = millis();
  }
}
void tarefaBuzina() { //função para acionar a buzina a cada 500ms
  unsigned long current = millis() - previousTimeBuzzer;

  if ((millis() - previousTimeBuzzer) < 500) {
    tone(8, 700);
  } else {
    noTone(8);
  }
  if ((millis() - previousTimeBuzzer) > 1000) {
    previousTimeBuzzer = millis();
  }

}