from machine import Pin, I2C, PWM
from time import sleep, time_ns
from lcd_api import LcdApi
from i2c_lcd import I2cLcd
import utime


i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)  # Adresse prüfen mit I2C Scan falls nötig

trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

servo = PWM(Pin(15))
servo.freq(50)

def set_servo_angle(angle):
    duty = int(1638 + (angle / 180) * (8192 - 1638))  # 0.5ms - 2.5ms Impulsbreite
    servo.duty_u16(duty)

def measure_distance():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(10)
    trigger.low()

    while echo.value() == 0:
        signaloff = utime.ticks_us()
    while echo.value() == 1:
        signalon = utime.ticks_us()

    time_passed = utime.ticks_diff(signalon, signaloff)
    distance_cm = (time_passed * 0.0343) / 2

    return distance_cm


lcd.clear()
lcd.putstr("Entfernung:")

while True:
    try:
        dist = measure_distance()
        lcd.move_to(0, 1)
        lcd.putstr("{:6.2f} cm     ".format(dist))

        # Servo analog anzeigen (max 180° bei z.B. 403 cm)
        max_dist = 403
        angle = min((dist / max_dist) * 180, 180)
        set_servo_angle(angle)

        sleep(0.2)

    except Exception as e:
        lcd.clear()
        lcd.putstr("Fehler: {}".format(e))
        sleep(1)