#include <Arduino.h>

// -------------------------- Pin Definition (MODIFY PER W10 SCHEMATIC) --------------------------
#define GPS_TX_PIN 43    // Master TX → L76K RX
#define GPS_RX_PIN 44    // Master RX → L76K TX
#define GPS_BAUDRATE 9600 // L76K default baud rate (confirm via W10 schematic)

// Hardware Serial for GPS (ESP32 has multiple hardware serial ports for stability)
HardwareSerial GPS_SERIAL(2); // Use Serial2 (ESP32-S3: Serial0=USB, Serial1/2=hardware UART)

// -------------------------- NMEA Data Storage --------------------------
// Store key GPS data (from GNGGA frame)
struct GPSData {
  String utcTime;       // UTC time (HHMMSS)
  String latitude;      // Latitude (ddmm.mmmm)
  String latDir;        // Latitude direction (N/S)
  String longitude;     // Longitude (dddmm.mmmm)
  String lonDir;        // Longitude direction (E/W)
  uint8_t fixQuality;   // Fix quality (0=invalid, 1=GPS fix, 2=DGPS fix)
  uint8_t satCount;     // Number of satellites in use
  float hdop;           // Horizontal dilution of precision
  float altitude;       // Altitude (meters above sea level)
  String status;        // GPS status (Valid/Invalid)
} gpsData;

// -------------------------- NMEA Parser Function --------------------------
// Parse GNGGA frame (core GPS positioning data)
void parseGNGGA(String nmeaFrame) {
  // GNGGA format: $GNGGA,hhmmss.ss,ddmm.mmmm,N,dddmm.mmmm,E,1,08,1.0,545.4,M,46.9,M,,*76
  int commaIndex[15]; // Store index of each comma in NMEA frame
  int idx = 0;
  commaIndex[idx++] = nmeaFrame.indexOf(',');
  
  // Extract all comma positions
  while (commaIndex[idx-1] != -1 && idx < 15) {
    commaIndex[idx] = nmeaFrame.indexOf(',', commaIndex[idx-1] + 1);
    idx++;
  }

  // Validate frame format
  if (idx < 10) return;

  // Extract key data
  gpsData.utcTime = nmeaFrame.substring(commaIndex[0]+1, commaIndex[1]);
  gpsData.latitude = nmeaFrame.substring(commaIndex[1]+1, commaIndex[2]);
  gpsData.latDir = nmeaFrame.substring(commaIndex[2]+1, commaIndex[3]);
  gpsData.longitude = nmeaFrame.substring(commaIndex[3]+1, commaIndex[4]);
  gpsData.lonDir = nmeaFrame.substring(commaIndex[4]+1, commaIndex[5]);
  gpsData.fixQuality = nmeaFrame.substring(commaIndex[5]+1, commaIndex[6]).toInt();
  gpsData.satCount = nmeaFrame.substring(commaIndex[6]+1, commaIndex[7]).toInt();
  gpsData.hdop = nmeaFrame.substring(commaIndex[7]+1, commaIndex[8]).toFloat();
  gpsData.altitude = nmeaFrame.substring(commaIndex[8]+1, commaIndex[9]).toFloat();

  // Update status
  gpsData.status = (gpsData.fixQuality > 0) ? "Valid" : "Invalid";
}

// Parse GPRMC frame (minimum recommended GPS data)
void parseGPRMC(String nmeaFrame) {
  // GPRMC format: $GPRMC,hhmmss.ss,A,ddmm.mmmm,N,dddmm.mmmm,E,0.00,0.00,ddmmyy,,,A*62
  int commaIndex[12];
  int idx = 0;
  commaIndex[idx++] = nmeaFrame.indexOf(',');
  
  while (commaIndex[idx-1] != -1 && idx < 12) {
    commaIndex[idx] = nmeaFrame.indexOf(',', commaIndex[idx-1] + 1);
    idx++;
  }

  if (idx < 4) return;
  // Update status (A=Valid, V=Invalid)
  String rmcStatus = nmeaFrame.substring(commaIndex[1]+1, commaIndex[2]);
  if (rmcStatus == "A") gpsData.status = "Valid";
  else if (rmcStatus == "V") gpsData.status = "Invalid";
}

// -------------------------- L76K Initialization --------------------------
void initL76K() {
  // Send configuration commands to L76K (adjust per requirements)
  // 1. Set output baud rate to 9600 (match GPS_BAUDRATE)
  GPS_SERIAL.println("$PQBAUD,9600*25"); 
  delay(100);
  // 2. Enable GNGGA and GPRMC frames (disable unused frames to reduce data)
  GPS_SERIAL.println("$PQCUSP,0,1,1,0,0,0,0,0,0,0,0,0*41"); 
  delay(100);
  // 3. Set update rate to 1Hz (1 second per update)
  GPS_SERIAL.println("$PQCUPDATE,1*21"); 
  delay(100);
  
  Serial.println("L76K GPS module initialized!");
}

// -------------------------- Print GPS Data --------------------------
void printGPSData() {
  Serial.println("==================== GPS DATA ====================");
  Serial.print("UTC Time: "); Serial.println(gpsData.utcTime);
  Serial.print("Latitude: "); Serial.print(gpsData.latitude); Serial.print(" "); Serial.println(gpsData.latDir);
  Serial.print("Longitude: "); Serial.print(gpsData.longitude); Serial.print(" "); Serial.println(gpsData.lonDir);
  Serial.print("Fix Quality: "); Serial.println(gpsData.fixQuality);
  Serial.print("Satellites: "); Serial.println(gpsData.satCount);
  Serial.print("HDOP: "); Serial.println(gpsData.hdop);
  Serial.print("Altitude: "); Serial.print(gpsData.altitude); Serial.println(" m");
  Serial.print("Status: "); Serial.println(gpsData.status);
  Serial.println("==================================================\n");
}

// -------------------------- Setup Function --------------------------
void setup() {
  // Initialize debug serial (USB)
  Serial.begin(115200);
  delay(10); // Wait for USB serial to connect
  
  // Initialize GPS serial
  GPS_SERIAL.begin(GPS_BAUDRATE, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  delay(1000); // Wait for L76K to power on
  
  // Configure L76K
  initL76K();
  
  Serial.println("W10 GPS System Ready (Quectel L76K)");
  Serial.println("Move to open area for GPS signal...\n");
}

// -------------------------- Loop Function --------------------------
void loop() {
  // Read data from L76K
  if (GPS_SERIAL.available() > 0) {
    String nmeaFrame = GPS_SERIAL.readStringUntil('\n');
    nmeaFrame.trim(); // Remove whitespace and newline
    
    // Parse key NMEA frames
    if (nmeaFrame.startsWith("$GNGGA")) {
      parseGNGGA(nmeaFrame);
      printGPSData(); // Print data after parsing GNGGA
    }
    else if (nmeaFrame.startsWith("$GPRMC")) {
      parseGPRMC(nmeaFrame); // Update status from GPRMC
    }
  }
  
  delay(100); // Avoid overloading serial
}