import board
import digitalio
import time
import pwmio

# ====================== PIN SETUP ======================
def init_btn(pin):
    io = digitalio.DigitalInOut(pin)
    io.direction = digitalio.Direction.INPUT
    io.pull = digitalio.Pull.UP 
    return io

# Buttons
btn_arm   = init_btn(board.GP14) # Red
btn_reset = init_btn(board.GP15) # Black
btn_tilt  = init_btn(board.GP22) # Yellow

# LEDs
led_green  = digitalio.DigitalInOut(board.GP2)
led_green.direction = digitalio.Direction.OUTPUT
led_red    = digitalio.DigitalInOut(board.GP3)
led_red.direction = digitalio.Direction.OUTPUT
led_yellow = digitalio.DigitalInOut(board.GP4)
led_yellow.direction = digitalio.Direction.OUTPUT

# Buzzer & Servo
buzzer = pwmio.PWMOut(board.GP13, frequency=1000, duty_cycle=0, variable_frequency=True)
servo = pwmio.PWMOut(board.GP6, frequency=50, duty_cycle=0)

# Ultrasonic
trig = digitalio.DigitalInOut(board.GP18)
trig.direction = digitalio.Direction.OUTPUT
trig.value = False
echo = digitalio.DigitalInOut(board.GP17)
echo.direction = digitalio.Direction.INPUT

# ====================== FUNCTIONS ======================
def get_distance():
    trig.value = False
    time.sleep(0.000002)
    trig.value = True
    time.sleep(0.00001)
    trig.value = False
    
    timeout = time.monotonic() + 0.04
    while not echo.value:
        if time.monotonic() > timeout: return 999
    start = time.monotonic()
    while echo.value:
        if time.monotonic() > timeout: return 999
    return (time.monotonic() - start) * 17150

# ====================== STABILIZATION ======================
print("Cleaning electrical noise...")
buzzer.duty_cycle = 0
led_green.value = led_red.value = led_yellow.value = False
set_servo_angle = lambda a: setattr(servo, 'duty_cycle', int(((a / 180) * 5000 + 2500) * 65535 / 20000))
set_servo_angle(0)

# WAIT FOR BUTTONS TO SETTLE
time.sleep(2)
while btn_arm.value == False:
    print("WAITING: Red Button (GP14) is SHORTED to GND. Fix wiring!")
    time.sleep(1)

system_armed = False
alarm_active = False
print("--- SYSTEM READY ---")
print("Press RED button to Arm.")

# ====================== MAIN LOGIC ======================
while True:
    # --- PART 3: TILT OVERRIDE ---
    if btn_tilt.value == False:
        print("TILT!")
        led_yellow.value = True
        buzzer.frequency = 3000
        buzzer.duty_cycle = 32768
        time.sleep(0.5)
        buzzer.duty_cycle = 0
        led_yellow.value = False
        while btn_tilt.value == False: pass
        continue

    # --- PART 1: ARMING ---
    if not system_armed:
        if btn_arm.value == False:
            time.sleep(0.1) # Final Debounce
            if btn_arm.value == False:
                print(">>> SYSTEM ARMED")
                system_armed = True
                led_green.value = True
                buzzer.frequency = 1000
                buzzer.duty_cycle = 32768
                time.sleep(0.2)
                buzzer.duty_cycle = 0
                while btn_arm.value == False: pass

    # --- PART 2: INTRUSION ---
    elif system_armed:
        led_green.value = True # Ensure Green stays ON
        dist = get_distance()
        
        # Trigger Alarm (Ignore 0 or 999)
        if 2 < dist < 50:
            alarm_active = True
            print(">>> INTRUSION!")

        if alarm_active:
            led_red.value = True
            buzzer.frequency = 800
            buzzer.duty_cycle = 32768
            
            if btn_reset.value == False:
                alarm_active = False
                led_red.value = False
                buzzer.duty_cycle = 0
                print(">>> RESET")
                time.sleep(0.5)
    
    time.sleep(0.05)