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

Adafruit_MPU6050 mpu;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10);

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

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  Serial.println("Accelerometer range set to: +-2G");

  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.println("Gyro range set to: +- 500 deg/s");
  
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("Filter bandwidth set to: 21 Hz");
  
  delay(100);
}

void loop() {

  Serial.println("-----------------------------------------------------------------------------");

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  float acc=sqrt((a.acceleration.x)*(a.acceleration.x)+(a.acceleration.y)*(a.acceleration.y)+(a.acceleration.z)*(a.acceleration.z));
  Serial.print("Net acceleration = ");
  Serial.print(acc);
  Serial.println(" m/s^2");

  Serial.println("Angle with the axes calibrated from the start time so it may include some error times.");

  int xa=int(((g.gyro.x)*180*millis()/(1000*PI)))%360;
  int ya=int(((g.gyro.y)*180*millis()/(1000*PI)))%360;
  int za=int(((g.gyro.z)*180*millis()/(1000*PI)))%360;
  Serial.print("Angle with X axis = ");
  Serial.print(xa);
  Serial.println("°");
  Serial.print("Angle with X axis = ");
  Serial.print(ya);
  Serial.println("°");
  Serial.print("Angle with X axis = ");
  Serial.print(za);
  Serial.println("°");
  delay(5000);
}