#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

void setup(void) {
  lcd.begin(16, 2);
  lcd.print("Steps: ");
  lcd.setCursor(0, 1);
  lcd.print(0);
  Serial.begin(115200);
  while (!Serial) delay(10);
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  delay(100);
}

int MIN_SAMPLES = 50; // Min samples for step
int THRESHOLD = 5; // Minimum magnitude for step
int steps = 0; // Number of steps
int samples = 0; // How many samples have been taken
float avg[3] = {0, 0, 0}; // Average acceleration

void loop() {
  sensors_event_t acc, gyro, temp;
  mpu.getEvent(&acc, &gyro, &temp);
  // get difference from last reading
  float delta[3] = {
    acc.acceleration.x - avg[0],
    acc.acceleration.y - avg[1],
    acc.acceleration.z - avg[2]
  };
  // calculate magnitude
  float mag = sqrt(
    (delta[0] * delta[0]) +
    (delta[1] * delta[1]) +
    (delta[2] * delta[2])
  );
  // we must have collected enough samples
  if (mag > THRESHOLD && samples >= MIN_SAMPLES) {
    steps++;
    lcd.setCursor(0, 1);
    lcd.print(steps);
    avg[0] = 0; avg[1] = 0; avg[2] = 0;
    samples = 0;
  }
  // calculate new average
  avg[0] = (avg[0] + acc.acceleration.x) / 2;
  avg[1] = (avg[1] + acc.acceleration.y) / 2;
  avg[2] = (avg[2] + acc.acceleration.z) / 2;
  // cap samples so we don't overflow
  samples = min(samples + 1, MIN_SAMPLES);
  delay(20);
}