#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"


#define NUM_MPUS 2

MPU6050 mpu[NUM_MPUS] = 
{
  MPU6050(),
  MPU6050(),
  /*MPU6050(),
  MPU6050(),
  MPU6050(),*/
};

const int AD0pin[NUM_MPUS] = {16,17,/*18,19,23*/};
const int INTpin[NUM_MPUS] = {12,14,/*27,26,25*/};

volatile bool interrupt[NUM_MPUS] = {false, false,/*false,false,false*/};  

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

bool dmpsReady[NUM_MPUS] = {false, false,/*false,false,false*/}; // set true if DMP init was successful
uint8_t intsStatus[NUM_MPUS] = {0,0,/*0,0,0,*/};
uint8_t devsStatus[NUM_MPUS] = {0,0,/*0,0,0,*/}; // return status after each device operation (0 = success, !0 = error)
uint16_t packetsSize[NUM_MPUS] = {0,0,/*0,0,0,*/};
uint16_t fifosCount[NUM_MPUS] = {0,0,/*0,0,0,*/};
uint8_t fifoBuffers[NUM_MPUS][64] = {{0}, {0}, /*{0}, {0}, {0}*/};

int currentMPU = 0;


Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector 

void dmpDataReady() {
  interrupt[currentMPU] = true;
}

unsigned long previousMillis;
const unsigned long interval = 100;

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

  Wire.begin(); 

  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);
    InitializeMPU(i);

    Serial.println("====== Finished MPU[i] Init. ======");
    delay(100);
  }

  Serial.println("Initialization done");
  previousMillis = micros();

}

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

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

    for(int i=0; i<NUM_MPUS; i++)
    {
      SelectMPU(i);
      getMPUValue(i);
    }
   // Serial.println(F("======================================================================================\n"));
  }

}


void getMPUValue(int mpuIndex)
{
  // if programming failed, don't try to do anything
  if (!dmpsReady[mpuIndex]) return;
  if (!interrupt[mpuIndex]) return;

  // Réinitialisation de l'interruption
  interrupt[mpuIndex] = false;

  // Lire la FIFO
    fifosCount[mpuIndex] = mpu[mpuIndex].getFIFOCount();
    if (fifosCount[mpuIndex] >= 1024) 
    {
        // Trop de données, réinitialisation
        mpu[mpuIndex].resetFIFO();
        Serial.println(F("FIFO 1 overflow!"));
        delay(1);
    } 
    else if (fifosCount[mpuIndex] >= packetsSize[mpuIndex]) 
    {

      // Lecture du paquet DMP
      mpu[mpuIndex].getFIFOBytes(fifoBuffers[mpuIndex], packetsSize[mpuIndex]);

      // display Euler angles in degrees
      mpu[mpuIndex].dmpGetQuaternion(&q, fifoBuffers[mpuIndex]);
      mpu[mpuIndex].dmpGetEuler(euler, &q);
      Serial.print(mpuIndex);
      Serial.print(",");
      Serial.print(euler[0] * 180/M_PI);
      Serial.print(",");
      Serial.print(euler[1] * 180/M_PI);
      Serial.print(",");
      Serial.println(euler[2] * 180/M_PI);

    }
}

void InitializeMPU(int mpuIndex)
{
  Serial.print("Initializing MPU");
   Serial.println(mpuIndex);
  mpu[mpuIndex].initialize();

  pinMode(INTpin[mpuIndex], INPUT);

  // Vérifier la connexion
  Serial.println( mpu[mpuIndex].testConnection() ? F("MPU6050 connection success.") : F("MPU6050 connection failed."));

  devsStatus[mpuIndex] =  mpu[mpuIndex].dmpInitialize();

  // Apply custom offsets
   mpu[mpuIndex].setXGyroOffset(-26);
   mpu[mpuIndex].setYGyroOffset(14);
   mpu[mpuIndex].setZGyroOffset(11);
   mpu[mpuIndex].setZAccelOffset(2011);

  if (devsStatus[mpuIndex] == 0) 
  {
    // Calibrage et initialisation DMP
    mpu[mpuIndex].CalibrateAccel(6);
    mpu[mpuIndex].CalibrateGyro(6);
    mpu[mpuIndex].PrintActiveOffsets();

    // Activation du DMP
    Serial.println(F("Enabling DMP..."));
    mpu[mpuIndex].setDMPEnabled(true);

    // Configuration de l'interruption
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(INTpin[mpuIndex]));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(INTpin[mpuIndex]), dmpDataReady, RISING);

    // Récupération de l'état d'interruption
    intsStatus[mpuIndex] = mpu[mpuIndex].getIntStatus();

    // Le DMP est prêt
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpsReady[mpuIndex] = true;

    // Récupérer la taille du paquet DMP
    packetsSize[mpuIndex] = mpu[mpuIndex].dmpGetFIFOPacketSize();
  } 
  else 
  {
    // Gestion des erreurs
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devsStatus[mpuIndex]);
    Serial.println(F(")"));
  }
}

// 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)
    {
      currentMPU = i;
      digitalWrite(AD0pin[i],LOW);  // selected, 0x68
    }
    else
      digitalWrite(AD0pin[i],HIGH); // not selected, 0x69
  }
}