Wokwi - Online ESP32, STM32, Arduino Simulator


#define red_S1_S4_mask        0b00100000 // Mask associated to PORTB Red Led for traffic light S14
#define yellow_S1_S4_mask     0b00010000 // Mask associated to PORTB Yellow Led for traffic light S14
#define green_S1_S4_mask      0b00001000 // Mask associated to PORTB Green Led for traffic light S14

#define red_S2_S3_mask        0b00000100 // Mask associated to PORTB Red Led for traffic light S23
#define yellow_S2_S3_mask     0b00000010 // Mask associated to PORTB Yellow Led for traffic light S23
#define green_S2_S3_mask      0b00000001 // Mask associated to PORTB Green Led for traffic light S23

#define pedestrian_S1_S4_mask 0b10000000 // Mask associated to PORTD Pedestrian Call Button for traffic light S14
#define traffic_S1_S4_mask    0b01000000 // Mask associated to PORTD Traffic Switch for traffic light S14
#define violation_S1_mask     0b00100000 // Mask associated to PORTD Violation Led for traffic light S1
#define violation_S4_mask     0b00010000 // Mask associated to PORTD Violation Led for traffic light S4

#define pedestrian_S2_S3_mask 0b00001000 // Mask associated to PORTD Pedestrian Call Button for traffic light S23
#define traffic_S2_S3_mask    0b00000100 // Mask associated to PORTD Traffic Switch for traffic light S23
#define violation_S2_mask     0b00000001 // Mask associated to PORTC Violation Led for traffic light S2
#define violation_S3_mask     0b00000010 // Mask associated to PORTC Violation Led for traffic light S3

#define speed_sensor_S1_mask  0b00000100 // Mask associated to PORTC Speed Potentiometer for traffic light S1
#define speed_sensor_S4_mask  0b00001000 // Mask associated to PORTC Speed Potentiometer for traffic light S4

#define speed_sensor_S2_mask  0b00010000 // Mask associated to PORTC Speed Potentiometer for traffic light S2
#define speed_sensor_S3_mask  0b00100000 // Mask associated to PORTC Speed Potentiometer for traffic light S3

#define key_mask              0b01000000 // Mask associated to PORTC Key Switch for all traffic lights

// Global Variables
volatile uint8_t trafficLightStatusS1 = 0;
volatile uint8_t trafficLightStatusS4 = 0;
volatile uint8_t trafficLightStatusS2 = 0;
volatile uint8_t trafficLightStatusS3 = 0;

// Global Variables bits
#define isPedestrianCallInHandle 0b10000000
#define pedestrianBit            0b01000000
#define keyBit                   0b00100000
#define violationBit             0b00010000
#define trafficBit               0b00001000
#define redBit                   0b00000100
#define yellowBit                0b00000010
#define greenBit                 0b00000001

#define setBit(trafficLightsSxSyStatus, mask) (trafficLightsSxSyStatus |= (mask))
#define clearBit(trafficLightsSxSyStatus, mask) (trafficLightsSxSyStatus &= ~(mask))
#define isBitSet(trafficLightsSxSyStatus, mask) ((trafficLightsSxSyStatus & (mask)) != 0)

volatile uint16_t systemTimerS1S4 = 0;
volatile uint16_t systemTimerS2S3 = 0;
volatile uint32_t timeSinceLastPedestrianCallForS1S4 = 0;
volatile uint32_t timeSinceLastPedestrianCallForS2S3 = 0;

void setup() {
  //Serial.begin(115200);
  setupTrafficLightsS1S4();
  setupTrafficLightsS2S3();
  setupKey();
  setupAnalogicToDigitalConverter();
  // Timer Setup
  cli(); // Critical Section
  TCCR1A = 0; // Timer1 normal mode
  TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10); // CTC Mode, prescaler 64
  OCR1A = 249; // Interrupt every 1 ms thanks to 64 & 249
  TIMSK1 = _BV(OCIE1A); // Interrupt for OCR1A
  sei(); // Critical Section End
}

void setupTrafficLightsS1S4() {
  DDRB |= 0b00111000;
  PORTD |= _BV(7); // Input Pullup for pedestrian_S1_S4
  PORTD |= _BV(6); // Input Pullup for traffic_S1_S4
  trafficLightStatusS1 = setBit(trafficLightStatusS1, yellowBit); // Blinking Yellow
  trafficLightStatusS4 = setBit(trafficLightStatusS4, yellowBit); // Blinking Yellow
  PORTB |= yellow_S1_S4_mask;
}

void setupTrafficLightsS2S3() {
  DDRB |= 0b00000111;
  PORTD |= _BV(3); // Input Pullup for pedestrian_S1_S4
  PORTD |= _BV(2); // Input Pullup for traffic_S1_S4
  trafficLightStatusS2 = setBit(trafficLightStatusS2, yellowBit); // Blinking Yellow
  trafficLightStatusS3 = setBit(trafficLightStatusS3, yellowBit); // Blinking Yellow
  PORTB |= yellow_S2_S3_mask;
}

void setupKey() {
  PORTC |= key_mask;
}

void setupAnalogicToDigitalConverter() {
  ADMUX =  0b01000000; // REFS0 Voltage Reference
  ADCSRA = 0b10000111; // ADEN, ADPS2, ADPS1, ADPS0 (ADC enable, division factor 128)
}

// Timer update

ISR(TIMER1_COMPA_vect) {
  systemTimerS1S4++; // Every millisecond systemTimerS1S4++
  systemTimerS2S3++; // Every millisecond systemTimerS2S3++
  if (! isBitSet(trafficLightStatusS1, isPedestrianCallInHandle) 
      || ! isBitSet(trafficLightStatusS4, isPedestrianCallInHandle))
        timeSinceLastPedestrianCallForS1S4++;
  if (! isBitSet(trafficLightStatusS2, isPedestrianCallInHandle) 
      || ! isBitSet(trafficLightStatusS3, isPedestrianCallInHandle))
        timeSinceLastPedestrianCallForS2S3++;
}

void loop() {
  handleS1();
  handleS2();
  //handleS4();
  //handleS3();
}

///////////////////////////////////////////////////////////////////////////////////
// S1 TRAFFIC LIGHT HANDLING
///////////////////////////////////////////////////////////////////////////////////

void handleS1() {
  // handleS1Detections();
  handleS1PedestrianCallDetection();
  handleS1TrafficDetection();
  handleS1SpeedDetection();
  if (! isKeyActive()) 
    handleS1KeyDetection();
  if (isPedestrianCallActiveForS1() || isS1HandlingAPedestrianCall())
    handlePedestrianCallForS1();
  if (isKeyActive() && ! isS1HandlingAPedestrianCall()) 
    handleKeyS1();
  else if (! isS1HandlingAPedestrianCall())
    handleTrafficForS1();
}

void handlePedestrianCallForS1() {
  if (isS1HandlingAPedestrianCall() && (isNoColorForS1() || isGreenOnForS1() || isYellowOnForS1() || isRedOnForS1())) {
    doNoColorRecoveryForS1(); // recovery in case of no color
    doGreenForS1(); // recovery in case of green
    handleYellowRedPedestrianForS1();
  } else if (isPedestrianCallActiveForS1() && (isCrossingClear() && isNoTraffic()) || isKeyActive()) {
    setBit(trafficLightStatusS1, isPedestrianCallInHandle);
  } else if (isPedestrianCallActiveForS1() && isTrafficActiveForS1() && timeSinceLastPedestrianCallForS1S4 >= 300000) {
    setBit(trafficLightStatusS1, isPedestrianCallInHandle);
  }
}

void handleYellowRedPedestrianForS1() {
  doYellowForS1();
  if (isRedOnForS1() && systemTimerS1S4 >= 20000) {
    clearBit(trafficLightStatusS1, redBit);
    clearBit(trafficLightStatusS1, isPedestrianCallInHandle);
    clearBit(trafficLightStatusS1, pedestrianBit);
    PORTB &= ~red_S1_S4_mask;
    systemTimerS1S4 = 0;
    timeSinceLastPedestrianCallForS1S4 = 0;
    if (! isTrafficActiveForS1() || isNoTraffic() || isKeyActive()) {
      setBit(trafficLightStatusS1, yellowBit);
      PORTB |= yellow_S1_S4_mask;
    } else if (isTrafficActiveForS1()) {
      setBit(trafficLightStatusS1, greenBit);
      PORTB |= green_S1_S4_mask;
    }
  }
}

void handleKeyS1() {
  handleBlinkingYellowS1();
  handleS1KeyDetection();
  if (! isKeyActive()) {
    // Acquiring green for traffic on both flows
    if (isTrafficActiveForS1() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
      clearBit(trafficLightStatusS1, yellowBit);
      PORTB &= ~yellow_S1_S4_mask;
      setBit(trafficLightStatusS1, greenBit);
      PORTB |= green_S1_S4_mask;
    }
    systemTimerS1S4 = 0;
  }
}

void handleTrafficForS1() {
  if (isTrafficActiveForS1() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
    startFAIRCycleForS1();
  } else if (isTrafficActiveForS1() && (! isTrafficActiveForS2() || ! isTrafficActiveForS3())) {
    handleMainitainingGreenS1();
  } else if (! isTrafficActiveForS1() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
    handleYellowRedS1();
  } else if (isNoTraffic()) {
    handleBlinkingYellowS1();
  }
}

void startFAIRCycleForS1() {
  // Recovery from key in case S4 acquired green color upon key exit

  // NON FUNZIONAAA
  if (isYellowOnForS1() && isGreenOnForS4()) { 
    //Serial.println("here");
    clearBit(trafficLightStatusS1, yellowBit);
    setBit(trafficLightStatusS1, greenBit);
    systemTimerS1S4 = 0;
  }
  doNoColorRecoveryForS1(); // Immediate recovery from no color, if active
  doYellowForS1();
  doRedForS1();
  doGreenForS1();
}

void doNoColorRecoveryForS1() {
  if (isNoColorForS1()) {
    setBit(trafficLightStatusS1, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doYellowForS1() {
  if (isYellowOnForS1() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS1, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS1, redBit);
    PORTB |= red_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doRedForS1() {
  if (isRedOnForS1() && systemTimerS1S4 >= 10000) {
    clearBit(trafficLightStatusS1, redBit);
    PORTB &= ~red_S1_S4_mask;
    setBit(trafficLightStatusS1, greenBit);
    PORTB |= green_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doGreenForS1() {
  if (isS2HandlingAPedestrianCall() || isS3HandlingAPedestrianCall()) {
    if (isGreenOnForS1() && systemTimerS1S4 >= 20000) {
      clearBit(trafficLightStatusS1, greenBit);
      PORTB &= ~green_S1_S4_mask;
      setBit(trafficLightStatusS1, yellowBit);
      PORTB |= yellow_S1_S4_mask;
      systemTimerS1S4 = 0;
    }
  } else if (isGreenOnForS1() && systemTimerS1S4 >= 10000) {
    clearBit(trafficLightStatusS1, greenBit);
    PORTB &= ~green_S1_S4_mask;
    setBit(trafficLightStatusS1, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleMainitainingGreenS1() {
  if (! isGreenOnForS1()) { // recovery from red, yellow or no color
    clearBit(trafficLightStatusS1, redBit);
    PORTB &= ~red_S1_S4_mask;
    clearBit(trafficLightStatusS1, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS1, greenBit);
    PORTB |= green_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleYellowRedS1() {
  if (! isYellowOnForS1() && ! isRedOnForS1()) { // recovery from green or no color
    clearBit(trafficLightStatusS1, greenBit);
    PORTB &= ~green_S1_S4_mask;
    setBit(trafficLightStatusS1, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  } else if (isYellowOnForS1() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS1, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS1, redBit);
    PORTB |= red_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleBlinkingYellowS1() {
  if (! isYellowOnForS1() && ! isNoColorForS1()) { // recovery from red or green
    clearBit(trafficLightStatusS1, redBit);
    PORTB &= ~red_S1_S4_mask; 
    clearBit(trafficLightStatusS1, greenBit);
    PORTB &= ~green_S1_S4_mask;
    systemTimerS1S4 = 0;
  } else if (isYellowOnForS1() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS1, yellowBit);
    PORTB &= ~yellow_S1_S4_mask; 
    systemTimerS1S4 = 0;
  }
  doNoColorForS1();
}

void doNoColorForS1() {
  if (isNoColorForS1() && systemTimerS1S4 >= 1000) {
    setBit(trafficLightStatusS1, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

///////////////////////////////////////////////////////////////////////////////////
// S4 TRAFFIC LIGHT HANDLING
///////////////////////////////////////////////////////////////////////////////////

void handleS4() {
  handleS4PedestrianCallDetection();
  handleS4TrafficDetection();
  handleS4SpeedDetection();
  if (! isKeyActive()) 
    handleS4KeyDetection();
  if (isPedestrianCallActiveForS4() || isS4HandlingAPedestrianCall())
    handlePedestrianCallForS4();
  if (isKeyActive() && ! isS4HandlingAPedestrianCall()) 
    handleKeyS4();
  else if (! isS4HandlingAPedestrianCall())
    handleTrafficForS4();
}

void handlePedestrianCallForS4() {
  if (isS4HandlingAPedestrianCall() && (isNoColorForS4() || isGreenOnForS4() || isYellowOnForS4() || isRedOnForS4())) {
    doNoColorForS4(); // recovery in case of no color
    doGreenForS4(); // recovery in case of green
    handleYellowRedPedestrianForS4();
  } else if (isPedestrianCallActiveForS4() && (isCrossingClear() && isNoTraffic()) || isKeyActive()) {
    setBit(trafficLightStatusS4, isPedestrianCallInHandle);
  } else if (isPedestrianCallActiveForS4() && isTrafficActiveForS4() && timeSinceLastPedestrianCallForS1S4 >= 300000) {
    setBit(trafficLightStatusS4, isPedestrianCallInHandle);
  }
}

void handleYellowRedPedestrianForS4() {
  doYellowForS4();
  if (isRedOnForS4() && systemTimerS1S4 >= 20000) {
    clearBit(trafficLightStatusS4, redBit);
    clearBit(trafficLightStatusS4, isPedestrianCallInHandle);
    clearBit(trafficLightStatusS4, pedestrianBit);
    PORTB &= ~red_S1_S4_mask;
    systemTimerS1S4 = 0;
    timeSinceLastPedestrianCallForS1S4 = 0;
    if (! isTrafficActiveForS4() || isNoTraffic() || isKeyActive()) {
      setBit(trafficLightStatusS4, yellowBit);
      PORTB |= yellow_S1_S4_mask;
    } else if (isTrafficActiveForS4()) {
      setBit(trafficLightStatusS4, greenBit);
      PORTB |= green_S1_S4_mask;
    }
  }
}

void handleKeyS4() {
  handleBlinkingYellowS4();
  handleS4KeyDetection();
  if (! isKeyActive()) {
    // Acquiring green for traffic on both flows
    if (isTrafficActiveForS4() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
      clearBit(trafficLightStatusS4, yellowBit);
      PORTB &= ~yellow_S1_S4_mask;
      setBit(trafficLightStatusS4, greenBit);
      PORTB |= green_S1_S4_mask;
      systemTimerS1S4 = 0;
    }
  }
}

void handleTrafficForS4() {
  if (isTrafficActiveForS4() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
    startFAIRCycleForS4();
  } else if (isTrafficActiveForS4() && (! isTrafficActiveForS2() || ! isTrafficActiveForS3())) {
    handleMainitainingGreenS4();
  } else if (! isTrafficActiveForS4() && (isTrafficActiveForS2() || isTrafficActiveForS3())) {
    handleYellowRedS4();
  } else if (isNoTraffic()) {
    handleBlinkingYellowS4();
  }
}

void startFAIRCycleForS4() {
  // Recovery from key in case S1 acquired green color upon key exit
  if (isYellowOnForS4() && isGreenOnForS1()) {
    clearBit(trafficLightStatusS4, yellowBit);
    setBit(trafficLightStatusS4, greenBit);
  }
  doNoColorForS4(); // recovery from no color if active
  doYellowForS4();
  doRedForS4();
  doGreenForS4();
}

void doNoColorForS4() {
  if (isNoColorForS4() && systemTimerS1S4 >= 1000) {
    setBit(trafficLightStatusS4, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doYellowForS4() {
  if (isYellowOnForS4() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS4, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS4, redBit);
    PORTB |= red_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doRedForS4() {
  if (isRedOnForS4() && systemTimerS1S4 >= 10000) {
    clearBit(trafficLightStatusS4, redBit);
    PORTB &= ~red_S1_S4_mask;
    setBit(trafficLightStatusS4, greenBit);
    PORTB |= green_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void doGreenForS4() {
  if (isS2HandlingAPedestrianCall() || isS3HandlingAPedestrianCall()) {
    if (isGreenOnForS4() && systemTimerS1S4 >= 20000) {
      clearBit(trafficLightStatusS4, greenBit);
      PORTB &= ~green_S1_S4_mask;
      setBit(trafficLightStatusS4, yellowBit);
      PORTB |= yellow_S1_S4_mask;
      systemTimerS1S4 = 0;
    }
  } else if (isGreenOnForS4() && systemTimerS1S4 >= 10000) {
    clearBit(trafficLightStatusS4, greenBit);
    PORTB &= ~green_S1_S4_mask;
    setBit(trafficLightStatusS4, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleMainitainingGreenS4() {
  if (! isGreenOnForS4()) { // recovery from red or no color
    clearBit(trafficLightStatusS4, redBit);
    PORTB &= ~red_S1_S4_mask;
    clearBit(trafficLightStatusS4, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS4, greenBit);
    PORTB |= green_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleYellowRedS4() {
  if (! isYellowOnForS4() && ! isRedOnForS4()) { // recovery from green or no color
    clearBit(trafficLightStatusS4, greenBit);
    PORTB &= ~green_S1_S4_mask;
    setBit(trafficLightStatusS4, yellowBit);
    PORTB |= yellow_S1_S4_mask;
    systemTimerS1S4 = 0;
  } else if (isYellowOnForS4() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS4, yellowBit);
    PORTB &= ~yellow_S1_S4_mask;
    setBit(trafficLightStatusS4, redBit);
    PORTB |= red_S1_S4_mask;
    systemTimerS1S4 = 0;
  }
}

void handleBlinkingYellowS4() {
  if (! isYellowOnForS4() && ! isNoColorForS4()) { // recovery from red or green
    clearBit(trafficLightStatusS4, redBit);
    PORTB &= ~red_S1_S4_mask; 
    clearBit(trafficLightStatusS4, greenBit);
    PORTB &= ~green_S1_S4_mask;
    systemTimerS1S4 = 0;
  } else if (isYellowOnForS4() && systemTimerS1S4 >= 2000) {
    clearBit(trafficLightStatusS4, yellowBit);
    PORTB &= ~yellow_S1_S4_mask; 
    systemTimerS1S4 = 0;
  }
  doNoColorForS4();
}

///////////////////////////////////////////////////////////////////////////////////
// S2 TRAFFIC LIGHT HANDLING
///////////////////////////////////////////////////////////////////////////////////

void handleS2() {
  handleS2PedestrianCallDetection();
  handleS2TrafficDetection();
  handleS2SpeedDetection();
  if (! isKeyActive()) 
    handleS2KeyDetection();
  if (isPedestrianCallActiveForS2() || isS2HandlingAPedestrianCall())
    handlePedestrianCallForS2();
  if (isKeyActive() && ! isS2HandlingAPedestrianCall()) 
    handleKeyS2();
  else if (! isS2HandlingAPedestrianCall())
    handleTrafficForS2();
}

void handlePedestrianCallForS2() {
  if (isS2HandlingAPedestrianCall() && (isNoColorForS2() || isGreenOnForS2() || isYellowOnForS2() || isRedOnForS2())) {
    doNoColorForS2(); // recovery in case of no color
    doGreenForS2(); // recovery from green
    handleYellowRedPedestrianForS2();
  } else if (isPedestrianCallActiveForS2() && ((isCrossingClear() && isNoTraffic()) || isKeyActive())) {
    setBit(trafficLightStatusS2, isPedestrianCallInHandle);
  } else if (isPedestrianCallActiveForS2() && isTrafficActiveForS2() && timeSinceLastPedestrianCallForS2S3 >= 300000) {
    setBit(trafficLightStatusS2, isPedestrianCallInHandle);
  }
}

void handleYellowRedPedestrianForS2() {
  doYellowForS2();
  if (isRedOnForS2() && systemTimerS2S3 >= 20000) {
    clearBit(trafficLightStatusS2, redBit);
    clearBit(trafficLightStatusS2, isPedestrianCallInHandle);
    clearBit(trafficLightStatusS2, pedestrianBit);
    PORTB &= ~red_S2_S3_mask;
    systemTimerS2S3 = 0;
    timeSinceLastPedestrianCallForS2S3 = 0;
    if (! isTrafficActiveForS2() || isNoTraffic() || isKeyActive()) {
      setBit(trafficLightStatusS2, yellowBit);
      PORTB |= yellow_S2_S3_mask;
    } else if (isTrafficActiveForS2()) {
      setBit(trafficLightStatusS2, greenBit);
      PORTB |= green_S2_S3_mask;
    }
  }
}

void handleKeyS2() {
  handleBlinkingYellowS2();
  handleS2KeyDetection();
  if (! isKeyActive()) {
    // Acquiring green for traffic on both flows
    if (isTrafficActiveForS2() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
      clearBit(trafficLightStatusS2, yellowBit);
      PORTB &= ~yellow_S2_S3_mask;
      setBit(trafficLightStatusS2, greenBit);
      PORTB |= green_S2_S3_mask;
    }
    systemTimerS2S3 = 0;
  }
}

void handleTrafficForS2() {
  if (isTrafficActiveForS2() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
    startFAIRCycleForS2();
  } else if (isTrafficActiveForS2() && (! isTrafficActiveForS1() || ! isTrafficActiveForS4())) {
    handleMainitainingGreenS2();
  } else if (! isTrafficActiveForS2() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
    handleYellowRedS2();
  } else if (isNoTraffic()) {
    handleBlinkingYellowS2();
  }
}

void startFAIRCycleForS2() {
  // Recovery from key in case S3 acquired green color upon key exit
  if (isYellowOnForS2() && isGreenOnForS3()) {
    clearBit(trafficLightStatusS2, yellowBit);
    setBit(trafficLightStatusS2, greenBit);
    systemTimerS2S3 = 0;
  }
  doNoColorRecoveryForS2(); // Immediate recovery from no color, if active
  doYellowForS2();
  doRedForS2();
  doGreenForS2();
}

void doNoColorRecoveryForS2() {
  if (isNoColorForS2()) {
    setBit(trafficLightStatusS2, yellowBit);
    PORTB |= yellow_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doYellowForS2() {
  if (isYellowOnForS2() && systemTimerS2S3 >= 2000) {
    clearBit(trafficLightStatusS2, yellowBit);
    PORTB &= ~yellow_S2_S3_mask;
    setBit(trafficLightStatusS2, redBit);
    PORTB |= red_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doRedForS2() {
  if (isRedOnForS2() && systemTimerS2S3 >= 10000) {
    clearBit(trafficLightStatusS2, redBit);
    PORTB &= ~red_S2_S3_mask;
    setBit(trafficLightStatusS2, greenBit);
    PORTB |= green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doGreenForS2() {
  if (isS1HandlingAPedestrianCall() || isS4HandlingAPedestrianCall()) {
    if (isGreenOnForS2() && systemTimerS2S3 >= 20000) {
      clearBit(trafficLightStatusS2, greenBit);
      PORTB &= ~green_S2_S3_mask;
      setBit(trafficLightStatusS2, yellowBit);
      PORTB |= yellow_S2_S3_mask;
      systemTimerS2S3 = 0;
    }
  } else if (isGreenOnForS2() && systemTimerS2S3 >= 10000) {
      clearBit(trafficLightStatusS2, greenBit);
      PORTB &= ~green_S2_S3_mask;
      setBit(trafficLightStatusS2, yellowBit);
      PORTB |= yellow_S2_S3_mask;
      systemTimerS2S3 = 0;
  }
}

void handleMainitainingGreenS2() {
  if (! isGreenOnForS2()) { // recovery from red or no color
    clearBit(trafficLightStatusS2, redBit);
    PORTB &= ~red_S2_S3_mask;
    clearBit(trafficLightStatusS2, yellowBit);
    PORTB &= ~yellow_S2_S3_mask;
    setBit(trafficLightStatusS2, greenBit);
    PORTB |= green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void handleYellowRedS2() {
  if (! isYellowOnForS2() && ! isRedOnForS2()) { // recovery from green or no color
    clearBit(trafficLightStatusS2, greenBit);
    PORTB &= ~green_S2_S3_mask;
    setBit(trafficLightStatusS2, yellowBit);
    PORTB |= yellow_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
  doYellowForS2();
}

void handleBlinkingYellowS2() {
  if (! isYellowOnForS2() && ! isNoColorForS2()) { // recovery from red or green
    clearBit(trafficLightStatusS2, redBit);
    PORTB &= ~red_S2_S3_mask; 
    clearBit(trafficLightStatusS2, greenBit);
    PORTB &= ~green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
  if (isYellowOnForS2() && systemTimerS2S3 >= 2000) {
    clearBit(trafficLightStatusS2, yellowBit);
    PORTB &= ~yellow_S2_S3_mask; 
    systemTimerS2S3 = 0;
  }
  doNoColorForS2();
}

void doNoColorForS2() {
  if (isNoColorForS2() && systemTimerS2S3 >= 1000) {
    setBit(trafficLightStatusS2, yellowBit);
    PORTB |= yellow_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

///////////////////////////////////////////////////////////////////////////////////
// S3 TRAFFIC LIGHT HANDLING
///////////////////////////////////////////////////////////////////////////////////

void handleS3() {
  handleS3PedestrianCallDetection();
  handleS3TrafficDetection();
  handleS3SpeedDetection();
  if (! isKeyActive()) 
    handleS3KeyDetection();
  if (isPedestrianCallActiveForS3() || isS3HandlingAPedestrianCall())
    handlePedestrianCallForS3();
  if (isKeyActive() && ! isS3HandlingAPedestrianCall()) 
    handleKeyS3();
  else if (! isS3HandlingAPedestrianCall())
    handleTrafficForS3();
}

void handlePedestrianCallForS3() {
  if (isS3HandlingAPedestrianCall() && (isNoColorForS3() || isGreenOnForS3() || isYellowOnForS3() || isRedOnForS3())) {
    doNoColorForS3(); // recovery in case of no color
    doGreenForS3(); // recovery from green
    handleYellowRedPedestrianForS3();
  } else if (isPedestrianCallActiveForS3() && ((isCrossingClear() && isNoTraffic()) || isKeyActive())) {
    setBit(trafficLightStatusS3, isPedestrianCallInHandle);
  } else if (isPedestrianCallActiveForS3() && isTrafficActiveForS3() && timeSinceLastPedestrianCallForS2S3 >= 300000) {
    setBit(trafficLightStatusS3, isPedestrianCallInHandle);
  }
}

void handleYellowRedPedestrianForS3() {
  doYellowForS3();
  if (isRedOnForS3() && systemTimerS2S3 >= 20000) {
    clearBit(trafficLightStatusS3, redBit);
    clearBit(trafficLightStatusS3, isPedestrianCallInHandle);
    clearBit(trafficLightStatusS3, pedestrianBit);
    PORTB &= ~red_S2_S3_mask;
    systemTimerS2S3 = 0;
    timeSinceLastPedestrianCallForS2S3 = 0;
    if (! isTrafficActiveForS3() || isNoTraffic() || isKeyActive()) {
      setBit(trafficLightStatusS3, yellowBit);
      PORTB |= yellow_S2_S3_mask;
    } else if (isTrafficActiveForS3()) {
      setBit(trafficLightStatusS3, greenBit);
      PORTB |= green_S2_S3_mask;
    }
  }
}

void handleKeyS3() {
  handleBlinkingYellowS3();
  handleS3KeyDetection();
  if (! isKeyActive()) {
    // Acquiring green for traffic on both flows
    if (isTrafficActiveForS3() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
      clearBit(trafficLightStatusS3, yellowBit);
      PORTB &= ~yellow_S2_S3_mask;
      setBit(trafficLightStatusS3, greenBit);
      PORTB |= green_S2_S3_mask;
      systemTimerS2S3 = 0;
    }
  }
}

void handleTrafficForS3() {
  if (isTrafficActiveForS3() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
    startFAIRCycleForS3();
  } else if (isTrafficActiveForS3() && (! isTrafficActiveForS1() || ! isTrafficActiveForS4())) {
    handleMainitainingGreenS3();
  } else if (! isTrafficActiveForS3() && (isTrafficActiveForS1() || isTrafficActiveForS4())) {
    handleYellowRedS3();
  } else if (isNoTraffic()) {
    handleBlinkingYellowS3();
  }
}

void startFAIRCycleForS3() {
  // Recovery from key in case S2 acquired green color upon key exit
  if (isYellowOnForS3() && isGreenOnForS2()) {
    clearBit(trafficLightStatusS3, yellowBit);
    setBit(trafficLightStatusS3, greenBit);
  }
  doNoColorForS3(); // recovery from no color if active
  doYellowForS3();
  doRedForS3();
  doGreenForS3();
}

void doNoColorForS3() {
  if (isNoColorForS3() && systemTimerS2S3 >= 1000) {
    setBit(trafficLightStatusS3, yellowBit);
    PORTB |= yellow_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doYellowForS3() {
  if (isYellowOnForS3() && systemTimerS2S3 >= 2000) {
    clearBit(trafficLightStatusS3, yellowBit);
    PORTB &= ~yellow_S2_S3_mask;
    setBit(trafficLightStatusS3, redBit);
    PORTB |= red_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doRedForS3() {
  if (isRedOnForS3() && systemTimerS2S3 >= 10000) {
    clearBit(trafficLightStatusS3, redBit);
    PORTB &= ~red_S2_S3_mask;
    setBit(trafficLightStatusS3, greenBit);
    PORTB |= green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void doGreenForS3() {
  if (isS1HandlingAPedestrianCall() || isS4HandlingAPedestrianCall()) {
    if (isGreenOnForS3() && systemTimerS2S3 >= 20000) {
      clearBit(trafficLightStatusS3, greenBit);
      PORTB &= ~green_S2_S3_mask;
      setBit(trafficLightStatusS3, yellowBit);
      PORTB |= yellow_S2_S3_mask;
      systemTimerS2S3 = 0;
    }
  } else if (isGreenOnForS3() && systemTimerS2S3 >= 10000) {
      clearBit(trafficLightStatusS3, greenBit);
      PORTB &= ~green_S2_S3_mask;
      setBit(trafficLightStatusS3, yellowBit);
      PORTB |= yellow_S2_S3_mask;
      systemTimerS2S3 = 0;
  }
}

void handleMainitainingGreenS3() {
  if (! isGreenOnForS3()) { // recovery from red or no color
    clearBit(trafficLightStatusS3, redBit);
    PORTB &= ~red_S2_S3_mask;
    clearBit(trafficLightStatusS3, yellowBit);
    PORTB &= ~yellow_S2_S3_mask;
    setBit(trafficLightStatusS3, greenBit);
    PORTB |= green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
}

void handleYellowRedS3() {
  if (! isYellowOnForS3() && ! isRedOnForS3()) { // recovery from green or no color
    clearBit(trafficLightStatusS3, greenBit);
    PORTB &= ~green_S2_S3_mask;
    setBit(trafficLightStatusS3, yellowBit);
    PORTB |= yellow_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
  doYellowForS3();
}

void handleBlinkingYellowS3() {
  if (! isYellowOnForS3() && ! isNoColorForS3()) { // recovery from red or green
    clearBit(trafficLightStatusS3, redBit);
    PORTB &= ~red_S2_S3_mask; 
    clearBit(trafficLightStatusS3, greenBit);
    PORTB &= ~green_S2_S3_mask;
    systemTimerS2S3 = 0;
  }
  if (isYellowOnForS3() && systemTimerS2S3 >= 2000) {
    clearBit(trafficLightStatusS3, yellowBit);
    PORTB &= ~yellow_S2_S3_mask; 
    systemTimerS2S3 = 0;
  }
  doNoColorForS3();
}

///////////////////////////////////////////////////////////////////////////////////
// DETECTOR HANDLERS
///////////////////////////////////////////////////////////////////////////////////

////////
// S1 //
////////

void handleS1Detections() {
  handleS1PedestrianCallDetection();
  handleS1TrafficDetection();
  handleS1SpeedDetection();
  handleS1KeyDetection();
}

void handleS1PedestrianCallDetection() {
  !(PIND & pedestrian_S1_S4_mask) ? 
    setBit(trafficLightStatusS1, pedestrianBit) : NULL;
}

void handleS1KeyDetection() {
  if (!(PINC & key_mask)) {
    setBit(trafficLightStatusS1, keyBit);
  } else {
    clearBit(trafficLightStatusS1, keyBit);
  }
}

void handleS1TrafficDetection() {
  // To avoid unexpected change of behaviour while performing a pedestrian call handle
  // in case of No Traffic && Key


  // TODO il problema è qui


  if (! ((isNoTraffic() || isKeyActive()) && isSomeTrafficLightHandlingAPedestrianCall()))
    !(PIND & traffic_S1_S4_mask) ? 
      setBit(trafficLightStatusS1, trafficBit) : clearBit(trafficLightStatusS1, trafficBit);
}

void handleS1SpeedDetection() {
  if (isYellowOnForS1() && (isTrafficActiveForS1() || isS1HandlingAPedestrianCall())) {
    (analogReadOfSpeedDetector(2) >> 2) > 0 ? setBit(trafficLightStatusS1, violationBit) : 
      clearBit(trafficLightStatusS1, violationBit);
  } else {
    (analogReadOfSpeedDetector(2) >> 2) > 50 ? setBit(trafficLightStatusS1, violationBit) : 
      clearBit(trafficLightStatusS1, violationBit);
  }
  isBitSet(trafficLightStatusS1, violationBit) ? PORTD |= _BV(5) : PORTD &= ~_BV(5);
}

////////
// S4 //
////////

void handleS4Detections() {
  handleS4PedestrianCallDetection();
  handleS4TrafficDetection();
  handleS4SpeedDetection();
  handleS4KeyDetection();
}

void handleS4PedestrianCallDetection() {
  !(PIND & pedestrian_S1_S4_mask) ? 
    setBit(trafficLightStatusS4, pedestrianBit) : NULL;
}

void handleS4KeyDetection() {
  if (!(PINC & key_mask)) {
    setBit(trafficLightStatusS4, keyBit);
  } else {
    clearBit(trafficLightStatusS4, keyBit);
  }
}

void handleS4TrafficDetection() {
  // To avoid unexpected change of behaviour while performing a pedestrian call handle
  // in case of No Traffic && Key
  if (! ((isNoTraffic() || isKeyActive()) && isSomeTrafficLightHandlingAPedestrianCall()))
    !(PIND & traffic_S1_S4_mask) ? 
      setBit(trafficLightStatusS4, trafficBit) : clearBit(trafficLightStatusS4, trafficBit);
}

void handleS4SpeedDetection() {
  if (isYellowOnForS4() && (isTrafficActiveForS4() || isS4HandlingAPedestrianCall())) {
    (analogReadOfSpeedDetector(3) >> 2) > 0 ? setBit(trafficLightStatusS4, violationBit) : 
      clearBit(trafficLightStatusS4, violationBit);
  } else {
    (analogReadOfSpeedDetector(3) >> 2) > 50 ? setBit(trafficLightStatusS4, violationBit) : 
      clearBit(trafficLightStatusS4, violationBit);
  }
  isBitSet(trafficLightStatusS4, violationBit) ? PORTD |= _BV(4) : PORTD &= ~_BV(4);
}


////////
// S2 //
////////

void handleS2Detections() {
  handleS2PedestrianCallDetection();
  handleS2TrafficDetection();
  handleS2SpeedDetection();
  handleS2KeyDetection();
}

void handleS2PedestrianCallDetection() {
  !(PIND & pedestrian_S2_S3_mask) ? 
    setBit(trafficLightStatusS2, pedestrianBit) : NULL;
}

void handleS2KeyDetection() {
  if (!(PINC & key_mask)) {
    setBit(trafficLightStatusS2, keyBit);
  } else {
    clearBit(trafficLightStatusS2, keyBit);
  }
}

void handleS2TrafficDetection() {
  // To avoid unexpected change of behaviour while performing a pedestrian call handle
  // in case of No Traffic && Key
  if (! ((isNoTraffic() || isKeyActive()) && isSomeTrafficLightHandlingAPedestrianCall()))
    !(PIND & traffic_S2_S3_mask) ? 
      setBit(trafficLightStatusS2, trafficBit) : clearBit(trafficLightStatusS2, trafficBit);
}

void handleS2SpeedDetection() {
  if (isYellowOnForS2() && (isTrafficActiveForS2() || isS2HandlingAPedestrianCall())) {
    (analogReadOfSpeedDetector(4) >> 2) > 0 ? setBit(trafficLightStatusS2, violationBit) : 
      clearBit(trafficLightStatusS2, violationBit);
  } else {
    (analogReadOfSpeedDetector(4) >> 2) > 50 ? setBit(trafficLightStatusS2, violationBit) : 
      clearBit(trafficLightStatusS2, violationBit);
  }
  isBitSet(trafficLightStatusS2, violationBit) ? PORTC |= _BV(0) : PORTC &= ~_BV(0);
}

////////
// S3 //
////////

void handleS3Detections() {
  handleS3PedestrianCallDetection();
  handleS3TrafficDetection();
  handleS3SpeedDetection();
  handleS3KeyDetection();
}

void handleS3PedestrianCallDetection() {
  !(PIND & pedestrian_S2_S3_mask) ? 
    setBit(trafficLightStatusS3, pedestrianBit) : NULL;
}

void handleS3KeyDetection() {
  if (!(PINC & key_mask)) {
    setBit(trafficLightStatusS3, keyBit);
  } else {
    clearBit(trafficLightStatusS3, keyBit);
  }
}

void handleS3TrafficDetection() {
  // To avoid unexpected change of behaviour while performing a pedestrian call handle
  // in case of No Traffic && Key
  if (! ((isNoTraffic() || isKeyActive()) && isSomeTrafficLightHandlingAPedestrianCall()))
    !(PIND & traffic_S2_S3_mask) ? 
      setBit(trafficLightStatusS3, trafficBit) : clearBit(trafficLightStatusS3, trafficBit);
}

void handleS3SpeedDetection() {
  if (isYellowOnForS3() && (isTrafficActiveForS3() || isS3HandlingAPedestrianCall())) {
    (analogReadOfSpeedDetector(5) >> 2) > 0 ? setBit(trafficLightStatusS3, violationBit) : 
      clearBit(trafficLightStatusS3, violationBit);
  } else {
    (analogReadOfSpeedDetector(5) >> 2) > 50 ? setBit(trafficLightStatusS3, violationBit) : 
      clearBit(trafficLightStatusS3, violationBit);
  }
  isBitSet(trafficLightStatusS3, violationBit) ? PORTC |= _BV(1) : PORTC &= ~_BV(1);
}

///////////////////////////////////////////////////////////////////////////////////
// BOOLEAN CHECKS
///////////////////////////////////////////////////////////////////////////////////

boolean isCrossingClear() {
  return (analogReadOfSpeedDetector(2) + analogReadOfSpeedDetector(3)
    + analogReadOfSpeedDetector(4) + analogReadOfSpeedDetector(5)) == 0;
}

boolean isNoTraffic() {
  return (! isBitSet(trafficLightStatusS1, trafficBit)) && (! isBitSet(trafficLightStatusS4, trafficBit)) 
    && (! isBitSet(trafficLightStatusS2, trafficBit)) && (! isBitSet(trafficLightStatusS3, trafficBit));
}

boolean isSomeTrafficLightHandlingAPedestrianCall() {
  return isS1HandlingAPedestrianCall() || isS2HandlingAPedestrianCall()
    || isS3HandlingAPedestrianCall() || isS4HandlingAPedestrianCall();
}

// Pedestrian call checks

boolean isPedestrianCallActiveForS1() {
  return isBitSet(trafficLightStatusS1, pedestrianBit);
}

boolean isPedestrianCallActiveForS4() {
  return isBitSet(trafficLightStatusS4, pedestrianBit);
}

boolean isPedestrianCallActiveForS2() {
  return isBitSet(trafficLightStatusS2, pedestrianBit);
}

boolean isPedestrianCallActiveForS3() {
  return isBitSet(trafficLightStatusS3, pedestrianBit);
}

// Traffic checks

boolean isTrafficActiveForS1() {
  return isBitSet(trafficLightStatusS1, trafficBit);
}

boolean isTrafficActiveForS4() {
  return isBitSet(trafficLightStatusS4, trafficBit);
}

boolean isTrafficActiveForS2() {
  return isBitSet(trafficLightStatusS2, trafficBit);
}

boolean isTrafficActiveForS3() {
  return isBitSet(trafficLightStatusS3, trafficBit);
}

// Key active

boolean isKeyActive() {
  return (isBitSet(trafficLightStatusS1, keyBit) || isBitSet(trafficLightStatusS4, keyBit))
    || (isBitSet(trafficLightStatusS2, keyBit) || isBitSet(trafficLightStatusS3, keyBit));
}

// Is pedestrian call being handled

boolean isS1HandlingAPedestrianCall() {
  return isBitSet(trafficLightStatusS1, isPedestrianCallInHandle);
}

boolean isS4HandlingAPedestrianCall() {
  return isBitSet(trafficLightStatusS4, isPedestrianCallInHandle);
}

boolean isS2HandlingAPedestrianCall() {
  return isBitSet(trafficLightStatusS2, isPedestrianCallInHandle);
}

boolean isS3HandlingAPedestrianCall() {
  return isBitSet(trafficLightStatusS3, isPedestrianCallInHandle);
}

// Color boolean checks

boolean isRedOnForS1() {
  return isBitSet(trafficLightStatusS1, redBit);
}

boolean isRedOnForS4() {
  return isBitSet(trafficLightStatusS4, redBit);
}

boolean isRedOnForS2() {
  return isBitSet(trafficLightStatusS2, redBit);
}

boolean isRedOnForS3() {
  return isBitSet(trafficLightStatusS3, redBit);
}

boolean isYellowOnForS1() {
  return isBitSet(trafficLightStatusS1, yellowBit);
}

boolean isYellowOnForS4() {
  return isBitSet(trafficLightStatusS4, yellowBit);
}

boolean isYellowOnForS2() {
  return isBitSet(trafficLightStatusS2, yellowBit);
}

boolean isYellowOnForS3() {
  return isBitSet(trafficLightStatusS3, yellowBit);
}

boolean isNoColorForS1() {
  return ! isBitSet(trafficLightStatusS1, yellowBit) && ! isBitSet(trafficLightStatusS1, redBit)
    && ! isBitSet(trafficLightStatusS1, greenBit);
}

boolean isNoColorForS4() {
  return ! isBitSet(trafficLightStatusS4, yellowBit) 
      && ! isBitSet(trafficLightStatusS4, redBit) && ! isBitSet(trafficLightStatusS4, greenBit);
}

boolean isNoColorForS2() {
  return ! isBitSet(trafficLightStatusS2, yellowBit) && ! isBitSet(trafficLightStatusS2, redBit)
    && ! isBitSet(trafficLightStatusS2, greenBit);
}

boolean isNoColorForS3() {
  return ! isBitSet(trafficLightStatusS3, yellowBit) 
      && ! isBitSet(trafficLightStatusS3, redBit) && ! isBitSet(trafficLightStatusS3, greenBit);
}

boolean isGreenOnForS1() {
  return isBitSet(trafficLightStatusS1, greenBit);
}

boolean isGreenOnForS4() {
  return isBitSet(trafficLightStatusS4, greenBit);
}

boolean isGreenOnForS2() {
  return isBitSet(trafficLightStatusS2, greenBit);
}

boolean isGreenOnForS3() {
  return isBitSet(trafficLightStatusS3, greenBit);
}

///////////////////////////////////////////////////////////////////////////////////
// UTILITIES
///////////////////////////////////////////////////////////////////////////////////

int analogReadOfSpeedDetector(uint8_t pin) {
  ADMUX = (ADMUX & 0xF8) | (pin & 0x07);
  ADCSRA |= (1 << ADSC);
  while (ADCSRA & (1 << ADSC));
  return ADC;
}