#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_MPU6050.h>

#define BTN_PIN 5
#define TFT_DC 2
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Adafruit_MPU6050 mpu;

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

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 sensor!");
    while (1);
  }

  tft.begin();
  tft.setRotation(3); // Depending on your display orientation
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
}

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

  float roll = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;
  float pitch = atan(-a.acceleration.x / sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;
  float yaw = atan2(g.gyro.y, g.gyro.x) * 180.0 / PI;

  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(10, 10);
  tft.print("Roll: ");
  tft.println(roll);
  tft.setCursor(10, 30);
  tft.print("Pitch: ");
  tft.println(pitch);
  tft.setCursor(10, 50);
  tft.print("Yaw: ");
  tft.println(yaw);

  delay(100);
}