#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 #3

Based on the demo program by Xander Soldaat (c)2011 Robomatter


                            G P S  T E S T   D E M O
                                  Ron Kessler
                               Created 8/13/2018

Updated 8/14/2018



PURPOSE:	Demonstrates how to READ GPS data from the DexterIndustries dGPS device and display readings
					on the Lego display.


BEHAVIORS: None, This is a static test. I want you to see how sensitive the sensor is.
					When you see your data it will look something like this because of the way the sensor and NXT work:
						Lon: -117621552
						Lat:	33498172

						If you want to map these co-ordinates, use -117.621552 and 33.498172 because most GPS systems
						use decimal degrees format. The NXT cannot compute these for us. So just remember to add the
						decimal places.


SETUP:		DO NOT use the Motor/Sensor setup screen like we have used before. The code will handle this.

					IMPORTANT PART: Be sure to copy the common.h file and the dexterind-gps.h file to the SAME folder
					where your project is stored. Use the files I have supplied in this project folder.



/*
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER.

 * Original code by Xander Soldaat (xander_at_botbench.com)
 * 20 February 2011
 * version 0.2
 */

//---this file and the common.h file must be in the folder where your project is. I have included
 //them in this project for you.

#include "dexterind-gps.h"				//add this line so we can access this file. Only needed for this file
																	//not common.h

task main () {

  long longitude = 0;
  long latitude = 0;
  bool linkstatus = false;

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

  while (true)
  	{
	    //---get info
	    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);
	    if (linkstatus)
	      nxtDisplayTextLine(7, "Statelite: [ON]");
	    else
	      nxtDisplayTextLine(7, "Satelite: [OFF]");
	    nxtDrawLine(0, 20, 99, 20);

	    //---read 2X /second
	    wait1Msec(500);
  	}
}