###    Import libraries   ###
from machine import Pin, PWM, Timer
import time

###    definitons   ###
red = Pin(4, Pin.OUT)
green = Pin(2, Pin.OUT)
servo = PWM(Pin(25), freq=50)
trig = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)

#reason : error (name TimeoutError isn't defined)  code from platform
class TimeoutError(Exception):
    pass

###   global variables    ###
last_time = time.ticks_ms()

# Hardware timer
trigger_timer = Timer(0)
tim1 = Timer(1)

###    functions    ### 
def set_angle(angle):
    duty = int((angle / 180) * 102 + 26)
    servo.duty(duty)

def turn_Off_leds():
    red.value(0)
    green.value(0)

def send_trigger():
    # Ensure TRIG is low
    trig.off()
    # Schedule TRIG high after 2 µs
    trigger_timer.init(period=2, mode=Timer.ONE_SHOT, callback=set_trigger_high)

def set_trigger_high(timer):
    trig.on()
    # Schedule TRIG low after 10 µs
    trigger_timer.init(period=10, mode=Timer.ONE_SHOT, callback=set_trigger_low)

def set_trigger_low(timer):
    trig.off()
    # Now the ultrasonic pulse has been sent

def get_distance_cm():
    send_trigger()
    try:
        pulse_duration = time_pulse_us_safe(echo, 1, 30000)
    except TimeoutError:
        return None
    return pulse_duration / 58.0

def time_pulse_us_safe(pin, pulse_level, timeout_us):
    start_time = time.ticks_us()

    # Wait for pulse to start
    while pin.value() != pulse_level:
        if time.ticks_diff(time.ticks_us(), start_time) > timeout_us:
            raise TimeoutError("Echo start timeout")

    pulse_start = time.ticks_us()

    # Wait for pulse to end
    while pin.value() == pulse_level:
        if time.ticks_diff(time.ticks_us(), pulse_start) > timeout_us:
            raise TimeoutError("Echo end timeout")

    pulse_end = time.ticks_us()

    return time.ticks_diff(pulse_end, pulse_start)


def veichle_detected():
    turn_Off_leds()
    print("-------------------")
    print("Vehicle detcted")
    print("gate is opening...")
    set_angle(90)
    green.value(1)

def nothing_detected():
        turn_Off_leds()
        red.value(1)
        set_angle(0)
        print("--------------------")
        print("NO Veichle Detectd X")



###   Super loop   ###
while True:
    now = time.ticks_ms()

    distance = get_distance_cm()

    if distance is None:
        print("Failed to read distance")
        continue   # skip this loop iteration

    distance = int(distance)

    if distance <= 50:
        #tim1.init(period = 10 , callback = veichle_detected , mode = Timer.ONE_SHOT)
        # if period incresed ex 100 code returns "Failed to read distance"

        veichle_detected()
        if time.ticks_diff( now, last_time) >= 1000 :
            last_time = now

    else :
        #tim1.init(period = 10 , callback = nothing_detected , mode = Timer.ONE_SHOT)
        nothing_detected()
        if time.ticks_diff( now, last_time) >= 1000 :
            last_time = now

'''
next_time = time.ticks_ms()
interval = 1000  # milliseconds
while True:
    # --- your task here ---
    print("Task running at", time.ticks_ms())

    # schedule the next run
    next_time += interval

    # wait until the scheduled time (When the current time reaches next_time, the condition fails and the loop exits.)
    while time.ticks_diff(next_time, time.ticks_ms()) > 0:
        pass
        '''
      #if time.ticks_diff(now, last_time) >= 1000:
      #  led.value(not led.value())
       # last_time = now