using System;
using System.Text;
using System.Threading;
using GHI.Processor;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SWModules;

namespace HWModules
{
    public sealed class GPS1 : gpsUBlox
    {
       private GPS1()
            : base(new I2CDevice.Configuration(0x42, 100), 6)
        {}

        private static GPS1 instance;
        public static GPS1 Instance
        {
            get { return instance ?? (instance = new GPS1()); }
        }

    }

    public class gpsUBlox : I2CPortBase
    {
        public struct GPSPosition
        {
            public StringBuilder latSB;
            public StringBuilder lonSB;
            public int numSats;
            public StringBuilder gpsTimeOfFix;
            public StringBuilder rawMessage;
        }
        public GPSPosition latestPosition = new GPSPosition();

        public gpsUBlox(I2CDevice.Configuration config, int slotNum)
            : base(config, slotNum)
        {
            latestPosition.latSB = new StringBuilder(16);
            latestPosition.lonSB = new StringBuilder(16);
            latestPosition.numSats = -1;
            latestPosition.gpsTimeOfFix = new StringBuilder(16);
            latestPosition.rawMessage = new StringBuilder(256);
            latestSVStatus.rawMessage = new StringBuilder(512);
        }

        private static DateTime pubx04DateTime = new DateTime();
        private static StringBuilder pubx04ReturnSB = new StringBuilder(128);
        private static byte[] pubx04ReturnBytes = new byte[128];
        private static readonly byte[] commandPUBX04 = UTF8Encoding.UTF8.GetBytes("$PUBX,04*37\r\n");
        private static readonly byte[] bytesPUBX04 = UTF8Encoding.UTF8.GetBytes("UBX,04");
        public void synchToGPSTime()
        {
            for (int i = 0; i < 3; i++)
            {
                Array.Clear(pubx04ReturnBytes, 0, pubx04ReturnBytes.Length);
                pubx04ReturnSB.Clear();
                pubx04ReturnBytes = sendI2CCmd(commandPUBX04);
                if (GHI.Utilities.Arrays.Contains(pubx04ReturnBytes, bytesPUBX04) >= 0)
                {
                    pubx04ReturnSB.Append(UTF8Encoding.UTF8.GetChars(pubx04ReturnBytes));
                    int hour = (pubx04ReturnSB[7] - 48) * 10 + (pubx04ReturnSB[8] - 48);
                    int minute = (pubx04ReturnSB[9] - 48) * 10 + (pubx04ReturnSB[10] - 48);
                    int second = (pubx04ReturnSB[11] - 48) * 10 + (pubx04ReturnSB[12] - 48);
                    int day = (pubx04ReturnSB[17] - 48) * 10 + (pubx04ReturnSB[18] - 48);
                    int month = (pubx04ReturnSB[19] - 48) * 10 + (pubx04ReturnSB[20] - 48);
                    int year = 2000 + (pubx04ReturnSB[21] - 48) * 10 + (pubx04ReturnSB[22] - 48);

                    //TODO P3 It sure would be nice to figure out a way to do this without using "new".  But DateTime is immutable
                    if (year >= 2018)
                    {
                        pubx04DateTime = new DateTime(year, month, day, hour, minute, second);
                        Utility.SetLocalTime(pubx04DateTime);

                        RealTimeClock.SetDateTime(pubx04DateTime);
                        RealTimeClock.GetDateTime();
                    }
                    else
                    {
                        Utility.SetLocalTime(RealTimeClock.GetDateTime());
                    }

                    //TODO P2 these should be entries in the log
                    //Debug.Print("Attempting GPS time synch, try number: " + i.ToString() + ";  Set system time to: " + pubx04DateTime.ToString());
                    break;
                }
                else
                {
                    Utility.SetLocalTime(RealTimeClock.GetDateTime());

                    pubx04ReturnSB.Clear();
                    pubx04ReturnSB.Append(UTF8Encoding.UTF8.GetChars(pubx04ReturnBytes));
                    //Debug.Print("Try Number: " + i.ToString() + ";  Error synching to GPS time with PUBX,04");
                    //Debug.Print(pubx04ReturnSB.ToString());
                }
            }
        }


        private static DateTime gpsTryTime = new DateTime(1900, 1, 1);
        private static TimeSpan gpsTryTimeSpan = new TimeSpan(0, 0, 30);
        private static int gpsTryNum = 0;
        private static gpsUBlox.GPSPosition gpsPosition = new gpsUBlox.GPSPosition();

        public int getGPSPosition(int maxNumTries) //TODO P2 consider using UBX-NAV-PVT message
        {
            if ((DateTime.Now - gpsTryTime) >= gpsTryTimeSpan)
            {
                sbTemp.Clear();
                sbTemp.Append("Trying GPS get position try num: ");
                sbTemp.Append(gpsTryNum.ToString());
                sbTemp.Append(" of ");
                sbTemp.Append(maxNumTries.ToString());
                EngrLogger.writeToColumns(sbTemp);
                gpsPosition = getPosition();
                if (gpsPosition.numSats >= 4)      
                {
                    return (gpsPosition.numSats);
                }
                else
                {
                    gpsTryTime = DateTime.Now;
                    gpsTryNum++;
                    return (-1);
                }
            }
            else
            {
                if (gpsTryNum >= maxNumTries)
                {
                    gpsTryNum = 0;
                    return (-2);
                }
                else
                    return (0);
            }
        }

        private static StringBuilder pubx00ReturnSB = new StringBuilder(128);
        private static byte[] pubx00ReturnBytes = new byte[128];
        private static readonly byte[] commandPUBX00 = UTF8Encoding.UTF8.GetBytes("$PUBX,00*33\r\n");
        private static readonly byte[] bytesPUBX00 = UTF8Encoding.UTF8.GetBytes("UBX,00");

        public GPSPosition getPosition()
        {
            latestPosition.latSB.Clear();
            latestPosition.lonSB.Clear();
            latestPosition.numSats = -1;

            for (int i = 0; i < 3; i++)
            {
                pubx00ReturnBytes = sendI2CCmd(commandPUBX00);
                pubx00ReturnSB.Clear();
                pubx00ReturnSB.Append(UTF8Encoding.UTF8.GetChars(pubx00ReturnBytes));
                //Debug.Print("PUBX,00 Return: " + pubx00ReturnSB.ToString());
                if (GHI.Utilities.Arrays.Contains(pubx00ReturnBytes, bytesPUBX00) >= 0)
                {
                    parsePUBX00(pubx00ReturnBytes, ref latestPosition);
                    break;
                }
                else
                {
                    sbTemp.Clear();
                    sbTemp.Append("Try Number: ");
                    sbTemp.Append(i.ToString());
                    sbTemp.Append(";  Error getting GPS Position with PUBX,00");
                    EngrLogger.writeToColumns(sbTemp);
                }
            }

            sbTemp.Clear();
            sbTemp.Append("Got Position: lat = ");
            sbTemp.Append(latestPosition.latSB);
            sbTemp.Append("  lon = ");
            sbTemp.Append(latestPosition.lonSB);
            sbTemp.Append("   num sats = ");
            sbTemp.Append(latestPosition.numSats.ToString());
            EngrLogger.writeToColumns(sbTemp);

            return (latestPosition);
        }
 
        public struct SVStatus
        {
            public StringBuilder rawMessage;
        }
        public SVStatus latestSVStatus = new SVStatus();

        private static StringBuilder pubx03ReturnSB = new StringBuilder(512);
        private static byte[] pubx03ReturnBytes = new byte[512];
        private static readonly byte[] commandPUBX03 = UTF8Encoding.UTF8.GetBytes("$PUBX,03*30\r\n");
        private static readonly byte[] bytesPUBX03 = UTF8Encoding.UTF8.GetBytes("UBX,03");

        public SVStatus getSatStatus()
        {
            Array.Clear(pubx03ReturnBytes, 0, pubx03ReturnBytes.Length);
            for (int i = 0; i < 3; i++)
            {
                pubx03ReturnBytes = sendI2CCmd(commandPUBX03);
                pubx03ReturnSB.Clear();
                pubx03ReturnSB.Append(UTF8Encoding.UTF8.GetChars(pubx03ReturnBytes));
                //Debug.Print("PUBX,03 Return: " + pubx03ReturnSB.ToString());
                if (GHI.Utilities.Arrays.Contains(pubx03ReturnBytes, bytesPUBX03) >= 0)
                {
                    sbTemp.Clear();
                    sbTemp.Append("GPS Sat Status: ");
                    sbTemp.Append(pubx03ReturnSB);
                    EngrLogger.writeToColumns(sbTemp); 
                    break;
                }
                else
                {
                    sbTemp.Clear();
                    sbTemp.Append("Try Number: ");
                    sbTemp.Append(i.ToString());
                    sbTemp.Append(";  Error getting GPS Sat Status with PUBX,03");
                    EngrLogger.writeToColumns(sbTemp);
                }
            }
            latestSVStatus.rawMessage.Clear();
            latestSVStatus.rawMessage.Append(UTF8Encoding.UTF8.GetChars(pubx03ReturnBytes, 0, 256));

            return (latestSVStatus);
        }

        public static byte[] latBytes = new byte[16];
        public static byte[] lonBytes = new byte[16];
        public static byte[] numSatsBytes = new byte[16];


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

        public static StringBuilder positionSB = new StringBuilder(64);

        public void parsePUBX00(byte[] inBytes, ref GPSPosition position)
        {
            try
            {
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 2, ref fieldBytes);
                position.gpsTimeOfFix.Append(UTF8Encoding.UTF8.GetChars(fieldBytes));

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 3, ref fieldBytes);
                position.latSB.Append(UTF8Encoding.UTF8.GetChars(fieldBytes));
                position.latSB.Append(" ");
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 4, ref fieldBytes);
                position.latSB.Append(UTF8Encoding.UTF8.GetChars(fieldBytes));

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 5, ref fieldBytes);
                position.lonSB.Append(UTF8Encoding.UTF8.GetChars(fieldBytes));
                position.lonSB.Append(" ");
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 6, ref fieldBytes);
                position.lonSB.Append(UTF8Encoding.UTF8.GetChars(fieldBytes));

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 18, ref fieldBytes);
                position.numSats = bytesToInt32(fieldBytes);
          
                position.rawMessage.Clear();
                position.rawMessage.Append(UTF8Encoding.UTF8.GetChars(inBytes, 0, latestPosition.rawMessage.Capacity));
            }
            catch
            {
                position.latSB.Clear();
                position.latSB.Append("Error");
                position.lonSB.Clear();
                position.lonSB.Append("Error");
                position.numSats = -1;

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

        private 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);
        }

        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';

        private static I2CDevice.I2CTransaction[] cmdXaction = new I2CDevice.I2CTransaction[1];
        private static I2CDevice.I2CTransaction[] numBytesXAction = new I2CDevice.I2CTransaction[2];
        private static I2CDevice.I2CTransaction[] readFFXAction = new I2CDevice.I2CTransaction[2];

        private readonly byte[] numBytesAddr = new byte[2] { 0xFD, 0xFE };
        private static byte[] numBytesValue = new byte[2];
        private static byte[] dummyReadArray = new byte[2];
        private static byte[] evk7Bytes = new byte[256];
        private readonly byte[] ffAddr = new byte[1] { 0xFF };
        private static byte[] ffValue = new byte[1];

        private static StringBuilder sbTemp = new StringBuilder(128);

        private byte[] sendI2CCmd(byte[] command)
        {
            cmdXaction[0] = CreateWriteTransaction(command);
            //cmdXaction[1] = CreateReadTransaction(dummyReadArray);

            if (Execute(cmdXaction, 2000) == 0)
                EngrLogger.writeToColumns("ERROR: Timeout error gpsUBlox sendI2CCmd method first Execute command");

            //Create I2C transaction to read number of bytes available
            int evk7BytesIndex = 0;
            int numBytes = 0;

            numBytesXAction[0] = CreateWriteTransaction(numBytesAddr);
            numBytesXAction[1] = CreateReadTransaction(numBytesValue);

            Array.Clear(numBytesValue, 0, numBytesValue.Length);
            Array.Clear(evk7Bytes, 0, evk7Bytes.Length);

            for (int i = 0; i < 10; i++)
            {
                if (Execute(numBytesXAction, 2000) == 0)
                {
                    EngrLogger.writeToColumns("ERROR: Timeout gpsUBlox sendI2CCmd method second Execute command");
                }
                else
                {
                    numBytes = (256 * (int)numBytesValue[0]) + numBytesValue[1];
                    //Debug.Print("Register value: " + numBytesValue[0].ToString() + " " + numBytesValue[1]);
                    if ((numBytes > 0) && (numBytes < 0xFFFF))
                        break;
                }
                Thread.Sleep(200);
            }

            //Read address FF until message is complete
            if ((numBytes > 0) && (numBytes < 0xFFFF))
            {
                // Create I2C transaction to get buffer FF value
                readFFXAction[0] = CreateWriteTransaction(ffAddr);
                readFFXAction[1] = CreateReadTransaction(ffValue);

                evk7BytesIndex = 0;
                for (int i = 0; i < numBytes; i++)
                {
                    if (Execute(readFFXAction, 1000) == 0)
                    {
                        EngrLogger.writeToColumns("ERROR: Timeout error gpsUBlox sendI2CCmd method third Execute command");
                    }
                    else
                    {
                        //Debug.Print("FF Value = " + ffValue[0].ToString() + "  " + ((char)ffValue[0]).ToString());
                        if (evk7BytesIndex == 0)
                        {
                            if (ffValue[0] == 85)   //Wait for "U" start of text character.  The $P gets lost somewhere
                            {
                                evk7Bytes[evk7BytesIndex] = ffValue[0];
                                evk7BytesIndex = evk7BytesIndex + 1;
                            }
                        }
                        else
                        {
                            if (ffValue[0] == 10)
                            {
                                evk7Bytes[evk7BytesIndex] = ffValue[0];
                                evk7BytesIndex = evk7BytesIndex + 1;
                                break;
                            }
                            else
                            {
                                evk7Bytes[evk7BytesIndex] = ffValue[0];
                                evk7BytesIndex = evk7BytesIndex + 1;
                            }
                        }
                        if (evk7BytesIndex >= evk7Bytes.Length)
                            break;
                    }
                }
            }
            return (evk7Bytes);
        }
    }
}