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

// LCD address (0x27 for most I2C LCDs, 0x3F for some others)
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Adjust the address, columns, and rows as needed

// Pin definitions
const int switch1Pin = 2;
const int switch2Pin = 3;
const int switch3Pin = 4;
const int bypassButtonPin = 5;
const int ledPin = 13;  // Output LED pin
const int statusLedGreenPin = 6;  // Green LED pin
const int statusLedRedPin = 7;    // Red LED pin
const int statusLedBluePin = 12;  // Blue LED pin

// Variables to store the switch states
bool switch1State = false;
bool switch2State = false;
bool switch3State = false;
bool bypassState = false;

void setup() {
  // Initialize the pins
  pinMode(switch1Pin, INPUT_PULLUP);
  pinMode(switch2Pin, INPUT_PULLUP);
  pinMode(switch3Pin, INPUT_PULLUP);
  pinMode(bypassButtonPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(statusLedGreenPin, OUTPUT);
  pinMode(statusLedRedPin, OUTPUT);
  pinMode(statusLedBluePin, OUTPUT);

  // Initialize LCD
  lcd.begin(16, 2);  // Initialize with number of columns and rows
  lcd.backlight();
  
  // Ensure LEDs are initially off
  digitalWrite(ledPin, LOW);
  digitalWrite(statusLedGreenPin, LOW);
  digitalWrite(statusLedRedPin, LOW);
  digitalWrite(statusLedBluePin, LOW);
}

void loop() {
  // Read the states of the switches and the bypass button
  switch1State = digitalRead(switch1Pin) == LOW;
  switch2State = digitalRead(switch2Pin) == LOW;
  switch3State = digitalRead(switch3Pin) == LOW;
  bypassState = digitalRead(bypassButtonPin) == LOW;

  if (bypassState) {
    // Bypass is on
    digitalWrite(ledPin, HIGH);  // LED is on regardless of other switches
    digitalWrite(statusLedGreenPin, LOW);
    digitalWrite(statusLedRedPin, LOW);
    digitalWrite(statusLedBluePin, LOW);
    lcd.setCursor(0, 0);
    lcd.print("System Bypass");
    lcd.setCursor(0, 1);
    
    if (!switch2State && !switch3State) {
      lcd.print("RfTnk Empty");
    } else if (switch2State && switch3State) {
      lcd.print("RfTnk Full");
    } else if (switch2State && !switch3State) {
      lcd.print("RfTnk Ready");
    } else if (!switch2State && switch3State) {
      lcd.print("RfTnk Error");
    }
  } else {
    // Bypass is off
    if (switch1State) {
      // Switch 1 is on
      if (!switch2State) {
        // S2 is off
        digitalWrite(ledPin, HIGH);
        digitalWrite(statusLedGreenPin, LOW);
        digitalWrite(statusLedBluePin, HIGH);
        digitalWrite(statusLedRedPin, LOW);
        lcd.setCursor(0, 0);
        lcd.print("GrTnk Ready");
        
        if (!switch3State) {
          // S3 is off
          lcd.setCursor(0, 1);
          lcd.print("RfTnk Filling ^");
        } else {
          // S3 is on
          lcd.setCursor(0, 1);
          lcd.print("RfTnk Ready");
        }
      } else {
        // S2 is on
        if (switch3State) {
          // S3 is on
          digitalWrite(ledPin, LOW);
          digitalWrite(statusLedGreenPin, HIGH);
          digitalWrite(statusLedBluePin, LOW);
          digitalWrite(statusLedRedPin, LOW);
          lcd.setCursor(0, 0);
          lcd.print("GrTnk Ready");
          lcd.setCursor(0, 1);
          lcd.print("RfTnk Full");
        } else {
          // S3 is off
          digitalWrite(ledPin, LOW);
          digitalWrite(statusLedGreenPin, LOW);
          digitalWrite(statusLedBluePin, LOW);
          digitalWrite(statusLedRedPin, HIGH);
          lcd.setCursor(0, 0);
          lcd.print("GrTnk Ready");
          lcd.setCursor(0, 1);
          lcd.print("RfTnk InUse");
        }
      }
    } else {
      // Switch 1 is off
      digitalWrite(ledPin, LOW);
      digitalWrite(statusLedGreenPin, LOW);
      digitalWrite(statusLedBluePin, LOW);
      digitalWrite(statusLedRedPin, HIGH);
      lcd.setCursor(0, 0);
      lcd.print("GrTnk Empty");
      
      if (!switch2State && !switch3State) {
        lcd.setCursor(0, 1);
        lcd.print("RfTnk Empty");
      } else if (switch2State && switch3State) {
        lcd.setCursor(0, 1);
        lcd.print("RfTnk Full");
      } else if (switch2State && !switch3State) {
        lcd.setCursor(0, 1);
        lcd.print("RfTnk Ready");
      } else if (!switch2State && switch3State) {
        lcd.setCursor(0, 1);
        lcd.print("RfTnk Error");
      }
    }
  }

  delay(500);  // Update display and LED states every 500 ms
}

// Function to blink the status LED
void blinkStatusLed(int pin, bool state) {
  static unsigned long previousMillis = 0;
  const long interval = 500;  // Interval in milliseconds for blinking

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    if (state) {
      digitalWrite(pin, !digitalRead(pin));  // Toggle the LED state
    } else {
      digitalWrite(pin, LOW);  // Turn off the LED
    }
  }
}