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(UTF8Encoding.UTF8.GetChars(evk7Chars));
            //Debug.Print("GPS I2C read = " + 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(UTF8Encoding.UTF8.GetChars(messageBytes, 0, messageIndex - 1));
                    //Debug.Print("EVK7 Message = " + 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;
                }
            }
        }
    }
}