from machine import Pin, I2C, time_pulse_us
import time
import ssd1306

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

# LED
led = Pin(15, Pin.OUT)

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

def get_distance():
    trig.low()
    time.sleep_us(2)

    trig.high()
    time.sleep_us(10)
    trig.low()

    duration = time_pulse_us(echo, 1)

    distance = (duration * 0.0343) / 2
    return distance

while True:
    try:
        distance = get_distance()

        if distance <= 0:
            frequency = 0.5
        else:
            frequency = distance / 40

        # Avoid too fast blinking
        if frequency > 10:
            frequency = 10

        delay = 1 / (2 * frequency)

        # LED blink
        led.toggle()
        time.sleep(delay)
        led.toggle()
        time.sleep(delay)

        # OLED display
        oled.fill(0)
        oled.text("Distance:", 0, 0)
        oled.text("{:.2f} cm".format(distance), 0, 15)

        oled.text("Frequency:", 0, 35)
        oled.text("{:.2f} Hz".format(frequency), 0, 50)

        oled.show()

    except:
        oled.fill(0)
        oled.text("Sensor Error", 0, 20)
        oled.show()
        time.sleep(1)