#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <ArduinoMqttClient.h>
#include <ESP8266WiFi.h>

#include "config.h"

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

#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);
WiFiClient client;
MqttClient mqttClient(client);

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 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);

  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println(WiFi.localIP());

  mqttClient.setUsernamePassword(MQTT_USERNAME, MQTT_PASSWORD);
  if (!mqttClient.connect(MQTT_SERVER, MQTT_PORT)) {
    Serial.print("MQTT connection failed! Error code = ");
    Serial.println(mqttClient.connectError());

    while (1);
  } else {
    Serial.println("MQTT connected");
  }

  Serial.println("Setup finished");
}

TinyGPSPlus gps;
void loop() {
  //Serial.println("Running loop");
  
  while (serialGPS.available() > 0) {
    if (gps.encode(serialGPS.read())) {
      if (gps.location.isValid()
          && gps.date.isValid()
          && gps.time.isValid()) {

        mqttClient.beginMessage("/gps/data");
        mqttClient.print(gps.date.year());
        
        if (gps.date.month() < 10) {
          mqttClient.print("0");
        }
        mqttClient.print(gps.date.month());
        
        if (gps.date.day() < 10) {
          mqttClient.print("0");
        }
        mqttClient.print(gps.date.day());
        
        if (gps.time.hour() < 10) {
          mqttClient.print("0");
        }
        mqttClient.print(gps.time.hour());
        
        if (gps.time.minute() < 10) {
          mqttClient.print("0");
        }
        mqttClient.print(gps.time.minute());
        
        if (gps.time.second() < 10) {
          mqttClient.print("0");
        }
        mqttClient.print(gps.time.second());
        mqttClient.print(" ");
        mqttClient.print(String(gps.location.lat(), 8));
        mqttClient.print(" ");
        mqttClient.print(String(gps.location.lng(), 8));
        mqttClient.endMessage();
        
        okLED();
      } else {
        Serial.println("Error: GPS not valid");
        errorLED(5);
      }
    } else {
      //Serial.println("Warning: GPS not readable");
    }
  }
  
  //Serial.println("Sleeping...");
  //ESP.deepSleep(MAX_SLEEP_TIME_MS * 1000);
}