#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Pin definitions
const int phaseBPin = 9;
const int phaseYPin = 8;
const int phaseRPin = 7;

const int relayBPin = 10;
const int relayYPin = 11;
const int relayRPin = 12;

// OLED display width and height
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Create an instance for the OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  // Initialize the phase pins as inputs
  pinMode(phaseBPin, INPUT);
  pinMode(phaseYPin, INPUT);
  pinMode(phaseRPin, INPUT);

  // Initialize the relay pins as outputs
  pinMode(relayBPin, OUTPUT);
  pinMode(relayYPin, OUTPUT);
  pinMode(relayRPin, OUTPUT);

  // Ensure all relays are initially LOW (OFF)
  digitalWrite(relayBPin, LOW);
  digitalWrite(relayYPin, LOW);
  digitalWrite(relayRPin, LOW);

  // Initialize the OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    // If the OLED display cannot initialize, do nothing
    for (;;);
  }
  display.clearDisplay();
  display.display();
}

void loop() {
  // Read the phase pin states
  bool phaseB = digitalRead(phaseBPin);
  bool phaseY = digitalRead(phaseYPin);
  bool phaseR = digitalRead(phaseRPin);

  // Priority-based relay control: Phase R > Phase Y > Phase B
  if (phaseR == HIGH) {
    // If phase R is available, make only relay R HIGH
    digitalWrite(relayRPin, HIGH);
    digitalWrite(relayBPin, LOW);
    digitalWrite(relayYPin, LOW);
  } else if (phaseY == HIGH) {
    // If phase R is not available, and phase Y is available, make only relay Y HIGH
    digitalWrite(relayYPin, HIGH);
    digitalWrite(relayBPin, LOW);
    digitalWrite(relayRPin, LOW);
  } else if (phaseB == HIGH) {
    // If both R and Y are unavailable, and phase B is available, make only relay B HIGH
    digitalWrite(relayBPin, HIGH);
    digitalWrite(relayYPin, LOW);
    digitalWrite(relayRPin, LOW);
  } else {
    // If all phases are LOW, make all relays LOW
    digitalWrite(relayBPin, LOW);
    digitalWrite(relayYPin, LOW);
    digitalWrite(relayRPin, LOW);
  }

  // Display phase input and relay output status on the OLED
  display.clearDisplay();

  // Display the input statuses (B, Y, R)
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Auto Phase Changer...");
  display.setCursor(0, 20);
  display.print("In  B:");
  display.print(phaseB == HIGH ? "H" : "L");
  display.print("  Y:");
  display.print(phaseY == HIGH ? "H" : "L");
  display.print("  R:");
  display.print(phaseR == HIGH ? "H" : "L");

  // Display the relay output statuses (B, Y, R)
  display.setCursor(0, 40);
  display.print("Out B:");
  display.print(digitalRead(relayBPin) == HIGH ? "H" : "L");
  display.print("  Y:");
  display.print(digitalRead(relayYPin) == HIGH ? "H" : "L");
  display.print("  R:");
  display.print(digitalRead(relayRPin) == HIGH ? "H" : "L");

  // Show the display
  display.display();

  // Short delay to debounce input
  delay(100);
}