from picozero import Servo
from machine import Pin, I2C
from time import sleep, ticks_us, ticks_diff
from lcd_api import LcdApi
from pico_i2c_lcd import I2cLcd

# === LCD Setup (I2C0: GP4 = SDA, GP5 = SCL) ===
i2c = I2C(0, sda=Pin(4), scl=Pin(5), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)  # Adjust I2C address if needed

# === Servo on GP7 ===
servo = Servo(7)

# === Ultrasonic Sensor ===
trig = Pin(15, Pin.OUT)
echo = Pin(14, Pin.IN)

# === Distance Measurement Function ===
def measure_distance():
    trig.low()
    sleep(0.002)
    trig.high()
    sleep(0.00001)
    trig.low()

    while echo.value() == 0:
        start = ticks_us()
    while echo.value() == 1:
        end = ticks_us()

    duration = ticks_diff(end, start)
    distance_cm = (duration * 0.0343) / 2
    return distance_cm

# === Start State ===
lcd.clear()
lcd.putstr("Clear")
servo.min()

# === Main Loop ===
try:
    while True:
        distance = measure_distance()
        if distance < 20:
            lcd.clear()
            lcd.putstr("Object detected")
            servo.max()
        else:
            lcd.clear()
            lcd.putstr("Clear")
            servo.min()
        sleep(0.5)

except KeyboardInterrupt:
    servo.off()
    lcd.clear()