#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <math.h>

Adafruit_MPU6050 mpu;

#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Temps entre les échantillons
float dt = 0.01;  // ajustez selon vos besoins

void setup() {
  Serial.begin(9600);
  tft.begin();
  Wire.begin();

  // Vérifiez si le MPU-6050 est correctement connecté
  if (!mpu.begin()) {
    Serial.println("Erreur de connexion MPU-6050, vérifiez les connexions !");
    while (1);
  }

  tft.setRotation(1);  // Ajustez la rotation de l'écran selon vos besoins

  tft.fillScreen(ILI9341_BLACK);  // Remplissez l'écran avec une couleur de fond
}

void loop() {
  float roll, pitch;
  getAngle(roll, pitch);
  writeValue(roll, pitch);

  
}

void getAngle(float &roll, float &pitch) {
  // Lire les données du gyroscope
  sensors_event_t accelEvent, gyroEvent, tempEvent;
  mpu.getEvent(&accelEvent, &gyroEvent, &tempEvent);

  // Calculer les angles d'Euler
  roll = atan2(-accelEvent.acceleration.y, accelEvent.acceleration.z) * 180.0 / M_PI;
  pitch = atan2(accelEvent.acceleration.x, sqrt(accelEvent.acceleration.y * accelEvent.acceleration.y + accelEvent.acceleration.z * accelEvent.acceleration.z)) * 180.0 / M_PI;
  
  // Afficher les angles d'Euler
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(" Pitch: ");
  Serial.println(pitch);

  delay(10);  // Petit délai entre chaque itération pour éviter une mise à jour trop fréquente
}


void writeValue(float &roll, float &pitch){
  // Affichez du texte
  tft.setTextSize(2);     // Définissez la taille du texte
  tft.setTextColor(ILI9341_RED);  // Définissez la couleur du texte
  tft.setCursor(10, 10);  // Définissez la position du curseur en pixels
  tft.print("Hello, World!");

  tft.setTextSize(3);     // Définissez la taille du texte
  tft.setTextColor(ILI9341_WHITE);  // Définissez la couleur du texte

  // Effacez et affichez le texte Roll
  clearText(100, 50, 160, 30);
  tft.setCursor(50, 50);
  tft.print("Roll: ");
  tft.print(roll);

  // Effacez et affichez le texte Pitch
  clearText(100, 90, 160, 30);
  tft.setCursor(50, 90);
  tft.print("Pitch: ");
  tft.print(pitch);

  // Attendez quelques secondes
  delay(10);
}

void clearText(int x, int y, int width, int height) {
  tft.fillRoundRect(x, y, width, height, 0, ILI9341_BLACK);
}