using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using System.Threading;
using System.IO.Ports;
using System.Text;


namespace CPF
{
    public static class EVK7GPS
    {
        private static StructQueue.qStruct qS = new StructQueue.qStruct();

        private static StringBuilder sbTemp = new StringBuilder(128);
        private static I2CDevice.Configuration evk7I2CConfig = new I2CDevice.Configuration(0x42, 100);

        public static void init()
        {
            qS.byteArray = new byte[StructQueue.byteArrayWidth];

            //evk7Port.ReadTimeout = 5000;
            //evk7Port.Open();

            //evk7Port.DiscardInBuffer();
            //evk7Port.DiscardOutBuffer();
            //evk7Port.DataReceived += new SerialDataReceivedEventHandler(evk7DataReceived);
        }

        private static byte[] txBytes = new byte[64];
        private const string evk7PUBX00Cmd = "$PUBX,00*33\r\n";
        public static void sendPUBX00()
        {
            Array.Clear(txBytes, 0, txBytes.Length);
            Array.Copy(UTF8Encoding.UTF8.GetBytes(evk7PUBX00Cmd), txBytes, evk7PUBX00Cmd.Length);
            //evk7Port.Write(txBytes, 0, evk7PUBXCmd.Length);
        }

        private const string evk7PUBX04Cmd = "$PUBX,04*37\r\n";
        public static void sendPUBX04()
        {
            Array.Clear(txBytes, 0, txBytes.Length);
            Array.Copy(UTF8Encoding.UTF8.GetBytes(evk7PUBX04Cmd), txBytes, evk7PUBX04Cmd.Length);
            //        evk7Port.Write(txBytes, 0, evk7PUBX04Cmd.Length);
        }

        private static byte LF = 10;
        private static byte[] bytesRead = new byte[2048];
        private static byte[] messageBytes = new byte[2048];
        private static int messageIndex = 0;

        private static void evk7DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            int bytesToRead = 0;
            //bytesToRead = evk7Port.BytesToRead;

            try
            {
                //evk7Port.Read(bytesRead, 0, bytesToRead);
            }
            catch
            {
                Debug.Print("EVK7 GPS Read Exception");          //TODO do something smarter here
            }

            for (int i = 0; i < bytesToRead; i++)
            {
                messageBytes[messageIndex] = bytesRead[i];
                if (messageBytes[messageIndex] == LF)
                {
                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CurrentState;

                    //sbTemp.Clear();
                    //sbTemp.Append("EVK7 Message = ");
                    //sbTemp.Append(UTF8Encoding.UTF8.GetChars(messageBytes, 0, messageIndex - 1));
                    //Debug.Print(sbTemp.ToString());

                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(messageBytes, qS.byteArray, messageIndex + 1);
                    qS.qRecordType = StructQueue.QRecordType.EVK7;
                    Program.sQueue.Enqueue(qS);

                    messageIndex = 0;
                    Array.Clear(messageBytes, 0, messageBytes.Length);
                }
                else
                {
                    messageIndex = messageIndex + 1;
                }
            }
        }

        private static DateTime pubx04DateTime = new DateTime(2015, 1, 1, 0, 0, 0);
        public static void setSystemTime(StringBuilder inSB)
        {
            //Use this for serial port PUBX,04
            //int hour = (inSB[9] - 48) * 10 + (inSB[10] - 48);
            //int minute = (inSB[11] - 48) * 10 + (inSB[12] - 48);
            //int second = (inSB[13] - 48) * 10 + (inSB[14] - 48);
            //int day = (inSB[19] - 48) * 10 + (inSB[20] - 48);
            //int month = (inSB[21] - 48) * 10 + (inSB[22] - 48);
            //int year = 2000 + (inSB[23] - 48) * 10 + (inSB[24] - 48);

            //Use this for I2C PUBX,04
            int hour = (inSB[7] - 48) * 10 + (inSB[8] - 48);
            int minute = (inSB[9] - 48) * 10 + (inSB[10] - 48);
            int second = (inSB[11] - 48) * 10 + (inSB[12] - 48);
            int day = (inSB[17] - 48) * 10 + (inSB[18] - 48);
            int month = (inSB[19] - 48) * 10 + (inSB[20] - 48);
            int year = 2000 + (inSB[21] - 48) * 10 + (inSB[22] - 48);


            pubx04DateTime = new DateTime(year, month, day, hour, minute, second);
            Utility.SetLocalTime(pubx04DateTime);
            //Debug.Print("setSystemTime inSB" + inSB.ToString() + "  Time: "+ hour.ToString() + minute.ToString() + second.ToString() + "  Date: " +
            //                year.ToString() + month.ToString() + day.ToString());

        }

        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 static byte[] numBytesAddr = new byte[2] { 0xFD, 0xFE };
        private static byte[] numBytesValue = new byte[2];
        private static byte[] evk7Chars = new byte[256];
        private static byte[] ffAddr = new byte[1] { 0xFF };
        private static byte[] ffValue = new byte[1];

        private static StringBuilder returnSB = new StringBuilder(256);

        public static StringBuilder sendI2CCmd(string command)
        {
            Program.g400I2C.Config = evk7I2CConfig;

            //Create I2C transaction to write command

            byte[] cmdByte = new byte[2];
            cmdByte[0] = 0xFF;

            byte[] cmdArray = UTF8Encoding.UTF8.GetBytes(command);

            cmdXaction[0] = I2CDevice.CreateWriteTransaction(cmdArray);

            if (Program.g400I2C.Execute(cmdXaction, 1000) == 0)
                Debug.Print("Error writing I2C command array");

            // Debug.Print("Done sending command \r\n");

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

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

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

            // Debug.Print("Checking number of bytes in buffer");
            for (int i = 0; i < 10; i++)
            {
                if (Program.g400I2C.Execute(numBytesXAction, 1000) == 0)
                {
                    Debug.Print("Failed to perform numBytes I2C transaction");
                }
                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] = I2CDevice.CreateWriteTransaction(ffAddr);
                readFFXAction[1] = I2CDevice.CreateReadTransaction(ffValue);

                evk7CharsIndex = 0;
                for (int i = 0; i < numBytes; i++)
                {
                    if (Program.g400I2C.Execute(readFFXAction, 1000) == 0)
                    {
                        Debug.Print("Failed to perform readFF I2C transaction");
                    }
                    else
                    {
                        //Debug.Print("FF Value = " + ffValue[0].ToString() + "  " + ((char)ffValue[0]).ToString());
                        if (evk7CharsIndex == 0)
                        {
                            if (ffValue[0] == 85)   //Wait for "U" start of text character.  The $P gets lost somewhere
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                            }
                        }
                        else
                        {
                            if (ffValue[0] == 10)
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                                break;
                            }
                            else
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                            }
                        }
                        if (evk7CharsIndex >= evk7Chars.Length)
                            break;
                    }
                }
            }
            returnSB.Clear();
            returnSB.Append(UTF8Encoding.UTF8.GetChars(evk7Chars));

            sbTemp.Clear();
            sbTemp.Append("GPS I2C read = ");
            sbTemp.Append(returnSB);
            // Debug.Print(sbTemp.ToString());

            if (command.IndexOf("PUBX,04") >= 0)
            {
            }
            else
            {
                qS.timeStamp = DateTime.Now;
                qS.state = Program.CurrentState;

                sbTemp.Clear();
                sbTemp.Append("GPS I2C read = ");
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(evk7Chars));
                //Debug.Print(sbTemp.ToString());

                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(evk7Chars, qS.byteArray, evk7CharsIndex);
                qS.qRecordType = StructQueue.QRecordType.EVK7;
                qS.errorCode = 0;
                Program.sQueue.Enqueue(qS);
            }

            return (returnSB);
        }

        public struct Position
        {
            public double lat;
            public double lon;
            public int numSats;
        }
        private static Position parsedPosition;

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

        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 byte[] fieldBytes = new byte[16];
        private static byte[] decMinBytes = new byte[16];
        private static byte[] degMinBytes = new byte[16];

        public static void parsePUBX00(byte[] inBytes)
        {
            try
            {
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Array.Clear(decMinBytes, 0, decMinBytes.Length);
                Array.Clear(degMinBytes, 0, degMinBytes.Length);
                Array.Clear(latBytes, 0, latBytes.Length);

                Utils.getField(inBytes, comma, 3, ref fieldBytes);
                sbTemp.Clear();
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(fieldBytes, 0, Array.IndexOf(fieldBytes, nullByte)));
                sbTemp.Append(',');
 
                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 4, ref fieldBytes);
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(fieldBytes, 0, Array.IndexOf(fieldBytes, nullByte)));
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), latBytes, sbTemp.Length);

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Array.Clear(decMinBytes, 0, decMinBytes.Length);
                Array.Clear(degMinBytes, 0, degMinBytes.Length);
                Array.Clear(lonBytes, 0, lonBytes.Length);

                Utils.getField(inBytes, comma, 5, ref fieldBytes);
                sbTemp.Clear();
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(fieldBytes, 0, Array.IndexOf(fieldBytes, nullByte)));
                sbTemp.Append(',');

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 6, ref fieldBytes);
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(fieldBytes, 0, Array.IndexOf(fieldBytes, nullByte)));
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), lonBytes, sbTemp.Length);

                Array.Clear(fieldBytes, 0, fieldBytes.Length);
                Utils.getField(inBytes, comma, 18, ref fieldBytes);
                Array.Copy(fieldBytes, numSatsBytes, Array.IndexOf(fieldBytes, nullByte));
            }
            catch
            {
                parsedPosition.lat = 0.0;
                parsedPosition.lon = 0.0;
                Array.Clear(latBytes, 0, latBytes.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes("0.0"), latBytes, latBytes.Length);
                Array.Clear(lonBytes, 0, lonBytes.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes("0.0"), lonBytes, lonBytes.Length);
            }
        }


    }
}
