import time
from machine import Pin, I2C, time_pulse_us
from lcd1602 import LCD1602
from vl53l0x import VL53L0X # muss als vl53l0x.py auf dem Board liegen

# --- LCD Setup (RS=6, E=7, D4=8, D5=9, D6=10, D7=11) ---
lcd = LCD1602(6, 7, 8, 9, 10, 11)

# --- HC-SR04 Setup ---
trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

def read_hcsr04():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()
    try:
        duration = time_pulse_us(echo, 1, 30000)  # Warte max. 30ms
        distance = (duration / 2) / 29.1  # cm
    except OSError:
        distance = -1  # kein Messwert
    return distance

# --- VL53L0X Setup ---
i2c = I2C(0, sda=Pin(4), scl=Pin(5), freq=400000)
sensor = VL53L0X(i2c)

# --- Hauptloop ---
while True:
    dist_us = read_hcsr04()
    dist_tof  = sensor.range/10  # Adafruit-Treiber liefert .range in mm modifiziert zu cm

    lcd.clear()
    lcd.move_to(0, 0)
    if dist_us >= 0:
        lcd.putstr("US: {:.1f} cm".format(dist_us))
    else:
        lcd.putstr("US: ---")

    lcd.move_to(0, 1)
    lcd.putstr("ToF: {} cm".format(dist_tof))

    time.sleep(0.5)