from machine import Pin, I2C
import time
import math

# ==============================
# MPU6050 I2C Address and Registers
# ==============================
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F

# ==============================
# GPIO Pin Configuration
# ==============================
LED_PIN = 16      # GPIO pin for LED
BUZZER_PIN = 17   # GPIO pin for Buzzer

# ==============================
# Fall Detection Thresholds
# ==============================
FALL_THRESHOLD = 2.5       # g-force threshold for fall detection
FREE_FALL_THRESHOLD = 0.5  # g-force for free fall detection
IMPACT_DURATION = 0.1      # seconds
STATIONARY_THRESHOLD = 0.5 # g-force threshold for stationary detection
STATIONARY_TIME = 1.5      # seconds to confirm fall

# ==============================
# MPU6050 Class
# ==============================
class MPU6050:
    def __init__(self, i2c, addr=MPU6050_ADDR):
        self.i2c = i2c
        self.addr = addr
        # Wake up MPU6050
        self.i2c.writeto_mem(self.addr, PWR_MGMT_1, b'\x00')
        time.sleep(0.1)
        print("✓ MPU6050 initialized")

    def read_raw_data(self, reg):
        """Read raw 16-bit data from MPU6050"""
        data = self.i2c.readfrom_mem(self.addr, reg, 2)
        value = (data[0] << 8) | data[1]
        if value > 32768:
            value -= 65536
        return value

    def get_accel_data(self):
        """Get accelerometer data in g-force"""
        accel_x = self.read_raw_data(ACCEL_XOUT_H)
        accel_y = self.read_raw_data(ACCEL_YOUT_H)
        accel_z = self.read_raw_data(ACCEL_ZOUT_H)
        # Convert to g-force (±2g range, sensitivity = 16384 LSB/g)
        ax = accel_x / 16384.0
        ay = accel_y / 16384.0
        az = accel_z / 16384.0
        return ax, ay, az

    def get_total_acceleration(self):
        """Calculate total acceleration magnitude"""
        ax, ay, az = self.get_accel_data()
        total_accel = math.sqrt(ax**2 + ay**2 + az**2)
        return total_accel

# ==============================
# Fall Detector Class
# ==============================
class FallDetector:
    def __init__(self, mpu, led_pin, buzzer_pin):
        self.mpu = mpu
        self.led = Pin(led_pin, Pin.OUT)
        self.buzzer = Pin(buzzer_pin, Pin.OUT)
        self.fall_detected = False
        self.impact_time = None
        self.alarm_active = False

        # Turn off outputs initially
        self.led.value(0)
        self.buzzer.value(0)
        print("✓ Fall detector initialized")

    def trigger_alarm(self):
        """Activate LED and buzzer alarm"""
        print("\n" + "="*50)
        print("⚠️  FALL DETECTED!")
        print("⚠️  EMERGENCY ALERT ACTIVATED!")
        print("="*50 + "\n")
        self.alarm_active = True

        # Flash LED and buzzer pattern
        for i in range(15):
            self.led.value(1)
            self.buzzer.value(1)
            time.sleep(0.15)
            self.led.value(0)
            self.buzzer.value(0)
            time.sleep(0.15)
            print(f"🚨 ALERT #{i+1}")

        self.alarm_active = False

    def reset_alarm(self):
        """Turn off LED and buzzer"""
        self.led.value(0)
        self.buzzer.value(0)

    def detect_fall(self):
        """Main fall detection algorithm"""
        total_accel = self.mpu.get_total_acceleration()

        # Stage 1: Detect sudden acceleration (impact) or free fall
        if total_accel > FALL_THRESHOLD or total_accel < FREE_FALL_THRESHOLD:
            if not self.fall_detected:
                print(f"\n⚡ Impact detected! Acceleration: {total_accel:.2f}g")
                self.impact_time = time.time()
                self.fall_detected = True
                # Quick LED blink to indicate detection
                self.led.value(1)
                time.sleep(0.05)
                self.led.value(0)

        # Stage 2: Check stationary phase after impact
        if self.fall_detected and self.impact_time:
            time_since_impact = time.time() - self.impact_time

            if time_since_impact > IMPACT_DURATION:
                # Check if user is stationary after fall
                if 0.5 < total_accel < 1.5:
                    if time_since_impact > STATIONARY_TIME:
                        # Confirm fall
                        self.trigger_alarm()
                        self.fall_detected = False
                        self.impact_time = None
                        time.sleep(3)
                    else:
                        print(f"⏳ Checking... {time_since_impact:.1f}s", end='\r')
                else:
                    # False alarm (movement detected)
                    print(f"\n✓ False alarm - Motion detected: {total_accel:.2f}g")
                    self.fall_detected = False
                    self.impact_time = None

        return total_accel

# ==============================
# Main Program
# ==============================
def main():
    print("\n" + "="*50)
    print("      FALL DETECTION SYSTEM")
    print("   MPU6050 + Raspberry Pi Pico")
    print("="*50)
    print(f"LED Pin: GP{LED_PIN}")
    print(f"Buzzer Pin: GP{BUZZER_PIN}")
    print(f"Fall Threshold: {FALL_THRESHOLD}g")
    print(f"Free