#include <NewSoftSerial.h>
#include <TinyGPS.h>

/* 
  This code extracts GPS data and creates a CSV file.
  A GPS device is assumed to be hooked up to pins 2(rx) and 3(tx) at 4800 baud.
  A OpenLog device is assumed to be hooked up to pins 4(rx) and 5(tx) at 9600 baud.  (rate can be changed in the setup() method)
  The NewSoftSerial library is used to provide multiple serial devices.
  The TinyGPS libary is used to interpret the GPS data.
  
  The default serial pins (0 and 1) have been left un-assigned to avoid problems sending code to the arduino, and/or another serial output 
  device can be connected to these.
*/

TinyGPS gps;
NewSoftSerial nss(2, 3);    //change to match the actual pins used
NewSoftSerial logger(4,5);  //change to match the actual pins used

void gpsdump(TinyGPS &gps);
bool feedgps();
void printFloat(double f, int digits = 2);

//Change the log interval to something useful.
//If it is too short you can expect problems, as it takes some time to process the data and spit out the results.
int log_interval = 5000;

void setup()
{
  Serial.begin(4800);
  nss.begin(4800);       //change baud rate as needed.
  logger.begin(9600);    //change baud rate as needed.
  
  Serial.println("year,month,day,hour,minute,seconds,latitude,longitude,altitude(cm),speed(kmph),course");
  logger.println("year,month,day,hour,minute,seconds,latitude,longitude,altitude(cm),speed(kmph),course");
}

void loop()
{
  bool newdata = false;
  unsigned long start = millis();

  // Every 5 seconds we print an update
  while (millis() - start < log_interval)
  {
    if (feedgps())
      newdata = true;
  }
  
  if (newdata)
  {
    gpsdump(gps);
  }
}

void printFloat(double number, int digits, bool toLogger = false)
{
  // Handle negative numbers
  if (number < 0.0)
  {
     if (!toLogger) { Serial.print('-'); } else { logger.print('-'); }
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  if (!toLogger) { Serial.print(int_part); } else { logger.print(int_part); }
  
  // Print the decimal point, but only if there are digits beyond
  if (digits > 0) {
    if (!toLogger) { Serial.print('.'); } else { logger.print('.'); }
  }

  // Extract digits from the remainder one at a time
  while (digits-- > 0)
  {
    remainder *= 10.0;
    int toPrint = int(remainder);
    if (!toLogger) { Serial.print(toPrint); } else { logger.print(toPrint); }
    remainder -= toPrint; 
  } 
}

void gpsdump(TinyGPS &gps)
{
  long lat, lon, alt;
  float flat, flon, falt, fspeed;
  unsigned long age, date, time, chars;
  int year;
  byte month, day, hour, minute, second, hundredths;
  unsigned short sentences, failed;
  char sfalt[100], sflat[100], sflon[100], sfspeed[100], sfcourse[100];
  
  gps.get_position(&lat, &lon, &age);
  feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
  gps.f_get_position(&flat, &flon, &age);
  feedgps();
  gps.get_datetime(&date, &time, &age);
  feedgps();
  gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
  feedgps();
  alt = gps.altitude();
  fspeed = gps.f_speed_kmph();
  

  feedgps();
  
    
  //outputing a comma delimited string in the following format:
  //  year,month,day,hour,minute,second,latitude,longitude,altitude(cm),speed(kmph),course
  
  //log to file (via the OpenLog device attached as "logger")
  logger.print(year); logger.print(",");
  logger.print(static_cast<int>(month)); logger.print(",");
  logger.print(static_cast<int>(day)); logger.print(",");
  feedgps();
  logger.print(static_cast<int>(hour)); logger.print(",");
  logger.print(static_cast<int>(minute)); logger.print(",");
  logger.print(static_cast<int>(second)); logger.print("."); logger.print(static_cast<int>(hundredths)); logger.print(",");
  feedgps();
  printFloat(flat,5, true); logger.print(",");
  printFloat(flon,5, true); logger.print(",");
  logger.print(alt); logger.print(",");
  feedgps();
  printFloat(gps.f_speed_kmph(), 2, true); logger.print(",");
  printFloat(gps.f_course(), 2, true);
  logger.println();
  feedgps();  

  //log to the serial port so we can monitor what is going on
  // --- ERASE this section if serial monitoring is not needed ---
  Serial.print(year); Serial.print(",");
  Serial.print(static_cast<int>(month)); Serial.print(",");
  Serial.print(static_cast<int>(day)); Serial.print(",");
  feedgps();
  Serial.print(static_cast<int>(hour)); Serial.print(",");
  Serial.print(static_cast<int>(minute)); Serial.print(",");
  Serial.print(static_cast<int>(second)); Serial.print("."); Serial.print(static_cast<int>(hundredths)); Serial.print(",");
  feedgps();
  printFloat(flat,5,false); Serial.print(",");
  printFloat(flon,5,false); Serial.print(",");
  Serial.print(alt); Serial.print(",");
  feedgps();
  printFloat(gps.f_speed_kmph(),2,false); Serial.print(",");
  printFloat(gps.f_course(),2,false);
  Serial.println();
  feedgps();
}
  
bool feedgps()
{
  while (nss.available())
  {
    if (gps.encode(nss.read()))
      return true;
  }
  return false;
}