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

MPU6050 mpu;

int stepCount = 0;
long lastStepTime = 0;
const int stepDelay = 300;  // Minimum time between steps (milliseconds)

// Gyroscope thresholds for step detection
const float gyroThreshold = 150.0;  // Gyro threshold (degrees per second)

int16_t gx, gy, gz;

void setup() {
  Serial.begin(115200);
  
  // Initialize I2C communication on ESP32
  Wire.begin(21, 22);  // SDA on GPIO21, SCL on GPIO22
  
  // Initialize the MPU6050 sensor
  Serial.println("Initializing MPU6050...");
  mpu.initialize();

  // Check if MPU6050 connection is successful
  if (mpu.testConnection()) {
    Serial.println("MPU6050 initialized successfully");
  } else {
    Serial.println("MPU6050 connection failed");
    while (1); // Stop execution if MPU6050 fails to connect
  }
}

void detectStep() {
  // Read MPU6050 sensor data (raw int16_t values)
  mpu.getRotation(&gx, &gy, &gz);
  
  // Convert gyroscope values to degrees per second
  float gz_deg = gz / 131.0;  // z-axis is typically used for step detection

  // Detect steps based on gyroscope data exceeding the threshold
  if (abs(gz_deg) > gyroThreshold) {
    long currentTime = millis();
    
    // Check if enough time has passed since the last detected step
    if (currentTime - lastStepTime > stepDelay) {
      stepCount++;
      lastStepTime = currentTime;  // Update the last step time
      Serial.print("Step detected! Total steps: ");
      Serial.println(stepCount);
    }
  }

  delay(100);  // Adjust the delay as needed
}

void loop() {
  detectStep();
}