#include <Wire.h>
#include <MPU6050.h>
#include <SD.h>

MPU6050 mpu;
const int chipSelect = 10;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // Initialize MPU6050
  Serial.println("Initializing MPU6050...");
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }
  Serial.println("MPU6050 connected.");

  // Initialize SD card
  Serial.println("Initializing SD card...");
  if (!SD.begin(chipSelect)) {
    Serial.println("SD card initialization failed!");
    while (1);
  }
  Serial.println("SD card ready.");
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  // Read raw data
  mpu.getAcceleration(&ax, &ay, &az);
  mpu.getRotation(&gx, &gy, &gz);

  // Print to Serial Monitor
  Serial.print("Accel: ");
  Serial.print(ax); Serial.print(", ");
  Serial.print(ay); Serial.print(", ");
  Serial.print(az); Serial.print(" | Gyro: ");
  Serial.print(gx); Serial.print(", ");
  Serial.print(gy); Serial.print(", ");
  Serial.println(gz);

  // Log to SD card
  File dataFile = SD.open("mpu_log.txt", FILE_WRITE);
  if (dataFile) {
    dataFile.print(ax); dataFile.print(",");
    dataFile.print(ay); dataFile.print(",");
    dataFile.print(az); dataFile.print(",");
    dataFile.print(gx); dataFile.print(",");
    dataFile.print(gy); dataFile.print(",");
    dataFile.println(gz);
    dataFile.close();
  } else {
    Serial.println("Error opening mpu_log.txt");
  }

  delay(500); // Adjust logging rate
}