from machine import Pin, I2C
import time
import machine

# =============================
# I2C LCD SETUP
# =============================
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
LCD_ADDR = 0x27

def lcd_send(addr, data, mode):
    buf = [(data & 0xF0) | mode | 0x08, (data & 0xF0) | mode | 0x0C, (data & 0xF0) | mode | 0x08]
    i2c.writeto(addr, bytes(buf))
    buf = [((data << 4) & 0xF0) | mode | 0x08, ((data << 4) & 0xF0) | mode | 0x0C, ((data << 4) & 0xF0) | mode | 0x08]
    i2c.writeto(addr, bytes(buf))

def lcd_init(addr):
    for cmd in [0x33, 0x32, 0x28, 0x0C, 0x06, 0x01]:
        lcd_send(addr, cmd, 0)
        time.sleep_ms(5)

def lcd_print(text, line):
    if line == 1: lcd_send(LCD_ADDR, 0x80, 0)
    if line == 2: lcd_send(LCD_ADDR, 0xC0, 0)
    # Menambah spasi agar teks lama terhapus
    text = text + "                " 
    for char in text[:16]: 
        lcd_send(LCD_ADDR, ord(char), 1)

lcd_init(LCD_ADDR)

# =============================
# HARDWARE SETUP
# =============================
trigger = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)
buzzer = Pin(19, Pin.OUT)

# =============================
# FUNGSI BACA JARAK
# =============================
def baca_jarak():
    trigger.off()
    time.sleep_us(2)
    trigger.on()
    time.sleep_us(10)
    trigger.off()
    
    # Menggunakan fungsi bawaan machine dengan timeout 30ms
    duration = machine.time_pulse_us(echo, 1, 30000)
    
    if duration < 0: 
        return 999.0 # Jarak dianggap jauh jika tidak ada echo
    
    return (duration * 0.0343) / 2

# =============================
# PROGRAM UTAMA
# =============================
lcd_print("Radar Mobil", 1)

while True:
    jarak = baca_jarak()
    print("Jarak: {:.2f} cm".format(jarak))

    # Tampilkan ke LCD
    lcd_print("Jarak: {:.1f} cm".format(jarak), 2)

    # Logika Buzzer (nyala jika jarak < 5cm)
    if 0 < jarak < 5:
        buzzer.on()
    else:
        buzzer.off()

    time.sleep(0.1)