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

// The number of MPU-6050 sensors in this example is 5.
#define NUM_MPUS 5

// Create objects. Each sensor is on the "Wire" I2C bus.
Adafruit_MPU6050 mpuArrays[NUM_MPUS] = 
{
  Adafruit_MPU6050(),
  Adafruit_MPU6050(),
  Adafruit_MPU6050(),
  Adafruit_MPU6050(),
  Adafruit_MPU6050(),
};

// For the AD0 pins of the MPU-6050.
// To select runtime the I2C address.
const int AD0pin[NUM_MPUS] = {16, 17, 18, 19, 23};

const int mpuAddr = 0x68;  // I2C address
const int mpuAddr_not_used = 0x69; // not even used in the code.

unsigned long previousMillis;
const unsigned long interval = 1000;

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

  Wire.begin();  // default pins: SDA=21, SCL=22.

  for(int i=0; i<NUM_MPUS; i++)
  {
    pinMode(AD0pin[i],OUTPUT);
    digitalWrite(AD0pin[i],HIGH); // default high for 0x69
  }

  // Initialize all the MPU-6050 sensors.
  Serial.println(F("Initializing the sensors and calculating offsets. Do not move MPU-6050 sensors."));
  Serial.println(F("Attention: Calculating the offset is good in real life, but it is better to skip this function in Wokwi."));
  delay(500);
  for(int i=0; i<NUM_MPUS; i++)
  {
    // Always select the proper sensor first.
    SelectMPU(i);

    // Run the library initialization function .begin
    bool status = mpuArrays[i].begin();
    
    if(!status)
    {
      Serial.print("Failed to find " );
      Serial.print(i);
      Serial.print(" MPU6050 chip");
      Serial.println();
    }
    
  
    mpuArrays[i].setAccelerometerRange(MPU6050_RANGE_8_G);
    mpuArrays[i].setGyroRange(MPU6050_RANGE_500_DEG);
    mpuArrays[i].setFilterBandwidth(MPU6050_BAND_21_HZ);
  }
  Serial.println("Initialization done");
  Serial.println("Click on a MPU-6050 module and change the acceleration and gyro.");

  previousMillis = millis();
}

void loop() 
{
  unsigned long currentMillis = millis();

  if(currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;

    for(int i=0; i<NUM_MPUS; i++)
    {
      SelectMPU(i);
      /* Get new sensor events with the readings */
      sensors_event_t a, g, temp;
      mpuArrays[i].getEvent(&a, &g, &temp);

      /* Print out the values */
      Serial.print("Acceleration X: ");
      Serial.print(a.acceleration.x);
      Serial.print(", Y: ");
      Serial.print(a.acceleration.y);
      Serial.print(", Z: ");
      Serial.print(a.acceleration.z);
      Serial.println(" m/s^2");

      Serial.print("Rotation X: ");
      Serial.print(g.gyro.x);
      Serial.print(", Y: ");
      Serial.print(g.gyro.y);
      Serial.print(", Z: ");
      Serial.print(g.gyro.z);
      Serial.println(" rad/s");

      Serial.print("Temperature: ");
      Serial.print(temp.temperature);
      Serial.println(" degC");

      Serial.println("");
    }
    Serial.println(F("======================================================================================\n"));
  }
}

// Choose a MPU, parameter starts at zero for the first sensor.
// AD0 = high, I2C address = 0x69, not selected
// AD0 = low,  I2C address = 0x68, selected
void SelectMPU(int selection)
{
  for(int i=0; i<NUM_MPUS; i++)
  {
    if(i == selection)
      digitalWrite(AD0pin[i],LOW);  // selected, 0x68
    else
      digitalWrite(AD0pin[i],HIGH); // not selected, 0x69
  }
}