#include <Servo.h>

// Pin configuration
const int trigPin = A0; // Pin Trig sensor ultrasonik (pin analog)
const int echoPin = 13; // Pin Echo sensor ultrasonik (pin digital)
const int ledHijau = 9; // LED hijau (pin digital)
const int ledKuning = 10; // LED kuning (pin digital)
const int ledMerah = 11; // LED merah (pin digital)
const int buzzer = 12; // Buzzer (pin digital)
const int servoPin = 8; // Pin servo (pin digital)

// Servo object
Servo myServo;

// Variabel untuk waktu dan pengukuran
long duration;
int distance;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledHijau, OUTPUT);
  pinMode(ledKuning, OUTPUT);
  pinMode(ledMerah, OUTPUT);
  pinMode(buzzer, OUTPUT);

  myServo.attach(servoPin); // Inisialisasi servo
  myServo.write(0); // Servo awal di posisi 0 derajat

  Serial.begin(9600); // Untuk monitoring
}

void loop() {
  // Mengukur jarak
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // Konversi ke cm

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Logika pengendalian berdasarkan ketinggian air
  if (distance < 200) { // Kondisi aman (hijau)
    digitalWrite(ledHijau, HIGH);
    digitalWrite(ledKuning, LOW);
    digitalWrite(ledMerah, LOW);
    myServo.write(0); // Servo pada posisi 0 derajat
    digitalWrite(buzzer, LOW); // Buzzer mati
  } 
  else if (distance >= 200 && distance <= 350) { // Kondisi waspada (kuning)
    digitalWrite(ledHijau, LOW);
    digitalWrite(ledKuning, HIGH);
    digitalWrite(ledMerah, LOW);
    myServo.write(90); // Servo pada posisi 90 derajat
    for (int i = 0; i < 3; i++) { // Buzzer berbunyi 3 kali
      digitalWrite(buzzer, HIGH);
      delay(1000);
      digitalWrite(buzzer, LOW);
      delay(1000);
    }
  } 
  else if (distance > 350) { // Kondisi bahaya (merah)
    digitalWrite(ledHijau, LOW);
    digitalWrite(ledKuning, LOW);
    digitalWrite(ledMerah, HIGH);
    myServo.write(180); // Servo pada posisi 180 derajat
    digitalWrite(buzzer, HIGH); // Buzzer berbunyi terus-menerus
  }

  delay(100); // Loop time
}