#include <Servo.h>

const int trigPin = 4;     // Trig Pin of Ultrasonic Sensor
const int echoPin = 3;     // Echo Pin of Ultrasonic Sensor
const int servoPin = 5;     // Pin for Servo
const int greenLED = 6;     // Pin for Green LED
const int yellowLED = 7;    // Pin for Yellow LED
const int redLED = 8;     // Pin for Red LED

int soundSpeed = 340;       // Speed of sound in cm/s

Servo myServo;              // Create a servo object

void setup() {
  pinMode(trigPin, OUTPUT);   // Set trig pin as output
  pinMode(echoPin, INPUT);   // Set echo pin as input
  pinMode(greenLED, OUTPUT);  // Set green LED pin as output
  pinMode(yellowLED, OUTPUT); // Set yellow LED pin as output
  pinMode(redLED, OUTPUT);   // Set red LED pin as output

  myServo.attach(servoPin);   // Attach servo to the specified pin

  Serial.begin(9600);          // Initialize serial communication at a baud rate of 9600
}

void loop() {
  int distance = ultrasonicRead();   // Read distance from ultrasonic sensor
  int speed = map(distance, 0, 400, 0, 100); // Map distance to speed (km/h)

  if (speed <= 20) {              // If speed is safe (0-20 km/h)
    digitalWrite(greenLED, HIGH);   // Turn on green LED
    myServo.write(0);             // Open the servo
  } else if (speed <= 40) {        // If speed is semi-safe (21-40 km/h)
    digitalWrite(greenLED, LOW);    // Turn off green LED
    digitalWrite(yellowLED, HIGH); // Turn on yellow LED
    myServo.write(90);             // Half-open the servo
  } else {                         // If speed is dangerous (>40 km/h)
    digitalWrite(greenLED, LOW);    // Turn off green LED
    digitalWrite(yellowLED, LOW);   // Turn off yellow LED
    digitalWrite(redLED, HIGH);    // Turn on red LED
    myServo.write(180);            // Close the servo fully
  }

  Serial.print("speed: ");      // Print distance to serial monitor
  Serial.print(speed);
  Serial.println(" units");         // Print unit of measurement

  delay(500);                    // Delay for half a second to avoid rapid serial output
}

int ultrasonicRead() {
  unsigned long start = micros();
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  unsigned long duration = micros() - start;
  float distance = (duration * soundSpeed) / 2;
  return distance;
}