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

// Define OLED display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Define MPU6050 sensors
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;
Adafruit_MPU6050 mpu3;
Adafruit_MPU6050 mpu4;

// I2C addresses for the MPU6050 sensors
#define MPU1_ADDR 0x68
#define MPU2_ADDR 0x69
#define MPU3_ADDR 0x68
#define MPU4_ADDR 0x69

void setup() {
  // Initialize serial communication
  Serial.begin(115200);
  Wire.begin(21, 22);

  // Initialize OLED display
  if (!display.begin(SSD1306_I2C_ADDRESS, 21, 22)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("MPU6050 and SSD1306");
  display.display();
  delay(1000);

  // Initialize MPU6050 sensors
  if (!mpu1.begin(MPU1_ADDR)) {
    Serial.println("Failed to find MPU6050 chip 1");
    while (1);
  }
  if (!mpu2.begin(MPU2_ADDR)) {
    Serial.println("Failed to find MPU6050 chip 2");
    while (1);
  }
  if (!mpu3.begin(MPU3_ADDR)) {
    Serial.println("Failed to find MPU6050 chip 3");
    while (1);
  }
  if (!mpu4.begin(MPU4_ADDR)) {
    Serial.println("Failed to find MPU6050 chip 4");
    while (1);
  }

  // Set up the sensors
  mpu1.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu2.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu3.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu4.setAccelerometerRange(MPU6050_RANGE_2_G);

  mpu1.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu2.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu3.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu4.setGyroRange(MPU6050_RANGE_250_DEG);

  mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
  mpu2.setFilterBandwidth(MPU6050_BAND_21_HZ);
  mpu3.setFilterBandwidth(MPU6050_BAND_21_HZ);
  mpu4.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
}

void loop() {
  sensors_event_t a1, g1, temp1;
  sensors_event_t a2, g2, temp2;
  sensors_event_t a3, g3, temp3;
  sensors_event_t a4, g4, temp4;

  mpu1.getEvent(&a1, &g1, &temp1);
  mpu2.getEvent(&a2, &g2, &temp2);
  mpu3.getEvent(&a3, &g3, &temp3);
  mpu4.getEvent(&a4, &g4, &temp4);

  display.clearDisplay();
  display.setCursor(0, 0);

  display.print("MPU6050 Sensor 1:");
  display.print(" X: "); display.print(a1.acceleration.x);
  display.print(" Y: "); display.print(a1.acceleration.y);
  display.print(" Z: "); display.println(a1.acceleration.z);

  display.print("MPU6050 Sensor 2:");
  display.print(" X: "); display.print(a2.acceleration.x);
  display.print(" Y: "); display.print(a2.acceleration.y);
  display.print(" Z: "); display.println(a2.acceleration.z);

  display.print("MPU6050 Sensor 3:");
  display.print(" X: "); display.print(a3.acceleration.x);
  display.print(" Y: "); display.print(a3.acceleration.y);
  display.print(" Z: "); display.println(a3.acceleration.z);

  display.print("MPU6050 Sensor 4:");
  display.print(" X: "); display.print(a4.acceleration.x);
  display.print(" Y: "); display.print(a4.acceleration.y);
  display.print(" Z: "); display.println(a4.acceleration.z);

  display.display();
  delay(500);
}