#include <Wire.h>
#include <BluetoothSerial.h>

BluetoothSerial SerialBT;
const int MPU2 = 0x69, MPU1 = 0x68;
int16_t AcX1, AcY1, AcZ1, Tmp1, GyX1, GyY1, GyZ1;
int16_t AcX2, AcY2, AcZ2, Tmp2, GyX2, GyY2, GyZ2;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU1);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU2);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Serial.begin(115200);
  SerialBT.begin("ESP32_BT"); // Bluetooth device name
}

void loop() {
  // Get values for the first MPU having an address of 0x68
  GetMpuValue1(MPU1);
  SerialBT.print("MPU1,AcX=");
  SerialBT.print(AcX1);
  SerialBT.print(",AcY=");
  SerialBT.print(AcY1);
  SerialBT.print(",AcZ=");
  SerialBT.print(AcZ1);
  SerialBT.print(",GyX=");
  SerialBT.print(GyX1);
  SerialBT.print(",GyY=");
  SerialBT.print(GyY1);
  SerialBT.print(",GyZ=");
  SerialBT.println(GyZ1);

  // Get values for the second MPU having an address of 0x69
  GetMpuValue2(MPU2);
  SerialBT.print("MPU2,AcX=");
  SerialBT.print(AcX2);
  SerialBT.print(",AcY=");
  SerialBT.print(AcY2);
  SerialBT.print(",AcZ=");
  SerialBT.print(AcZ2);
  SerialBT.print(",GyX=");
  SerialBT.print(GyX2);
  SerialBT.print(",GyY=");
  SerialBT.print(GyY2);
  SerialBT.print(",GyZ=");
  SerialBT.println(GyZ2);

  Serial.println("Data Sent!"); // Debugging line

  delay(1000); // Adjust the delay as needed
}

void GetMpuValue1(const int MPU) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); // request a total of 14 registers
  AcX1 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY1 = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ1 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp1 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX1 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY1 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ1 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

void GetMpuValue2(const int MPU) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); // request a total of 14 registers
  AcX2 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY2 = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ2 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp2 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX2 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY2 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ2 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}