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

Adafruit_MPU6050 mpu;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  Wire.begin(); // default pin was SCL -> 22 and SDA -> 21

  while (!mpu.begin()) {
    Serial.println(mpu.begin());
    Serial.println("Failed to find MPU6050 chip");
    delay(1000);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

float upper_acc_fall = 100;
float upper_w_fall = 100;

void loop() {
  // put your main code here, to run repeatedly:
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float ax = a.acceleration.x;
  float ay = a.acceleration.y;
  float az = a.acceleration.z;

  float wx = g.gyro.x;
  float wy = g.gyro.y;
  float wz = g.gyro.z;

  float acc = sqrt(ax * ax + ay * ay + az * az);
  float w = sqrt(wx * wx + wy * wy + wz * wz);

  if (acc > upper_acc_fall && w > upper_w_fall) {
    Serial.println("Fall detected!");
  }

  Serial.printf("Accel: %f,%f,%f\n", ax, ay, az);
  Serial.printf("Gyro: %f,%f,%f\n", wx, wy, wz);
  Serial.printf("Temperature %f\n", temp.temperature);

  delay(500); // this speeds up the simulation
}