from machine import Pin, I2C, PWM
import time
from hcsr04 import HCSR04
from lcd_i2c import I2cLcd

# initialize I2C LCD
def init_lcd():
    i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)
    lcd = I2cLcd(i2c, 0x27, 2, 16)
    return lcd

# measure distance
def measure_distance(sensor):
    return sensor.distance_cm()

# display distance on LCD
def display_distance(lcd, distance):
    lcd.clear()
    lcd.putstr("Distance:\n{} cm".format(distance))
    print('Distance:', distance, 'cm')

# control servo based on distance
def control_servo(servo, distance):
    if distance is not None:
        if distance <= 50:  # Activate servo if distance is below or equal to 50cm
            servo.duty(115)
        else:
            servo.duty(77)
    else:
        servo.duty(0)  # Turn off the servo if distance reading fails

def main():
    # Initialize LCD
    lcd = init_lcd()

    # Initialize Ultrasonic Sensor
    sensor = HCSR04(trigger_pin=19, echo_pin=13)

    # Initialize Servo
    servo_pin = 18
    servo = PWM(Pin(servo_pin), freq=50)

    while True:
        # Measure distance
        distance = measure_distance(sensor)
        
        # Display distance
        display_distance(lcd, distance)

        # Control servo based on distance
        control_servo(servo, distance)
        
        # Wait for 5 seconds
        time.sleep(5)

if __name__ == "__main__":
    main()