const int greenLight = 2;
const int yellowLight = 3;
const int redLight = 4;
const int echoPin = 12;
const int trigPin = 13;

bool greenLightOn = false;

void setup() {
  pinMode(redLight, OUTPUT);
  pinMode(yellowLight, OUTPUT);
  pinMode(greenLight, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  long distance = readDistance();
  launchLight(distance);
}

long readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

void launchLight(long distance) {
  if (distance < 50) {
    if (!greenLightOn) {
      digitalWrite(greenLight, HIGH);
      digitalWrite(yellowLight, LOW);
      digitalWrite(redLight, LOW);
      greenLightOn = true;
    }
  } else {
    if (greenLightOn) {
      digitalWrite(greenLight, LOW);
      digitalWrite(yellowLight, HIGH);
      digitalWrite(redLight, LOW);
      delay(2000);
      digitalWrite(yellowLight, LOW);
      digitalWrite(redLight, HIGH);
      greenLightOn = false;
    }
  }
}