//TO-DO

#include <SdFat.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define OUTPUT_READABLE_WORLDACCEL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

SdFat sd;
SdFile myFile;

const int chipSelect = 3;

//const int ledRecordPin = 7;
const int  redLed= 7;
const int greenLed = 5;
const int blueLed = 6;

int startButtonState = 0;
const int startButtonPin = 8;
signed short int isRecording = 1; // if odd, then it is not recording; if even it is.
boolean isStartButtontHigh = false; // to prevent repeated HIGH detecttion for the button pin
int accelX;
int accelY;
int accelZ;
char data[30]; //MPU data to be written to SD card

char dataTime[60]; // A separator for millis() data
unsigned long ellapsedTime; //Contais millis() data
signed short int ledTime = 0; //Contais an increment for LED
signed short int ledInterval = 500; //How much time the red led will be HIGH

boolean fifoOverflow = false; //Red light.
boolean canRecord = false; //Blue light. Timer will confirm if it is ok to start recording
//isRecording is green light

void setup() {
  Serial.begin(115200);
  pinMode(10,OUTPUT); //SPI requirements
  digitalWrite(10,HIGH); // Idem
  pinMode(blueLed,OUTPUT);
  pinMode(greenLed,OUTPUT);
  pinMode(redLed,OUTPUT);
  pinMode(startButtonPin,INPUT);
  ledControl(); //Warning users they should wait, as data will not be recorded

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(100, true);
  #endif

  mpu.initialize();
  mpu.testConnection();

  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(93);
  mpu.setYGyroOffset(-37);
  mpu.setZGyroOffset(22);

  mpu.setXAccelOffset(-4654);
  mpu.setYAccelOffset(1446);
  mpu.setZAccelOffset(1389);

  if (devStatus == 0) {
      mpu.setDMPEnabled(true);

      attachInterrupt(0, dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      dmpReady = true;

      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      // ERROR!
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}

void loop() {
    ellapsedTime = millis();
    ledTime++; //Start incrementing ledTime

    if(ellapsedTime > 5000) { //Wait until 60 seconds for MPU data stabilize
      canRecord=true; //Now data can be recorded
      ledControl();
    }

    if( ledTime > ledInterval){
      fifoOverflow = false;
      ledControl();
    }

    //MPU CODE STARTS HERE
    if (!dmpReady) return;

    while (!mpuInterrupt && fifoCount < packetSize) {} //////Muitas vezes programa pode travar aqui

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();

        fifoOverflow = true;
        ledTime=0;
        ledControl();
        sprintf(data,"Overflow: %lu",ellapsedTime);
        writteToFile(isRecording, data);

        Serial.println("OVERFLOW");


    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, packetSize);

        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_WORLDACCEL
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
        #endif

      if( canRecord ){ //do not record until at least 60 seconds are passed
           startButtonState = digitalRead(startButtonPin);

          if (startButtonState == HIGH && isStartButtontHigh) {
              isStartButtontHigh = false;
              isRecording += 1;
              Serial.println("GRAVANDO");
              handleFile(isRecording);
           }
           else if (startButtonState == LOW) {
               isStartButtontHigh = true;
           }

           accelX = aaWorld.x;
           accelY = aaWorld.y;
           accelZ = aaWorld.z;

           sprintf(data,"%d\t%d\t%d\t%lu",accelX, accelY, accelZ, ellapsedTime);

           writteToFile(isRecording, data);
           ledControl();
        }
    }
}

void dmpDataReady() {
    mpuInterrupt = true;
}

void ledControl(){
  //Turn all leds off
  digitalWrite(greenLed,LOW);
  digitalWrite(redLed,LOW);
  digitalWrite(blueLed,LOW);

  //Check what should be on
  if( !canRecord ){
    digitalWrite(blueLed,HIGH); //Wait for MPU data to stabilize. Nothing is being recorded.
  }

  if( fifoOverflow ){
    digitalWrite(redLed,HIGH); //FIFO overflow!
  }

  if( !fifoOverflow ){
    digitalWrite(redLed,LOW);
  }

  if( !fifoOverflow && canRecord &&  isRecording % 2 == 0 ){
    digitalWrite(greenLed,HIGH); //Recording
  }

}

void handleFile(int isRecording){

  //if isRecording is even
  if(isRecording % 2 == 0){

      if (!sd.begin(chipSelect, SPI_HALF_SPEED)) sd.initErrorHalt();

      if (!myFile.open("MPU.txt", O_RDWR | O_CREAT | O_AT_END)) {
          sd.errorHalt("opening for write failed");

       }
       sprintf(dataTime,"Starting record: %lu",ellapsedTime);
       myFile.println( dataTime );
  }else{

      sprintf(dataTime,"Stopping record: %lu",ellapsedTime);
      myFile.println( dataTime );
      myFile.close();
      mpu.resetFIFO();
  }
}

void writteToFile(int isRecording, char* data){
  if(isRecording % 2 == 0){
    myFile.println(data);
  }
}