#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Declaration for SSD1306 display connected using I2C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
    Wire.begin(6, 7); // SDA to GPIO6, SCL to GPIO7

      // Initialize the MPU6050 sensor
        Serial.println("Initializing MPU6050...");
          mpu.initialize();
            if (!mpu.testConnection()) {
                Serial.println("Failed to connect to MPU6050!");
                    while (1);
                      }
                        Serial.println("MPU6050 initialized successfully.");

                          // Initialize the SSD1306 display
                            if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Adjust 0x3C if your OLED has a different address
                                Serial.println(F("SSD1306 allocation failed"));
                                    for (;;);
                                      }

                                        display.display();
                                          delay(2000); // Pause for 2 seconds
                                            display.clearDisplay();
                                              display.setTextSize(1); 
                                                display.setTextColor(SSD1306_WHITE); 
                                                  display.setCursor(0, 0); 
                                                    display.print("MPU6050 & OLED");
                                                      display.display();
                                                      }

                                                      void loop() {
                                                        // Read accelerometer and gyroscope data
                                                          int16_t ax, ay, az;
                                                            int16_t gx, gy, gz;
                                                              
                                                                mpu.getAcceleration(&ax, &ay, &az);
                                                                  mpu.getRotation(&gx, &gy, &gz);
                                                                    
                                                                      // Display data on OLED
                                                                        display.clearDisplay();
                                                                          display.setCursor(0, 0); 
                                                                            display.print("Accel: ");
                                                                              display.print("X: "); display.print(ax);
                                                                                display.print(" Y: "); display.print(ay);
                                                                                  display.print(" Z: "); display.println(az);
                                                                                    
                                                                                      display.print("Gyro: ");
                                                                                        display.print("X: "); display.print(gx);
                                                                                          display.print(" Y: "); display.print(gy);
                                                                                            display.print(" Z: "); display.println(gz);
                                                                                              
                                                                                                display.display();
                                                                                                  
                                                                                                    delay(500);
                                                                                                    }