#include "gps.h"
#include "settings.h"

// The serial connection to the GPS device
SoftwareSerial* serial_gps;

void gps_begin() {
	serial_gps = new SoftwareSerial(PIN_GPS_RX, PIN_GPS_TX);
	// Initialise the software-based serial connection to the GPS device
	serial_gps->begin(BAUD_GPS);
}

GPSLocation gps_fetch() {
	// The GPS message decoder
	TinyGPSPlus	gps;
	GPSLocation result;
	
	Serial.print(F("[gps] Working: "));
	uint32_t ms_last_update = millis();
	// We WILL discover our location - if it's the last thing we do!
	while(true) {
		if(millis() > 5000 && gps.charsProcessed() < 10) {
			Serial.println(F("\n[error] GPS device init failed"));
			while(true) { delay(1); }
		}
		
		// Make sure there's something to read
		if(serial_gps->available() <= 0)
			continue;
		
		// If it failed, go around again
		if(!gps.encode(serial_gps->read()))
			continue;
		
		// If we haven't acquired a lock yet, go around again
		// Also go around again if the location is the same hasn't yet been updated (GPS devices are strange)
		if(!gps.location.isValid()) {
			// NOTE: It can really take a rather long time to get a lock. Just wait patiently :-)
			if(millis() - ms_last_update > 500) Serial.print(".");
			ms_last_update = millis();
			continue;
		}
		
		// Hooray!
		Serial.println(F("ok"));
		
		result.lat = gps.location.lat();
		result.lng = gps.location.lng();
		#ifdef SD_DEBUG_ENABLED
		snprintf(result.time, 19, "%04d-%02d-%02d %02d:%02d:%02d",
			gps.date.year(),
			gps.date.month(),
			gps.date.day(),
			gps.time.hour(),
			gps.time.minute(),
			gps.time.second()
		);
		#endif
		return result;
	}
}

void gps_end() {
	serial_gps->end();
	delete serial_gps;
	//delete gps;
	// Serial.println(F("[warning] Putting the GPS device to sleep isn't implemented yet."));
}