// BufferedInput_20_GPS_57600_testing.ino
//
// This example reads GPS data from a SafeStringStream at 57600 baud with a 64byte rx buffer
// with BufferedOutput and extra 8mS of loop() code
// and with an extra 20 chars of BufferedInput
//
// Only the $GPRMC message is parsed.
//
//  by Matthew Ford
//  Copyright(c)2020 Forward Computing and Control Pty. Ltd.
//  This example code is in the public domain.
//
// download and install the SafeString library from Arduino library manager
// or from www.forward.com.au/pfod/ArduinoProgramming/SafeString/index.html
#include "SafeString.h"
#include "SafeStringStream.h"
#include "SafeStringReader.h"
#include "BufferedOutput.h"
#include "BufferedInput.h"

createBufferedInput(bufferedIn, 20);

createBufferedOutput(output, 80, DROP_UNTIL_EMPTY);

const uint32_t TESTING_BAUD_RATE = 19200; // how fast to release the data from sfStream to be read by this sketch
cSF(sfTestData, 180); // the test data SafeString, will be filled in setup()
cSF(rxBuf, 64); // the extra rxbuffer for the SafeStringStream to mimic the Uno hardware serial rx buffer
SafeStringStream sfStream(sfTestData, rxBuf); // set the SafeString to be read from and the SafeString providing the rx buffer

// create an sfReader instance of SafeStringReader class
// that will handle input lines upto 80 chars long terminated by 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, 80, '\n');

// UTC date/time
int year;
int month;
int day;
int hour;
int minute;
float seconds;
int latDegs = 0; float latMins = 0.0;
int longDegs = 0; float longMins = 0.0;
float speed = 0.0;
float angle = 0.0;


void setup() {
  Serial.begin(115200);    // Open serial communications and wait a few seconds
  for (int i = 10; i > 0; i--) {
    Serial.print(' '); Serial.print(i);
    delay(500);
  }
  Serial.println();
  Serial.print(F("Automated Serial testing at ")); Serial.println(TESTING_BAUD_RATE);
  Serial.println(F(" with 80char BufferedOutput and 25mS extra loop processing"));
  Serial.println(F(" and 20char BufferedInput, 115200 Serial baud rate"));
  Serial.println(F(" and anotherTask taking 25mS extra loop processing time"));
  output.connect(Serial); // connect the buffered output to Serial
  SafeString::setOutput(output); // enable error messages and debug() output to be sent to Serial  these error msgs always be displayed but will block!!
  bufferedIn.connect(sfStream); // add extra buffering to the test data stream, in addition to the 64byte rx buffer already in sfStream
  sfReader.connect(bufferedIn); // read the test data from the extra BufferedInput
  sfReader.echoOn(); // echos the read data back to be re-read again
  sfTestData = F(
                 "$GPRMC,194509.000,A,4042.6142,N,07400.4168,W,2.03,221.11,160412,,,A*77\n"
                 "$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48\n"
                 "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\n"
               ); // initialized the test data
  Serial.println("Test Data: -");
  Serial.println(sfTestData);
  Serial.println();
  sfStream.begin(sfTestData, TESTING_BAUD_RATE); // start releasing sfTestData at 9600 baud,  do this last in setup()
}

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, int &d, float& m, 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 returns
  d = degs;
  m = mins;
}

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, latMins, 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, longMins, 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;
}

void print2digits(Print& out, int num) {
  if (num <= 9) {
    out.print('0');
  }
  out.print(num);
}

void printPosition() {
  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("  ")); output.print(longDegs); output.print(' '); output.print(longMins); output.print(F("'"));
  output.print(F(", ")); output.print(latDegs); output.print(' '); output.print(latMins);  output.print(F("'"));
  output.println();
}

void processGPSInput() {
  if (sfReader.read()) {
    output.print(F(" InBufUsed:")); output.print(bufferedIn.maxBufferUsed());
    output.print(F(" mxAv:")); output.print(bufferedIn.maxStreamAvailable());
    output.print(F(" sfStreamOv:")); output.print(sfStream.RxBufferOverflow());
    output.println();
    sfReader.trim(); // remove and leading/trailing white space
    output.println(sfReader);
    if (!checkSum(sfReader)) { // is the check sum OK
      output.print("!! bad checksum : ");  output.println(sfReader);
    } else { // check sum OK so select msgs to process
      if (sfReader.startsWith("$GPRMC,")) {  // this is the one we want
        if (parseGPRMC(sfReader)) {
          printPosition(); // print new data
        }
      } else { // some other msg
      }
    }
  } // else token is empty
}

void anotherTask() {
  delay(25); // simulate more code here that takes 25mS to run
}

void loop() {
  bufferedIn.nextByteIn(); // <<<<<<<<< important add this to read more bytes from the input into the BufferedInput
  output.nextByteOut(); // <<<<<<<<< important add this to send next buffered bytes to Serial
  processGPSInput(); // task to read and process GPS messages
  anotherTask();  // some other task
}