from machine import Pin, I2C
import time
import ssd1306

# -------- Ultrasonic Sensor --------
trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

# -------- OLED Setup --------
i2c = I2C(0, scl=Pin(1), sda=Pin(0))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

# -------- Distance Function --------
def measure_distance():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()

    while echo.value() == 0:
        pass
    start = time.ticks_us()

    while echo.value() == 1:
        pass
    end = time.ticks_us()

    duration = time.ticks_diff(end, start)
    distance = (duration * 0.0343) / 2  # cm
    return distance

# -------- Main Loop --------
while True:
    distance = measure_distance()

    print("Distance:", round(distance, 2), "cm")

    oled.fill(0)
    oled.text("Ultrasonic Sensor", 0, 0)
    oled.text("Distance:", 0, 20)
    oled.text(str(round(distance, 2)) + " cm", 0, 40)
    oled.show()

    time.sleep(1)