from machine import Pin, I2C, PWM, time_pulse_us
from pico_i2c_lcd import I2cLcd
import utime

# ---------------- LCD Setup ----------------
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)

# ---------------- Ultrasonic Sensor ----------------
trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

# ---------------- LEDs ----------------
l_green = Pin(4, Pin.OUT)
l_red = Pin(5, Pin.OUT)

# ---------------- Servo Setup ----------------
servo = PWM(Pin(6))
servo.freq(50)  # 50 Hz for servo

def servo_angle(angle):
    # Convert angle (0-180) to duty cycle
    min_duty = 1638   # 0.5 ms pulse
    max_duty = 8192   # 2.5 ms pulse

    duty = int(min_duty + (angle / 180) * (max_duty - min_duty))
    servo.duty_u16(duty)

# Gate Closed initially
servo_angle(0)

# ---------------- Distance Function ----------------
def g_dist():
    trig.low()
    utime.sleep_us(2)

    trig.high()
    utime.sleep_us(10)
    trig.low()

    duration = time_pulse_us(echo, 1, 30000)

    if duration < 0:
        return 999

    d_cm = (duration * 0.0343) / 2
    return d_cm

# ---------------- Main Loop ----------------
while True:
    dist = g_dist()

    print("Distance:", dist, "cm")

    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("Dist:{:.1f} cm".format(dist))

    # person detected within 20 cm
    if dist <= 20:

        l_red.value(0)
        l_green.value(1)

        servo_angle(90)      # Open gate

        lcd.move_to(0, 1)
        lcd.putstr("Gate Opened")

    else:

        l_red.value(1)
        l_green.value(0)

        servo_angle(0)       # Close gate

        lcd.move_to(0, 1)
        lcd.putstr("Gate Closed")

    utime.sleep(1)

    '''
    here is another model of code

    from machine import Pin, time_pulse_us,I2C,PWM
from pico_i2c_lcd import I2cLcd
import utime

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

i2c=I2C(0,sda=Pin(0),scl=Pin(1),freq=400000)
lcd=I2cLcd(i2c,0x27,2,16)

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

while True:

    trig.low()
    utime.sleep_us(2)

    trig.high()
    utime.sleep_us(10)
    trig.low()

    duration = time_pulse_us(echo, 1)

    distance = (duration * 0.0343) / 2
#avoid this inbetween code ok using both servo and lcd
    lcd.clear()
    lcd.putstr("distance:")
    lcd.move_to(0,1)
    lcd.putstr(str(distance))
#avoid this above code ok
    print("Distance:", distance, "cm")
    
    if distance<20:
        lcd.clear()
        lcd.putstr("door opened")
        servo.duty_u16(4915)
    
        
    else:
        lcd.clear()
        lcd.putstr("door closed")
        servo.duty_u16(1638)

    utime.sleep(4)
    '''