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

// Define ultrasonic sensor pins for Generator 1
const int sensor1Trigger = 2;
const int sensor1Echo = 3;
const int relay1Switch = 11; // Relay for Generator 1 switch
const int relay1 = 8;

// Define ultrasonic sensor pins for Generator 2
const int sensor2Trigger = 4;
const int sensor2Echo = 5;
const int relay2Switch = 12; // Relay for Generator 2 switch
const int relay2 = 9;

// Define ultrasonic sensor pins for Generator 3
const int sensor3Trigger = 6;
const int sensor3Echo = 7;
const int relay3Switch = 13; // Relay for Generator 3 switch
const int relay3 = 10;

// Define LCD module
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Function prototypes
float getFuelLevel(int triggerPin, int echoPin);
int selectGenerator(float fuelLevel1, float fuelLevel2, float fuelLevel3, int currentGenerator);
int findNextGenerator(float fuelLevel1, float fuelLevel2, float fuelLevel3);
void activateGenerator(int generator);
void deactivateGenerator(int generator);

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

  // Set up ultrasonic sensors
  pinMode(sensor1Trigger, OUTPUT);
  pinMode(sensor1Echo, INPUT);
  pinMode(relay1Switch, OUTPUT);

  pinMode(sensor2Trigger, OUTPUT);
  pinMode(sensor2Echo, INPUT);
  pinMode(relay2Switch, OUTPUT);

  pinMode(sensor3Trigger, OUTPUT);
  pinMode(sensor3Echo, INPUT);
  pinMode(relay3Switch, OUTPUT);

  // Set up relay modules for generators
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  pinMode(relay3, OUTPUT);

  // Deactivate all relays at the beginning
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  digitalWrite(relay3, LOW);
  digitalWrite(relay1Switch, LOW);
  digitalWrite(relay2Switch, LOW);
  digitalWrite(relay3Switch, LOW);

  // Set up LCD
  lcd.begin(16, 2);

  // Display "God is my Light" with glowing LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("God is my Light");

  // Turn on the LCD backlight and delay for 2 seconds
  lcd.backlight();
  delay(2000);

  // Turn off the LCD backlight
  lcd.noBacklight();
}

void loop() {
  float fuelLevel1 = getFuelLevel(sensor1Trigger, sensor1Echo);
  float fuelLevel2 = getFuelLevel(sensor2Trigger, sensor2Echo);
  float fuelLevel3 = getFuelLevel(sensor3Trigger, sensor3Echo);

  static int selectedGenerator = 0; // Static variable to store the currently selected generator
  bool activateNextGenerator = false;

  // If no generator is selected or the fuel level of the selected generator is below 20%
  if (selectedGenerator == 0 || (selectedGenerator == 1 && fuelLevel1 <= 20) ||
      (selectedGenerator == 2 && fuelLevel2 <= 20) || (selectedGenerator == 3 && fuelLevel3 <= 20)) {
    selectedGenerator = selectGenerator(fuelLevel1, fuelLevel2, fuelLevel3, selectedGenerator);
  }

  // Display on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Generator: ");
  lcd.print(selectedGenerator);

  lcd.setCursor(0, 1);
  lcd.print("Fuel Level: ");

  // Display fuel level based on the selected generator
  switch (selectedGenerator) {
    case 1:
      lcd.print(fuelLevel1);
      break;
    case 2:
      lcd.print(fuelLevel2);
      break;
    case 3:
      lcd.print(fuelLevel3);
      break;
  }

  lcd.print("%");

  // Keep the LCD backlight on
  lcd.backlight();

  // Check if all fuel levels are below 20%
  if (fuelLevel1 <= 20 && fuelLevel2 <= 20 && fuelLevel3 <= 20) {
    // Deactivate all generator relays
    deactivateGenerator(1);
    deactivateGenerator(2);
    deactivateGenerator(3);
  } else {
    // Check if the fuel level of the selected generator is below 20%
    if ((selectedGenerator == 1 && fuelLevel1 <= 20) ||
        (selectedGenerator == 2 && fuelLevel2 <= 20) ||
        (selectedGenerator == 3 && fuelLevel3 <= 20)) {
      // Find the next generator with the highest fuel level
      int nextGenerator = findNextGenerator(fuelLevel1, fuelLevel2, fuelLevel3);
      // Activate the next generator
      activateGenerator(nextGenerator);
      // Set the flag to true for activating the next generator in the next iteration
      activateNextGenerator = true;
    }
  }

  // Delay to avoid flickering
  delay(1000);

  // Deactivate the next generator if it was activated in the previous iteration
  if (activateNextGenerator) {
    deactivateGenerator(selectedGenerator);
  }

  // ... (Your existing loop code)
}

float getFuelLevel(int triggerPin, int echoPin) {
  // Function to measure fuel level using ultrasonic sensor

  // Generate ultrasonic pulse
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  // Measure the time it takes for the echo to return
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  float distance = duration * 0.034 / 2;

  // Assuming a linear relationship between distance and fuel level
  // Adjust the conversion factor based on your actual setup
  float fuelLevel = map(distance, 0, 100, 100, 0);

  // Ensure the calculated fuel level is within the valid range
  fuelLevel = constrain(fuelLevel, 0, 100);

  return fuelLevel;
}

int findNextGenerator(float fuelLevel1, float fuelLevel2, float fuelLevel3) {
  // Determine the generator with the highest fuel level
  if (fuelLevel2 >= fuelLevel1 && fuelLevel2 >= fuelLevel3) {
    return 2;
  } else {
    return 3;
  }
}

int selectGenerator(float fuelLevel1, float fuelLevel2, float fuelLevel3, int currentGenerator) {
  // Select the generator with the highest fuel level that is above 20%
  if (fuelLevel1 > 20 && (fuelLevel1 >= fuelLevel2 && fuelLevel1 >= fuelLevel3)) {
    activateGenerator(1);
    return 1;
  } else if (fuelLevel2 > 20 && (fuelLevel2 >= fuelLevel1 && fuelLevel2 >= fuelLevel3)) {
    activateGenerator(2);
    return 2;
  } else if (fuelLevel3 > 20 && (fuelLevel3 >= fuelLevel1 && fuelLevel3 >= fuelLevel2)) {
    activateGenerator(3);
    return 3;
  } else {
    // If no generator is above 20%, stay with the current generator
    return currentGenerator;
  }
}

void activateGenerator(int generator) {
  // Deactivate all relays
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  digitalWrite(relay3, LOW);
  digitalWrite(relay1Switch, LOW);
  digitalWrite(relay2Switch, LOW);
  digitalWrite(relay3Switch, LOW);

  // Activate the selected relay for generator switch
  if (generator == 1) {
    digitalWrite(relay1Switch, HIGH);
    delay(100);  // Add a delay to stabilize the relay state
  } else if (generator == 2) {
    digitalWrite(relay2Switch, HIGH);
    delay(100);
  } else {
    digitalWrite(relay3Switch, HIGH);
    delay(100);
  }

  // Activate the selected relay for generator
  if (generator == 1) {
    digitalWrite(relay1, HIGH);
    delay(100);  // Add a delay to stabilize the relay state
  } else if (generator == 2) {
    digitalWrite(relay2, HIGH);
    delay(100);
  } else {
    digitalWrite(relay3, HIGH);
    delay(100);
  }
}

void deactivateGenerator(int generator) {
  // Deactivate the specified generator
  switch (generator) {
    case 1:
      digitalWrite(relay1, LOW);
      digitalWrite(relay1Switch, LOW);
      delay(100);  // Add a delay to stabilize the relay state
      break;
    case 2:
      digitalWrite(relay2, LOW);
      digitalWrite(relay2Switch, LOW);
      delay(100);
      break;
    case 3:
      digitalWrite(relay3, LOW);
      digitalWrite(relay3Switch, LOW);
      delay(100);
      break;
  }
}