from machine import Pin, SPI
import time
from ili9341 import ILI9341, color565

# Setup Display (SPI0)
spi = SPI(0, baudrate=40000000, sck=Pin(18), mosi=Pin(19))
display = ILI9341(spi=spi, cs=Pin(17), dc=Pin(16), rst=Pin(15),
                  width=240, height=320, rotation=0)

# Ultraschallsensor Pins
TRIG = Pin(3, Pin.OUT)
ECHO = Pin(2, Pin.IN)

# Funktion zur Messung der Entfernung
def measure_distance():
    TRIG.low()
    time.sleep_us(2)
    TRIG.high()
    time.sleep_us(10)
    TRIG.low()

    timeout = 1000000  # 1 Sekunde
    pulse_start = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), pulse_start) > timeout:
            raise Exception("Timeout waiting for echo high")
    start = time.ticks_us()

    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout:
            raise Exception("Timeout waiting for echo low")
    end = time.ticks_us()

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

# Display Startbildschirm
display.fill(color565(0, 0, 0))  # Schwarz
display.text("Ultraschall Messung", 10, 10, color565(255, 255, 0))
display.show()

# Mess-Schleife
while True:
    try:
        dist = measure_distance()
        display.fill_rect(0, 40, 240, 30, color565(0, 0, 0))  # Bereich löschen
        display.text("Entfernung: {:.1f} cm".format(dist), 10, 40, color565(0, 255, 0))
    except Exception as e:
        display.fill_rect(0, 40, 240, 30, color565(0, 0, 0))
        display.text("Messfehler", 10, 40, color565(255, 0, 0))
    display.show()
    time.sleep(0.5)