#include <Wire.h>

// I2C address of MPU9250
const int MPU_ADDR = 0x68;

// Sensor data variables
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ,MagX, MagY, MagZ;

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

  // Initialize I2C communication
  Wire.begin(0, 2);  // GPIO0 (SDA), GPIO2 (SCL)

  // Wake up MPU9250
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Set to active mode
  Wire.endTransmission(true);

  // Configure gyroscope and accelerometer
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1C);  // GYRO_CONFIG register
  Wire.write(0x18);  // 250 deg/s
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1B);  // ACCEL_CONFIG register
  Wire.write(0x01);  // +-2g
  Wire.endTransmission(true);

  // Configure magnetometer
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x01);  // CTRL1 register
  Wire.write(0x05);  // Enable magnetometer and bypass mode
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x02);  // CTRL2 register
  Wire.write(0x00);  // 100 Hz continuous mode
  Wire.endTransmission(true);

  // Configure magnetometer sensitivity (adjust if needed)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x10);  // T1 register (X sensitivity)
  Wire.write(0x48);  // Value for 1200 Ga/LSB
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x11);  // T2 register (Y sensitivity)
  Wire.write(0x48);  // Value for 1200 Ga/LSB
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x12);  // T3 register (Z sensitivity)
  Wire.write(0x4B);  // Value for 1200 Ga/LSB
  Wire.endTransmission(true);
}

void loop() {
  // Read accelerometer data
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // Starting register for accelerometer data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);  // Request 6 bytes for x, y, z axes

  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();

  // Read temperature data (not used in this example)
  Wire.read();
  Wire.read();

  // Read gyroscope data
  Wire.requestFrom(MPU_ADDR, 6, true);  // Request 6 bytes for x, y, z axes

  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();

  // Read magnetometer data
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x4A);  // Starting register for magnetometer data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);  // Request 6 bytes for x, y, z axes

  MagX = Wire.read() << 8 | Wire.read();
  MagY = Wire.read() << 8 | Wire.read();
  MagZ = Wire.read() << 8 | Wire.read();

  Serial.print("Accelerometer: ");
  Serial.print(AcX);
  Serial.print(", ");
  Serial.print(AcY);
  Serial.print(", ");
  Serial.println(AcZ);

  Serial.print("Gyroscope: ");
  Serial.print(GyX);
  Serial.print(", ");
  Serial.print(GyY);
  Serial.print(", ");
  Serial.println(GyZ);

  Serial.print("Magnetometer: ");
  Serial.print(MagX);
  Serial.print(", ");
  Serial.print(MagY);
  Serial.print(", ");
  Serial.println(MagZ);

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