#include <LiquidCrystal_I2C.h>

#define LCD_ADDR 0x27
#define LCD_COLS 16
#define LCD_ROWS 2

LiquidCrystal_I2C lcd(LCD_ADDR, LCD_COLS, LCD_ROWS);

// LEDs (phase indicators)
#define R_LED PB0   // Red
#define Y_LED PB5   // Green
#define B_LED PB4   // Blue

// Slide switches (fault inputs)
#define SW_R PA0
#define SW_Y PA1
#define SW_B PA2

void setup() {
  // LCD
  lcd.init();
  lcd.backlight();

  // LEDs
  pinMode(R_LED, OUTPUT);
  pinMode(Y_LED, OUTPUT);
  pinMode(B_LED, OUTPUT);

  // Switches (IMPORTANT)
  pinMode(SW_R, INPUT_PULLUP);
  pinMode(SW_Y, INPUT_PULLUP);
  pinMode(SW_B, INPUT_PULLUP);

  lcd.setCursor(0, 0);
  lcd.print("R   Y   B");
}

void loop() {

  // -------- R PHASE --------
  digitalWrite(R_LED, HIGH);
  digitalWrite(Y_LED, LOW);
  digitalWrite(B_LED, LOW);

  lcd.setCursor(0, 1);
  if (digitalRead(SW_R) == LOW)
    lcd.print("F   ");
  else
    lcd.print("NF  ");

  lcd.print("    "); // clear Y B
  delay(1000);

  // -------- Y PHASE --------
  digitalWrite(R_LED, LOW);
  digitalWrite(Y_LED, HIGH);
  digitalWrite(B_LED, LOW);

  lcd.setCursor(4, 1);
  if (digitalRead(SW_Y) == LOW)
    lcd.print("F   ");
  else
    lcd.print("NF  ");

  delay(1000);

  // -------- B PHASE --------
  digitalWrite(R_LED, LOW);
  digitalWrite(Y_LED, LOW);
  digitalWrite(B_LED, HIGH);

  lcd.setCursor(8, 1);
  if (digitalRead(SW_B) == LOW)
    lcd.print("F   ");
  else
    lcd.print("NF  ");

  delay(1000);
}