/*
  Emergency Vehicle Traffic Clearance System
  STM32 Nucleo-C031C6 - Wokwi

  Lane A:
    RED    -> PA0
    YELLOW -> PA1
    GREEN  -> PA4

  Lane B:
    RED    -> PB0
    YELLOW -> PC1
    GREEN  -> PC0

  Lane C:
    RED    -> PA8
    YELLOW -> PA9
    GREEN  -> PC7

  Emergency Button:
    PC13 (INPUT_PULLUP)
*/

#define LANE_A_RED    PA_0
#define LANE_A_YELLOW PA_1
#define LANE_A_GREEN  PA_4

#define LANE_B_RED    PB_0
#define LANE_B_YELLOW PC_1
#define LANE_B_GREEN  PC_0 

#define LANE_C_RED    PA_8
#define LANE_C_YELLOW PA_9
#define LANE_C_GREEN  PC_7

#define RF_BUTTON     PC_13

#define GREEN_TIME      4000
#define YELLOW_TIME     1000
#define EMERGENCY_HOLD  6000

typedef enum {
  STATE_LANE_A_GREEN,
  STATE_LANE_A_YELLOW,
  STATE_LANE_B_GREEN,
  STATE_LANE_B_YELLOW,
  STATE_LANE_C_GREEN,
  STATE_LANE_C_YELLOW,
  STATE_EMERGENCY
} TrafficState;

TrafficState currentState = STATE_LANE_A_GREEN;

unsigned long stateStartTime = 0;

void allRed() {

  digitalWrite(LANE_A_RED, HIGH);
  digitalWrite(LANE_A_YELLOW, LOW);
  digitalWrite(LANE_A_GREEN, LOW);

  digitalWrite(LANE_B_RED, HIGH);
  digitalWrite(LANE_B_YELLOW, LOW);
  digitalWrite(LANE_B_GREEN, LOW);

  digitalWrite(LANE_C_RED, HIGH);
  digitalWrite(LANE_C_YELLOW, LOW);
  digitalWrite(LANE_C_GREEN, LOW);
}

void setLaneGreen(int lane) {

  allRed();

  if (lane == 0) {
    digitalWrite(LANE_A_RED, LOW);
    digitalWrite(LANE_A_GREEN, HIGH);
  }

  else if (lane == 1) {
    digitalWrite(LANE_B_RED, LOW);
    digitalWrite(LANE_B_GREEN, HIGH);
  }

  else if (lane == 2) {
    digitalWrite(LANE_C_RED, LOW);
    digitalWrite(LANE_C_GREEN, HIGH);
  }
}

void setLaneYellow(int lane) {

  allRed();

  if (lane == 0) {
    digitalWrite(LANE_A_RED, LOW);
    digitalWrite(LANE_A_YELLOW, HIGH);
  }

  else if (lane == 1) {
    digitalWrite(LANE_B_RED, LOW);
    digitalWrite(LANE_B_YELLOW, HIGH);
  }

  else if (lane == 2) {
    digitalWrite(LANE_C_RED, LOW);
    digitalWrite(LANE_C_YELLOW, HIGH);
  }
}

void enterState(TrafficState newState) {

  currentState = newState;
  stateStartTime = millis();

  switch (newState) {

    case STATE_LANE_A_GREEN:
      setLaneGreen(0);
      Serial.println("Lane A GREEN");
      break;

    case STATE_LANE_A_YELLOW:
      setLaneYellow(0);
      Serial.println("Lane A YELLOW");
      break;

    case STATE_LANE_B_GREEN:
      setLaneGreen(1);
      Serial.println("Lane B GREEN");
      break;

    case STATE_LANE_B_YELLOW:
      setLaneYellow(1);
      Serial.println("Lane B YELLOW");
      break;

    case STATE_LANE_C_GREEN:
      setLaneGreen(2);
      Serial.println("Lane C GREEN");
      break;

    case STATE_LANE_C_YELLOW:
      setLaneYellow(2);
      Serial.println("Lane C YELLOW");
      break;

    case STATE_EMERGENCY:

      // Emergency vehicle gets Lane A GREEN
      setLaneGreen(0);

      Serial.println("EMERGENCY OVERRIDE ACTIVE");
      break;
  }
}

void setup() {

  Serial.begin(115200);

  pinMode(LANE_A_RED, OUTPUT);
  pinMode(LANE_A_YELLOW, OUTPUT);
  pinMode(LANE_A_GREEN, OUTPUT);

  pinMode(LANE_B_RED, OUTPUT);
  pinMode(LANE_B_YELLOW, OUTPUT);
  pinMode(LANE_B_GREEN, OUTPUT);

  pinMode(LANE_C_RED, OUTPUT);
  pinMode(LANE_C_YELLOW, OUTPUT);
  pinMode(LANE_C_GREEN, OUTPUT);

  pinMode(RF_BUTTON, INPUT_PULLUP);

  allRed();

  enterState(STATE_LANE_A_GREEN);
}

void loop() {

  unsigned long now = millis();

  bool rfSignal = (digitalRead(RF_BUTTON) == LOW);

  // Emergency detected
  if (rfSignal && currentState != STATE_EMERGENCY) {

    enterState(STATE_EMERGENCY);
    return;
  }

  // Stay in emergency mode
  if (currentState == STATE_EMERGENCY) {

    if (!rfSignal && (now - stateStartTime >= EMERGENCY_HOLD)) {

      Serial.println("Emergency Cleared");

      enterState(STATE_LANE_A_GREEN);
    }

    return;
  }

  switch (currentState) {

    case STATE_LANE_A_GREEN:

      if (now - stateStartTime >= GREEN_TIME) {
        enterState(STATE_LANE_A_YELLOW);
      }

      break;

    case STATE_LANE_A_YELLOW:

      if (now - stateStartTime >= YELLOW_TIME) {
        enterState(STATE_LANE_B_GREEN);
      }

      break;

    case STATE_LANE_B_GREEN:

      if (now - stateStartTime >= GREEN_TIME) {
        enterState(STATE_LANE_B_YELLOW);
      }

      break;

    case STATE_LANE_B_YELLOW:

      if (now - stateStartTime >= YELLOW_TIME) {
        enterState(STATE_LANE_C_GREEN);
      }

      break;

    case STATE_LANE_C_GREEN:

      if (now - stateStartTime >= GREEN_TIME) {
        enterState(STATE_LANE_C_YELLOW);
      }

      break;

    case STATE_LANE_C_YELLOW:

      if (now - stateStartTime >= YELLOW_TIME) {
        enterState(STATE_LANE_A_GREEN);
      }

      break;

    default:
      break;
  }
}