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

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

# I2C setup for Raspberry Pi Pico
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400000)

# Buzzer setup for alerts
buzzer = Pin(15, Pin.OUT)

# Button setup for acknowledging fall detection
button = Pin(14, Pin.IN, Pin.PULL_DOWN)

# Function to read raw data from MPU6050
def read_raw_data(addr):
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)[0]
    low = i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)[0]
    value = (high << 8) | low
    if value > 32768:
        value = value - 65536
    return value

# Initialize MPU6050
def mpu6050_init():
    i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, b'\x00')  # Wake up MPU6050

# Function to detect falls based on acceleration
def detect_fall():
    # Read accelerometer values
    accel_x = read_raw_data(ACCEL_XOUT_H)
    accel_y = read_raw_data(ACCEL_XOUT_H + 2)
    accel_z = read_raw_data(ACCEL_XOUT_H + 4)
    
    # Convert to g-force
    ax = accel_x / 16384.0
    ay = accel_y / 16384.0
    az = accel_z / 16384.0

    # Compute the total magnitude of acceleration (vector sum)
    acceleration_magnitude = math.sqrt(ax ** 2 + ay ** 2 + az ** 2)

    print(f"Acceleration magnitude: {acceleration_magnitude:.2f}g")
    
    # Threshold for fall detection (adjust based on testing)
    fall_threshold_high = 2.5  # Sudden large acceleration
    fall_threshold_low = 0.5   # Sudden stop (near-zero acceleration)

    # Fall detection logic
    if acceleration_magnitude > fall_threshold_high or acceleration_magnitude < fall_threshold_low:
        print("Fall detected!")
        alert_user()
        return True
    return False

# Function to alert the user when a fall is detected
def alert_user():
    # Activate the buzzer for 5 seconds
    for _ in range(10):
        buzzer.on()
        sleep(0.2)
        buzzer.off()
        sleep(0.3)

# Main loop
mpu6050_init()

try:
    while True:
        if detect_fall():
            # Wait until the user acknowledges the fall with the button
            print("Waiting for user to acknowledge fall detection...")
            while button.value() == 0:
                sleep(0.1)
            print("Fall acknowledged, system reset.")

        sleep(0.5)  # Delay between checks

except KeyboardInterrupt:
    print("Program stopped by user.")