from machine import Pin, PWM, time_pulse_us
import time

# Magic Numbers
trig_pin = 5
echo_pin = 19
servo_pin = 18
last_read_ultra = 0
DEBOUNCE_LAST_READ = 500
MAX_DIST = 20
CLOSED_SERVO_ANGLE = 0
OPEN_SERVO_ANGLE = 90

# Ultrasonic Sensor
trig = Pin(trig_pin, Pin.OUT)      
echo = Pin(echo_pin, Pin.IN)

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

Door_status = "CLOSED"
DEBUG = True

# LOG Function
def log(state, message):
    if DEBUG:
        print("[{}] [{} ms] [{}]".format(state, time.ticks_ms(), message))

# FIX: Changed variable name to start_log so it doesn't overwrite log()
start_log = ("START", "System started successfully")
log(start_log[0], start_log[1])

# METHOD CALC DISTANCE ULTRASONIC
def calc_distance():
    trig.off()
    time.sleep_us(2)

    trig.on()
    time.sleep_us(10)

    trig.off()

    duration = time_pulse_us(echo, 1)
    
    # Avoid division by zero if pulse times out
    if duration <= 0:
        return 999.0

    distance = (duration * 0.0343) / 2
    return distance

def set_servo_angle(deg):
    duty = int((((deg) / 180.0) * 2.0 + 0.5) / 20.0 * 1023)
    servo.duty(duty)

# Initialize door to closed
set_servo_angle(CLOSED_SERVO_ANGLE)

while True:
    now = time.ticks_ms()
    if time.ticks_diff(now, last_read_ultra) > DEBOUNCE_LAST_READ:
        last_read_ultra = now

        distance = calc_distance()
        
        if DEBUG:
            log("sensor", "Measured distance: {:.1f} cm".format(distance))

        if distance < MAX_DIST:
            if Door_status == "CLOSED":  
                set_servo_angle(OPEN_SERVO_ANGLE)
                Door_status = "OPEN"
                log("servo", "Door Status updated to: {}".format(Door_status))
        else:
            if Door_status == "OPEN":
                set_servo_angle(CLOSED_SERVO_ANGLE)
                Door_status = "CLOSED"
                log("servo", "Door Status updated to: {}".format(Door_status))