from machine import Pin, I2C, PWM
import ssd1306
from time import sleep
from hcsr04 import HCSR04

# Inisialisasi sensor ultrasonik
ultra_rizal = HCSR04(trigger_pin=13, echo_pin=12, echo_timeout_us=100000)

# LED
led_hijau_rizal = Pin(27, Pin.OUT)
led_kuning_rizal = Pin(26, Pin.OUT)
led_merah_rizal = Pin(25, Pin.OUT)

buzzer_rizal = PWM(Pin(14))
buzzer_rizal.duty(0)  # Awal tidak berbunyi

# OLED
i2c_rizal = I2C(0, scl=Pin(22), sda=Pin(21))
oled_rizal = ssd1306.SSD1306_I2C(128, 64, i2c_rizal)

def fuzzy_kedekatan_rizal(jarak_rizal):
    if jarak_rizal <= 10:
        return 'Dekat Sekali', 100
    elif jarak_rizal <= 20:
        return 'Dekat', 80
    elif jarak_rizal <= 40:
        return 'Agak Dekat', 50
    elif jarak_rizal <= 80:
        return 'Agak Jauh', 20
    else:
        return 'Jauh Sekali', 0

while True:
    try:
        jarak_rizal = ultra_rizal.distance_cm()
        status_rizal, risiko_rizal = fuzzy_kedekatan_rizal(jarak_rizal)

        # Logika LED dan Buzzer
        if risiko_rizal >= 80:
            buzzer_rizal.freq(1500)
            buzzer_rizal.duty(500)
            led_merah_rizal.on()
            led_kuning_rizal.off()
            led_hijau_rizal.off()
            sleep(0.2)
            buzzer_rizal.duty(0)
        elif risiko_rizal >= 30:
            buzzer_rizal.freq(1000)
            buzzer_rizal.duty(400)
            led_merah_rizal.off()
            led_kuning_rizal.on()
            led_hijau_rizal.off()
            sleep(0.4)
            buzzer_rizal.duty(0)
        else:
            led_merah_rizal.off()
            led_kuning_rizal.off()
            led_hijau_rizal.on()

        # Tampilkan di OLED
        oled_rizal.fill(0)
        oled_rizal.text("Jarak: {:.1f} cm".format(jarak_rizal), 0, 0)
        oled_rizal.text("Status: {}".format(status_rizal), 0, 16)
        oled_rizal.text("Risiko: {}".format(risiko_rizal), 0, 32)
        oled_rizal.show()

        sleep(1)

    except Exception as e:
        print("Error:", e)