// Serial1_GPS_buffered.ino
//
// This example reads GPS data from a Serial1
// e.g on a Mega2560 or Adafruit Feather 32u4 (used here)
//
// Only the $GPRMC message is parsed.
// Serial and GPS_serial both running at 9600
// User commands are:-
//  dms for output in degs min'ss.ss
//  degs for output in decimal degs.degs
//
//
// download and install the SafeString library from
// www.forward.com.au/pfod/ArduinoProgramming/SafeString/index.html
// for SafeString and BufferedOutput
#include "SafeString.h"
#include "SafeStringReader.h"
#include "BufferedOutput.h"
#include "millisDelay.h"

millisDelay printWaitingDelay;
unsigned long PRINT_WAITING_DELAY_MS = 5000;

// create an sfGPS_Reader instance of SafeStringReader class
// that will handle input lines upto 120 chars long terminated by newline
// the createSafeStringReader( ) macro creates both the SafeStringReader (sfGPS_Reader) and the necessary SafeString that holds input chars until a delimiter is found
// args are (ReaderInstanceName, expectedMaxInputLength, delimiters)
createSafeStringReader(sfGPS_Reader, 120, '\n');

createBufferedOutput(GPS_serial, 120, BLOCK_IF_FULL); // create an extra output buffer for Serial1, will block if buffer full, this buffers GPS commands sends

// =================== USER COMMANDS =====================
// create an sfGPSreader instance of SafeStringReader class
// that will handle input lines upto 20 chars long terminated by space, comma, carriage return or newline
// the createSafeStringReader( ) macro creates both the SafeStringReader (sfReader) and the necessary SafeString that holds input chars until a delimiter is found
// args are (ReaderInstanceName, expectedMaxInputLength, delimiters)
createSafeStringReader(sfReader, 20, " ,\r\n");

createBufferedOutput(output, 180, DROP_UNTIL_EMPTY); // create an extra output buffer for Serial output

// UTC date/time
int year;
int month;
int day;
int hour;
int minute;
float seconds;
float latDegs = 0; // google maps compatible
float longDegs = 0; // google maps compatible
float speed = 0.0;
float angle = 0.0;
bool haveFix = false;
bool dms = true;


void setup() {
  Serial.begin(115200);    // Open serial communications and wait a few seconds use highest baud rate available
  for (int i = 10; i > 0; i--) {
    Serial.print(' '); Serial.print(i);
    delay(500);
  }
  Serial.println();
  Serial.println("Serial and GPS_serial both running at 9600");
  Serial.println("User commands are:-");
  Serial.println(" dms for output in degs min'ss.ss");
  Serial.println(" degs for output in decimal degs.degs");

  output.connect(Serial); // connect the output buffer to the Serial, this flushes Serial
  sfReader.connect(output); // connect SafeStringReader for user cmds
  sfReader.echoOn();

  SafeString::setOutput(output); // enable error messages and debug() output to be sent to output

  Serial1.begin(9600);
  GPS_serial.connect(Serial1); // connect the output buffer to the Serial1, this flushes Serial1
  GPS_serial.println("$PMTK220,1000*1F"); // updates 1/sec
  GPS_serial.println("$PMTK300,1000,0,0,0,0*1C"); // fix speed 1/sec
  GPS_serial.println("$PMTK314,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); // turn on GPRMC, GPGGA and GPGSA
  Serial.println("GPS initilaization cmds sent");
  sfGPS_Reader.connect(GPS_serial);  // connect SafeStringReader for GPS data
  printWaitingDelay.start(PRINT_WAITING_DELAY_MS);
}

bool checkSum(SafeString &msg) {
  int idxStar = msg.indexOf('*');
  // could do these checks also
  // BUT SafeString will just return empty sfCheckSumHex and so fail to hexToLong conversion below
  //  if (idxStar < 0) {
  //    return false; // missing * //this also checks for empty string
  //  }
  //  // check for 2 chars
  //  if (((msg.length()-1) - idxStar) != 2) { // msg.length() -1 is the last idx of the msg
  //    return false; // too few or to many chars after *
  //  }
  cSF(sfCheckSumHex, 2);
  msg.substring(sfCheckSumHex, idxStar + 1); // next 2 chars SafeString will complain and return empty substring if more than 2 chars
  long sum = 0;
  if (!sfCheckSumHex.hexToLong(sum)) {
    return false; // not a valid hex number
  }
  for (size_t i = 1; i < idxStar; i++) { // skip the $ and the *checksum
    sum ^= msg[i];
  }
  return (sum == 0);
}

void parseTime(SafeString &timeField) {
  float timef = 0.0;
  if (!timeField.toFloat(timef)) { // an empty field is not a valid float
    return; // not valid skip the rest
  }
  uint32_t time = timef;
  hour = time / 10000;
  minute = (time % 10000) / 100;
  seconds = fmod(timef, 100.0);
}

void parseDegMin(SafeString &degField, float &d, int degDigits) {
  cSF(sfSub, 10); // temp substring
  degField.substring(sfSub, 0, degDigits); // degs, 2 for lat, 3 for long
  int degs = 0;
  if (!sfSub.toInt(degs)) {
    return; // invalid
  }
  float mins = 0.0;
  degField.substring(sfSub, degDigits, degField.length()); // mins
  if (!sfSub.toFloat(mins)) {
    return; // invalid
  }
  // both deg/mins valid update return
  d = degs + (mins / 60.0);
}

void parseDate(SafeString &dateField) {
  long lDate = 0;
  if (!dateField.toLong(lDate)) {
    return; // invalid
  }
  day = lDate / 10000;
  month = (lDate % 10000) / 100;
  year = (lDate % 100);
}
/**
   Fields: (note fields can be empty)
    123519.723   Fix taken at 12:35:19,723 UTC
    A            Status A=active or V=Void.
    4807.038,N   Latitude 48 deg 07.038' N
    01131.000,E  Longitude 11 deg 31.000' E
    022.4        Speed over the ground in knots
    084.4        Track angle in degrees True
    230394       Date 23rd of March 1994
    003.1,W      Magnetic Variation
*/
// just leaves existing values unchanged if new ones are not valid
// returns false if msg Not Active
bool parseGPRMC(SafeString &msg) {
  cSF(sfField, 11); // temp SafeString to received fields, max field len is <11;
  char delims[] = ",*"; // fields delimited by , or *
  bool returnEmptyFields = true; // return empty field for ,,
  int idx = 0;
  idx = msg.stoken(sfField, idx, delims, returnEmptyFields);
  if (sfField != "$GPRMC") {  // first field should be $GPRMC else called with wrong msg
    return false;
  }

  cSF(sfTimeField, 11); // temp SafeString to hold time for later passing, after checking 'A'
  idx = msg.stoken(sfTimeField, idx, delims, returnEmptyFields); // time, keep for later

  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // A / V
  if (sfField != 'A') {
    return false; // not active
  }
  // else A so update time
  parseTime(sfTimeField);

  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // Lat
  parseDegMin(sfField, latDegs, 2);

  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // N / S or empty
  if (sfField == 'S') {
    latDegs = -latDegs;
  }
  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // Long
  parseDegMin(sfField, longDegs, 3);

  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // N / S or empty
  if (sfField == 'W') {
    longDegs = -longDegs;
  }
  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // speed
  if (!sfField.toFloat(speed) ) {
    // invalid, speed not changed
  }
  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // track angle true
  if (!sfField.toFloat(angle) ) {
    // invalid, angle not changed
  }
  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // date
  parseDate(sfField);

  idx = msg.stoken(sfField, idx, delims, returnEmptyFields); // magnetic variation
  // skip parsing this for now
  return true;
}

// output degs mm'ss.ss"
void addDMS(Print& out, float degs) {
  int d = degs;
  out.print(d);
  out.print(' ');
  float mins = (degs - d) * 60; // 60mins/deg
  int m = mins;
  out.print(m);
  out.print("'");
  float secs = (mins - m) * 60;
  out.print(secs, 2);
  out.print('"');
}

void print2digits(Print& out, int num) {
  if (num <= 9) {
    out.print('0');
  }
  out.print(num);
}
void printPosition() {
  // only need to add this if missing the results
  //output.clearSpace(70); // only clears space the extra buffer not the Serial tx buffer
  output.print(F(" > > > "));
  output.print(F("20")); print2digits(output, year);  output.print('/');  print2digits(output, month);  output.print('/');  print2digits(output, day);
  output.print(F(" "));
  print2digits(output, hour); output.print(':'); print2digits(output, minute); output.print(':'); print2digits(output, seconds);
  output.print(F(" Position (Lat Long): "));
  if (dms) {
    bool negLat = false;
    if (latDegs < 0) {
      negLat = true;
      latDegs = -latDegs;
    }
    bool negLong = false;
    if (longDegs < 0) {
      negLong = true;
      longDegs = -longDegs;
    }
    addDMS(output, latDegs);
    output.print(negLat ? 'S' : 'N');
    output.print("  ");
    addDMS(output, longDegs);
    output.print(negLong ? 'W' : 'E');

  } else { // degs.degs
    output.print(latDegs, 6);
    output.print(" ");
    output.print(longDegs, 6);
  }
  output.println();
}

void waitingForFix() {
  if (printWaitingDelay.justFinished()) {
    if (!haveFix) {
      printWaitingDelay.repeat();
      output.println("Waiting for Fix..");
    }
  }
}

void handleGPS() {
  waitingForFix();
  if (sfGPS_Reader.read()) {
    sfGPS_Reader.trim(); // remove and leading/trailing white space
    if (checkSum(sfGPS_Reader)) { // if the check sum is OK
      if (sfGPS_Reader.startsWith("$GPRMC,")) {  // this is the one we want
        output.println(sfGPS_Reader);
        if (parseGPRMC(sfGPS_Reader)) {
          haveFix = true;
          printPosition(); // print new data
        }
      } else {  // ignore but print first 10 chars
        cSF(tmp, 10);
        output.print(sfGPS_Reader.substring(tmp, 0, 7)); output.println("...");
      }
    } else {
      output.clearSpace(12); // make space for at least the start of this error
      output.print("!! bad checksum : "); output.println(sfGPS_Reader);
    }
  } // else token is empty
}

void handleCMDS() {
  if (sfReader.read()) { // echo commands
    sfReader.toLowerCase(); // do compares in lower case
    if (sfReader == "dms") {
      output.println(" Setting DMS mode.");
      dms = true;
    } else if (sfReader == "degs") {
      output.println(" Setting DEGS mode.");
      dms = false;
    } else {
      // unknown cmd
      output.println(" -- invalid. Only dms or degs are valid commands");
    }
  } // else token is empty
}

void loop() {
  output.nextByteOut();
  handleCMDS();
  GPS_serial.nextByteOut();
  handleGPS();
}