#pragma once
#include <TinyGPS++.h>
#include <Time.h>
#include "config.h"

class Gps {
  protected:
  TinyGPSPlus gps;

  public:
  void begin() {
    // uBlox NEO-7M can't persist settings, so we update them on runtime to get a higher update rate
    // commands extracted via u-center (https://www.youtube.com/watch?v=iWd0gCOYsdo)
    uint8_t ubloxconfig[216] = {
      // enable GPGGA & RMC sentences (only these are evaluated by TinyGPS++)
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x05, 0x38, //GGA
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x09, 0x54, //RMC
      // disable all other NMEA sentences to save bandwith
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x2B, //GLL
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x32, //GSA
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x39, //GSV
      0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x05, 0x47, //VTG
      // setup SBAS search to EGNOS only
      0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x01, 0x03, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x84, 0x15,
      // set NAV5 model to automotive, static hold on 0.5m/s, 3m
      0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27,
      0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x32, 0x3C, 0x00, 0x00,
      0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x85, 0x78,
      // 150ms update interval
      0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x96, 0x00, 0x01, 0x00, 0x01, 0x00, 0xAC, 0x3E,
      // 100ms update interval (needs higher baudrate, whose config doesnt work?)
      //0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12,
      // uart to baud 115200 and nmea only  -> wont work?! TODO :^(
      //0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2,
      //0x01, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xBF, 0x78,
      // save changes
      0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00,
      0x00, 0x00, 0x03, 0x1D, 0xAB
    };

    Serial.begin(GPS_BAUD);
    Serial.write(ubloxconfig, sizeof(ubloxconfig));
    //Serial.end();
    //Serial.begin(115200);
  }

  /**
   * parse all available bytes from the Serial input
   * should be called frequently to avoid buffer overflows
   */
  void pollGPS() {
    while (Serial.available() > 0)
      gps.encode(Serial.read());
  }

  /**
   * poll until we have a valid location.
   * retries at most once, returns success state
   */
  bool updateLocation() {
    // abort if the GPS device does not push valid data  within one update cycle
    static const unsigned int timeout = 2 * GPS_INTERVAL;
    unsigned long start = millis();

    do {
      pollGPS();
      if (millis() - start > timeout) return false;
    } while (!gps.location.isUpdated() && !gps.location.isValid() && gps.location.age() >= GPS_INTERVAL);
    return true;
  }

  /**
   * poll until we have a valid date & time.
   * retries at most once, returns success state
   */
  bool updateTime() {
    // abort if the GPS device does not push valid data  within one update cycle
    static const unsigned int timeout = 2 * GPS_INTERVAL;
    unsigned long start = millis();

    do {
      pollGPS();
      if (millis() - start > timeout) return false;
    } while ( !(gps.date.isUpdated() && gps.time.isUpdated() && gps.time.isValid() && gps.time.isValid()) );

    // in case we didnt timeout, resync the local clock (Time.h)
    setTime(gps.time.hour(), gps.time.minute(), gps.time.second(),
      gps.date.day(), gps.date.month(), gps.date.year());

    return true;
  }

  TinyGPSLocation& getLocation() {
    return gps.location;
  }

  /**
   * fill a char[20] with an iso8601 formatted date from now
   */
  void getISODate(char* result) {
    sprintf(result, "%04d-%02d-%02dT%02d:%02d:%02dZ",
      year(), month(), day(), hour(), minute(), second());
  }
};