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

Adafruit_MPU6050 mpu;

int timeSaved[400];
int tempx[400];
int tempy[400];
int tempz[400];
int written;

String filename = "accelerometerData";
String writeData = "";

File acceldata;

unsigned long pInterval = 12.5;
unsigned long previousTime = 0;

const int CSpin = 10;

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

  if (!mpu.begin() && !SD.begin(CSpin)) {
    Serial.println("Faulty MPU6050 or SD connection");
    while(1) {
      delay(20);
    }
  }
  Serial.println("MPU6050 connected");
  Serial.println("SD connected");

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_94_HZ);
}

void loop() {

  recordData();
  saveData();

}

void recordData() {
//the sensor is filtering data to give me 94 results a second.
//i am going to pull data from the sensor at a lower rate than that, likely at ~80hz
  
  while(written < 399) {
    unsigned long currentTime = millis();
    if (currentTime - previousTime >= pInterval) {

      previousTime = currentTime;
      sensors_event_t a, g, temp;
      mpu.getEvent(&a, &g, &temp);

      timeSaved[written] = currentTime;
      tempx[written] = a.acceleration.x;
      tempy[written] = a.acceleration.y;
      tempz[written] = a.acceleration.z;

      written = written + 1;

    }
  }
  written = 0;
}

void saveData() {

  acceldata = SD.open(filename + ".csv");
  for (int i = 0; i < 400; i++) {
    writeData = String(timeSaved[i-1]) + "," + String(tempx[i-1]) + "," + String(tempy[i-1]) + "," + String(tempz[i-1]);
    acceldata.println(writeData);
  }

}