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

#define TFT_CS 15
#define TFT_DC 2
#define TFT_RST 4

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

const int ledPin = 13;
const float GYRO_THRESHOLD = 5.0;
int cursorY = 0;  // Variable to keep track of Y position

void setup() {
  Serial.begin(115200);
  pinMode(ledPin, OUTPUT);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    while(1) delay(10);
  }
  Serial.println("MPU6050 ready!");

  tft.begin();
  delay(50); // Allow some delay for display to initialize
  tft.setRotation(3);
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
}

void loop() {
  sensors_event_t gyroEvent;
  mpu.getGyroSensor()->getEvent(&gyroEvent);

  gyroEvent.gyro.z *= 1.5;

  Serial.print("Adjusted Gyro Z: "); Serial.println(gyroEvent.gyro.z);

  if (cursorY > tft.height() - 20) {  // Check if cursor is near or at the bottom of the display
    tft.fillScreen(ILI9341_BLACK);  // Clear screen
    cursorY = 0;  // Reset cursor Y position
  }

  tft.setCursor(0, cursorY);  // Set the cursor at the current Y position
  tft.print("Gyro Z: ");
  tft.println(gyroEvent.gyro.z);
  cursorY += 30;  // Move the Y position down by 30 pixels for next iteration

  if (abs(gyroEvent.gyro.z) > GYRO_THRESHOLD) {
    digitalWrite(ledPin, HIGH);
    delay(500);
    digitalWrite(ledPin, LOW);
  }

  delay(500);
}