#include <Wire.h>
#include <MPU6050_tockn.h>

// Define MPU6050 addresses
const int MPU6050_1_Address = 0x68;
const int MPU6050_2_Address = 0x69;
const int MPU6050_3_Address = 0x68;

// Initialize MPU6050 objects
MPU6050 mpu6050_1(Wire);
MPU6050 mpu6050_2(Wire);
MPU6050 mpu6050_3(Wire);

void setup() {
  Serial.begin(115200);
  
  // Initialize I2C communication
  Wire.begin();

  // Initialize MPU6050 1
  if (!mpu6050_1.begin(MPU6050_1_Address)) {
    Serial.println("Failed to find MPU6050 1 device!");
    while (1);
  }

  // Initialize MPU6050 2
  if (!mpu6050_2.begin(MPU6050_2_Address)) {
    Serial.println("Failed to find MPU6050 2 device!");
    while (1);
  }

  // Initialize MPU6050 3
  if (!mpu6050_3.begin(MPU6050_3_Address)) {
    Serial.println("Failed to find MPU6050 3 device!");
    while (1);
  }

  // Set accelerometer range
  mpu6050_1.setAccelerometerRange(MPU6050_RANGE_16G);
  mpu6050_2.setAccelerometerRange(MPU6050_RANGE_16G);
  mpu6050_3.setAccelerometerRange(MPU6050_RANGE_16G);
}

void loop() {
  // Read MPU6050 1 data
  VectorInt16 accel1 = mpu6050_1.readRawAccel();
  VectorInt16 gyro1 = mpu6050_1.readRawGyro();
  
  // Read MPU6050 2 data
  VectorInt16 accel2 = mpu6050_2.readRawAccel();
  VectorInt16 gyro2 = mpu6050_2.readRawGyro();
  
  // Read MPU6050 3 data
  VectorInt16 accel3 = mpu6050_3.readRawAccel();
  VectorInt16 gyro3 = mpu6050_3.readRawGyro();
  
  // Print data to Serial Monitor
  Serial.print("MPU6050 1: ");
  Serial.print("Accel(XYZ): ");
  Serial.print(accel1.x);
  Serial.print(", ");
  Serial.print(accel1.y);
  Serial.print(", ");
  Serial.print(accel1.z);
  Serial.print(" | ");
  Serial.print("Gyro(XYZ): ");
  Serial.print(gyro1.x);
  Serial.print(", ");
  Serial.print(gyro1.y);
  Serial.print(", ");
  Serial.println(gyro1.z);
  
  Serial.print("MPU6050 2: ");
  Serial.print("Accel(XYZ): ");
  Serial.print(accel2.x);
  Serial.print(", ");
  Serial.print(accel2.y);
  Serial.print(", ");
  Serial.print(accel2.z);
  Serial.print(" | ");
  Serial.print("Gyro(XYZ): ");
  Serial.print(gyro2.x);
  Serial.print(", ");
  Serial.print(gyro2.y);
  Serial.print(", ");
  Serial.println(gyro2.z);
  
  Serial.print("MPU6050 3: ");
  Serial.print("Accel(XYZ): ");
  Serial.print(accel3.x);
  Serial.print(", ");
  Serial.print(accel3.y);
  Serial.print(", ");
  Serial.print(accel3.z);
  Serial.print(" | ");
  Serial.print("Gyro(XYZ): ");
  Serial.print(gyro3.x);
  Serial.print(", ");
  Serial.print(gyro3.y);
  Serial.print(", ");
  Serial.println(gyro3.z);
  
  Serial.println();
  
  delay(1000); // Delay between readings
}