#include <MPU9250_asukiaaa.h>
#include <SD.h>
#include <SPI.h>
#include <MPU6500_WE.h>
#include <Wire.h>

File myFile;

#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 21
#define SCL_PIN 22
#endif
#define MPU6500_ADDR 0x68

MPU6500_WE myMPU6500 = MPU6500_WE(MPU6500_ADDR);

float aX, aY, aZ, aSqrt;
int state = 1;
const int chipSelect = BUILTIN_SDCARD;
int result;
const int buzzer = 4; 
const int LED_RED = 3;
const int LED_GREEN = 2;
float t;
float aX1, aX2, aX3, aX4, aX5, aX6, aX7, aX8, aX9, aX10;
float aY1, aY2, aY3, aY4, aY5, aY6, aY7, aY8, aY9, aY10;
float aZ1, aZ2, aZ3, aZ4, aZ5, aZ6, aZ7, aZ8, aZ9, aZ10;
float average_aX, average_aY, average_aZ;
float pitch, roll, yaw, gX, gY, gZ;
float deltat;


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

  pinMode(buzzer, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  
   //while (!Serial) {
    //; // wait for serial port to connect.
  //}


  //Serial.print("Initializing SD card...");

  if (!SD.begin(chipSelect)) {
    //Serial.println("initialization failed!");
    tone(buzzer, 1000); // Send 1KHz sound signal...
    digitalWrite(LED_RED, HIGH);
    return;
  }
  //Serial.println("initialization done.");
  
  //while(!Serial);
    //Serial.println("started");

  if(!myMPU6500.init()){
      Serial.println("MPU6500 does not respond");
  }
  else{
      Serial.println("MPU6500 is connected");
  }

  Serial.println("Position you MPU6500 flat and don't move it - calibrating...");
  delay(1000);
  myMPU6500.autoOffsets();
  Serial.println("Done!");

  myMPU6500.setSampleRateDivider(5);
  
  myMPU6500.setAccRange(MPU6500_ACC_RANGE_2G);

  myMPU6500.enableAccDLPF(true);

  myMPU6500.setAccDLPF(MPU6500_DLPF_6);

  myMPU6500.enableGyrDLPF();
  myMPU6500.setGyrDLPF(MPU6500_DLPF_4);
  myMPU6500.setSampleRateDivider(5);
  myMPU6500.setGyrRange(MPU6500_GYRO_RANGE_250);
}

void loop() {

  if(state == 1){
    myFile = SD.open("test.txt", FILE_WRITE);
  
    // if the file opened okay, write to it:
    if (myFile) {

       xyzFloat accRaw = myMPU6500.getAccRawValues();
       xyzFloat accCorrRaw = myMPU6500.getCorrectedAccRawValues();
       xyzFloat gValue = myMPU6500.getGValues();
       xyzFloat gyrRaw = myMPU6500.getGyrRawValues();
       xyzFloat gyrCorrRaw = myMPU6500.getCorrectedGyrRawValues();
       
       float resultantG = myMPU6500.getResultantG(gValue);
  


       Serial.print("g values (x,y,z):");
       Serial.print(String(gValue.x)+",");
       myFile.print(String(gValue.x)+",");
       Serial.print(String(gValue.y)+",");
       myFile.print(String(gValue.y)+",");
       Serial.println(String(gValue.z)+",");
       myFile.print(String(gValue.z)+",");
       Serial.print(String(gyrCorrRaw.x)+",");
       myFile.print(String(gyrCorrRaw.x)+",");
       Serial.print(String(gyrCorrRaw.y)+",");
       myFile.print(String(gyrCorrRaw.y)+",");
       Serial.println(String(gyrCorrRaw.z)+",");
       myFile.print(String(gyrCorrRaw.z)+",");
       t = millis();

       myFile.println(String(t)+",");


        
       digitalWrite(LED_GREEN, HIGH);
        
        delay(1);
      } else {
        myFile.println("Cannot read accel values " + String(result));
        tone(buzzer, 10000); // Send 1KHz sound signal...
        digitalWrite(LED_RED, HIGH);
      }
        // close the file:
      } else {
      // if the file didn't open, print an error:
        //Serial.println("error opening test.txt");
    }
  }