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

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int GREEN_LED = D2;
const int YELLOW_LED = D3;
const int RED_LED = D4;
const int BUTTON = D5;

//==================================================
// STATE MACHINE
//==================================================

enum TrafficState
{
  NORMAL_GREEN,
  NORMAL_YELLOW,
  NORMAL_RED,

  TO_EMERGENCY,
  EMERGENCY,

  TO_NORMAL
};

TrafficState currentState = NORMAL_GREEN;

unsigned long stateStartTime;

//==================================================
// TIMINGS
//==================================================

const unsigned long GREEN_TIME = 3000;
const unsigned long YELLOW_TIME = 1000;
const unsigned long RED_TIME = 3000;

const unsigned long EMERGENCY_TIME = 4000;

//==================================================
// LCD HELPER
//==================================================

void showLCD(const char *line1,
             const char *line2)
{
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print(line1);

  lcd.setCursor(0, 1);
  lcd.print(line2);
}

//==================================================
// LIGHT STATES
//==================================================

void setGreen()
{
  digitalWrite(GREEN_LED, HIGH);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);

  showLCD(
      "Traffic Normal",
      "Green Signal");
}

void setYellow()
{
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, HIGH);
  digitalWrite(RED_LED, LOW);

  showLCD(
      "Traffic Normal",
      "Yellow Signal");
}

void setRed()
{
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, HIGH);

  showLCD(
      "Traffic Normal",
      "Red Signal");
}

void setAmbulanceGreen()
{
  digitalWrite(GREEN_LED, HIGH);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);

  showLCD(
      "Ambulance",
      "Crossing");
}

//==================================================
// SETUP
//==================================================

void setup()
{
  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  pinMode(BUTTON, INPUT_PULLUP);

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

  setGreen();

  stateStartTime = millis();
}

//==================================================
// LOOP
//==================================================

void loop()
{
  //------------------------------------------------
  // Ambulance Request
  //------------------------------------------------

  if ((currentState == NORMAL_GREEN ||
       currentState == NORMAL_YELLOW ||
       currentState == NORMAL_RED) &&
      digitalRead(BUTTON) == LOW)
  {
    currentState = TO_EMERGENCY;

    stateStartTime = millis();

    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, HIGH);
    digitalWrite(RED_LED, LOW);

    showLCD(
        "Ambulance",
        "Incoming");
  }

  //------------------------------------------------
  // State Machine
  //------------------------------------------------

  switch (currentState)
  {
  case NORMAL_GREEN:

    if (millis() - stateStartTime >= GREEN_TIME)
    {
      currentState = NORMAL_YELLOW;

      stateStartTime = millis();

      setYellow();
    }

    break;

  case NORMAL_YELLOW:

    if (millis() - stateStartTime >= YELLOW_TIME)
    {
      currentState = NORMAL_RED;

      stateStartTime = millis();

      setRed();
    }

    break;

  case NORMAL_RED:

    if (millis() - stateStartTime >= RED_TIME)
    {
      currentState = NORMAL_GREEN;

      stateStartTime = millis();

      setGreen();
    }

    break;

  case TO_EMERGENCY:

    if (millis() - stateStartTime >= 2000)
    {
      currentState = EMERGENCY;

      stateStartTime = millis();

      setAmbulanceGreen();
    }

    break;

  case EMERGENCY:

    if (millis() - stateStartTime >= EMERGENCY_TIME)
    {
      currentState = TO_NORMAL;

      stateStartTime = millis();

      digitalWrite(GREEN_LED, LOW);
      digitalWrite(YELLOW_LED, HIGH);
      digitalWrite(RED_LED, LOW);

      showLCD(
          "Traffic",
          "Restoring");
    }

    break;

  case TO_NORMAL:

    if (millis() - stateStartTime >= 2000)
    {
      currentState = NORMAL_GREEN;

      stateStartTime = millis();

      setGreen();
    }

    break;
  }
}