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




#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_SDA_PIN 1
#define OLED_SCL_PIN 0
#define MPU_SDA_PIN 3 
#define MPU_SCL_PIN 2 

TwoWire oledWire = TwoWire(0); // OLED display Wire object
TwoWire mpuWire = TwoWire(1);  // MPU6050 Wire object

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &oledWire, -1, OLED_SDA_PIN, OLED_SCL_PIN);


void setup() {
  Serial.begin(115200);
  oledWire.begin(OLED_SDA_PIN, OLED_SCL_PIN, 100000);
  

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;
  }
  delay(2000);
  display.clearDisplay();
  
  // Initialize the MPU-6050 and test if it is connected.
  mpuWire.begin(MPU_SDA_PIN, MPU_SCL_PIN, 100000);
  mpuWire.write( 0x6B);                           // PWR_MGMT_1 register
  Wire.write( 0);                              // set to zero (wakes up the MPU-6050)
  auto error = Wire.endTransmission();
  if( error != 0)
  {
    Serial.println(F( "Error, MPU-6050 not found"));
    for(;;);                                   // halt the sketch if error encountered
  }




}

void loop() {
  mpuWire.begin(MPU_SDA_PIN, MPU_SCL_PIN, 100000);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start
  // Read accelerometer data from MPU6050
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Display accelerometer data on OLED display
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("Accel X: ");
  display.println(ax);
  display.print("Accel Y: ");
  display.println(ay);
  display.print("Accel Z: ");
  display.println(az);
  display.display();

  delay(1); // Adjust delay as needed for desired refresh rate
}