#include <LiquidCrystal_I2C.h>
#define I2C_ADDR 0x27
#define LCD_COLUMNS 20
#define LCD_LINES 4
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define ECHO1_PIN 6
#define TRIG1_PIN 7
#define ECHO2_PIN 4
#define TRIG2_PIN 5

void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT); //Buzzer
  pinMode(12, OUTPUT); // LED Verde
  pinMode(11, OUTPUT); // LED Amarelo
  pinMode(10, OUTPUT); // LED Vermelho
  
  pinMode(TRIG1_PIN, OUTPUT);
  pinMode(ECHO1_PIN, INPUT);
  pinMode(TRIG2_PIN, OUTPUT);
  pinMode(ECHO2_PIN, INPUT);
  
  lcd.init();
  lcd.backlight();
}

float readDistanceCM1() {
  digitalWrite(TRIG1_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG1_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG1_PIN, LOW);
  
  int duration1 = pulseIn(ECHO1_PIN, HIGH);

  return duration1 * 0.034 / 2;
}

float readDistanceCM2() {
  digitalWrite(TRIG2_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG2_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG2_PIN, LOW);
  
  int duration2 = pulseIn(ECHO2_PIN, HIGH);

  return duration2 * 0.034 / 2;
}


void loop() {

  noTone(13);

  float distance1 = readDistanceCM1();

  float distance2 = readDistanceCM2();

  float min_distance = min(distance1, distance2);

  lcd.setCursor(5,0);
  lcd.print("Distancias");

  lcd.setCursor(1,1);
  lcd.print("Dist 1: ");
  lcd.print(distance1);
  lcd.print(" cm  ");

  lcd.setCursor(1,2);
  lcd.print("Dist 2: ");
  lcd.print(distance2);
  lcd.print(" cm  ");

  if (min_distance > 100)
  {

    digitalWrite(12, HIGH);
    digitalWrite(11, LOW);
    digitalWrite(10, LOW);

  }
  else if (min_distance > 50 && min_distance < 100)
  {

    digitalWrite(12, LOW);
    digitalWrite(11, HIGH);
    digitalWrite(10, LOW);

  }
  else if (min_distance > 30 && min_distance < 50)
  {

    digitalWrite(12, LOW);
    digitalWrite(11, HIGH);
    delay(200);
    digitalWrite(11, LOW);
    digitalWrite(10, LOW);

  }
  else if (min_distance < 30)
  {

    digitalWrite(12, LOW);
    digitalWrite(11, LOW);
    tone(13, 2000);
    digitalWrite(10, HIGH);

  }
  delay(100);
}