#pragma config(Sensor, S1,     touchSensorL,   sensorTouch)
#pragma config(Sensor, S2,     lightSensor,    sensorLightActive)
#pragma config(Sensor, S3,     touchSensorR,   sensorTouch)
#pragma config(Sensor, S4,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
#pragma config(Sensor, S1,     DGPS,                sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
RESCUE ROBOTICS WORKSHOP
Save GPS data to a file

                              File IO: GPS data
                                 Ron Kessler
                              Created 8/17/2018

Updated: 8/17/2018
				Did final compile/test. Saves latitude and longitude to the NXT File (GPStest.txt).


PURPOSE: We learned how to save sample data but now we need to save more complex data.
				We will get actual readings from GPS and save one set of values. Then we will read
				those values back and display them.

SETUP: 	Use the motor/sensor setup screen to define you GPS sensor. I am using port 1.
*/

#include "dexterind-gps.h"				//add this file. Must be in the same folder as this project!
																	//also needs common.h in the same folder

//---STEP 1: Create helper functions and variables
long longitude = 0;
long latitude = 0;
//bool linkstatus = false;

void ReadGPS()
{
	//---delay to help sensor link up to statelite
  wait1Msec(5000);
  eraseDisplay();				//start clean

  while (latitude <= 0)		//I just want the first good reading to save
  	{
	    //---get GPS data now
	    longitude = DGPSreadLongitude(DGPS);
	    latitude = DGPSreadLatitude(DGPS);
	    //linkstatus = DGPSreadStatus(DGPS);

	    //---how to display stuff on the display.
	    nxtDisplayCenteredTextLine(0, "GPS Readings...");
	    nxtDrawLine(0, 52, 99, 52);

	    nxtDisplayTextLine(3, "Lon: %d", longitude);			// the %d is a formatting command
	    nxtDisplayTextLine(4, "Lat: %d", latitude);
		}

}

void Save2File()
{
	//---STEP 1: Define the stuff we need to access this file
	TFileHandle   fileNumber;								//internal number used to locate the file.
  TFileIOResult IOException;							//tells us if there is an error during the IO
  string        myFileName = "GPStest.txt";	//my file name.
  int           fileSize = 100;						//same as when we saved 2 file




	Delete(myFileName,IOException);					//don't do this if you want to append to existing data!
  OpenWrite(fileNumber, IOException, myFileName, fileSize);	//creates the file if it doesn't exist
  WriteLong(fileNumber, IOException, longitude);
  WriteLong(fileNumber, IOException, latitude);
  Close(fileNumber, IOException);

  if(IOException == 0)
  	nxtDisplayTextLine(7, "Data was saved!");
 	else
   	nxtDisplayTextLine(7, "Error while saving....");

   	wait1Msec(10000);			//let us see the result for a few seconds
}




//***************START OF MAIN CODE*******************
task main()
{
	ReadGPS();
	Save2File();

}