from machine import Pin, I2C
from time import sleep
import math

# MPU6050 setup
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B

# Initialize I2C for Pico (GP0 = SDA, GP1 = SCL)
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400000)
i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, b'\x00')  # Wake MPU6050

# LED and Buzzer setup
led = Pin(15, Pin.OUT)       # GP15 → LED
buzzer = Pin(14, Pin.OUT)    # GP14 → Buzzer

def read_raw_data(addr):
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)
    low = i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)
    value = (high[0] << 8) | low[0]
    if value > 32767:
        value -= 65536
    return value

def get_accel_data():
    acc_x = read_raw_data(ACCEL_XOUT_H)
    acc_y = read_raw_data(ACCEL_XOUT_H + 2)
    acc_z = read_raw_data(ACCEL_XOUT_H + 4)
    Ax = acc_x / 16384.0
    Ay = acc_y / 16384.0
    Az = acc_z / 16384.0
    return Ax, Ay, Az

# Thresholds (adjust as needed)
LOW_THRESHOLD = 0.5   # below this = possible fall
HIGH_THRESHOLD = 2.0  # above this = strong impact

print("=== Fall Detection with LED & Buzzer ===")

while True:
    Ax, Ay, Az = get_accel_data()
    total_acc = math.sqrt(Ax**2 + Ay**2 + Az**2)

    print("Ax={:.2f} Ay={:.2f} Az={:.2f} | Total Acc={:.2f}g".format(Ax, Ay, Az, total_acc))

    # ---- IF / ELSE ----
    if LOW_THRESHOLD <= total_acc <= HIGH_THRESHOLD:
        print("Normal movement\n")
        led.off()
        buzzer.off()
    else:
        print("⚠️ Fall detected!\n")
        led.on()
        buzzer.on()
        sleep(1)
        led.off()
        buzzer.off()

    sleep(0.5)