import time
import board
import busio
import digitalio
from adafruit_displayio_ssd1306 import SSD1306_I2C


trigger = digitalio.DigitalInOut(board.D3)
trigger.direction = digitalio.Direction.OUTPUT
echo = digitalio.DigitalInOut(board.D2)
echo.direction = digitalio.Direction.INPUT

def distance():
    timepassed = 0
    trigger.value = False
    time.sleep(0.000002)
    trigger.value = True
    time.sleep(0.000005)
    trigger.value = False
    while echo.value == False:
        signaloff = time.monotonic_ns()
    while echo.value == True:
        signalon = time.monotonic_ns()
    timepassed = signalon - signaloff
    return timepassed

WIDTH = 128
HEIGHT = 64
i2c = busio.I2C(board.GP17, board.GP16, frequency=200000)
oled = SSD1306_I2C(WIDTH, HEIGHT, i2c)
while True:
    oled.fill(0)
    measured_time = distance()
    distance_cm = (measured_time * 0.0343) / 2
    text = "Distance: {:.1f} cm".format(distance_cm)
    label_text = label.Label(oled, font_size=20, text=text)
    label_text.x = 0
    label_text.y = 0
    oled.show()