from machine import Pin, I2C
from time import sleep
from i2c_lcd import I2cLcd

# === Setup I2C LCD ===
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)

# === Define Pins ===
btn_green = Pin(25, Pin.IN, Pin.PULL_UP)  # ON/OFF toggle button
btn_red = Pin(33, Pin.IN, Pin.PULL_UP)    # Emergency stop toggle
btn_black = Pin(26, Pin.IN, Pin.PULL_UP)  # Safety interlock (must hold)

ir1 = Pin(36, Pin.IN)  # IR1 for relay
ir2 = Pin(39, Pin.IN)  # IR2 for reject count

relay = Pin(4, Pin.OUT)
led_green = Pin(14, Pin.OUT)
led_red = Pin(27, Pin.OUT)
buzzer = Pin(2, Pin.OUT)  # Simulated buzzer with GPIO2 (Wokwi)

# === Variables ===
system_on = False
paused_by_emergency = False  # <-- Emergency toggle state
reject_count = 0
object_detected_ir1 = False
object_detected_ir2 = False
green_button_prev = 1
red_button_prev = 1  # <-- Track emergency button state
last_status = ""
relay_triggered_once = False

# === Display Init ===
lcd.clear()
lcd.putstr("System Ready")

def update_lcd(msg):
    global last_status
    if msg != last_status:
        lcd.clear()
        lcd.putstr(msg)
        last_status = msg

def reset_all():
    global reject_count, object_detected_ir1, object_detected_ir2, relay_triggered_once
    reject_count = 0
    object_detected_ir1 = False
    object_detected_ir2 = False
    relay_triggered_once = False
    relay.off()
    led_green.off()
    led_red.off()
    buzzer.off()
    update_lcd("System OFF")

while True:
    # === Green Button Toggle (1 press ON, 1 press OFF) ===
    if green_button_prev == 1 and btn_green.value() == 0:  # falling edge
        system_on = not system_on
        paused_by_emergency = False  # Clear emergency pause when toggling
        if system_on:
            update_lcd("System ON")
            relay.on()
            sleep(1)
            relay.off()
            relay_triggered_once = True
        else:
            reset_all()
        sleep(0.3)
    green_button_prev = btn_green.value()

    # === Emergency Pause/Resume Toggle ===
    if red_button_prev == 1 and btn_red.value() == 0:  # falling edge
        paused_by_emergency = not paused_by_emergency
        if paused_by_emergency:
            update_lcd("!! EMERGENCY !!")
            relay.off()
            led_green.off()
            led_red.on()
            buzzer.off()
        else:
            update_lcd("Resuming...")
        sleep(0.3)
    red_button_prev = btn_red.value()

    # === Run if ON, Not Emergency Paused, and Safety Pressed ===
    if system_on and not paused_by_emergency and not btn_black.value():
        if reject_count > 2:
            led_red.on()
            led_green.off()
            buzzer.on()
            update_lcd(f"Reject: {reject_count}")
        else:
            led_red.off()
            buzzer.off()
            led_green.on()
            update_lcd(f"Running\nReject: {reject_count}")

        # === IR1: Relay trigger after safety ===
        if ir1.value() == 0 and not object_detected_ir1:
            object_detected_ir1 = True
            relay.on()
            sleep(5)
            relay.off()
        if ir1.value() == 1:
            object_detected_ir1 = False

        # === IR2: Reject counter ===
        if ir2.value() == 0 and not object_detected_ir2:
            object_detected_ir2 = True
            reject_count += 1
            update_lcd(f"Reject: {reject_count}")
            sleep(0.5)
        if ir2.value() == 1:
            object_detected_ir2 = False

    else:
        # System not allowed to run
        relay.off()
        led_green.off()
        buzzer.off()
        if not system_on:
            update_lcd("System OFF")
        elif paused_by_emergency:
            update_lcd("!! EMERGENCY !!")
        elif btn_black.value():
            update_lcd("Hold Safety Btn")

    sleep(0.1)