#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);
float FL, FR, BL, BR;


long readUltrasonicDistance(int triggerPin, int echoPin) {
  pinMode(triggerPin, OUTPUT);

  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  pinMode(echoPin, INPUT);
  return pulseIn(echoPin, HIGH);
}

void setup() {
  Serial.begin(9600);

  pinMode(11, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(A3, OUTPUT);
  
  lcd.init();
  lcd.backlight();
  lcd.print("Alarma - Distancia");
  delay(3000);
  
  lcd.clear();
}

void triggerSound(float value1, float value2, float value3, float value4) {
  if (value1 < 100 || value2 < 100 || value3 < 100 || value4 < 100){
    tone(A3, 240);
    delay(300);
    noTone(A3);
    delay(300);
    return;
  }
  
  if (
    (value1 > 100 && value1 <= 200) || 
    (value2 > 100 && value2 <= 200) || 
    (value3 > 100 && value3 <= 200) || 
    (value4 > 100 && value4 <= 200)
  ) {
    tone(A3, 120);
    delay(300);
    noTone(A3);
    delay(300);
    return;
  }
  
  if (
    (value1 > 200 && value1 <= 400) || 
    (value2 > 200 && value2 <= 400) || 
    (value3 > 200 && value3 <= 400) || 
    (value4 > 200 && value4 <= 400)
  ) {
    tone(A3, 20);
    delay(300);
    noTone(A3);
    delay(300);
    return;
  }
  
  noTone(A3);
}

void lightLed(int pin, float value){
  if (value < 100){
    digitalWrite(pin, HIGH);
    return;
  }
  digitalWrite(pin, LOW);
}

void loop() {
  FL = 0.0344 / 2 * readUltrasonicDistance(5, 4);
  FR = 0.0344 / 2 * readUltrasonicDistance(3, 2);
  BL = 0.0344 / 2 * readUltrasonicDistance(9, 8);
  BR = 0.0344 / 2 * readUltrasonicDistance(7, 6);

  Serial.print("\t");

  lcd.setCursor(2,0);
  lcd.print("FL");

  lcd.setCursor(12,0);
  lcd.print("FR");

  lcd.setCursor(1,1);
  lcd.print(FL,1);

  lcd.setCursor(11,1);
  lcd.print(FR,1);

  lcd.setCursor(2,2);
  lcd.print("BL");

  lcd.setCursor(12,2);
  lcd.print("BR");

  lcd.setCursor(1,3);
  lcd.print(BL,1);

  lcd.setCursor(11,3);
  lcd.print(BR,1);

  lightLed(11, FL);
  lightLed(10, FR);
  lightLed(13, BL);
  lightLed(12, BR);

  triggerSound(FL, FR, BL, BR);

  delay(100); // Remover para sonido constante
  lcd.clear();
}