#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <SD.h>

#define BUFFER_SIZE 256
#define SERIAL_TX_PIN 4
#define SERIAL_RX_PIN 5

#define SD_CS_PIN 15

#define MAX_LOOP_RUN 120
#define LOOP_DELAY_RUN_MS 200

#define LED_PIN 2

#define LED_ERROR_DEALY_MS 100
#define LED_PROGRESS_DELAY_MS 200

#define MAX_SLEEP_TIME_MS 60 * 1000

SoftwareSerial serialGPS(SERIAL_RX_PIN, SERIAL_TX_PIN); 

void blinkLED(int count, int delayMS) {
  for (int i = 0; i < count; i++) {
    digitalWrite(LED_PIN, LOW);
    delay(delayMS);
    digitalWrite(LED_PIN, HIGH);
    delay(delayMS);
  }  
}

void errorLED(int count) {
  blinkLED(count, LED_ERROR_DEALY_MS);
}

void progressLED(int count) {
  blinkLED(count, LED_PROGRESS_DELAY_MS);
}

void okLED() {
  blinkLED(1, 1000);
}

void writeTrackPoint(File gpxFile, TinyGPSPlus gps) {
  float latitude = gps.location.lat();
  String latitudeStr = String(latitude , 6);
  float longitude = gps.location.lng();
  String longitudeStr = String(longitude , 6);

  gpxFile.print("<trkpt lat=\"");
  gpxFile.print(latitudeStr);
  gpxFile.print("\" lon=\"");
  gpxFile.print(longitudeStr);
  gpxFile.print("\"><time>");
  gpxFile.print(gps.date.year());
  gpxFile.print("-");
  gpxFile.print(gps.date.month());
  gpxFile.print("-");
  gpxFile.print(gps.date.day());
  gpxFile.print("T");
  gpxFile.print(gps.time.hour());
  gpxFile.print(":");
  gpxFile.print(gps.time.minute());
  gpxFile.print(":");
  gpxFile.print(gps.time.second());
  gpxFile.println("Z</time></trkpt>");
}

void setup() {
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  serialGPS.begin(9600);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  if (!SD.begin(SD_CS_PIN)) {
    Serial.println("SD startup failed");
    errorLED(10);
    while(1);
  }

  Serial.println("Setup finished");
}

void loop() {
  Serial.println("Running loop");
  File gpxFile = SD.open("gpx.txt", FILE_WRITE);
  if (!gpxFile) {
    errorLED(15);
    Serial.println("Unable to open file for writing");
    return;
  }
  
  TinyGPSPlus gps;
  int run = 0;
  bool success = false;
  while (!success && run < MAX_LOOP_RUN) {
    while (serialGPS.available() > 0) {
      if (gps.encode(serialGPS.read())) {
        if (gps.location.isValid()
            && gps.date.isValid()
            && gps.time.isValid()) {

          success = true;

          writeTrackPoint(gpxFile, gps);
          
          Serial.println("location, date and time are valid");
        } else {
          run++;
          errorLED(2);
          delay(LOOP_DELAY_RUN_MS);
        }
      }
    }
  }

  gpxFile.close();

  if (success) {
    okLED();
  } else {
    errorLED(5);
  }

  Serial.println("Sleeping...");
  ESP.deepSleep(MAX_SLEEP_TIME_MS * 1000);
}