#include <Wire.h> // Library for I2C peripherals, graphical display
#include <NewPing.h> // Include the library for the ultrasonic sensor
#include <Adafruit_GFX.h> // Library for graphical display
#include <Adafruit_SSD1306.h>

// Screen definition
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int LED = 14;
int BUZZER = 12;

// Define the pins for the ultrasonic sensor
#define TRIG_PIN 25 // Tx2 on ESP32
#define ECHO_PIN 26 // Rx2 on ESP32
#define PUMP_PIN 5 // Define the pin for the pump relay (NO input)
#define VALVE_PIN 4 // Define the pin for the valve relay (NC input)
#define HEIGHT_MAX 25 // Adjust this distance according to your needs. Define the maximum distance to consider the tank full
#define HEIGHT_MIN 10 // Adjust this distance according to your needs. // Define the minimum distance to activate the pump
#define TIME_STOP_PUMP 2000 // 2 seconds // Time to wait to open the valve after the pump fills the tank (in milliseconds)
#define TIME_STOP_VALVE 2000 // 2 seconds // Time to wait after turning off the pump before opening the valve (in milliseconds)
#define INVALID_DISTANCE -1 // only read mistake
NewPing sonar(TRIG_PIN, ECHO_PIN); // Create an instance of the ultrasonic sensor

bool pump_on = false; // Variable to track if the pump has been turned on

void openValve() // Function to open the valve
{
  digitalWrite(VALVE_PIN, LOW); // If the relay is triggered by HIGH, change LOW to HIGH
}

void closeValve() // Function to close the valve
{
  digitalWrite(VALVE_PIN, HIGH); // If the relay is triggered by LOW, change HIGH to LOW
}

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  
  Serial.begin(9600);

  // Initialize the OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Error initializing the OLED display"));
    for(;;);
  }

  // Initialize the pins of the pump relay and the valve as outputs
  pinMode(PUMP_PIN, OUTPUT);
  pinMode(VALVE_PIN, OUTPUT);

  // Turn off the pump and close the valve initially
  digitalWrite(VALVE_PIN, HIGH); // If the relay of the pump is triggered by LOW, change HIGH to LOW
  closeValve();
}

void loop() {
  // Display distance on the OLED screen
  int distance = sonar.ping_cm(); // Perform a reading of the ultrasonic sensor to measure the distance

  // Check if the distance reading is valid
  if (distance == INVALID_DISTANCE) {
    // Handle error: unable to read distance
    Serial.println("Error: Unable to read distance from sensor");
    // Otras acciones para manejar el error
    // Verificar el estado actual del sistema y continuar según sea necesario
    if (pump_on) {
      // Si la bomba está encendida (llenando el tanque), continuar llenando el tanque
      closeValve(); // Cerrar la válvula para detener la evacuación
      pump_on = false; // Marcar que la bomba ya no está encendida
      return; // Salir de la función loop para evitar continuar con el resto del código
    } else {
      // Si la bomba no está encendida (evacuando el tanque), continuar evacuando el tanque
      return; // Salir de la función loop para evitar continuar con el resto del código
    }
  }

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);

  int16_t x, y;
  uint16_t w, h;

  display.getTextBounds("HEIGHT", 0, 0, &x, &y, &w, &h);
  display.setCursor((SCREEN_WIDTH - w) / 2, 0); // Set the position to display the distance
  display.println("HEIGHT:");

  display.setTextSize(3);
  display.getTextBounds(String(distance), 0, 0, &x, &y, &w, &h);
  display.setCursor((SCREEN_WIDTH - w) / 2, (SCREEN_WIDTH - h) / 3.5);
  display.print(distance, 1); // Show the measured distance on the display
  display.display();

  if (distance <= HEIGHT_MIN) { // Verify if the water level is low (minimum distance reached)
    if (!pump_on) { // Turn on the pump if the tank is empty and the pump is not already on
      digitalWrite(PUMP_PIN, LOW); // If the pump relay is triggered by HIGH, change LOW to HIGH
      delay(TIME_STOP_VALVE); //  Wait 2 seconds before opening the valve
      openValve();
      pump_on = true;
    }
  } else if (distance >= HEIGHT_MAX) {
    // Turn off the pump and close the valve if the water level is high
    digitalWrite(PUMP_PIN, HIGH); // If the pump relay is triggered by LOW, change HIGH to LOW
    delay(TIME_STOP_PUMP); // Wait 2 seconds before opening the pump
    closeValve();
    pump_on = false;
  }
  if (distance <= HEIGHT_MIN + 0.5 || distance >= HEIGHT_MAX - 0.5) { // Turn on LED and buzzer outside the new established range
    digitalWrite(LED, HIGH);
    tone(BUZZER, 1000); // Turn on the buzzer at a frequency of 1000 Hz
  } else { // Turn off LED and buzzer inside the new established range
    digitalWrite(LED, LOW);
    noTone(BUZZER); // Turn off the buzzer
  }

  delay(200); // Wait a brief period of time before the next reading
}