#include <Servo.h>

// Pin sensor ultrasonik
const int trigPin = 9;
const int echoPin = 10;

// Servo
Servo servoMotor;
const int servoPin = 6;

// Variabel jarak
long duration;
int distance;

// Jarak maksimum untuk mendeteksi gelas (dalam cm)
const int detectionThreshold = 10;

// Status untuk mendeteksi gelas baru
bool isGlassPresent = false;

void setup() {
  // Inisialisasi serial monitor
  Serial.begin(9600);

  // Inisialisasi pin sensor ultrasonik
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Inisialisasi servo
  servoMotor.attach(servoPin);
  servoMotor.write(90); // Servo dalam posisi awal (off)
}

void loop() {
  // Mengukur jarak dengan sensor ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Membaca hasil pantulan
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  // Debug jarak di serial monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Jika ada gelas di dekat sensor dan belum terdeteksi sebelumnya
  if (distance > 0 && distance <= detectionThreshold && !isGlassPresent) {
    isGlassPresent = true; // Tandai gelas telah terdeteksi
    Serial.println("Gelas terdeteksi! Mengaktifkan servo.");
    servoMotor.write(0); // Servo bergerak ke posisi aktif
    delay(2000);          // Tunggu 2 detik
    servoMotor.write(90);  // Servo kembali ke posisi awal
    Serial.println("Servo berhenti.");
  }

  // Jika gelas sudah tidak ada, reset status
  if (distance > detectionThreshold) {
    isGlassPresent = false; // Siap mendeteksi gelas baru
  }

  delay(500); // Delay untuk pembacaan berikutnya
}