#include <LiquidCrystal.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

LiquidCrystal lcd (12, 11, 10, 9, 8, 7);
Adafruit_MPU6050 mpu;

float pitch = 0;
float yaw = 0;
float deltatime = 0;
unsigned long lastTime;
unsigned long currentTime;

void setup() 
{
 Serial.begin(9600);
 lcd.begin(16, 2);
 lcd.setCursor(1,1);
 lcd.print("Initializing...");

 Wire.begin();
 if (!mpu.begin()) {
    lcd.print("MPU6050 Error");
    while (1);
  }

 lcd.setCursor(0, 0);
    lcd.print("MPU6050 Ready");
    delay(1000);

 lastTime = micros();
}

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

  unsigned long currentTime = micros();
  float deltatime = (currentTime - lastTime) / 1000000.0; // Convert to seconds
  lastTime = currentTime;

  pitch += g.gyro.x * deltatime; 
  yaw += g.gyro.z * deltatime; 

  lcd.setCursor(0, 0); 
  lcd.print("Pitch: ");
  lcd.print(pitch, 1); 
  lcd.print(" deg");

  lcd.setCursor(0, 1); 
  lcd.print("Yaw:   ");
  lcd.print(yaw, 1);   
  lcd.print(" deg");

  delay(50); 
}