from machine import Pin, PWM, ADC, I2C
import utime
from i2c_lcd import I2cLcd

# Inisialisasi pin
infrared = ADC(Pin(27))
induksi = ADC(Pin(26))
kapasitif = ADC(Pin(25))
scan = Pin(16, Pin.IN, Pin.PULL_DOWN)
reset = Pin(17, Pin.IN, Pin.PULL_UP)
buzzer = PWM(Pin(23))
led1 = Pin(32, Pin.OUT)
led2 = Pin(33, Pin.OUT)
OrganicTrig = Pin(4, Pin.OUT)
OrganicEcho = Pin(0, Pin.IN)  # Perhatikan: Pin 0 bisa konflik dengan USB serial
AnorganicTrig = Pin(19, Pin.OUT)
AnorganicEcho = Pin(18, Pin.IN)
HumanTrig = Pin(2, Pin.OUT)
HumanEcho = Pin(15, Pin.IN)
servo1 = PWM(Pin(12))
servo2 = PWM(Pin(14)) # Perbaikan: servo2 menggunakan pin 14
servo1.freq(50)
servo2.freq(50)

# Inisialisasi LCD
i2c = I2C(0, sda=Pin(21), scl=Pin(22), freq=400000)
lcd = I2cLcd(i2c, 0x27, 4, 20)

# Fungsi ultrasonik
def read_ultrasonic(trig_pin, echo_pin):
    trig_pin.off()
    utime.sleep_us(2)
    trig_pin.on()
    utime.sleep_us(10)
    trig_pin.off()
    while echo_pin.value() == 0:
        pass
    start = utime.ticks_us()
    while echo_pin.value() == 1:
        pass
    end = utime.ticks_us()
    duration = utime.ticks_diff(end, start)
    distance = (duration * 0.0343) / 2
    return distance

# Fungsi LCD
def lcd_display(line1, line2, line3, line4):
    lcd.clear()
    lcd.putstr(f"{line1:<20}\n{line2:<20}\n{line3:<20}\n{line4:<20}")

# Fungsi servo
def move_servo(servo, angle):
    duty = int((angle / 180 * 102) + 26)
    servo.duty(duty)

# Inisialisasi awal
lcd_display("TEMPAT SAMPAH", "OTOMATIS", "KKN 49 UMG", "KOTAKUSUMA")

# Main loop
while True:
    if reset.value() == 0:
        lcd_display("TEMPAT SAMPAH", "OTOMATIS", "KKN 49 UMG", "KOTAKUSUMA")
        led1.value(0)
        led2.value(0)
        utime.sleep_ms(500)  # Debouncing
        continue

    if read_ultrasonic(HumanTrig, HumanEcho) < 50:
        lcd_display("TUNJUKKAN SAMPAH", "", "", "")
        utime.sleep_ms(200)

        if scan.value() == 1:
            infrared_val = infrared.read()
            kapasitif_val = kapasitif.read()
            induksi_val = induksi.read()

            if infrared_val < 200 and kapasitif_val > 200 and induksi_val == 0:
                lcd_display("SAMPAH ORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah")
                move_servo(servo1, 90)
                buzzer.duty(512)
                utime.sleep(1)
                buzzer.duty(0)
                move_servo(servo1, 0)
            elif infrared_val < 200 and kapasitif_val < 200 and induksi_val == 0:
                lcd_display("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah")
                move_servo(servo2, 90)
                buzzer.duty(512)
                utime.sleep(1)
                buzzer.duty(0)
                move_servo(servo2, 0)
            elif infrared_val > 200 and kapasitif_val < 200 and induksi_val == 0:
                lcd_display("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah")
                move_servo(servo2, 90)
                buzzer.duty(512)
                utime.sleep(1)
                buzzer.duty(0)
                move_servo(servo2, 0)
            elif infrared_val > 200 and kapasitif_val == 0 and induksi_val > 0:
                lcd_display("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah")
                move_servo(servo2, 90)
                buzzer.duty(512)
                utime.sleep(1)
                buzzer.duty(0)
                move_servo(servo2, 0)
            else:
                lcd_display("TIDAK TERDETEKSI", "===============", "COBA LAGI", "")
                utime.sleep(1)

    # Deteksi kapasitas tempat sampah
    if read_ultrasonic(OrganicTrig, OrganicEcho) < 10:
        led1.value(1)
    else:
        led1.value(0)

    if read_ultrasonic(AnorganicTrig, AnorganicEcho) < 10:
        led2.value(1)
    else:
        led2.value(0)

    utime.sleep_ms(100)