// This is an example of how to communicate over I2C on Arduino
// Based on the MPU6050 example
// https://wokwi.com/arduino/projects/305937248748044864

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

#define FirstMPUAddr 0x68     
#define SecondMPUAddr 0x69

Adafruit_MPU6050 mpu;
Adafruit_MPU6050 mpu2;

// High and low bytes of X-Acceleration
char XAccelHighByte;
char XAccelLowByte;

int16_t XAccelCombined;  // Combined High and Low bytes of X-Acceleration
// Must tell it that this is an int16_t instead of an int
// or else it does not handle negative numbers correctly!

void setup(void) {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");  // println() adds a newline at the end

  delay(100);

  // Start the I2C system
  Wire.begin();

  // Scan bus for all I2C devices:
  byte error, address;
  int nDevices;
  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
      nDevices++;
    }
    else if (error==4) {
      Serial.print("Unknown error at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }
  delay(100);   




  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready at default address 0x68!");

  //begin(uint8_t i2c_addr = MPU6050_I2CADDR_DEFAULT, TwoWire *wire = &Wire,
  //      int32_t sensorID = 0);
  while (!mpu2.begin(SecondMPUAddr, &Wire, 1)) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready at address 0x69!");


}

sensors_event_t event;
sensors_event_t event2;

void loop() {

  // Get Accelerometer readings using the Adafruit Library:
  mpu.getAccelerometerSensor()->getEvent(&event);

  Serial.print("[");   // print() does not add a newline
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(event.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(event.acceleration.z);
  Serial.println(" m/s^2");

  mpu2.getGyroSensor()->getEvent(&event2);
  Serial.print("[");   // print() does not add a newline
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(event2.gyro.x);
  Serial.print(", Y: ");
  Serial.print(event2.gyro.y);
  Serial.print(", Z: ");
  Serial.print(event2.gyro.z);
  Serial.println(" m/s^2");


  /////////////////////////////////////////////////////////////////////////
  // Test reading a single value from the MPU even without the Library:
  // First, we will get a single register value and print it out.
  // 
  // First, we start by saying this is the peripheral we are talking to:
  Wire.beginTransmission(FirstMPUAddr);
  // Tell the MPU that we want the data from the Slave Register 0x3B:
  // This is the High Byte of the X-Acceleration
  Wire.write(0x3B);
  // Send a Restart - we are not done getting the data, so don't end the 
  // transmission (don't send a Stop bit).
  Wire.endTransmission(false);

  // Now we will request 1 byte from the MPU.  The third parameter 
  // 'true' says that we are  ending the transmission after we get the data 
  // (i.e. send the Stop bit).
  Wire.requestFrom(FirstMPUAddr, 1, true);

  // Now we will assign the byte from before to the 
  // value we are reading.
  XAccelHighByte = Wire.read();
  Serial.print("XAccelHighByte_Read_by_itself = 0b");
  Serial.println(XAccelHighByte,BIN);
  /////////////////////////////////////////////////////////////////////////



  /////////////////////////////////////////////////////////////////////////
  // Now, we will read the high and low bytes then combine them into a single
  // value.  We will read them with two separate transactions.
  //
  // Test reading a value from the MPU even without the Library:
  // First, we start by saying this is the peripheral we are talking to:
  Wire.beginTransmission(FirstMPUAddr);
  // Tell the MPU that we want the data from the Slave Register 0x3B:
  // This is the High Byte of the X-Acceleration.
  // Look at the MPU "Register Map" datasheet to see this.
  Wire.write(0x3B);
  // Send a Restart - we are not done getting the data, so don't end the 
  // transmission (don't send a Stop bit).
  Wire.endTransmission(false);

  // Now we will request 1 byte from the MPU.  The third parameter 
  // 'true' says that we are ending the transmission after we get the data.
  // NOTE: We must do it this way even though we still need to do a second 
  // separate transmission for the Low Byte.  It DOES NOT WORK to do:
  //    Wire.requestFrom(FirstMPUAddr, 1, false);  
  // and then proceed directly to 
  //    Wire.write(0x3C);   
  // (skipping the Wire.beginTransmission(FirstMPUAddr);)
  // This gives BAD data!!
  Wire.requestFrom(FirstMPUAddr, 1, true);

  // Now we will assign the byte from before to the value we are reading.
  XAccelHighByte = Wire.read();

  // Do all these steps again, we need to get the Low Byte now.
  Wire.beginTransmission(FirstMPUAddr);

  // Tell the MPU that we want the data from the Slave Register 0x3C:
  // This is the Low Byte of the X-Acceleration.
  Wire.write(0x3C);
  // Send a Restart - we are not done getting the data, so don't end the 
  // transmission (don't send a Stop bit).
  Wire.endTransmission(false);

  // Now we will request 1 byte from the MPU.  The third parameter 
  // 'true' says that we are done with the transmission, send a Stop bit.
  Wire.requestFrom(FirstMPUAddr, 1, true);

  // Now we will assign the byte from before to the value we are reading.
  XAccelLowByte = Wire.read();

  Serial.print("XAccelHighByte = 0b");
  Serial.println(XAccelHighByte,BIN);
  Serial.print("XAccelLowByte = 0b");
  Serial.println(XAccelLowByte,BIN);

  // Combine the bytes to make a whole number:
  // NOTE that when you print it out, it does not look correct if the acceleration
  // is negative -- because we are casting it to an int16_t.  
  // This is needed to handle the negative numbers correctly.
  XAccelCombined = (XAccelHighByte << 8 | XAccelLowByte);

  Serial.print("XAccelCombined = 0b");
  Serial.println(XAccelCombined,BIN);
  Serial.print("XAccelCombined = ");
  Serial.println(XAccelCombined*(1.0/16384.0)*9.80665f);  
     // put it in the same units as the Adafruit library.
     // Note the 16384.0 to avoid integer division!
     // 16384 is the scale factor for the +/-2g full scale range.
     // See the chart near the bottom of the Register Map datasheet on p.30.
  
  /////////////////////////////////////////////////////////////////////////



  /////////////////////////////////////////////////////////////////////////
  // This section reads two values in a row-- if you say you want to read 
  // two values in sequential registers, it just outputs the second register 
  // right after the one you started at.  This keeps us from having to do 
  // multiple transmissions.  You could even read all the registers in one
  // go if you wanted!
  //
  //
  // Test reading a value from the MPU even without the Library:
  // First, we start by saying this is the peripheral we are talking to:
  Wire.beginTransmission(FirstMPUAddr);
  // Tell the MPU that we want the data from the Slave Register 0x3B:
  // This is the High Byte of the X-Acceleration.
  // Look at the MPU "Register Map" datasheet to see this.
  Wire.write(0x3B);
  // Send a Restart - (don't send a Stop bit).
  Wire.endTransmission(false);

  // Now we will request **2 bytes** from the MPU.  The third parameter 
  // 'true' says that we are ending the transmission after we get the data.
  // This will give us the values at both 0x3B and 0x3C, in two separate reads.
  Wire.requestFrom(FirstMPUAddr, 2, true);

  // Busy-wait until we have both bytes available in the I2C buffer:
  while (Wire.available() < 2);

  // Now we will read in both bytes, in order!
  XAccelHighByte = Wire.read();
  XAccelLowByte = Wire.read();

  Serial.print("XAccelHighByte_ReadSequentially = 0b");
  Serial.println(XAccelHighByte,BIN);
  Serial.print("XAccelLowByte_ReadSequentially = 0b");
  Serial.println(XAccelLowByte,BIN);

  // Combine the bytes to make a whole number:
  XAccelCombined = (XAccelHighByte << 8 | XAccelLowByte);

  Serial.print("XAccelCombined_ReadSequentially = 0b");
  Serial.println(XAccelCombined,BIN);
  Serial.print("XAccelCombined_ReadSequentially = ");
  Serial.println(XAccelCombined*(1.0/16384.0)*9.80665f);  
     // put it in the same units as the Adafruit library.
     // Note the 16384.0 to avoid integer division!
     // 16384 is the scale factor for the +/-2g full scale range.
     // See the chart near the bottom of the Register Map datasheet on p.30.
  
  /////////////////////////////////////////////////////////////////////////


  delay(1000);  // This apparently speeds up the simulation
}