#include <Wire.h>

const int MPU9250_ADDR = 0x68;  // Change to 0x69 if the AD0 pin is pulled high
const int AK8963_ADDR = 0x0C;  // Address of the magnetometer
const int PWR_MGMT_1 = 0x6B;
const int ACCEL_CONFIG = 0x1C;
const int GYRO_CONFIG = 0x1B;
const int MAG_MODE = 0x0A;
const int ACCEL_XOUT_H = 0x3B;
const int GYRO_XOUT_H = 0x43;
const int MAG_XOUT_L = 0x03;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  MPU9250_Init();
  AK8963_Init();
}

void loop() {
  readSensors();
  delay(100);
}

void MPU9250_Init() {
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(ACCEL_CONFIG);
  Wire.write(0x10);
  Wire.endTransmission();
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(GYRO_CONFIG);
  Wire.write(0x18);
  Wire.endTransmission();
}

void AK8963_Init() {
  // Initialize the magnetometer
  Wire.beginTransmission(AK8963_ADDR);
  Wire.write(MAG_MODE);  // Magnetometer mode (16-bit output)
  Wire.endTransmission();
}

int readWord(int reg) {
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(MPU9250_ADDR, 2);
  byte high = Wire.read();
  byte low = Wire.read();
  int value = (high << 8) | low;
  if (value >= 0x8000) {
    return -((65535 - value) + 1);
  }
  return value;
}

void readSensors() {
  int accelX = readWord(ACCEL_XOUT_H);
  int accelY = readWord(ACCEL_XOUT_H + 2);
  int accelZ = readWord(ACCEL_XOUT_H + 4);
  
  int gyroX = readWord(GYRO_XOUT_H);
  int gyroY = readWord(GYRO_XOUT_H + 2);
  int gyroZ = readWord(GYRO_XOUT_H + 4);
  
  int magX = readWord(MAG_XOUT_L);
  int magY = readWord(MAG_XOUT_L + 2);
  int magZ = readWord(MAG_XOUT_L + 4);
  
  Serial.print("Accelerometer: ");
  Serial.print(accelX);
  Serial.print(", ");
  Serial.print(accelY);
  Serial.print(", ");
  Serial.print(accelZ);
  Serial.print(" | Gyroscope: ");
  Serial.print(gyroX);
  Serial.print(", ");
  Serial.print(gyroY);
  Serial.print(", ");
  Serial.print(gyroZ);
  Serial.print(" | Magnetometer: ");
  Serial.print(magX);
  Serial.print(", ");
  Serial.print(magY);
  Serial.print(", ");
  Serial.print(magZ);
  Serial.println();
}