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
    {
        //public static SerialPort evk7Port = new SerialPort("COM5", 19200, Parity.None, 8, StopBits.One);

        private static object evk7Lock = new object();

        private static StructQueue.qStruct qS = new StructQueue.qStruct();

        //     private static byte[] evk7Chars = new byte[256];
        private static StringBuilder sbTemp = new StringBuilder(128);
        private static I2CDevice.Configuration evk7I2CConfig = new I2CDevice.Configuration(0x42, 100);
        //     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[] ffAddr = new byte[1] { 0xFF };
        //    private static byte[] ffValue = new byte[1];
        private static int evk7CharsIndex = 0;
        private static int numBytes = 0;

        public static byte[] readI2C()
        {
            Program.g400I2C.Config = evk7I2CConfig;

            //Create I2C transaction to read number of bytes available
            numBytesXAction[0] = I2CDevice.CreateWriteTransaction(numBytesAddr);
            numBytesXAction[1] = I2CDevice.CreateReadTransaction(numBytesValue);

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

            for (int i = 0; i < 10; i++)
            {
                if (Program.g400I2C.Execute(numBytesXAction, 1000) == 0)
                {
                    sbTemp.Clear();
                    sbTemp.Append("ERROR: GPS I2C bytes available transaction");
                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                    qS.qRecordType = StructQueue.QRecordType.EVK7;
                    qS.errorCode = -1;
                    lock (evk7Lock) { Program.sQueue.Enqueue(qS); }

                    numBytes = 0;
                }
                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(500);
            }

            //If we've got bytes in the GPS buffer
            if ((numBytes > 0) && (numBytes < 0xFFFF))
            {
                //Just in case all the bytes aren't in the GPS buffer
                Thread.Sleep(200);

                // Create I2C transaction to get buffer FF value
                readFFXAction[0] = I2CDevice.CreateWriteTransaction(ffAddr);
                readFFXAction[1] = I2CDevice.CreateReadTransaction(ffValue);

                evk7CharsIndex = 0;
                //Read for no more than 1 second
                for (int i = 0; i < 1000; i++)
                {
                    //Read the FF register
                    if (Program.g400I2C.Execute(readFFXAction, 1000) == 0)
                    {
                        sbTemp.Clear();
                        sbTemp.Append("ERROR: GPS I2C readFF transaction");
                        Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                        qS.qRecordType = StructQueue.QRecordType.EVK7;
                        qS.errorCode = -1;
                        lock (evk7Lock) { Program.sQueue.Enqueue(qS); }
                    }
                    else
                    {
                        //Wait for the '$' STX character
                        if (evk7CharsIndex == 0)
                        {
                            if (ffValue[0] == 36)
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                            }
                        }
                        //After we've got the STX character
                        else
                        {
                            //Break out of loop when LF ETX shows up 
                            if (ffValue[0] == 10)
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                                break;
                            }
                            else
                            {
                                evk7Chars[evk7CharsIndex] = ffValue[0];
                                evk7CharsIndex = evk7CharsIndex + 1;
                            }
                            //Break and deal with error if we don't get a LF character
                            if (evk7CharsIndex >= evk7Chars.Length - 5)
                            {
                                //TODO: Increment error counter
                                break;
                            }
                        }
                    }
                }
                Thread.Sleep(1);
            }

            qS.timeStamp = DateTime.Now;
            qS.state = Program.CPFState;

            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;
            lock (evk7Lock) { Program.sQueue.Enqueue(qS); }

            return (evk7Chars);
        }

        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 evk7PUBXCmd = "$PUBX,00*33\r\n";
        public static void getPUBX()
        {
            Array.Clear(txBytes, 0, txBytes.Length);
            Array.Copy(UTF8Encoding.UTF8.GetBytes(evk7PUBXCmd), txBytes, evk7PUBXCmd.Length);
            //evk7Port.Write(txBytes, 0, evk7PUBXCmd.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.CPFState;

                    //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;
                    lock (evk7Lock) { 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)
        {
            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 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.CPFState;

                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;
                lock (evk7Lock) { Program.sQueue.Enqueue(qS); }
            }

            return (returnSB);
        }

    }
}