#include <Wire.h>
#include <MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <math.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Initialize MPU6050
MPU6050 mpu;

// Initialize OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

volatile int steps = 0;
int16_t ax, ay, az;
float prevMagnitude = 0.0;
float threshold = 1.2;  // Lower threshold for slow walking
float stepFrequency = 0; // Steps per second
unsigned long lastStepTime = 0;
unsigned long lastPeakTime = 0;
unsigned long debounceTime = 200; // Debounce time in ms

void setup() {
  Serial.begin(115200);
  
  // MPU6050 Initialization
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }

  // OLED Initialization
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED initialization failed!");
    while (1);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Device Initializing...");
  display.display();
  delay(2000);

  Serial.println("Setup complete.");
}

void loop() {
  // Get acceleration data
  mpu.getAcceleration(&ax, &ay, &az);

  // Convert raw values to G-forces
  float accelX = ax / 16384.0;
  float accelY = ay / 16384.0;
  float accelZ = az / 16384.0;

  // Calculate magnitude of acceleration
  float magnitude = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);

  // High-pass filter to remove gravity component
  float dynamicAcceleration = magnitude - 1.0; // Subtract gravity (1G)

  // Step detection using peak recognition
  if (dynamicAcceleration > threshold && (millis() - lastStepTime > debounceTime)) {
    unsigned long currentTime = millis();
    float timeSinceLastPeak = (currentTime - lastPeakTime) / 1000.0; // Convert to seconds
    lastPeakTime = currentTime;

    // Calculate step frequency (steps per second)
    if (timeSinceLastPeak > 0) {
      stepFrequency = 1.0 / timeSinceLastPeak;
    }

    // Count the step if frequency is within typical walking/running range
    if (stepFrequency > 0.5 && stepFrequency < 3.0) { // 0.5-3 Hz for walking/running
      steps++;
      lastStepTime = currentTime;
    }
  }

  // Display steps on Serial Monitor
  Serial.print("Steps: ");
  Serial.println(steps);

  // Update OLED display
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Wearable Stats");
  display.setCursor(0, 15);
  display.print("Steps: ");
  display.println(steps);
  display.setCursor(0, 30);
  display.print("Freq: ");
  display.print(stepFrequency, 2);
  display.println(" Hz");
  display.display();

  delay(50); // Smooth updates
}