from machine import Pin, PWM, time_pulse_us
import time

# -------------------------
# Pin Setup
# -------------------------

# Person sensor
trig_person = Pin(2, Pin.OUT)
echo_person = Pin(3, Pin.IN)

# Bin level sensor
trig_bin = Pin(4, Pin.OUT)
echo_bin = Pin(5, Pin.IN)

# LED and buzzer
led = Pin(14, Pin.OUT)
buzzer = Pin(16, Pin.OUT)

# Servo
servo = PWM(Pin(15))
servo.freq(50)

# -------------------------
# Servo Function
# -------------------------
def set_servo_angle(angle):
    duty = int(1638 + (angle / 180) * (8192 - 1638))
    servo.duty_u16(duty)

def open_lid():
    set_servo_angle(90)

def close_lid():
    set_servo_angle(0)

# -------------------------
# Ultrasonic Function
# -------------------------
def get_distance(trig, echo):
    trig.low()
    time.sleep_us(2)

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

    duration = time_pulse_us(echo, 1, 30000)

    if duration < 0:
        return None

    distance = (duration * 0.0343) / 2
    return distance

# -------------------------
# Thresholds
# -------------------------
PERSON_THRESHOLD = 50   # cm
BIN_FULL_THRESHOLD = 20 # cm

# -------------------------
# Initial State
# -------------------------
close_lid()
led.off()
buzzer.off()

# -------------------------
# Main Loop
# -------------------------
while True:
    person_distance = get_distance(trig_person, echo_person)
    bin_distance = get_distance(trig_bin, echo_bin)

    print("Person:", person_distance, "cm")
    print("Bin:", bin_distance, "cm")
    print("----------------------")
    time.sleep(0.8)

    # Lid Control
    if person_distance is not None and person_distance < PERSON_THRESHOLD:
        open_lid()
    else:
        close_lid()

    # Full Bin Alert
    if bin_distance is not None and bin_distance < BIN_FULL_THRESHOLD:
        led.off()

        buzzer.on()
        time.sleep(0.2)
        buzzer.off()
        time.sleep(0.2)
    else:
        led.on()
        buzzer.off()
        time.sleep(0.3)