#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const int fl = A0; // Sensor frontal izquierdo
const int fr = A1; // Sensor frontal derecho
const int bl = A2; // Sensor trasero izquierdo
const int br = A3; // Sensor trasero derecho
const int Imin = 4; // Corriente mínima en mA
const int Imax = 20; // Corriente máxima en mA
const int Dmin = 50; // Distancia correspondiente a 4 mA en mm
const int Dmax = 200; // Distancia correspondiente a 20 mA en mm
//const int numReadings = 5; // Número de mediciones para calcular el promedio

LiquidCrystal_I2C lcd(0x27, 24, 4);

void setup() {
  // put your setup code here, to run once:
  pinMode(fl, INPUT);
  pinMode(fr, INPUT);
  pinMode(bl, INPUT);
  pinMode(br, INPUT);
  lcd.init();  // Inicializa el display
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Ad.Izq: ");
  lcd.setCursor(0, 1);
  lcd.print("Ad.Der: ");
  lcd.setCursor(0, 2);
  lcd.print("At.Izq: ");
  lcd.setCursor(0, 3);
  lcd.print("Ad.Der: ");

void loop() {
  // put your main code here, to run repeatedly:
  
  float Dist1 = 0;
  float Dist2 = 0;
  float Dist3 = 0;
  float Dist4 = 0;
  int mediciones = 5;

  for (int i = 0; i < mediciones; i++) {
   Dist1 += getDistance(fl);
   Dist2 += getDistance(fr);
   Dist3 += getDistance(bl);
   Dist4 += getDistance(br);
  }

  float promedio1 = Dist1 / mediciones;
  float promedio2 = Dist2 / mediciones;
  float promedio3 = Dist3 / mediciones;
  float promedio4 = Dist4 / mediciones;
  
  lcd.setCursor(8, 0);
  lcd.print(promedio1 * 10);
  lcd.print(" mm");
  
  lcd.setCursor(8, 1);
  lcd.print(promedio2 * 10);
  lcd.print(" mm");

  lcd.setCursor(8, 2);
  lcd.print(promedio3 * 10);
  lcd.print(" mm");

  lcd.setCursor(8, 3);
  lcd.print(promedio4 * 10);
  lcd.print(" mm");

  delay(1000);
}

float getDistance(float fl) {
  digitalWrite(fl, LOW);
  delayMicroseconds(2);
  
  digitalWrite(fl, HIGH);
  delayMicroseconds(10);
  digitalWrite(fl, LOW);
  
  //float duration = pulseIn(echoPin, HIGH);
  
  //float distance = (duration / 2) * 0.0343;
  
  //return distance;
}