#include <Wire.h>
#include <MPU6050.h>

// Initialize MPU6050
MPU6050 mpu;

// Global variables for step counting
float GyroZ = 0;       // Processed gyroscope Z-axis reading (float)
float filt1 = 0;       // Filtered gyro data

// Moving Average Filter Parameters
const int MA_SIZE = 5;                // Moving average window size
float gyroBuffer[MA_SIZE] = {0};      // Buffer for gyroZ
int bufferIndex = 0;                   // Current index in the buffer
float gyroSum = 0;                     // Sum of the buffer elements

int stepCount = 0;                     // Total number of steps
float threshold = 0.5;                 // Threshold for step detection

// Global variables for fall detection
float accelMagnitude = 0;
bool fallDetected = false;

// Function to read gyroscope and accelerometer data from the MPU6050
void getSensorData() {
    int16_t rawAccelX, rawAccelY, rawAccelZ;
    int16_t rawGyroX, rawGyroY, rawGyroZ;

    // Read all six axes
    mpu.getMotion6(&rawAccelX, &rawAccelY, &rawAccelZ, &rawGyroX, &rawGyroY, &rawGyroZ);

    // Convert raw gyroscope Z-axis data to degrees/second
    GyroZ = rawGyroZ / 131.0; // MPU6050 scaling factor for ±250°/s

    // Calculate acceleration magnitude for fall detection
    float accelX = rawAccelX / 16384.0; // Assuming ±2g scale
    float accelY = rawAccelY / 16384.0;
    float accelZ = rawAccelZ / 16384.0;
    accelMagnitude = sqrt(pow(accelX, 2) + pow(accelY, 2) + pow(accelZ, 2));
}

// Function to apply a simple moving average filter to gyroZ
void filterData() {
    // Subtract the oldest value from the sum
    gyroSum -= gyroBuffer[bufferIndex];
    
    // Add the new gyroZ value to the buffer and sum
    gyroBuffer[bufferIndex] = GyroZ;
    gyroSum += GyroZ;
    
    // Move to the next index, wrapping around if necessary
    bufferIndex = (bufferIndex + 1) % MA_SIZE;
    
    // Calculate the moving average
    filt1 = gyroSum / MA_SIZE;
}

// Function to detect steps based on simple peak detection
void detectSteps() {
    static float prevFilt1 = 0; // Previous filtered gyroscope reading

    // Detect a step when the filtered gyroZ crosses the threshold from below
    if (filt1 > threshold && prevFilt1 <= threshold) {
        stepCount++;
    }

    // Update the previous filtered gyroscope reading
    prevFilt1 = filt1;
}

// Function to detect falls
void detectFall() {
    // Simple fall detection based on acceleration magnitude
    if (accelMagnitude < 0.8) { // Adjust threshold based on testing
        fallDetected = true;
    } else {
        fallDetected = false;
    }
}

void setup() {
    // Initialize Serial for debugging
    Serial.begin(9600);
    
    // Initialize I2C communication for MPU6050
    Wire.begin();
    mpu.initialize();
    
    // Check MPU6050 connection
    if (!mpu.testConnection()) {
        Serial.println("MPU6050 connection failed!");
        while (1); // Halt if connection fails
    } else {
        Serial.println("MPU6050 connected.");
    }
}

void loop() {
    // Read sensor data (accelerometer and gyroscope)
    getSensorData();
    
    // Apply filter to gyro data
    filterData();
    
    // Detect steps
    detectSteps();
    
    // Detect falls
    detectFall();
    
    // Output the current step count and fall detection status every second
    static unsigned long lastPrint = 0;
    if (millis() - lastPrint > 1000) { // Update every 1000 ms (1 second)
        Serial.print("Steps counted: ");
        Serial.println(stepCount);
        Serial.println(fallDetected ? "Fall detected!" : "No fall detected.");
        lastPrint = millis(); // Update the last print time
    }
    
    // Short delay for loop stability (sampling frequency ~100Hz)
    delay(50);
}