from machine import Pin, PWM
import time

# ==========================================
# 1. KONFIGURASI PIN & INISIALISASI
# ==========================================

# Sensor Ultrasonik LUAR (Untuk mendeteksi tangan)
trig_out = Pin(19, Pin.OUT)
echo_out = Pin(18, Pin.IN)

# Sensor Ultrasonik DALAM (Untuk cek kapasitas sampah)
# Hubungkan Trig ke Pin 5 (D5) dan Echo ke Pin 17 (TX2)
trig_in = Pin(5, Pin.OUT)
echo_in = Pin(17, Pin.IN)

# Servo Motor (Penggerak Tutup) - Pin 21
servo = PWM(Pin(21), freq=50)

# Buzzer (Peringatan) - Pin 26
buzzer = PWM(Pin(26), freq=1000)

# Variabel Pengaturan Servo (Sesuaikan jika posisi buka/tutup kurang pas)
min_duty = 40  # Nilai untuk posisi tutup rapat
max_duty = 115 # Nilai untuk posisi buka 90-110 derajat

# Status Awal
servo_open = False
last_alarm_time = 0

# ==========================================
# 2. FUNGSI-FUNGSI PENDUKUNG
# ==========================================

def get_distance(trig, echo):
    """Fungsi universal untuk menghitung jarak sensor ultrasonik"""
    trig.off()
    time.sleep_us(2)
    trig.on()
    time.sleep_us(10)
    trig.off()
    
    # Menghitung durasi pantulan gelombang (timeout 30ms agar tidak stuck)
    duration = machine.time_pulse_us(echo, 1, 30000) 
    
    if duration < 0:
        return 999  # Mengembalikan angka jauh jika sensor tidak mendeteksi benda
    
    # Rumus konversi durasi ke centimeter
    distance = (duration / 2) / 29.1
    return distance

def bip_bip():
    """Fungsi suara buzzer bip-bip 3 kali sebagai tanda penuh"""
    for _ in range(3):
        buzzer.duty(512) # Bunyi (50% volume)
        time.sleep(0.1)
        buzzer.duty(0)   # Mati
        time.sleep(0.1)

# Set posisi awal tutup tong sampah saat pertama kali dinyalakan
servo.duty(min_duty)
buzzer.duty(0)

# ==========================================
# 3. LOOP UTAMA (LOGIKA PROGRAM)
# ==========================================

while True:
    # Baca jarak dari kedua sensor
    dist_internal = get_distance(trig_in, echo_in)
    dist_external = get_distance(trig_out, echo_out)
    
    # Syarat Penuh: Jika jarak sampah ke sensor dalam kurang dari 5 cm
    is_full = dist_internal < 5 

    if is_full:
        # JIKA PENUH:
        # 1. Pastikan tutup segera menutup jika tadinya terbuka
        if servo_open:
            print("Sampah penuh! Menutup otomatis...")
            servo.duty(min_duty)
            servo_open = False
        
        print("STATUS: PENUH - Buzzer akan berbunyi tiap 5 detik")
        
        # 2. Logika Alarm (Bip-bip setiap jeda 5 detik)
        current_time = time.time()
        if current_time - last_alarm_time >= 5:
            bip_bip()
            last_alarm_time = current_time
            
    else:
        # JIKA TIDAK PENUH (Mode Normal):
        # 1. Logika Buka: Jika tangan < 15cm dan posisi masih tertutup
        if dist_external < 15 and not servo_open:
            print("Membuka: Tangan terdeteksi")
            servo.duty(max_duty)
            servo_open = True
            
            # Bunyi bip pendek tanda mulai membuka
            buzzer.duty(512)
            time.sleep(0.2)
            buzzer.duty(0)
            
            # Jeda waktu agar orang bisa masukkan sampah sebelum dicek lagi
            time.sleep(3) 
            
        # 2. Logika Tutup: Jika tangan sudah jauh (> 15cm) dan posisi masih terbuka
        elif dist_external > 15 and servo_open:
            print("Menutup: Tangan sudah menjauh")
            servo.duty(min_duty)
            servo_open = False
            time.sleep(1) # Jeda agar gerakan servo stabil

    # Delay singkat agar prosesor ESP32 tidak panas dan pembacaan sensor stabil
    time.sleep(0.1)