#include <Wire.h>
#include <SSD1306.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET -1

SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_MPU6050 mpu;

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

  if (!mpu.begin()) {
    Serial.println("Failed to initialize MPU6050 sensor!");
    while (1);
  }

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0,0);
  display.println("MPU6050 Data:");
  display.display();
}

void loop() {
  sensors_event_t accel_event, gyro_event, temp_event;
  mpu.getEvent(&accel_event, &gyro_event, &temp_event);

  float ax = accel_event.acceleration.x;
  float ay = accel_event.acceleration.y;
  float az = accel_event.acceleration.z;
  float gx = gyro_event.gyro.x;
  float gy = gyro_event.gyro.y;
  float gz = gyro_event.gyro.z;

  Serial.print("Accelerometer: ");
  Serial.print(ax);
  Serial.print(" ");
  Serial.print(ay);
  Serial.print(" ");
  Serial.println(az);

  Serial.print("Gyroscope: ");
  Serial.print(gx);
  Serial.print(" ");
  Serial.print(gy);
  Serial.print(" ");
  Serial.println(gz);

  display.clearDisplay();
  display.setCursor(0,0);
  display.print("Ax: ");
  display.print(ax);
  display.println(" m/s^2");
  display.print("Ay: ");
  display.print(ay);
  display.println(" m/s^2");
  display.print("Az: ");
  display.print(az);
  display.println(" m/s^2");
  display.print("Gx: ");
  display.print(gx);
  display.println(" rad/s");
  display.print("Gy: ");
  display.print(gy);
  display.println(" rad/s");
  display.print("Gz: ");
  display.print(gz);
  display.println(" rad/s");
  display.display();

  delay(1000);
}