SafeDispatch/SipComponent/Simoco/Utils.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace SipComponent.Simoco
{
    internal class Utils
    {

        /// <summary>
        /// Returns gps information from an array of bytes in short NMEA format
        /// </summary>
        /// <param name="nmea">array of bytes in short NMEA format</param>
        /// <param name="latitude">latitude</param>
        /// <param name="longitude">longitude</param>
        /// <param name="speed">speed</param>
        /// <param name="time">time expressed as UTC</param>
        public static void GPSInfo(byte[] nmea, out double latitude, out double longitude, out double speed,
            out DateTime time)
        {
            if (nmea.Length != 10)
                throw new ArgumentOutOfRangeException();
            
            DateTime now = DateTime.UtcNow;

            // | C NS EW Q speed speed speed speed | speed speed speed ndeg ndeg ndeg ndeg ndeg | ndeg ndeg nmin nmin nmin nmin nmin nmin | 
            // |       byte 0                      |                 byte 1                     |            byte 2
            bool fixValid = false;
            if (((nmea[0] >> 4) & 0x01) == 1)
                fixValid = true;
            if (fixValid)
            {
                sbyte NS = -1;
                if (((nmea[0] >> 6) & 0x01) == 1)
                    NS = 1;
                sbyte EW = -1;
                if (((nmea[0] >> 5) & 0x01) == 1)
                    EW = 1;

                byte speed_knots = (byte)((nmea[0] << 3) & 0x7F | (nmea[1] >> 5));

                byte latitudeDegrees = (byte)((nmea[1] << 2) & 0x7F | (nmea[2] >> 6));
                byte latitudeMinutes = (byte)(nmea[2] & 0x3F);

                // | nminf nminf nminf nminf nminf nminf nminf nminf | nminf nminf nminf nminf nminf nminf edeg edeg |
                // |                byte 3                           |                 byte 4                        |

                ushort latitudeFractionOfMin = (ushort)((nmea[3] >> 2) * 256 | (nmea[3] << 6) | (nmea[4] >> 2));

                //
                // | edeg edeg edeg edeg edeg edeg eminmm eminmm | eminmm eminmm eminmm eminmm eminf eminf eminf eminf |
                // |                byte 5                       |                 byte 6                              |
                //

                byte longitudeDegrees = (byte)((nmea[4] << 6) | (nmea[5] >> 2));
                byte longitudeMinutes = (byte)((nmea[5] << 4) & 0x3F | (nmea[6] >> 4));


                // | eminf eminf eminf eminf eminf eminf eminf eminf | eminf eminf h h h h h m | m m m m m s s s |
                // |                byte 7                           |        byte 8           |    byte 9       |

                ushort longitudeFractionOfMin = (ushort)(((nmea[6] << 2) & 0x3F | (nmea[7] >> 6) & 0xF3) * 256 |
                    (nmea[7] << 2) | ((nmea[8] >> 6) & 0xF3));

                byte hour = (byte)((nmea[8] >> 1) & 0x1F);
                byte minutes = (byte)((nmea[8] << 5) & 0x3F | (nmea[9] >> 3));
                byte ten_sec = (byte)(nmea[9] & 0x07);

                // viteza in km/h
                speed = speed_knots * 1.852;
                // latitudine in grade decimale
                latitude = (latitudeDegrees + double.Parse(string.Format("{0}{1}{2}",
                    latitudeMinutes,
                    System.Globalization.CultureInfo.CurrentCulture.NumberFormat.NumberDecimalSeparator,
                    latitudeFractionOfMin)) / 60) * NS;
                // longitude in grade decimale
                longitude = (longitudeDegrees + double.Parse(
                    string.Format("{0}{1}{2}",
                        longitudeMinutes,
                        System.Globalization.CultureInfo.CurrentCulture.NumberFormat.NumberDecimalSeparator,
                        longitudeFractionOfMin)) / 60) * EW;
                // time as unix time
                time = new DateTime(now.Year, now.Month, now.Day, hour, minutes, ten_sec * 10);
            }
            else
            {
                speed = latitude = longitude = 0;
                time = now;
            }




            //return string.Format(
            //    "{0}\r\n{1} degrees {2}.{3} minutes {4}\r\n{5} degrees {6}.{7} minutes {8}\r\nSpeed: {9} knots\r\nUTC time: {10:00}:{11:00}:{12:00}",
            //    fixValid ? "GPS valid fix" : "GPS no fix",
            //    latitudeDegrees, latitudeMinutes, latitudeFractionOfMin, N ? "North" : "South",
            //    longitudeDegrees, longitudeMinutes, longitudeFractionOfMin, E ? "East" : "West",
            //    speed, hour, minutes, ten_sec * 10
            //    );

        }

        public static void GetGPSFromStatusReport(byte[] deviceStatusReport, out double latitude, out double longitude)
        {
            byte[] latitudeBytes = new byte[4];
            byte[] longitudeBytes = new byte[4];
            Array.Copy(deviceStatusReport, 11, latitudeBytes, 0, 4);
            Array.Reverse(latitudeBytes);
            Array.Copy(deviceStatusReport, 15, longitudeBytes, 0, 4);
            Array.Reverse(longitudeBytes);
            latitude = BitConverter.ToInt32(latitudeBytes, 0) / 10000000.0;
            longitude = BitConverter.ToInt32(longitudeBytes, 0) / 10000000.0;
        }
    }
}