#include <LiquidCrystal_I2C.h>
#include "RTClib.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// long currentTime = 0;

// // Données pour les états
// enum State {ACC, GYR, TIME, MAX_STATE};

// State currentState = ACC;
// long statePrevious = 0;
// int stateDelay = 3000;

// // Données pour le LCD
// LiquidCrystal_I2C lcd(0x27,16,2);
// // Tampon pour écrire les strings
// char lcdBuff[2][16] = {"                ", "                "};
// // Tampon pour écrire un float
// char szF[6];

// bool toDelete = true;

// long lcdPrevious = 0;
// int lcdDelay = 100;

// Données pour le MPU
Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;
float ax, ay, az, gx, gy, gz;


// Données pour le RTC
RTC_DS1307 rtc;
DateTime now;

// char daysOfTheWeek[7][4] = {"Dim", "Lun", "Mar", "Mer", "Jeu", "Ven", "Sam"};
// long rtcPrevious = 0;
// int rtcDelay = 250;

// Retourne la valeur absolue maximale
float vMaxAbs (float v1, float v2) {
  if (abs(v2) > abs(v1))
    return v2;
  return v1;
}

// Efface le contenu de l'écran
// void lcdErase() {
//     lcd.setCursor(0,0);
//     lcd.print("                ");
//     lcd.setCursor(0,1);
//     lcd.print("                ");
// }

void mpuTask() {
  mpu.getEvent(&a, &g, &temp);
  ax = vMaxAbs(ax, a.acceleration.x);
  ay = vMaxAbs(ay, a.acceleration.y);
  az = vMaxAbs(az, a.acceleration.z);

  gx = vMaxAbs(gx, g.gyro.x);
  gy = vMaxAbs(gy, g.gyro.y);
  gz = vMaxAbs(gz, g.gyro.z);
}

// Tâche de l'écran
// void lcdTask() {
//   if (currentTime - lcdPrevious < lcdDelay) return;

//   lcdPrevious = currentTime;

//   if (toDelete) {
//     toDelete = false;
//     lcdErase();    
//   }

//   lcd.setCursor(0, 0);
//   lcd.print(lcdBuff[0]);
//   lcd.setCursor(0, 1);
//   lcd.print(lcdBuff[1]);
// }

// void taskAccelerometer() {
//   // Convertir un float en char[]
//   dtostrf(ax, 4, 1, szF);
//   // Écriture à la ligne 1
//   sprintf(lcdBuff[0], "Acc.x=%s", szF);
//   dtostrf(ay, 4, 1, szF);
//   sprintf(lcdBuff[1], "y=%s", szF);
//   dtostrf(az, 4, 1, szF);
//   sprintf(lcdBuff[1], "%s z=%s", lcdBuff[1], szF);
// }

// void taskGyro() {
//   dtostrf(gx, 4, 1, szF);
//   sprintf(lcdBuff[0], "Gyr.x=%s", szF);
//   dtostrf(gy, 4, 1, szF);
//   sprintf(lcdBuff[1], "y=%s", szF);
//   dtostrf(gz, 4, 1, szF);
//   sprintf(lcdBuff[1], "%s z=%s", lcdBuff[1], szF);
// }

// Tâche du RTC
// void taskRTC() {
//   if (currentTime - rtcPrevious < rtcDelay) return;

//   rtcPrevious = currentTime;

//   now = rtc.now();
//   sprintf(lcdBuff[0], "%s %d-%02d-%02d", 
//     daysOfTheWeek[now.dayOfTheWeek()],
//     now.year(), now.month(), now.day());
//   sprintf(lcdBuff[1], "%02d:%02d:%02d",
//     now.hour(), now.minute(), now.second());
// }

void setup() {
  Serial.begin(115200);
  
  // Initialisation du MPU
  ax = ay = az = gx = gy = gz = 0.0f;

  // Uncomment to TEST
  if (!mpu.begin(0x69)) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  // Serial.println("MPU6050 Found!");
  // mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  // mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  // mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // // Initialisation du LCD
  // lcd.init();              
  // lcd.backlight();

  // Initialisation de l'horloge
  if (! rtc.begin()) {
    Serial.println("Couldn't find RTC");
    Serial.flush();
  }

}

void loop() {
  // currentTime = millis();

  mpuTask();

  // switch(currentState) {
  //   case ACC:
  //     taskAccelerometer();
  //     break;
  //   case GYR:
  //     taskGyro();
  //     break;
  //   case TIME:
  //     taskRTC();
  //     break;
  //   default:
  //     currentState = ACC;
  // }

  // // Switch state and raise LCD delete flag
  // if (currentTime - statePrevious >= stateDelay) {
  //   statePrevious = currentTime;

  //   currentState = (currentState + 1) % MAX_STATE;

  //   toDelete = true;
  // }
  
  // lcdTask();

  now = rtc.now();

  Serial.print(millis());
  Serial.print(",");
  Serial.print(now.second());
  Serial.print(","); 
  Serial.print(ax);
  Serial.print(",");
  Serial.print(ay);
  Serial.print(",");  
  Serial.print(az);
  Serial.println("");

}