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;

        #region readI2C_stuff
        //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);
        // }
        #endregion

        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, evk7PUBX00Cmd.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;

        public static void parsePUBX(byte[] inBytes)
        {
            // Initialize the PUBX structure
            //
            Program.PUBX.HDOP = 0;

            sbTemp.Clear();
            sbTemp.Append("Parsing: ");
            sbTemp.Append(UTF8Encoding.UTF8.GetChars(inBytes));

            Debug.Print(sbTemp.ToString());
            //EngrLogger.writeToColumns(sbTemp);


            // $PUBX,00,time,lat,NS,long,EW,altRef,navStat,hAcc,vAcc,SOG,COG,vVel,diffAge,HDOP,VDOP,TDOP,numSvs,reserved,DR,*cs<CR><LF>
            //                  U                    B                      X                     :
            if ((inBytes[0] == 85) && (inBytes[1] == 66) && (inBytes[2] == 88) ) //&& (inBytes[3] == 66) && (inBytes[4] == 88)) //&& (inBytes[5] == 58))
            {
                // Extract the SBDI message parameters
                int sbdiState = 0;
                int NumCommasSkip = 0;
                int negative = 0;
                //int exp = 0;
                double stupid = 10;

                for (int j = Array.IndexOf(inBytes, (byte)'X') + 1; j < inBytes.Length; j++)
                {
                    switch (sbdiState)
                    {
                        // Skip to HDOP
                        case (0):
                            if (inBytes[j] == 44)   // found a comma (ASCII 44), move on from initial MO status state to MOMSN sbdiState
                                NumCommasSkip += 1;
                            if (NumCommasSkip == 15) sbdiState += 1;
                            break;

                        // Get HDOP left side of decimal
                        case (1):
                            if (inBytes[j] == 46)   // found the decimal point, move on to right side sbdiState
                                sbdiState += 1;
                            else if (inBytes[j] == 45) //found a negative sign
                                negative = 1;
                            else
                                if (inBytes[j] >= 48 && inBytes[j] <= 57) //skip over anything not 0-9
                                    Program.PUBX.HDOP = Program.PUBX.HDOP * 10 + ((double)inBytes[j] - 48);
                            break;

                        // Get HDOP right side
                        case (2):
                            if (inBytes[j] == 44)   // found a comma, move on to looking at negative
                            {
                                sbdiState += 1;
                                stupid = 10;
                            }
                            else
                            {
                                if (inBytes[j] >= 48 && inBytes[j] <= 57) //skip over anything not 0-9
                                {
                                    Program.PUBX.HDOP = Program.PUBX.HDOP + (((double)inBytes[j] - 48) / stupid);
                                    stupid = stupid * 10;
                                    //exp += 1;
                                }
                            }
                            break;
                        default:
                            break;
                    }  // switch
                }  // for loop

                if (negative == 1) Program.PUBX.HDOP = Program.PUBX.HDOP * -1;
                //Debug.Print(result.ToString("F3"));

            }
        }

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

        public static byte[] latBytes = new byte[16];
        public static byte[] lonBytes = 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);
            }
            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);
            }
        }

        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)
        {
            //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 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();
            if (evk7Chars.Length == 0) { return (returnSB); };//bail if nothing came back

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

    }
}