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

LiquidCrystal_I2C lcd(0x27, 16, 2); // Adjust columns and rows as needed

const int phase3Pin = A0;
const int phase2Pin = A1;
const int phase1Pin = A2;
const int solarPowerPin = A3;

const int relay1Pin = 8;
const int relay2Pin = 10;
const int relay3Pin = 4;
const int solarRelayPin = 7;
const int generatorSwitchRelayPin = 6; // Added generator switch relay pin
const int generatorRelayPin = 5;        // Added generator relay pin

// Define voltage thresholds with hysteresis
const int overVoltageThreshold = 245;
const int underVoltageThreshold = 195;
const int overVoltageResetThreshold = 235; // For hysteresis
const int underVoltageResetThreshold = 205; // For hysteresis

unsigned long previousMillis = 0;
const long interval = 500; // interval in milliseconds

// Calibration factor to convert ADC reading to actual voltage
const float voltageCalibrationFactor = 250.0 / 1023.0;

const int numReadings = 10; // Number of readings to average
int readings[numReadings];   // Array to store readings
int readIndex = 0;           // Index of the current reading
int total = 0;               // Running total
int average = 0;             // Average

void setup() {
  // Initialize LCD
  lcd.begin(16, 2); // Adjust rows as needed
  lcd.backlight();  // add the backlight
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("God Is My Light");
  lcd.setCursor(0, 1);
  lcd.print("EnerGuard Switch");
  delay(2000);

  // Configure relay pins as outputs
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(relay3Pin, OUTPUT);
  pinMode(solarRelayPin, OUTPUT);
  pinMode(generatorSwitchRelayPin, OUTPUT); // Added generator switch relay pin
  pinMode(generatorRelayPin, OUTPUT);       // Added generator relay pin

  lcd.clear();

  // Initialize all readings to 0
  for (int i = 0; i < numReadings; i++) {
    readings[i] = 0;
  }
}

float getAverageVoltage(int pin) {
  // Subtract the last reading
  total = total - readings[readIndex];
  
  // Read from the sensor
  readings[readIndex] = analogRead(pin);
  
  // Add the reading to the total
  total = total + readings[readIndex];
  
  // Advance to the next position in the array
  readIndex = readIndex + 1;
  
  // If we're at the end of the array, wrap around to the beginning
  if (readIndex >= numReadings) {
    readIndex = 0;
  }
  
  // Calculate the average
  average = total / numReadings;
  
  // Convert the average to voltage
  return average * voltageCalibrationFactor;
}

void voltageProtection(int pin, int relayPin, int nextRelayPin) {
  float voltage = getAverageVoltage(pin);

  static bool isRelayOn = false;

  if (isRelayOn) {
    if (voltage > overVoltageThreshold || voltage < underVoltageThreshold) {
      digitalWrite(relayPin, LOW);  // Turn off the relay for protection
      digitalWrite(nextRelayPin, HIGH); // Turn on the next relay in hierarchy
      isRelayOn = false;
    }
  } else {
    if (voltage < overVoltageResetThreshold && voltage > underVoltageResetThreshold) {
      digitalWrite(relayPin, HIGH); // Turn on the relay
      digitalWrite(nextRelayPin, LOW); // Turn off the next relay
      isRelayOn = true;
    }
  }
}

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

  // Perform actions every 500 milliseconds
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    // Perform voltage protection for each phase
    voltageProtection(phase1Pin, relay1Pin, relay2Pin);
    voltageProtection(phase2Pin, relay2Pin, relay3Pin);
    voltageProtection(phase3Pin, relay3Pin, relay1Pin);

    // Read analog values from pins
    int phase1Value = analogRead(phase1Pin);
    int phase2Value = analogRead(phase2Pin);
    int phase3Value = analogRead(phase3Pin);
    int solarPowerValue = analogRead(solarPowerPin);

    // Adjust these thresholds based on your sensor characteristics
    const int threshold = 990;

    // Determine which phase sensor is active (if any)
    int activePhase = 0;

    if (phase1Value > threshold)
      activePhase = 3;
    if (phase2Value > threshold)
      activePhase = 2;
    if (phase3Value > threshold)
      activePhase = 1;

    // Relay control based on phase status hierarchy
    switch (activePhase) {
      case 1:
        digitalWrite(relay1Pin, HIGH); // Change LOW to HIGH
        digitalWrite(relay2Pin, LOW);
        digitalWrite(relay3Pin, LOW);
        break;
      case 2:
        digitalWrite(relay1Pin, LOW);
        digitalWrite(relay2Pin, HIGH); // Change LOW to HIGH
        digitalWrite(relay3Pin, LOW);
        break;
      case 3:
        digitalWrite(relay1Pin, LOW);
        digitalWrite(relay2Pin, LOW);
        digitalWrite(relay3Pin, HIGH); // Change LOW to HIGH
        break;
      default:
        digitalWrite(relay1Pin, LOW);
        digitalWrite(relay2Pin, LOW);
        digitalWrite(relay3Pin, LOW);
    }

    // Solar relay control
    digitalWrite(solarRelayPin, (activePhase == 0 && solarPowerValue > threshold) ? HIGH : LOW);

    // Generator switch relay control
    digitalWrite(generatorSwitchRelayPin, (activePhase == 0 && solarPowerValue <= threshold) ? HIGH : LOW);

    // Generator relay control
    digitalWrite(generatorRelayPin, (activePhase == 0 && solarPowerValue <= threshold) ? HIGH : LOW);

    // Read analog voltage values from pins and convert to AC voltage
    float phase1Voltage = getAverageVoltage(phase1Pin); // Assuming maximum AC voltage is 250V
    float phase2Voltage = getAverageVoltage(phase2Pin);
    float phase3Voltage = getAverageVoltage(phase3Pin);
    float solarVoltage = getAverageVoltage(solarPowerPin);

    // Display power status and voltage readings on LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Power Status:");

    if (activePhase != 0) {
      lcd.setCursor(0, 1);
      lcd.print("Phase");
      lcd.print(activePhase);
      lcd.print(" :");
      lcd.print(phase1Voltage, 1); // Display voltage of the selected phase
      lcd.print("V");
    } else if (solarPowerValue > threshold) {
      lcd.setCursor(0, 1);
      lcd.print("Solar :");
      lcd.print(solarVoltage, 1); // Corrected variable name
      lcd.print("V");
    } else {
      lcd.setCursor(0, 1);
      lcd.print("Generator "); // Display generator voltage
      // You may need to add code to read generator voltage if applicable
    }
  }
}