#include <Servo.h>

Servo servo1;
const int trigPin = 9;
const int echoPin = 8;
const int greenLed = 6; // Groene LED voor open-indicatie
const int redLed = 5;   // Rode LED voor sluit-indicatie
const int whiteLed = 4; // Witte LED voor normale positie
const int buzzer = 3;   // Buzzer voor geluidsindicatie
long distance = 100;
long duration;

void setup() {
  servo1.attach(7);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(greenLed, OUTPUT);
  pinMode(redLed, OUTPUT);
  pinMode(whiteLed, OUTPUT);
  pinMode(buzzer, OUTPUT);
  servo1.write(90); // Start op 90 graden
  digitalWrite(greenLed, LOW);
  digitalWrite(redLed, LOW);
  digitalWrite(whiteLed, HIGH); // Zet witte LED aan voor normale positie
  digitalWrite(buzzer, LOW); // Zet buzzer uit
}

void loop() {
  ultra();

  if (distance > 0 && distance <= 20) { 
    digitalWrite(whiteLed, LOW); // Zet de witte LED uit wanneer de servo beweegt
    digitalWrite(greenLed, HIGH); // Zet de groene LED aan wanneer de servo opent
    moveServo(180); // Beweeg naar 180°
    delay(5000); // Blijf 5 seconden op 180°
    digitalWrite(greenLed, LOW); // Zet de groene LED uit na 5 seconden
  } else {
    moveServo(90);  // Beweeg terug naar 90°
    digitalWrite(whiteLed, HIGH); // Zet de witte LED weer aan bij normale positie
  }

  delay(100);
}

void ultra() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, 30000);
  if (duration > 0) {
    distance = duration * 0.034 / 2;
  } else {
    distance = 100;
  }
}

void moveServo(int targetAngle) {
  int currentAngle = servo1.read();

  if (currentAngle < targetAngle) {
    for (int pos = currentAngle; pos <= targetAngle; pos++) {
      servo1.write(pos);
      delay(20);
    }
  } else if (currentAngle > targetAngle) {
    digitalWrite(redLed, HIGH); // Zet de rode LED aan tijdens het sluiten

    // Buzzer piep wanneer richting 90° bewegen, maar niet bij 90°
    for (int pos = currentAngle; pos >= targetAngle; pos--) {
      servo1.write(pos);
      delay(20);

      // Trigger piep net voor 90° (tijdens het bewegen richting 90°)
      if (pos == 92) { // Activeer piep net voor 90°
        tone(buzzer, 1000, 200); // Zelfde toon voor waarschuwingsgeluid (1000 Hz, 200 ms)
      }
    }
    digitalWrite(redLed, LOW); // Zet de rode LED uit zodra de servo op 90° is
  }

  // Geluid bij 180° (voor openen), twee keer achter elkaar
  if (targetAngle == 180) {
    tone(buzzer, 1000, 200); // Zelfde toon voor blij geluid (1000 Hz, 200 ms)
    delay(300); // Kleine pauze tussen de twee piepjes
    tone(buzzer, 1000, 200); // Zelfde piep opnieuw
  }
  // Zet buzzer uit bij 90° (sluiten)
  else if (targetAngle == 90) {
    noTone(buzzer); // Zet de buzzer uit bij 90°
  }
  else {
    noTone(buzzer); // Zet de buzzer uit voor andere hoeken
  }
}
