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 for stationary detection after fall
STATIONARY_TIME = 1.5     # seconds to confirm fall

class MPU6050:
    def _init_(self, i2c, addr=MPU6050_ADDR):
        self.i2c = i2c
        self.addr = addr
        try:
            # Wake up MPU6050
            self.i2c.writeto_mem(self.addr, PWR_MGMT_1, b'\x00')
            time.sleep(0.1)
            print("MPU6050 initialized successfully")
        except Exception as e:
            print(f"MPU6050 init error: {e}")
    
    def read_raw_data(self, reg):
        """Read raw 16-bit data from MPU6050"""
        try:
            data = self.i2c.readfrom_mem(self.addr, reg, 2)
            value = (data[0] << 8) | data[1]
            # Convert to signed value
            if value > 32768:
                value -= 65536
            return value
        except Exception as e:
            print(f"Read error: {e}")
            return 0
    
    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 magnitude of acceleration vector"""
        ax, ay, az = self.get_accel_data()
        total_accel = math.sqrt(ax*2 + ay2 + az*2)
        return total_accel, ax, ay, az

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
        self.sample_count = 0
        
        # Turn off outputs initially
        self.led.value(0)
        self.buzzer.value(0)
        print("Fall detector ready")
        print()
    
    def trigger_alarm(self):
        """Activate LED and buzzer alarm"""
        print("\n" + "="*50)
        print("*** FALL DETECTED ***")
        print("*** EMERGENCY ALERT ACTIVATED ***")
        print("="*50)
        self.alarm_active = True
        
        # Flash and beep pattern
        for i in range(10):
            self.led.value(1)
            self.buzzer.value(1)
            print(f"ALERT #{i+1} - LED ON, BUZZER ON")
            time.sleep(0.2)
            self.led.value(0)
            self.buzzer.value(0)
            print(f"ALERT #{i+1} - LED OFF, BUZZER OFF")
            time.sleep(0.2)
        
        print("="*50)
        print("Alarm sequence complete")
        print("="*50)
        print()
        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, ax, ay, az = self.mpu.get_total_acceleration()
        
        # Print readings every 10 samples (once per second at 10Hz)
        self.sample_count += 1
        if self.sample_count >= 10:
            print(f"Accel: {total_accel:.2f}g | X:{ax:.2f} Y:{ay:.2f} Z:{az:.2f}")
            self.sample_count = 0
        
        # 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")
                print(f">>> Axis values - X:{ax:.2f} Y:{ay:.2f} Z:{az:.2f}")
                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 if person is stationary after impact (lying down)
        if self.fall_detected and self.impact_time:
            time_since_impact = time.time() - self.impact_time
            
            if time_since_impact > IMPACT_DURATION:
                # Check if person is now stationary
                if 0.5 < total_accel < 1.5:
                    print(f"Checking fall... {time_since_impact:.1f}s - Accel: {total_accel:.2f}g")
                    if time_since_impact > STATIONARY_TIME:
                        # Confirmed fall - person hasn't gotten up
                        self.trigger_alarm()
                        self.fall_detected = False
                        self.impact_time = None
                        time.sleep(3)  # Wait before next detection
                else:
                    # False alarm - person recovered quickly
                    print(f">>> False alarm - Motion detected: {total_accel:.2f}g")
                    print(">>> Person recovered quickly")
                    print()
                    self.fall_detected = False
                    self.impact_time = None
        
        return total_accel

def main():
    print()
    print("="*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 Fall Threshold: {FREE_FALL_THRESHOLD}g")
    print("="*50)
    print()
    
    try:
        # Initialize I2C (SDA=GP4, SCL=GP5)
        print("Initializing I2C...")
        i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=400000)
        print("I2C initialized")
        
        # Scan for I2C devices
        print("Scanning for I2C devices...")
        devices = i2c.scan()
        if devices:
            print(f"I2C devices found at addresses: {[hex(d) for d in devices]}")
        else:
            print("ERROR: No I2C devices found!")
            print("Check your MPU6050 connections:")
            print("  VCC -> 3.3V")
            print("  GND -> GND")
            print("  SDA -> GP4")
            print("  SCL -> GP5")
            return
        
        print()
        
        # Initialize MPU6050
        mpu = MPU6050(i2c)
        
        # Initialize Fall Detector
        detector = FallDetector(mpu, LED_PIN, BUZZER_PIN)
        
        print("="*50)
        print("SYSTEM READY - Monitoring for falls...")
        print("TIP: Click and drag the MPU6050 sensor to simulate movement")
        print("     Shake it hard to simulate a fall!")
        print("="*50)
        print()
        
        # Main loop
        loop_count = 0
        while True:
            if not detector.alarm_active:
                total_accel = detector.detect_fall()
                loop_count += 1
                
                # Print status every 50 loops (5 seconds)
                if loop_count >= 50:
                    print(f"Status: System active, monitoring... ({total_accel:.2f}g)")
                    loop_count = 0
                    
            time.sleep(0.1)  # Sample rate: 10Hz
    
    except KeyboardInterrupt:
        print()
        print("="*50)
        print("System stopped by user")
        print("="*50)
    except Exception as e:
        print()
        print(f"ERROR occurred: {e}")
        import sys
        sys.print_exception(e)
    finally:
        try:
            detector.reset_alarm()
            print("System shutdown complete")
        except:
            pass

# Auto-start the program
print("Starting Fall Detection System...")
print("Please wait...")
time.sleep(0.5)
main()