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

MPU6050 mpu;
File myFile;

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 successful!");
  } else {
    Serial.println("MPU6050 connection failed!");
  }

  // Initialize SD card
  Serial.print("Initializing SD card...");
  if (!SD.begin(chipSelect)) {
    Serial.println("SD card initialization failed!");
    return;
  }
  Serial.println("SD card ready.");
  
  // Open file for writing
  myFile = SD.open("mpu_log.txt", FILE_WRITE);
  if (myFile) {
    myFile.println("Time(ms),AccelX,AccelY,AccelZ,GyroX,GyroY,GyroZ");
    myFile.close();
  } else {
    Serial.println("Error opening file!");
  }
}

void loop() {
  // Read accelerometer and gyroscope
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  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
  myFile = SD.open("mpu_log.txt", FILE_WRITE);
  if (myFile) {
    myFile.print(millis()); myFile.print(",");
    myFile.print(ax); myFile.print(",");
    myFile.print(ay); myFile.print(",");
    myFile.print(az); myFile.print(",");
    myFile.print(gx); myFile.print(",");
    myFile.print(gy); myFile.print(",");
    myFile.println(gz);
    myFile.close();
  } else {
    Serial.println("Error writing to file");
  }

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