using System;
using Microsoft.SPOT;

using System.Text;
//using SWModules;

namespace GPSParser
{
    public class Program
    {
        private const byte dPoint = (byte)'.';
        private const byte comma = (byte)',';
        private const byte nullByte = (byte)'\0';
        private const byte north = (byte)'N';
        private const byte south = (byte)'S';
        private const byte east = (byte)'E';
        private const byte west = (byte)'W';

    
        public const char tabChar = '\t';

        public const char spaceChar = ' ';
        public const string spaceString = " ";
        public static readonly byte[] spaceBytes = { 0x20 };

        public const char commaChar = ',';

        public const string crlfString = "\r\n";
        public static readonly byte[] crlfBytes = { 0xD, 0xA };
        public static readonly byte[] lfBytes = { 0xA };
        public static readonly byte[] dollarSignBytes = { 0x24 };

        public const string nanString = "NaN";

        public struct GPSPosition
        {
            public StringBuilder latSB;
            public StringBuilder lonSB;
            public int numSats;
            public StringBuilder gpsTimeOfFix;
            public DateTime cpfDateOfFix;
            public TimeSpan gpsTTFF;
            public StringBuilder rawMessage;
        } public GPSPosition latestPosition;

        
        public static void Main()
        {
            GPSPosition gpsPosition = new GPSPosition();

            gpsPosition.latSB = new StringBuilder();
            gpsPosition.lonSB = new StringBuilder();
            gpsPosition.numSats = -1;
            gpsPosition.gpsTimeOfFix = new StringBuilder();
            gpsPosition.cpfDateOfFix = new DateTime();
            gpsPosition.gpsTTFF = new TimeSpan(0, 0, 0);
            gpsPosition.rawMessage = new StringBuilder();
            
            StringBuilder pubxMsg = new StringBuilder("UBX,00,150208.00,3646.21831,N,12152.28182,W,0.000,D3,2.6,4.6,2.582,208.67,-0.041,,0.78,1.18,0.74,12,0,0*6A");
            parsePUBX00(UTF8Encoding.UTF8.GetBytes(pubxMsg.ToString()), ref gpsPosition);

            updateLatLon(gpsPosition);

            Debug.Print(gpsPositionSB.ToString());
        }

        static StringBuilder gpsPositionSB = new StringBuilder();

        static void updateLatLon(GPSPosition position)
        {
            double lat = double.NaN;
            double lon = double.NaN;

 
            gpsPositionSB.Clear();
            gpsPositionSB.Append("# GPS fix obtained in (seconds) \t");
            gpsPositionSB.Append(position.gpsTTFF.ToString());
            gpsPositionSB.Append(crlfString);

            gpsPositionSB.Append("#Fix: \tLon\tLat\tmm/dd/yyyy\tHH:mm:sss\tnumSats\tnmeaLon\t\tnmeaLat\t\tgpsTimeOfFix");
            gpsPositionSB.Append(crlfString);

            gpsPositionSB.Append("Fix:");
            gpsPositionSB.Append(tabChar);
            lon = convertLonToDecDeg(position.lonSB.ToString());
            gpsPositionSB.Append(lon.ToString("f8"));

            gpsPositionSB.Append(tabChar);
            lat = convertLatToDecDeg(position.latSB.ToString());
            gpsPositionSB.Append(lat.ToString("f8"));

            gpsPositionSB.Append(tabChar);
            gpsPositionSB.Append(position.cpfDateOfFix.ToString("MM/dd/yyyy\tHH:mm:ss"));

            gpsPositionSB.Append(tabChar);
            gpsPositionSB.Append(position.numSats.ToString());

            gpsPositionSB.Append(tabChar);
            gpsPositionSB.Append(position.lonSB);

            gpsPositionSB.Append(tabChar);
            gpsPositionSB.Append(position.latSB);

            gpsPositionSB.Append(tabChar);
            gpsPositionSB.Append(position.gpsTimeOfFix);
        }

        static public void parsePUBX00(byte[] inBytes, ref GPSPosition position)
        {
            byte[] latBytes = new byte[16];
            byte[] lonBytes = new byte[16];
            byte[] numSatsBytes = new byte[16];


            byte[] fieldBytes = new byte[16];
            byte[] decMinBytes = new byte[16];
            byte[] degMinBytes = new byte[16]; 

            StringBuilder sbTemp = new StringBuilder(128);

            StringBuilder positionSB = new StringBuilder(64);
            try
            {
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 2, ref fieldBytes);
                position.gpsTimeOfFix.Clear();
                position.gpsTimeOfFix.Append(GetChars(fieldBytes));
                position.cpfDateOfFix = DateTime.Now;

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 3, ref fieldBytes);
                position.latSB.Clear();
                position.latSB.Append(GetChars(fieldBytes));
                position.latSB.Append(tabChar);

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 4, ref fieldBytes);
                position.latSB.Append(GetChars(fieldBytes));

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 5, ref fieldBytes);
                position.lonSB.Clear();
                position.lonSB.Append(GetChars(fieldBytes));
                position.lonSB.Append(tabChar);

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 6, ref fieldBytes);
                position.lonSB.Append(GetChars(fieldBytes));

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                getField(inBytes, comma, 18, ref fieldBytes);
                position.numSats = bytesToInt32(fieldBytes);

                position.rawMessage.Clear();
                position.rawMessage.Append(GetChars(inBytes));
            }
            catch
            {
               // EngrLogger.writeToColumns("[ERROR] parsing GPS PUBX00 message");
                position.latSB.Clear();
                position.latSB.Append(nanString);
                position.lonSB.Clear();
                position.lonSB.Append(nanString);
                position.numSats = -1;

                position.rawMessage.Clear();
                //position.rawMessage.Append(GetChars(inBytes, 0, latestPosition.rawMessage.Capacity));
            }
        }

        private static double convertLatToDecDeg(string sLat)
        {
            int decPtIndex = 0;
            double lat = double.NaN;
            double deg;
            double min;

            //TODO P1 need to make sure pubx always has 2 digits of minutes
            try
            {
                decPtIndex = sLat.IndexOf('.');
                min = double.Parse(sLat.Substring(decPtIndex - 2, sLat.Length - decPtIndex));
                deg = double.Parse(sLat.Substring(0, decPtIndex - 2));
                lat = deg + min / 60.0;
                if (sLat.Substring(sLat.Length - 1, 1) == "S")
                    lat = -1 * lat;
            }
            catch
            {
                lat = double.NaN;
            }

            return lat;
        }

        private static double convertLonToDecDeg(string sLon)
        {
            int decPtIndex = 0;
            double lon = double.NaN;
            double deg;
            double min;

            //TODO P1 need to make sure pubx always has 2 digits of minutes
            try
            {
                decPtIndex = sLon.IndexOf('.');
                min = double.Parse(sLon.Substring(decPtIndex - 2, sLon.Length - decPtIndex));
                deg = double.Parse(sLon.Substring(0, decPtIndex - 2));
                lon = deg + min / 60.0;
                if (sLon.Substring(sLon.Length - 1, 1) == "W")
                    lon = -1 * lon;
            }
            catch
            {
                lon = double.NaN;
            }

            return lon;
        }

         public static int getField(byte[] inBytes, byte token, int desiredField, ref byte[] returnField)
        {
            int currentField = 0;
            int returnFieldIndex = 0;
            bool inDesiredField;

            //TODO P2 I don't think we need this block and it doesn't look like returnFieldIndex means anything
            if (desiredField == 0)
                inDesiredField = true;
            else
                inDesiredField = false;

            Array.Clear(returnField, 0, returnField.Length);

            for (int i = 0; i < inBytes.Length; i++)
            {
                if ((inBytes[i] == token) && !inDesiredField)
                    currentField++;
                else if (currentField == desiredField)
                {
                    inDesiredField = true;
                    if ((inBytes[i] == token) || (inBytes[i] == 10) || (inBytes[i] == 13))
                        return (returnFieldIndex);
                    returnField[returnFieldIndex++] = inBytes[i];
                    if (returnFieldIndex >= returnField.Length)
                        return returnFieldIndex;
                }
            }
            return (0);
        }

        public static int getField(StringBuilder inSB, char token, int desiredField, ref StringBuilder returnField)
        {
            int currentField = 0;
            int returnFieldIndex = 0;
            bool inDesiredField;

            //TODO P2 I don't think we need this block and it doesn't look like returnFieldIndex means anything
            if (desiredField == 0)
                inDesiredField = true;
            else
                inDesiredField = false;

            returnField.Clear();
            
            for (int i = 0; i < inSB.Length; i++)
            {
                if ((inSB[i] == token) && !inDesiredField)
                    currentField++;
                else if (currentField == desiredField)
                {
                    inDesiredField = true;
                    if ((inSB[i] == token) || (inSB[i] == 10) || (inSB[i] == 13))
                        return (returnFieldIndex);
                    returnField.Append(inSB[i]);
                    if (returnFieldIndex >= returnField.Length)
                        return returnFieldIndex;
                }
            }
            return (0);
        }

        private const byte Nonprintreplacement = 13;

        public static char[] GetChars(byte[] bytes)
        {
            for (var i = 0; i < bytes.Length; i++)
                if (bytes[i] > 127)
                    bytes[i] = Nonprintreplacement;
            try
            {
                return UTF8Encoding.UTF8.GetChars(bytes);
            }
            catch
            {
                return null;
            }
        }

        static int bytesToInt32(byte[] inBytes)
        {
            int returnValue = 0;
            double power = 1;
            int numBytes = 1;

            bool didOneCalc = false;

            if (Array.IndexOf(inBytes, 0) > 3)
                numBytes = 4;
            else
                numBytes = Array.IndexOf(inBytes, 0);

            for (int i = numBytes - 1; i > -1; i--)
            {
                if ((inBytes[i] >= 48) && (inBytes[i] <= 57))
                {
                    returnValue = returnValue + (int)((inBytes[i] - 48) * power);
                    power = power * 10;
                    didOneCalc = true;
                }
            }

            if (didOneCalc)
                return (returnValue);
            else
                return (-1);
        }

    }
}