using System;
using Microsoft.SPOT;

namespace CPF
{
    public static class GPS
    {
        public static NMEA_Location     Location;
        public static NMEA_Date         Date;
        public static NMEA_Time         Time;
        public static NMEA_Speed        Speed;
        public static NMEA_Course       Course;
        public static NMEA_Altitude     Altitude;
        public static NMEA_Integer      Satellites;
        public static NMEA_Decimal      Hdop;

        public static void Initialize()
        {
            Location    = new NMEA_Location();
            Date        = new NMEA_Date();
            Time        = new NMEA_Time();
            Speed       = new NMEA_Speed();
            Course      = new NMEA_Course();
            Altitude    = new NMEA_Altitude();
            Satellites  = new NMEA_Integer();     
            Hdop        = new NMEA_Decimal();      
        }

        // Returns distance in meters between two positions, both specified
        // as signed decimal-degrees latitude and longitude. Uses great-circle
        // distance computation for hypothetical sphere of radius 6372795 meters.
        // Because Earth is no exact sphere, rounding errors may be up to 0.5%.

        public static double DistanceBetween(double lat1, double long1, double lat2, double long2)
        {
            const double RadiusOfEarth = 6372795.0;  // In Meters
            double delta = Math.ToRadians(long1 - long2);
            double sdlong = Math.Sin(delta);
            double cdlong = Math.Cos(delta);
            lat1 = Math.ToRadians(lat1);
            lat2 = Math.ToRadians(lat2);
            double slat1 = Math.Sin(lat1);
            double clat1 = Math.Cos(lat1);
            double slat2 = Math.Sin(lat2);
            double clat2 = Math.Cos(lat2);
            delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
            delta = Math.Square(delta);
            delta += Math.Square(clat2 * sdlong);
            delta = Math.Sqrt(delta);
            double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
            delta = Math.Atan2(delta, denom);
            return delta * RadiusOfEarth;
        }

        // Returns course in degrees (North=0, West=270) from position 1 to position 2,
        // both specified as signed decimal-degrees latitude and longitude.
        // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
        public static double CourseTo(double lat1, double long1, double lat2, double long2)
        {
            double dlon = Math.ToRadians(long2 - long1);
            lat1 = Math.ToRadians(lat1);
            lat2 = Math.ToRadians(lat2);
            double a1 = Math.Sin(dlon) * Math.Cos(lat2);
            double a2 = Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(dlon);
            a2 = Math.Cos(lat1) * Math.Sin(lat2) - a2;
            a2 = Math.Atan2(a1, a2);
            if (a2 < 0.0)
                a2 += Math.PI * 2; 
            return Math.ToDegrees(a2);
        }
    }
}