#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

TaskHandle_t tarefa1;
TaskHandle_t tarefa2;
TaskHandle_t tarefa3;
TaskHandle_t tarefa4;

int ledVermelho = 32;
int ledAzul = 33;

int trigPin = 26;
int echoPin = 27;

int buzzer = 25;

int tempoBuzzer = 1000;

void setup() {

  Serial.begin(115200);
  pinMode(ledVermelho, OUTPUT);
  pinMode(ledAzul, OUTPUT);

  pinMode(buzzer, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);

  }


  xTaskCreatePinnedToCore(blinkVermelho, "tarefa1", 10000, NULL, 0, &tarefa1, 0);
  xTaskCreatePinnedToCore(blinkAzul, "tarefa2", 10000, NULL, 1, &tarefa2, 1);

  xTaskCreatePinnedToCore(sensorHC, "tarefa3", 10000, NULL, 2, &tarefa3, 1);

  xTaskCreatePinnedToCore(acionaBuzzer, "tarefa4", 10000, NULL, 1, &tarefa4, 1);


}

void loop() {

  if (Serial.available()) {
    char c = Serial.read();

    if (c == 'a') {
      vTaskSuspend(tarefa1);
    } else if (c == 'b') {
      vTaskResume(tarefa1);
    }
  }


}

void blinkVermelho(void *pvParameters) {

  for (;;) {
    digitalWrite(ledVermelho, HIGH);
    delay(500);
    digitalWrite(ledVermelho, LOW);
    delay(500);
  }


}

void blinkAzul(void *pvParameters) {

  for (;;) {
    digitalWrite(ledAzul, HIGH);
    delay(100);
    digitalWrite(ledAzul, LOW);
    delay(100);
  }
}

void sensorHC(void *pvParameters) {

  for (;;) {

    int distancia;
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    unsigned long tempoSom = pulseIn(echoPin, HIGH);
    distancia = tempoSom / 58;

    Serial.print("Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");

    if (distancia >= 20) {
      tempoBuzzer = 1000;

      delay(2000);
      display.clearDisplay();

      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 10);

      display.println(distancia);
      display.display();
      
    } else if ((distancia < 20) && (distancia >= 15)) {
      tempoBuzzer = 500;
      delay(2000);
      display.clearDisplay();

      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 10);

      display.print(distancia);
      display.print(" cm");
      display.display();

    } else if ((distancia < 15) && (distancia >= 10)) {
      tempoBuzzer = 250;

      delay(2000);
      display.clearDisplay();

      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 10);

      display.println(distancia);
      display.display();

    } else if ((distancia < 10) && (distancia >= 5)) {
      tempoBuzzer = 100;
    } else {
      tempoBuzzer = 0;
    }


    delay(1000);
  }
}

void acionaBuzzer(void *pvParametters) {

  for (;;) {
    digitalWrite(ledVermelho, HIGH);
    digitalWrite(buzzer, HIGH);
    delay(tempoBuzzer);

    digitalWrite(ledVermelho, LOW);
    digitalWrite(buzzer, LOW);
    delay(tempoBuzzer);
  }

}