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

// LCD setup (16x2 or 16x4)
LiquidCrystal_I2C lcd(0x27, 16, 2); // Change address to 0x3F if needed

// Relay pin definitions
#define RELAY_MAINS 7
#define RELAY_GEN 8
#define RELAY_GEN_START 9
#define RELAY_GEN_STOP 10

// Voltage sensing pins
#define MAINS_SENSOR_PIN A0
#define GEN_SENSOR_PIN A1

// Thresholds & timings
#define VOLTAGE_THRESHOLD 200.0
#define GEN_START_DELAY 3000
#define GEN_WARMUP_DELAY 5000
#define GEN_SHUTDOWN_DELAY 3000

bool mainsAvailable = false;
bool generatorRunning = false;

void setup() {
  pinMode(RELAY_MAINS, OUTPUT);
  pinMode(RELAY_GEN, OUTPUT);
  pinMode(RELAY_GEN_START, OUTPUT);
  pinMode(RELAY_GEN_STOP, OUTPUT);

  digitalWrite(RELAY_MAINS, LOW);
  digitalWrite(RELAY_GEN, LOW);
  digitalWrite(RELAY_GEN_START, LOW);
  digitalWrite(RELAY_GEN_STOP, LOW);

  lcd.init();
  lcd.backlight();

  Serial.begin(9600);
  lcd.setCursor(0, 0);
  lcd.print("  ATS System Boot ");
  delay(2000);
  lcd.clear();
}

void loop() {
  float mainsVoltage = readVoltage(MAINS_SENSOR_PIN);
  float genVoltage = readVoltage(GEN_SENSOR_PIN);

  Serial.print("Mains: "); Serial.print(mainsVoltage);
  Serial.print("V  Gen: "); Serial.print(genVoltage); Serial.println("V");

  // Display voltages on LCD
  lcd.setCursor(0, 0);
  lcd.print("M:");
  lcd.print(mainsVoltage, 0);
  lcd.print("V G:");
  lcd.print(genVoltage, 0);
  lcd.print("V ");

  if (mainsVoltage >= VOLTAGE_THRESHOLD) {
    if (!mainsAvailable) {
      switchToMains();
      stopGenerator();
      mainsAvailable = true;
      generatorRunning = false;
    }
  } else {
    if (mainsAvailable) {
      startGenerator();
      mainsAvailable = false;
    }

    if (generatorRunning && genVoltage >= VOLTAGE_THRESHOLD) {
      switchToGenerator();
    }
  }

  // Line 2: System status
  lcd.setCursor(0, 1);
  if (mainsAvailable) {
    lcd.print("Source: MAINS     ");
  } else if (generatorRunning && genVoltage >= VOLTAGE_THRESHOLD) {
    lcd.print("Source: GENERATOR ");
  } else {
    lcd.print("Waiting Generator ");
  }

  delay(1000);
}

void switchToMains() {
  digitalWrite(RELAY_GEN, LOW);
  delay(500);
  digitalWrite(RELAY_MAINS, HIGH);
  Serial.println("Switched to MAINS");
}

void switchToGenerator() {
  digitalWrite(RELAY_MAINS, LOW);
  delay(500);
  digitalWrite(RELAY_GEN, HIGH);
  Serial.println("Switched to GENERATOR");
}

void startGenerator() {
  Serial.println("Starting generator...");
  lcd.setCursor(0, 1);
  lcd.print("Starting Generator");
  digitalWrite(RELAY_GEN_START, HIGH);
  delay(GEN_START_DELAY);
  digitalWrite(RELAY_GEN_START, LOW);
  delay(GEN_WARMUP_DELAY);
  generatorRunning = true;
}

void stopGenerator() {
  Serial.println("Stopping generator...");
  lcd.setCursor(0, 1);
  lcd.print("Stopping Generator");
  digitalWrite(RELAY_GEN, LOW);
  delay(500);
  digitalWrite(RELAY_GEN_STOP, HIGH);
  delay(GEN_SHUTDOWN_DELAY);
  digitalWrite(RELAY_GEN_STOP, LOW);
}

float readVoltage(int analogPin) {
  int raw = analogRead(analogPin);
  float voltage = (raw / 1023.0) * 250.0;  // Adjust this factor for your sensor
  return voltage;
}