import machine
import utime

i2c = machine.I2C(0, sda=machine.Pin(0), scl=machine.Pin(1), freq=400000)

trigger = machine.Pin(15, machine.Pin.OUT)
echo = machine.Pin(14, machine.Pin.IN)
servo = machine.PWM(machine.Pin(16))
servo.freq(50)

led_open = machine.Pin(18, machine.Pin.OUT)
led_close = machine.Pin(17, machine.Pin.OUT)

seuil_distance = 20
compteur = 0

def lcd_send(cmd, mode=0):
    high = (cmd & 0xF0) | mode | 0x08 | 0x04
    low = ((cmd << 4) & 0xF0) | mode | 0x08 | 0x04
    i2c.writeto(0x27, bytes([high, high & ~0x04, low, low & ~0x04]))

def lcd_write(txt, row=0):
    lcd_send(0x80 if row == 0 else 0xC0)
    for char in txt:
        lcd_send(ord(char), 1)

def get_distance():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(10)
    trigger.low()
    
    while echo.value() == 0:
        pass
    t1 = utime.ticks_us()
    
    while echo.value() == 1:
        pass
    t2 = utime.ticks_us()
    
    duration = utime.ticks_diff(t2, t1)
    return (duration * 0.0343) / 2

def set_servo(angle):
    duty = int(((angle / 180) * 2 + 0.5) / 20 * 65535)
    servo.duty_u16(duty)

for cmd in [0x33, 0x32, 0x28, 0x0C, 0x06, 0x01]:
    lcd_send(cmd)
utime.sleep_ms(10)

set_servo(0)
led_open.low()
led_close.high()

while True:
    dist = get_distance()
    
    if dist < seuil_distance:
        compteur += 1
        led_close.low()
        led_open.high()
        
        lcd_write("Status: OPEN    ", 0)
        lcd_write("Compteur: {}   ".format(compteur), 1)
        
        set_servo(90)
        utime.sleep(5)
        
        set_servo(0)
        led_open.low()
        led_close.high()
    else:
        lcd_write("Status: CLOSED  ", 0)
        lcd_write("Compteur: {}   ".format(compteur), 1)
        
    utime.sleep(0.2)