from machine import Pin, I2C
from mpu6050 import MPU6050
import time
import math

# --------------------------------
# ESP32 I2C configuration
# --------------------------------
i2c = I2C(0, scl=Pin(22), sda=Pin(21))

# Initialize MPU6050
mpu = MPU6050(i2c)

# --------------------------------
# MPU6050 conversion factors
# --------------------------------
ACC_SCALE = 16384.0
GYRO_SCALE = 131.0

# --------------------------------
# FALL DETECTION THRESHOLDS
# --------------------------------
ACC_THRESHOLD = 2.5       # g
GYRO_THRESHOLD = 200.0   # degree/second

print("Smart Helmet - Fall Detection")
print("--------------------------------")
print("Time,Ax,Ay,Az,Gx,Gy,Gz,AccelMag,GyroMag,Class")

while True:

    # Read raw accelerometer values
    ax_raw, ay_raw, az_raw = mpu.accel()

    # Read raw gyroscope values
    gx_raw, gy_raw, gz_raw = mpu.gyro()

    # --------------------------------
    # Convert accelerometer to g
    # --------------------------------
    ax = ax_raw / ACC_SCALE
    ay = ay_raw / ACC_SCALE
    az = az_raw / ACC_SCALE

    # --------------------------------
    # Convert gyroscope to degree/sec
    # --------------------------------
    gx = gx_raw / GYRO_SCALE
    gy = gy_raw / GYRO_SCALE
    gz = gz_raw / GYRO_SCALE

    # --------------------------------
    # Calculate acceleration magnitude
    # --------------------------------
    accel_mag = math.sqrt(
        ax * ax +
        ay * ay +
        az * az
    )

    # --------------------------------
    # Calculate gyroscope magnitude
    # --------------------------------
    gyro_mag = math.sqrt(
        gx * gx +
        gy * gy +
        gz * gz
    )

    # --------------------------------
    # FALL / NORMAL CLASSIFICATION
    # --------------------------------
    if accel_mag >= ACC_THRESHOLD or gyro_mag >= GYRO_THRESHOLD:
        classification = "Fall"
    else:
        classification = "Normal"

    # --------------------------------
    # Print CSV data
    # --------------------------------
    print(
        "{},{:.3f},{:.3f},{:.3f},{:.2f},{:.2f},{:.2f},{:.3f},{:.2f},{}".format(
            time.ticks_ms(),
            ax, ay, az,
            gx, gy, gz,
            accel_mag,
            gyro_mag,
            classification
        )
    )

    time.sleep_ms(100)