#include <Adafruit_MPU6050.h>
#include <SD.h>
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/semphr.h>

SemaphoreHandle_t serialMutex;
Adafruit_MPU6050 mpu;
const int chipSelect = 5 ; // Pino de seleção do chip SD (pode variar dependendo do seu hardware)
File dataFile;
sensors_event_t a, g, temp;

void coletacao (void *pvParameters){
  while(true){
    if(xSemaphoreTake(serialMutex, portMAX_DELAY)){
      Serial.print("Tarefa 1 usando a serial");
      Serial.print("\n");
      // Coleta os dados 
     coletacao();
     Serial.print("\n");
     xSemaphoreGive(serialMutex);  // Libera o mutex
      vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

}

void armazenagem (void *pvParameters){
  while(true){
    if(xSemaphoreTake(serialMutex, portMAX_DELAY)){
      Serial.print("Tarefa 2 usando a serial");
      Serial.print("\n");
      sdcard(); // Armazena os dados no cartão sd 
      Serial.print("\n");
      xSemaphoreGive(serialMutex);  // Libera o mutex
      vTaskDelay(500 / portTICK_PERIOD_MS);
    }
  }
}

void coletacao (){
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");
      
    }

void calibrar(){
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G); 
  //set define o intervalo de medição do acelerômetro (8g).
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) { //get obtém o intervalo configurado.
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
     // Imprime o alcance configurado no monitor serial.
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

}

void sdcard(){
    // Criar ou abrir o arquivo de dados
      dataFile = SD.open("/dataFile.txt", FILE_APPEND);
    if (dataFile) {
      //printa no arquivo os dados
         dataFile.print("Acceleration X: ");
         dataFile.print(a.acceleration.x);
         dataFile.print(", Y: ");
         dataFile.print(a.acceleration.y);
         dataFile.print(", Z: ");
         dataFile.print(a.acceleration.z);
         dataFile.println(" m/s^2");

         dataFile.print("Rotation X: ");
         dataFile.print(g.gyro.x);
         dataFile.print(", Y: ");
         dataFile.print(g.gyro.y);
         dataFile.print(", Z: ");
         dataFile.print(g.gyro.z);
         dataFile.println(" rad/s");

         dataFile.print("Temperature: ");
         dataFile.print(temp.temperature);
         dataFile.println(" degC");
        dataFile.close();
    }
   else {
		// Caso o arquivo não abra, essa será a mensagem:
    Serial.println("Erro ao abrir o arquivo para armazenar os dados");
  }
  dataFile = SD.open("/dataFile.txt", FILE_READ);
  if (dataFile) {
    Serial.println("dataFile aberto ... lendo...");

    //lendo o que está sendo armazenado no sd card
    while (dataFile.available()) {
      Serial.write(dataFile.read());
    }
    dataFile.close();
  } else {
    // se o arquivo não abrir para leitura
    Serial.println("Erro ao abrir dataFile.txt");
  }
}

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

    // Inicializar o MPU9250
    if (!mpu.begin(0x68)) {  // change to your own address
        while (1) {
            Serial.println("MPU connection failed. Please check your connection with connection_check example.");
            delay(5000);
        }
    }

    // Inicializar o cartão SD
    if (!SD.begin(chipSelect)) {
        Serial.println("SD card initialization failed!");
        while (1);
    }
    Serial.println("SD card initialized.");
    
    calibrar(); //calibrar antes de começar a coletar os daods

    serialMutex = xSemaphoreCreateMutex(); //cria o mutex
    xTaskCreate(coletacao, "Coletacao", 2000, NULL, 1, NULL);
    delay(500);
    xTaskCreate(armazenagem, "Armazenagem", 3000, NULL, 1, NULL);
    delay(500);
}



void loop() {
 //nada por enquanto
}