#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 4

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

Adafruit_MPU6050 mpu;

// Constants
#define ACCEL_THRESHOLD 1.0   // Adjust this threshold based on your movements
#define STEP_THRESHOLD 5      // Adjust this threshold based on your movements
#define CALORIES_PER_STEP 0.05
#define DISTANCE_PER_STEP 0.6

// Variables
int stepCount = 0;
float totalCalories = 0;
float totalDistance = 0;
bool stepDetected = false;

void setup(void) {
  Serial.begin(115200);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE, 0);
  display.setCursor(35, 10);
  display.print("ESP32");
  display.setCursor(25, 40);
  display.print("MPU6050");
  display.display();

  // Try to initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(1000);
}
void loop() {
  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate total acceleration
  float totalAcceleration = sqrt(pow(a.acceleration.x, 2) + pow(a.acceleration.y, 2) + pow(a.acceleration.z, 2));

  // Update previous acceleration value
  static float prevAcceleration = totalAcceleration;

  // Calculate the rate of change of acceleration
  float accelerationChange = abs(totalAcceleration - prevAcceleration);

  // Check for a step
  if (accelerationChange > ACCEL_THRESHOLD && totalAcceleration > STEP_THRESHOLD) {
    stepCount++;
    totalCalories += CALORIES_PER_STEP;
    totalDistance += DISTANCE_PER_STEP;
  }

  // Display data on OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE, 0);
  display.setCursor(0, 0);
  display.println("Fitness Tracker");
  display.setTextSize(1);
  display.setCursor(0, 20);
  display.print("Steps: ");
  display.println(stepCount);
  Serial.println(stepCount);

  display.print("Calories: ");
  display.println(totalCalories, 2);
  Serial.println(totalCalories);

  display.print("Distance: ");
  Serial.println(totalDistance);
  display.println(totalDistance, 2);

  // display.println("Accelerometer:");
  // display.print(a.acceleration.x);
  // display.print(",");
  // display.print(a.acceleration.y);
  // display.print(",");
  // display.println(a.acceleration.z);

  // display.println("Gyroscope:");
  // display.print(g.gyro.x);
  // display.print(",");
  // display.print(g.gyro.y);
  // display.print(",");
  // display.print(g.gyro.z);

  display.display();

  // Update previous acceleration value
  prevAcceleration = totalAcceleration;

  delay(10);
}