#include <LiquidCrystal_I2C.h>
#include <Servo.h>

const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;
const int trigPin3 = 6;
const int echoPin3 = 7;
const int servoPin = 9;
const int ledPin = 8;
const int ledPin2 = 10;

LiquidCrystal_I2C lcd(0x027, 16, 2);
Servo myServo;

void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  myServo.attach(servoPin);
  lcd.init();
  lcd.backlight();
}
bool hayAlgoEnfrente = false;
void loop() {
  // Measure distance from sensor 1
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  float duration1 = pulseIn(echoPin1, HIGH);
  float distance_cm1 = duration1 * 0.034 / 2;

  // Measure distance from sensor 2
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  float duration2 = pulseIn(echoPin2, HIGH);
  float distance_cm2 = duration2 * 0.034 / 2;

  // Measure distance from sensor 3
  digitalWrite(trigPin3, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW);
  float duration3 = pulseIn(echoPin3, HIGH);
  float distance_cm3 = duration3 * 0.034 / 2;

  // Control servo and LED based on distance
  if (distance_cm3 > 100) {
    
    if (distance_cm1 < 100) {
      myServo.write(45);
    } else if (distance_cm2 < 100) {
      myServo.write(135);
    } else if (hayAlgoEnfrente){
      //Habia algo enfrente, por lo que giramos a un lado de ambos
      myServo.write(45);
      hayAlgoEnfrente = false;
      delay(200);
      myServo.write(90);
    } else {
      myServo.write(90);
    }
    //Empezamos a movernos

    digitalWrite(ledPin, HIGH);
    //myServo.write(45);
    if (ledPin, HIGH)
      digitalWrite(ledPin2, LOW);
  } else {
    digitalWrite(ledPin, LOW);
    myServo.write(90);
    delay(10);
    digitalWrite(ledPin2, HIGH); 
    delay(100);
    hayAlgoEnfrente = true;
  }

  // Display distance from all sensors on LCD
  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Dist Rigth: ");
  lcd.print(distance_cm1);
  lcd.print(" cm");
  delay(400);
  lcd.setCursor(0, 1);
  lcd.print("Dist Left: ");
  lcd.print(distance_cm2);
  lcd.print(" cm");
  delay(400);
  lcd.setCursor(0, 0);
  lcd.print("Dist Front: ");
  lcd.print(distance_cm3);
  lcd.print(" cm ");
  delay(400);
}