#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <SD.h>

Adafruit_MPU6050 mpu;
File dataFile;

const int chipSelect = 53; // SD card chip select pin

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

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");


  // Initialize SD card
  if (!SD.begin(chipSelect)) {
    Serial.println("Failed to initialize SD card");
    while (1) {
      delay(10);
    }
  }
  /*
  // Open file for writing
  dataFile = SD.open("sensor_data.txt", FILE_WRITE);
  if (!dataFile) {
    Serial.println("Failed to open file");
    while (1) {
      delay(10);
    }
  }
  */
}

void writeData(const char* sensorName, float x, float y, float z) {
  // Format the sensor data as a string
  char dataString[100];
  sprintf(dataString, "%s - x: %f, y: %f, z: %f\n", sensorName, x, y, z);

  // Write the data to the file
  dataFile.println(dataString);
}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);

  Serial.print("[");
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(event.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(event.acceleration.z);
  Serial.println(" m/s^2");

  writeData("Accelerometer", event.acceleration.x, event.acceleration.y, event.acceleration.z);
  delay(500);
}