Gadgeteer SatNav Simple

using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Presentation;
using Microsoft.SPOT.Presentation.Controls;
using Microsoft.SPOT.Presentation.Media;
using Microsoft.SPOT.Touch;

using Gadgeteer.Networking;
using GT = Gadgeteer;
using GTM = Gadgeteer.Modules;
using Gadgeteer.Modules.Seeed;
using Gadgeteer.Modules.GHIElectronics;
using GHIElectronics.NETMF.System;

namespace Gadgeteer_Pointer_01
{
    public partial class Program
    {

        GT.Timer timer = new GT.Timer(2000, GT.Timer.BehaviorType.RunContinuously);
        bool gpsPositionHasBeenReceived = false;
        bool compassDirectionHasBeenRecieved = false;
        GTM.Seeed.Compass.SensorData compassSensorData;
        GT.Color displayColor = GT.Color.White;
        double targetDirection;
        double compassAngle;
        // risd
        //double targetLatitude = 41.823259499999992;
        //double targetLongitude = -71.406410000000008;
        // harvard 42.366788,-71.124008
        //double targetLatitude = 42.366788;
        //double targetLongitude = -71.124008;
        // microsoft boston 42.361794,-71.08135
        double targetLatitude = 42.361794;
        double targetLongitude = -71.08135;
        //
        uint xPos;
        uint yPos;

        void ProgramStarted()
        {
            //gps.InvalidPositionReceived += new GTM.Seeed.GPS.InvalidPositionReceivedHandler(gps_InvalidPositionReceived);
            gps.PositionReceived += new GTM.Seeed.GPS.PositionReceivedHandler(gps_PositionReceived);
            //gps.NMEASentenceReceived += new GPS.NMEASentenceReceivedHandler(gps_NMEASentenceReceived);

            // setup a compass reading robot event watcher
            compass.MeasurementComplete += new GTM.Seeed.Compass.MeasurementCompleteEventHandler(compass_MeasurementComplete);
            //compass.ContinuousMeasurementInterval = new TimeSpan(0, 0, 0, 0, 500);
            //compass.StartContinuousMeasurements();

            oledDisplay.SimpleGraphics.AutoRedraw = false;

            // create a timer robot
            timer.Tick += new GT.Timer.TickEventHandler(timer_Tick);
            timer.Start();

            // tell everyone that you have started
            Debug.Print("Program Started");
        }

        void timer_Tick(GT.Timer timer)
        {
            //Debug.Print("GPS last position " + gps.LastPosition + " age " + gps.LastValidPositionAge);
            compass.RequestMeasurement();


            if (gpsPositionHasBeenReceived && compassDirectionHasBeenRecieved)
            {
                displayColor = GT.Color.Gray;
                compassAngle = ((compassSensorData.Angle / 360d) * (2d * MathEx.PI));
                //radians
                // should be abs?
                targetDirection = (MathEx.Atan2(targetLatitude - gps.LastPosition.Latitude, targetLongitude - gps.LastPosition.Longitude));
                //targetDirection -= (MathEx.PI * 1.5);
                xPos = 64u + (uint)(MathEx.Cos(compassAngle - targetDirection + MathEx.PI) * 60d);
                yPos = 64u + (uint)(MathEx.Sin(compassAngle - targetDirection + MathEx.PI) * 60d);


                oledDisplay.SimpleGraphics.Clear();
                printOledDisplay(0, "GPS Last Position:");
                printOledDisplay(1, "Lat  :" + gps.LastPosition.Latitude.ToString().Substring(0, 5));
                printOledDisplay(2, "Long :" + gps.LastPosition.Longitude.ToString().Substring(0, 5));
                printOledDisplay(3, "Compass Reading");
                printOledDisplay(4, compassAngle.ToString().Substring(0, 5));
                printOledDisplay(5, "Target Direction");
                printOledDisplay(6, targetDirection.ToString().Substring(0, 5));
                drawCompass();
                clearOledDisplay();
            }
            else if (compassDirectionHasBeenRecieved)
            {
                compassAngle = ((compassSensorData.Angle / 360d) * (2d * MathEx.PI));

                oledDisplay.SimpleGraphics.Clear();
                printOledDisplay(0, "GPS Last Position:");
                printOledDisplay(1, "UNKNOWN");
                printOledDisplay(2, "Connecting...");
                printOledDisplay(3, "Compass Reading");
                // what if not a long double???
                printOledDisplay(4, compassAngle.ToString().Substring(0, 5));
                printOledDisplay(5, "Target Direction");
                printOledDisplay(6, "UNKNOWN");
                clearOledDisplay();
            }
            else
            {
                oledDisplay.SimpleGraphics.Clear();
                printOledDisplay(0, "Waiting...");

                clearOledDisplay();
            }
        }

        void clearOledDisplay()
        {
            //oledDisplay.SimpleGraphics.Clear();
            oledDisplay.SimpleGraphics.Redraw();
        }

        void drawCompass()
        {
            oledDisplay.SimpleGraphics.DisplayRectangle(GT.Color.Red, 0, GT.Color.Red, 64, 64, 5, 5);
            //oledDisplay.SimpleGraphics.DisplayRectangle(GT.Color.Red, 0, GT.Color.Red, ((xPos - 64) / 2) + 64, ((yPos - 64) / 2) + 64, 5, 5);
            oledDisplay.SimpleGraphics.DisplayRectangle(GT.Color.Red, 0, GT.Color.Red, xPos, yPos, 5, 5);
        }

        void printOledDisplay(uint lineNumber, string displayMessage)
        {
            uint xPos = 0;
            oledDisplay.SimpleGraphics.DisplayText(displayMessage, Resources.GetFont(Resources.FontResources.NinaB), displayColor, xPos, lineNumber * 12);

        }

        void compass_MeasurementComplete(GTM.Seeed.Compass sender, GTM.Seeed.Compass.SensorData sensorData)
        {
            compassSensorData = sensorData;
            compassDirectionHasBeenRecieved = true;
            //Debug.Print(sensorData.ToString());
        }

        void gps_InvalidPositionReceived(GTM.Seeed.GPS sender)
        {
            //Debug.Print("Invalid position");
        }

        void gps_PositionReceived(GTM.Seeed.GPS sender, GTM.Seeed.GPS.Position position)
        {
            gpsPositionHasBeenReceived = true;
            //Debug.Print(position);
        }

        void gps_NMEASentenceReceived(GPS sender, string nmeaSentence)
        {
            if (!gpsPositionHasBeenReceived)
            {
                //Debug.Print("NMEA Sentence: " + nmeaSentence);
            }
        }

    }
}