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

Adafruit_MPU6050 mpu;

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

  Serial.println("STM32 Blue Pill + MPU6050");

  // STM32 Blue Pill I2C
  // SDA = PB7
  // SCL = PB6
  Wire.begin(PB7, PB6);

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

  Serial.println("MPU6050 Found!");

  // Accelerometer range
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);

  // Gyroscope range
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);

  // Digital Low Pass Filter
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("MPU6050 initialized.");
  Serial.println();
}

void loop() {

  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;

  // Read sensor
  mpu.getEvent(&accel, &gyro, &temp);

  // ==========================
  // Accelerometer
  // ==========================

  Serial.print("ACCEL [m/s^2] ");

  Serial.print("X: ");
  Serial.print(accel.acceleration.x, 2);

  Serial.print("  Y: ");
  Serial.print(accel.acceleration.y, 2);

  Serial.print("  Z: ");
  Serial.print(accel.acceleration.z, 2);


  // ==========================
  // Gyroscope
  // ==========================

  Serial.print("    |    GYRO [rad/s] ");

  Serial.print("X: ");
  Serial.print(gyro.gyro.x, 2);

  Serial.print("  Y: ");
  Serial.print(gyro.gyro.y, 2);

  Serial.print("  Z: ");
  Serial.print(gyro.gyro.z, 2);


  // ==========================
  // Temperature
  // ==========================

  Serial.print("    |    TEMP: ");
  Serial.print(temp.temperature, 2);
  Serial.println(" C");

  delay(100);
}