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

# Initialize servo and ultrasonic sensor pins
servo = PWM(Pin(21), freq=50)
trigger = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)
led = Pin(2, Pin.OUT)

# Set up I2C with lower frequency
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=100000)  # Lowered frequency to 100 kHz
LCD_I2C_ADDRESS = 0x27  # Confirm this address after scanning

# I2C device scan to verify connection
devices = i2c.scan()
if LCD_I2C_ADDRESS not in devices:
    print("Warning: LCD not detected on I2C. Check wiring and address.")
else:
    print("LCD detected at address:", hex(LCD_I2C_ADDRESS))

# Initialize LCD if connected
lcd = I2cLcd(i2c, LCD_I2C_ADDRESS, 2, 16) if LCD_I2C_ADDRESS in devices else None

# Calibration constant for ultrasonic sensor
CALIBRATION_FACTOR = 1.08

# Function to set servo angle
def set_servo_angle(angle):
    duty = int(((angle / 180) * 102) + 26)
    servo.duty(duty)

# Function to get distance from ultrasonic sensor
def get_distance():
    trigger.off()
    time.sleep_us(2)
    trigger.on()
    time.sleep_us(10)
    trigger.off()

    pulse_start = 0
    pulse_end = 0

    timeout = time.ticks_ms() + 1000
    while echo.value() == 0:
        pulse_start = time.ticks_us()
        if time.ticks_ms() > timeout:
            return -1

    timeout = time.ticks_ms() + 1000
    while echo.value() == 1:
        pulse_end = time.ticks_us()
        if time.ticks_ms() > timeout:
            return -1

    pulse_duration = time.ticks_diff(pulse_end, pulse_start)
    distance = (pulse_duration * 0.0343) / 2
    distance *= CALIBRATION_FACTOR

    return distance

# Main loop
if lcd:  # Only proceed if LCD is connected
    while True:
        # Sweep servo from 0 to 180 degrees
        for angle in range(0, 181, 5):
            set_servo_angle(angle)
            distance = get_distance()

            if distance == -1:
                lcd.clear()
                lcd.putstr(f"Angle: {angle}°\nNo object")
                print(f"Angle: {angle}°, No object detected")
            else:
                lcd.clear()
                lcd.putstr(f"Angle: {angle}°\nDist: {distance:.2f} cm")
                print(f"Angle: {angle}°, Distance: {distance:.2f} cm")

            if distance > 0 and distance < 50:
                led.on()
            else:
                led.off()

            time.sleep(0.05)

        # Sweep servo from 180 to 0 degrees
        for angle in range(180, -1, -5):
            set_servo_angle(angle)
            distance = get_distance()

            if distance == -1:
                lcd.clear()
                lcd.putstr(f"Angle: {angle}°\nNo object")
                print(f"Angle: {angle}°, No object detected")
            else:
                lcd.clear()
                lcd.putstr(f"Angle: {angle}°\nDist: {distance:.2f} cm")
                print(f"Angle: {angle}°, Distance: {distance:.2f} cm")

            if distance > 0 and distance < 50:
                led.on()
            else:
                led.off()

            time.sleep(0.1)
else:
    print("LCD not connected. Check wiring and I2C address.")