from machine import Pin, PWM
import time

# ---------------- PINS ----------------
# Ultrasonic #1: Fill level (inside, facing down)
FILL_TRIG = 5
FILL_ECHO = 18

# Ultrasonic #2: User detection (front, facing outward)
USER_TRIG = 6
USER_ECHO = 7

# Servo
SERVO_PIN = 10   

fill_trig = Pin(FILL_TRIG, Pin.OUT)
fill_echo = Pin(FILL_ECHO, Pin.IN)

user_trig = Pin(USER_TRIG, Pin.OUT)
user_echo = Pin(USER_ECHO, Pin.IN)

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

# -------------- THRESHOLDS (cm) --------------
# Fill-level classification based on fill ultrasonic distance
EMPTY_DIST_CM = 95.0
HALF_DIST_CM  = 50.0
FULL_DIST_CM  = 10.0

# User near threshold based on user ultrasonic distance
USER_NEAR_CM = 30.0

# -------------- SERVO SETTINGS --------------
CLOSE_ANGLE = 0
OPEN_ANGLE  = 90      # adjust to match lid mechanics
OPEN_TIME_S = 2.0     # keep lid open for this long
COOLDOWN_MS = 500   # prevent rapid re-trigger

def set_servo_angle(angle):
    angle = max(0, min(180, int(angle)))

    # Typical servo pulse range
    min_us = 500
    max_us = 2500
    us = min_us + (max_us - min_us) * angle // 180

    # 50Hz period = 20,000us
    duty = int(us * 65535 / 20000)
    servo.duty_u16(duty)

def lid_open():
    set_servo_angle(OPEN_ANGLE)

def lid_close():
    set_servo_angle(CLOSE_ANGLE)

def read_distance_cm(trig: Pin, echo: Pin, timeout_us=30000):
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    # wait for echo HIGH
    start_wait = time.ticks_us()
    while echo.value() == 0:
        if time.ticks_diff(time.ticks_us(), start_wait) > timeout_us:
            return -1

    start = time.ticks_us()

    # wait for echo LOW
    while echo.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout_us:
            return -1

    end = time.ticks_us()
    duration = time.ticks_diff(end, start)

    return (duration * 0.0343) / 2  # cm

def classify_fill(dist_cm):
    if dist_cm < 0:
        return "UNKNOWN"
    if dist_cm >= EMPTY_DIST_CM:
        return "EMPTY"
    elif dist_cm >= HALF_DIST_CM:
        return "HALF"
    elif dist_cm <= FULL_DIST_CM:
        return "FULL"
    else:
        return "ALMOST FULL"

# Start closed
lid_close()
time.sleep(0.5)

last_open_ms = 0

while True:
    # Read fill level
    fill_dist = read_distance_cm(fill_trig, fill_echo)
    bin_state = classify_fill(fill_dist)

    # Read user presence
    user_dist = read_distance_cm(user_trig, user_echo)
    user_near = (user_dist != -1 and user_dist <= USER_NEAR_CM)

    now = time.ticks_ms()
    can_open = (bin_state not in ["FULL", "UNKNOWN"])

    # Action: open lid only if user near AND bin not full AND cooldown passed
    if user_near and can_open and time.ticks_diff(now, last_open_ms) > COOLDOWN_MS:
        print("OPENING: user_dist={}cm, fill_dist={}cm, state={}".format(
            "?" if user_dist == -1 else round(user_dist, 1),
            "?" if fill_dist == -1 else round(fill_dist, 1),
            bin_state
        ))
        lid_open()
        time.sleep(OPEN_TIME_S)
        lid_close()
        last_open_ms = now
        print("CLOSED")

    # Debug output
    print("FILL:", "?" if fill_dist == -1 else round(fill_dist, 1), "cm |",
          "STATE:", bin_state, "|",
          "USER:", "?" if user_dist == -1 else round(user_dist, 1), "cm |",
          "USER_NEAR:", user_near)

    time.sleep(0.4)

# Start with lid closed
lid_close()
time.sleep(0.5)

# ---- SERVO TEST (ADD THIS HERE) ----
lid_open()
time.sleep(1)
lid_close()
time.sleep(1)
lid_open()
time.sleep(1)
lid_close()
# ---- END SERVO TEST ----

last_open_ms = 0

while True:
    # main loop logic here