from machine import I2C, Pin, PWM, time_pulse_us
from time import sleep, ticks_ms
from pico_i2c_lcd import I2cLcd
from mpu6050 import MPU6050

# --- PINS ---
led = Pin(15, Pin.OUT)
buzzer = PWM(Pin(16))
buzzer.freq(1000)
buzzer.duty_u16(0)
button = Pin(14, Pin.IN, Pin.PULL_UP)

# --- SENSORS ---
# FREQ=10000 IS REQUIRED FOR WOKWI STABILITY
sleep(1) # Wait for simulator to stabilize
i2c_lcd = I2C(0, sda=Pin(0), scl=Pin(1), freq=10000)

print("Scanning I2C Bus 0...")
devices = i2c_lcd.scan()
if len(devices) == 0:
    print("No I2C devices found!")
else:
    print("I2C devices found:", [hex(device) for device in devices])

try:
    lcd = I2cLcd(i2c_lcd, 0x27, 2, 16)
    lcd.clear()
except:
    pass 

i2c_mpu = I2C(1, sda=Pin(26), scl=Pin(27), freq=400000)
mpu = MPU6050(i2c_mpu)

trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

# --- VARIABLES ---
current_state = "START"
last_lcd_update = 0
last_blink_time = 0
blink_state = False

def get_distance():
    trig.low()
    sleep(0.000002)
    trig.high()
    sleep(0.00001)
    trig.low()
    duration = time_pulse_us(echo, 1, 30000)
    if duration <= 0: return 400 
    return (duration * 0.0343) / 2

def check_fall():
    try:
        data = mpu.get_values()
        if abs(data["AcX"]) > 16000 or abs(data["AcY"]) > 16000: 
            return True
    except:
        pass
    return False

# --- MAIN LOOP ---
lcd.clear()
lcd.putstr("System Ready")

while True:
    now = ticks_ms()
    
    # 1. READ BUTTON (Instant)
    btn_pressed = (button.value() == 0)
    
    # 2. READ SENSORS (Only if button is NOT pressed)
    if not btn_pressed:
        dist = get_distance()
        is_fallen = check_fall()
    else:
        dist = 400
        is_fallen = False

    # 3. DECIDE STATE
    if btn_pressed:
        new_state = "PANIC"
    elif is_fallen:
        new_state = "FALL"
    elif dist < 30: 
        new_state = "OBSTACLE"
    else:
        new_state = "SAFE"

    # 4. INSTANT ALARM ACTION
    if new_state == "PANIC":
        buzzer.duty_u16(32768) 
        led.on()
    
    elif new_state == "FALL" or new_state == "OBSTACLE":
        if now - last_blink_time > 200:
            blink_state = not blink_state
            if blink_state:
                buzzer.duty_u16(32768)
                led.on()
            else:
                buzzer.duty_u16(0)
                led.off()
            last_blink_time = now
            
    else: 
        buzzer.duty_u16(0)
        led.off()

    # 5. SLOW LCD UPDATE (1 second interval)
    if now - last_lcd_update > 1000:
        if new_state == "PANIC":
            lcd.move_to(0,0)
            lcd.putstr("!! HELP !!      ")
            lcd.move_to(0,1)
            lcd.putstr("Button Pressed  ")
        elif new_state == "FALL":
            lcd.move_to(0,0)
            lcd.putstr("FALL DETECTED!  ")
            lcd.move_to(0,1)
            lcd.putstr("Sending Alert   ")
        elif new_state == "OBSTACLE":
            lcd.move_to(0,0)
            lcd.putstr("OBSTACLE!       ")
            lcd.move_to(0,1)
            lcd.putstr("Stop Now!       ")
        elif new_state == "SAFE":
            lcd.move_to(0,0)
            lcd.putstr("System Safe     ")
            lcd.move_to(0,1)
            lcd.putstr("Dist: {:.0f}cm      ".format(dist))
        
        last_lcd_update = now