#include <TinyGPS++.h>
#include <Wire.h>
#include <MPU6050.h>

#define GPS_BAUDRATE 9600  // The default baudrate of NEO-6M is 9600

TinyGPSPlus gps;  // the TinyGPS++ object
MPU6050 mpu;

void setup() {
  Serial.begin(9600);
  Serial2.begin(GPS_BAUDRATE);//serial2 for GPS UART

  Serial.println(F("ESP32 - GPS module_Simulation"));
  Wire.begin(21, 22); // ESP32 default I2C pins for MPU6050
  Serial.println("Initializing MPU6050...");
  if (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }
  Serial.println("MPU6050 ready!");
}

void loop() {
  if (Serial2.available() > 0) {
    if (gps.encode(Serial2.read())) {
      if (gps.location.isValid()) {
        Serial.print(F("Latitude: "));
        Serial.println(gps.location.lat(),6);

        Serial.print(F("Longitude: "));
        Serial.println(gps.location.lng(),6);

        Serial.print(F("Altitude: "));
        if (gps.altitude.isValid())
          Serial.println(gps.altitude.meters());
        else
          Serial.println(F("INVALID"));
      } else {
        Serial.println(F("Location: INVALID"));
      }

      Serial.print(F("Speed: "));
      if (gps.speed.isValid()) {
        Serial.print(gps.speed.kmph());
        Serial.println(F(" km/h"));
      } else {
        Serial.println(F("INVALID"));
      }

      Serial.print(F("GPS date&time: "));
      if (gps.date.isValid() && gps.time.isValid()) {
        Serial.print(gps.date.year());
        Serial.print(F("-"));
        Serial.print(gps.date.month());
        Serial.print(F("-"));
        Serial.print(gps.date.day());
        Serial.print(F(" "));
        Serial.print(gps.time.hour());
        Serial.print(F(":"));
        Serial.print(gps.time.minute());
        Serial.print(F(":"));
        Serial.println(gps.time.second());
      } else {
        Serial.println(F("INVALID"));
      }

      Serial.println();
    }
  }



// MPU6050 Data
  Vector normAccel = mpu.readNormalizeAccel();
  Vector normGyro = mpu.readNormalizeGyro();

  Serial.print("Accel (g): ");
  Serial.print(normAccel.XAxis);
  Serial.print(", ");
  Serial.print(normAccel.YAxis);
  Serial.print(", ");
  Serial.println(normAccel.ZAxis);

  Serial.print("Gyro (deg/s): ");
  Serial.print(normGyro.XAxis);
  Serial.print(", ");
  Serial.print(normGyro.YAxis);
  Serial.print(", ");
  Serial.println(normGyro.ZAxis);

  Serial.println("----------------------------------");

  // Check GPS connection
  if (millis() > 5000 && gps.charsProcessed() < 10){
    Serial.println(F("No GPS data received: check wiring"));

  delay(1000);
}
}