// Based on MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain

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

#define bufferLength 128

const int MPU=0x68;  // I2C address of the MPU-6050

int second = floor(millis() / 1000);
File myFile;

volatile byte buf1[bufferLength];
volatile byte buf2[bufferLength];
byte toSend[bufferLength];

volatile uint8_t counter1;
volatile uint8_t counter2;

volatile bool secondBuffer = false;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(116400);
  while (!Serial) {}
  pinMode(10, OUTPUT);
  if (!SD.begin(10)) {
    Serial.println("initialization failed!");
    return;
  }
  myFile = SD.open("test.txt", FILE_WRITE);
  Serial.println("All OK, wait 5s, then GO TIME");
  delay(5000);
  Serial.print("GO");
  Timer1.initialize(1000);
  Timer1.attachInterrupt(readData);
}

bool dead = false;
volatile uint8_t fucks = 0;
void loop() {
  second = floor(millis() / 1000);
  if(dead) {
    return;
  }
  if(second > 60 || fucks > 5) {
    if(!dead) {
      Serial.println("END");
      Serial.println(fucks);
      myFile.close();
      Serial.println("Closed");
      dead = true;
    }
    return;
  }
  
  if(counter1 >= bufferLength or counter2 >= bufferLength) {
    noInterrupts();
    if(counter1 >= bufferLength) {
      memcpy((char*)toSend, (char*)buf1, bufferLength);
      counter1 = 0;
    } else {
      memcpy((char*)toSend, (char*)buf2, bufferLength);
      counter2 = 0;
    }
    interrupts();
    myFile.write(toSend,bufferLength);
  }
}

void readData(){
  interrupts();
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,2,true);
  uint8_t acc_msb = Wire.read();
  uint8_t acc_lsb = Wire.read();
  noInterrupts();
  uint16_t time = micros() % 50000;
  uint8_t time_msb = time >> 8;
  uint8_t time_lsb = time;
  
  if(secondBuffer == false) {
    if(counter1 == bufferLength) {
      fucks += 1;
    } else {
      buf1[counter1] = acc_msb;
      buf1[counter1 + 1] = acc_lsb;
      buf1[counter1 + 2] = time_msb;
      buf1[counter1 + 3] = time_lsb;
      counter1 += 4;
      if(counter1 == bufferLength) {
        secondBuffer = true;
      }
    }
  } else {
    if(counter2 == bufferLength) {
      fucks += 1;
    } else {
      buf2[counter2] = acc_msb;
      buf2[counter2 + 1] = acc_lsb;
      buf2[counter2 + 2] = time_msb;
      buf2[counter2 + 3] = time_lsb;
      counter2 += 4;
      if(counter2 == bufferLength) {
        secondBuffer = false;
      }
    }
  }
}