// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
/* I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
 * for both classes must be in the include path of your project
 */
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#define NUMBER_OF_SENSORS 3 //// YOU MAY NEED TO CHANGE THIS

// Default I2C address is 0x68
// AD0 LOW(0) = 0x68 (Default for SparkFun breakout and InvenSense evaluation board)
// AD0 HIGH(1) = 0x69

// MPU Control Variables
MPU6050 mpu;
bool dmpReady; // Set true if DMP init was successful.
uint8_t devStatus;   // Return status after each device operation. (0 = success, !0 = error)
uint8_t mpuIntStatus; // Holds interrupt status byte from MPU.
uint16_t packetSize; // Expected DMP packet size. (Default is 42 bytes)
uint16_t fifoCount;  // Count of all bytes currently in FIFO.
uint8_t fifoBuffer[64]; // FIFO storage buffer.

// Orientation and Motion Variables
Quaternion q;        // [W, X, Y, Z] Quaternion container.
VectorFloat gravity;  // [X, Y, Z] Gravity vector
float ypr[3];         // [Yaw, Pitch, Roll] array container.

//Digital Pins Reference Variables
const int latchPin = 2; //ST_CP or RCLK
const int clockPin = 1; //SH_CP or SRCLK
const int dataPin = 3; //DS or SER

// Other Variables
String finalParts[NUMBER_OF_SENSORS];
String final = "";

//==============================================================

void switchSensor(int sensorNumber) {
  /* The shift register shifts out bits to the ADO lines. In binary:
   * 0001 = Sensor on Q0(QA) 2^0 = 1 = 0001 in binary
   * 0010 = Sensor on Q1(QB) 2^1 = 2 = 0010 in binary
   */
  int data = (int) 1 << (sensorNumber - 1); // Shift register serial data input.

  digitalWrite(latchPin, LOW); // Hold low for as long as you are transmitting data.
  shiftOutBits(dataPin, clockPin, ~data); // Shift out the bits into the shift register; negate data. (0001 = 1000)
  digitalWrite(latchPin, HIGH); // Ends transmission of data.
}

void shiftOutBits(int myDataPin, int myClockPin, byte myDataOut) {
  // This shifts 8 bits out MSB first.
  
  //Function Setup
  int l = 0;
  int pinState;

  pinMode(myClockPin, OUTPUT);
  pinMode(myDataPin, OUTPUT);

  // Clear everything out in case to prepare shift register.
  digitalWrite(myDataPin, 0);
  digitalWrite(myClockPin, 0);

  // For each bit in the byte myDataOut.
  for (l = 7; l >= 0; l--) {
    digitalWrite(myClockPin, 0);

    //if the value passed to myDataOut and a bitmask result
    // true then... so if we are at i=6 and our value is
    // %11010100 it would the code compares it to %01000000
    // and proceeds to set pinState to 1.
    if ( myDataOut & (1 << l) ) {
      pinState = 1;
      //////Serial.print(pinState);
    }
    else {
      pinState = 0;
      //////Serial.print(pinState);
    }
    // Sets the pin to HIGH or LOW depending on pinState.
    digitalWrite(myDataPin, pinState);
    // Register shifts on HIGH of myClockPin.
    digitalWrite(myClockPin, 1);
    // Stop sending on myDataPin.
    digitalWrite(myDataPin, 0);
  }
  // Stop shifting.
  digitalWrite(myClockPin, 0);
}

// ================================================================
// ===                  MAIN PROGRAM SETUP                      ===
// ================================================================

void setup() {
  delay(2000);
  
  Wire.begin(); // Join I2C bus. (I2Cdev library doesn't do this automatically.)
  TWBR = 24; // Sets frequency of the clock (SCL) higher.
  

  Serial.begin(115200); // Initialize serial communication with baud rate.

  pinMode(latchPin, OUTPUT);  // Tell the Arduino to send or recieve signals.

  int dmpReadyCounter = 0; // Counts number of sensors ready.

  for (int i = 1; i <= NUMBER_OF_SENSORS; i++) {
    /*  We read data from all sensors by switching addresses one by one, only reading from the first address (0x68).
     *  Therefore, we shift the addresses to the next sensor, and retrieve its value.
     */
    switchSensor(i);

    mpu.initialize();  // Intialize device.

    devStatus = mpu.dmpInitialize(); // Load and configure the DMP. (Digital Motion Processor)

    // Gyroscope offsets. (Change if necessary)
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);

    // Check success of DMP.
    if (devStatus == 0) {
      mpu.setDMPEnabled(true);

      dmpReadyCounter += 1; // Add one to count number of ready sensors.
      
      packetSize = mpu.dmpGetFIFOPacketSize(); // Get expected DMP packet size for later comparison
    } else {
      // Error!
      Serial.print("Error on sensor " + String(i) + "\n");
    }
    /////////Serial.print(String(digitalRead(8)) + String(digitalRead(9)) + String(digitalRead(10)) + String(digitalRead(11)) + "\n");
  }
  if (dmpReadyCounter == 3) {
    dmpReady = true; // Set DMP Ready flag. (Allows main loop to use the DMP.)
  }
}

// ================================================================
// ===                  MAIN PROGRAM LOOP                       ===
// ================================================================

void loop() {
  final = ""; // Reset final to nothing.
  // If DMP isn't ready...
  if (!(dmpReady)) {
    return;
  }

  for (int k = 1; k <= NUMBER_OF_SENSORS; k++) {

    switchSensor(k);

    // Check for overflow.
    if (fifoCount == 1024) {
      mpu.resetFIFO(); // Reset so we can continue cleanly.
    } else {
      fifoCount = mpu.getFIFOCount(); // Get current FIFO count.

      // Wait for correct avaliable data length.
      while (fifoCount < packetSize) {
        fifoCount = mpu.getFIFOCount();
      }
      
      mpu.getFIFOBytes(fifoBuffer, packetSize); // Read a packet from FIFO
      /* Track FIFO count in case there is more than 1 packet avaliable.
       *  (Read more without waiting for an interrupt.)
       */
      fifoCount -= packetSize;

      // Get values to process.
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      // Concatenate for outputting. (Displays in Euler Angles in degrees.)
      finalParts[k - 1] = String(ypr[0] * 180 / M_PI) + "," + String(ypr[1] * 180 / M_PI) + "," + String(ypr[2] * 180 / M_PI) + ":";
      final += finalParts[k - 1];
    }
  }
  Serial.print(final);
  Serial.print("\n");
}