// Demo for getting individual unified sensor data from the MPU6050
#include <Adafruit_MPU6050.h>
#include <SPI.h>
#include <SD.h>

Adafruit_MPU6050 mpu;
Adafruit_Sensor *mpu_temp, *mpu_accel, *mpu_gyro;

File myFile;

void setup(void) {
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // will pause Zero, etc until serial console opens
  }

  Serial.println("Initializing SD card...\n");
  if (!SD.begin(5)) {
    Serial.println("SD card missing!");
    while (1);
  }

  Serial.println("Adafruit MPU6050 test!");
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  Serial.println("MPU6050 Found!");
  mpu_temp = mpu.getTemperatureSensor();
  mpu_temp->printSensorDetails();

  mpu_accel = mpu.getAccelerometerSensor();
  mpu_accel->printSensorDetails();

  mpu_gyro = mpu.getGyroSensor();
  mpu_gyro->printSensorDetails();
}

void MPU_data() {
  /* Get a new normalized sensor event */
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  mpu_temp->getEvent(&temp);
  mpu_accel->getEvent(&accel);
  mpu_gyro->getEvent(&gyro);

  Serial.print("\t\tTemperature ");
  Serial.print(temp.temperature);
  Serial.println(" deg C");

  /* Display the results (acceleration is measured in m/s^2) */
  Serial.print("\t\tAccel X: ");
  Serial.print(accel.acceleration.x);
  Serial.print(" \tY: ");
  Serial.print(accel.acceleration.y);
  Serial.print(" \tZ: ");
  Serial.print(accel.acceleration.z);
  Serial.println(" m/s^2 ");

  /* Display the results (rotation is measured in rad/s) */
  Serial.print("\t\tGyro X: ");
  Serial.print(gyro.gyro.x);
  Serial.print(" \tY: ");
  Serial.print(gyro.gyro.y);
  Serial.print(" \tZ: ");
  Serial.print(gyro.gyro.z);
  Serial.println(" radians/s ");
  Serial.println();

  //Data Logging to SD Card
  if (SD.exists("Data.csv"))        //file name is Data.csv
  {
    myFile = SD.open("Data.csv", FILE_WRITE);

    myFile.print(temp.temperature); myFile.print(", ");
    myFile.print(accel.acceleration.x); myFile.print(", ");
    myFile.print(accel.acceleration.y); myFile.print(", ");
    myFile.print(accel.acceleration.z); myFile.print(", ");
    myFile.print(gyro.gyro.x); myFile.print(", ");
    myFile.print(gyro.gyro.y); myFile.print(", ");
    myFile.print(gyro.gyro.z); myFile.print(", ");
    myFile.println();
    myFile.close();
    Serial.println("\n1st data write to sd card-------------------------------------");
  }
  else
  {
    myFile = SD.open("Data.csv", FILE_WRITE);
    myFile.println("Temperature (C), Accel X, Accel Y, Accel Z, Gyro X, Gyro Y, Gyro Z");
    myFile.println("__________________________________________________________________");
    myFile.print(temp.temperature); myFile.print(", ");
    myFile.print(accel.acceleration.x); myFile.print(", ");
    myFile.print(accel.acceleration.y); myFile.print(", ");
    myFile.print(accel.acceleration.z); myFile.print(", ");
    myFile.print(gyro.gyro.x); myFile.print(", ");
    myFile.print(gyro.gyro.y); myFile.print(", ");
    myFile.print(gyro.gyro.z); myFile.print(", ");
    myFile.println();
    myFile.close();
    Serial.println("\nWrite to sd card------------------------------------------------");
  }
}

void loop() {
  MPU_data();
  delay(1000);
}