from machine import Pin, I2C
from ssd1306 import SSD1306_I2C  # Or the appropriate OLED library for your setup
import utime
import time

# Define pins for ultrasonic sensor
trigger_pin = Pin(5, Pin.OUT)
echo_pin = Pin(4, Pin.IN)

# I2C for OLED (adjust pins and address if needed)
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)  # Check your OLED's I2C address
oled = SSD1306_I2C(128, 64, i2c)

critical_distance = 50  # cm

def measure_distance():
    trigger_pin.value(1)
    utime.sleep_us(10)  # Trigger pulse for 10 microseconds
    trigger_pin.value(0)

    # Measure echo pulse duration (more robust method)
    pulse_start = utime.ticks_us()
    while echo_pin.value() == 0:
        pass
    pulse_end = utime.ticks_us()
    pulse_duration = utime.ticks_diff(pulse_end, pulse_start)

    # Calculate distance (sound speed: ~343 m/s or 29.15 us/cm)
    distance = (pulse_duration * 0.0343) / 2  # Divide by 2 for one-way distance
    return distance



while True:
    distance = measure_distance()
    print("Distance:", distance, "cm")

    oled.fill(0)  # Clear the OLED
    oled.text("Distance: " + str(round(distance, 2)) + " cm", 0, 0)  # Round to 2 decimals

    if distance < critical_distance:
        oled.text("Collision Imminent!", 0, 20)
    
    oled.show()
    time.sleep_ms(100)  # Small delay