#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 3
// Create objects. Each sensor is on the "Wire" I2C bus.
Adafruit_MPU6050 mpuArrays[NUM_MPUS] = 
{
   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] = {8, 9, 10};

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 = 500;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

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


  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);
  }

  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"));
  }
  delay(1);
}

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
  }
}