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

// Pin definitions for first HC-SR04
#define TriggerH1  12  
#define EchoH1  13 

// Pin definitions for second HC-SR04
#define TriggerH2  5
#define EchoH2  6

#define LedV  2   // Green LED
#define LedR  3   // Red LED
#define Buzz  4

long duracion, distancia1, distancia2;  // Variables for distances from both sensors
int estatura; 
int heightMeasurements[3];  // Array to store height measurements
int measurementIndex = 0;   // Index for the heightMeasurements array
unsigned long lastMeasurementTime = 0;  // Time of the last measurement
unsigned long measurementInterval = 1333;  // Time between measurements (4s / 3)
int consistentHeight = 0;   // Variable to store the consistent height
bool heightConsistent = false;
bool showInitialMessage = true;  // Flag to show the initial message

Adafruit_SSD1306 display(128, 64, &Wire, -1);

void setup() {
  Serial.begin(9600);
  pinMode(EchoH1, INPUT);
  pinMode(TriggerH1, OUTPUT);
  pinMode(EchoH2, INPUT);
  pinMode(TriggerH2, OUTPUT);
  pinMode(LedV, OUTPUT);      
  pinMode(LedR, OUTPUT);  

  digitalWrite(LedV, LOW); 
  digitalWrite(LedR, LOW); 

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

}
void inicio(){
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2); 
  display.setCursor(0, 0); 
  display.println("MIRADA AL");
  display.println("FRENTE");
  display.display();
  delay(1500); 
  tone(Buzz, 2500, 250);
}

void loop() {
  unsigned long currentTime = millis();

  // Check distance from first HC-SR04 sensor
  digitalWrite(TriggerH1, LOW);
  delayMicroseconds(2);
  digitalWrite(TriggerH1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerH1, LOW);
  duracion = pulseIn(EchoH1, HIGH);
  distancia1 = duracion / 58.2;

  // Check distance from second HC-SR04 sensor
  digitalWrite(TriggerH2, LOW);
  delayMicroseconds(2);
  digitalWrite(TriggerH2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerH2, LOW);
  duracion = pulseIn(EchoH2, HIGH);
  distancia2 = duracion / 58.2;

  // Control LEDs based on distance from second sensor
  if (distancia2 <= 40) {
    digitalWrite(LedR, HIGH);  // Turn on red LED
  } else {
    digitalWrite(LedR, LOW);   // Turn off red LED
  }

  // Only take height measurement if the person is detected within 40cm by second sensor
  if (distancia2 <= 40) {
    if (currentTime - lastMeasurementTime >= measurementInterval) {
      lastMeasurementTime = currentTime;

      // Calculate the height using the first sensor (204 cm from the floor)
      estatura = 204 - distancia1;

      heightMeasurements[measurementIndex] = estatura;
      measurementIndex = (measurementIndex + 1) % 3;

      // Check if all three measurements are the same
      if (heightMeasurements[0] == heightMeasurements[1] && heightMeasurements[1] == heightMeasurements[2]) {
        if (!heightConsistent || consistentHeight != heightMeasurements[0]) {
          consistentHeight = heightMeasurements[0];
          heightConsistent = true;

          // Update the OLED display with the consistent height
          display.clearDisplay();
          display.setCursor(10, 0);
          display.setTextSize(5); // Adjust text size as needed
          display.println(consistentHeight);
          display.setTextSize(2);
          display.setCursor(10, 40); // Adjust this value to fit your screen
          display.println("Cms");
          display.display();

          // Sound the buzzer 3 times
          for (int i = 0; i < 3; i++) {
            tone(Buzz, 1000, 200);
            delay(290);
            Serial.print("H");
            Serial.println(consistentHeight);
          }

          // Serial output of the consistent height
          Serial.print("H");
          Serial.println(consistentHeight);

          digitalWrite(LedV, HIGH);  // Turn on green LED
        }
      } else {
        heightConsistent = false;
      }

      // Show the initial message if no consistent height is found
      if (!heightConsistent) {

        digitalWrite(LedV, LOW);   // Turn off green LED

      }
    }
  } else {
    digitalWrite(LedV, LOW);   // Turn off green LED if distance > 40cm
    inicio();
  }
}