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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

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

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

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();       // Show splash screen
  delay(2000);             // Pause for 2 seconds
  display.clearDisplay();  // Clear screen


  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.println(F("MPU6050 and OLED"));
  display.display();  
  delay(1000);

}

void loop() {
    sensors_event_t a, g, temp; // Create sensor event objects
    mpu.getEvent(&a, &g, &temp); // Get current sensor data

    display.clearDisplay(); // Clear the display buffer
    display.setCursor(0, 0); // Set cursor to top-left of display

    // Print acceleration data
    display.print("AccelX: "); display.println(a.acceleration.x);
    display.print("AccelY: "); display.println(a.acceleration.y);
    display.print("AccelZ: "); display.println(a.acceleration.z);

    // Print gyroscope data
    display.print("GyroX: ");  display.println(g.gyro.x);
    display.print("GyroY: ");  display.println(g.gyro.y);
    display.print("GyroZ: ");  display.println(g.gyro.z);

    display.display(); // Push the buffer to the screen

    delay(500); // Wait for 500ms before updating again
}