#include <Wire.h> // Needed for I2C to GNSS
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> 
#include <LiquidCrystal.h>
#include <SdFat.h>
#include <SFEMP3Shield.h>

SFE_UBLOX_GNSS myGNSS;
LiquidCrystal lcd(12, 11, 10, 9, 5, 3);
long lastTime = 0;

SdFat sd;
SFEMP3Shield MP3player;

const int SD_CS = 53;  // Chip Select for SD card
int currentTrack = 1;  // Start from the fourth track

void setup() {
  Serial.begin(115200);
  while (!Serial); // Wait for user to open terminal

  Wire.begin();

  if (myGNSS.begin() == false) {
    Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
    while (1);
  }

  myGNSS.setI2COutput(COM_TYPE_UBX); // Set the I2C port to output UBX only
  myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); // Save the communications port settings

  // Initialize the LCD
  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");
  delay(1000);

  // Initialize the SD card
  if (!sd.begin(SD_CS, SPI_HALF_SPEED)) {
    Serial.println("SD card initialization failed!");
    return;
  }
  
  // Initialize the MP3 player shield
  uint8_t result = MP3player.begin();
  if (result != 0) {
    Serial.print("MP3 player initialization failed with error code: ");
    Serial.println(result);
    return;
  }
  
  MP3player.setVolume(0, 0);  // Set volume. 0 is loudest, 254 is lowest (almost mute)
}

void loop() {
  // Query GNSS module for latitude and longitude
  if (millis() - lastTime > 1000) {
    lastTime = millis();

    long latitude = myGNSS.getLatitude();
    long longitude = myGNSS.getLongitude();

    // Print to Serial Monitor
    Serial.print(F("Lat: "));
    Serial.print(latitude);
    Serial.print(F(" Long: "));
    Serial.println(longitude);

    // Print to LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Lat: ");
    lcd.print(latitude);
    lcd.setCursor(0, 1);
    lcd.print("Lng: ");
    lcd.print(longitude);
  }

  // Check if the MP3 player is playing
  if (!MP3player.isPlaying()) {
    // Play the current track
    MP3player.playTrack(currentTrack);
    
    // Display current track number
    Serial.print("Playing track: ");
    Serial.println(currentTrack);
    
    // Decrement the track number
    currentTrack++;

    // If we've played down to track 001, reset to track 004
    if (currentTrack > 6) {
      currentTrack = 1;
    }
    
    delay(1000);  // Wait for a bit before checking again
  }
}