using System;
using Microsoft.SPOT;

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

using Microsoft.SPOT.Hardware;
using Microsoft.SPOT.IO;

using GHI.Hardware.EMX;

namespace miniCPF
{
    class SBE41
    {
        private static OutputPort DR = new OutputPort(Pin.IO15, false);
        private static OutputPort SPM = new OutputPort(Pin.IO46, true);
        private static OutputPort serialTX;

        private static double pressure = Double.NaN;
        private static byte[] pressureByteArray = new byte[8];
        private static int pressureByteArrayIndex = 0;

        private static int portBytesToRead;
        private static byte[] portReadBytes = new byte[1024];
        private static byte[] portInByteArray = new byte[240];
        private static byte[] space = new byte[1] { 32 };
        private static int portInByteArrayIndex = -1;

        private static byte[] header = UTF8Encoding.UTF8.GetBytes("SBE41 ");
        private static byte[] message = new byte[256];

        private static int messageNum;
        private static SerialPort SBE41Port = new SerialPort("com4", 9600, Parity.None, 8, StopBits.One);

        private static Timer SBE41Timer = null;
        private static int timerState = 0;

        private static StringBuilder sb = new StringBuilder(256);

        private static Object SBE41Lock = new Object();

        private static byte[] parserField = new byte[16];
        private static int parserI = 0;
        private static int parserJ = 0;
        private static int decimalPtIndex = -1;
        private static double accum = 0;
        private static int lastByte = 0;
        private static int fieldSign = 1;
        private static byte token = 32;         //token (separator) = space
        private static byte CR = 13;
        private static byte comma = 44;
        private static byte negativeSign = 45;     //negative sign
        private static byte decimalPt = 46;     //decimal point
        private static int inArrayLength = 0;

        private static byte[] loggerMessage = new byte[256];
        private static int tokenCounter = 0;
        private static int loggerMessageIndex = 0;

        public static void Init()
        {
            SBE41Port.DataReceived += new SerialDataReceivedEventHandler(portDataReceivedHandler);

            //Initialize();
            SBE41Port.ReadTimeout = 5000;
            SBE41Port.WriteTimeout = 20;
            SBE41Port.Open();

            //ReleaseWakeUp();
            DR.Write(false);
            SPM.Write(true);

            SBE41Timer = new Timer(new TimerCallback(SBE41TimerCallback), null, 500, 2000);
            //Stop the timer by set SBE41Timer = null

            Array.Copy(header, message, header.Length);
        }

        private static void SBE41TimerCallback(object state)
        {
            switch (timerState)
            {
                case 0:
                    SBE41Port.Close();
                    serialTX = new OutputPort(Pin.IO6, true);
                    DR.Write(true);
                    SPM.Write(false);
                    // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 0  DR = true   SM = false   TX = true");
                    timerState = 1;
                    SBE41Timer.Change(0, 50);
                    break;
                case 1:
                    DR.Write(false);
                    // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 1  DR = false  SM = false   TX = true");
                    timerState = 2;
                    SBE41Timer.Change(0, 150);
                    break;
                case 2:
                    serialTX.Write(false);
                    // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 2  DR = false  SM = false   TX = false");
                    timerState = 3;
                    SBE41Timer.Change(0, 150);
                    break;
                case 3:
                    SPM.Write(true);
                    serialTX.Write(true);
                    //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 3  DR = false  SM = true    TX = true");
                    timerState = 4;
                    SBE41Timer.Change(0, 50);
                    break;
                case 4:
                    serialTX.Dispose();
                    SBE41Port.Open();
                    //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 3  DR = false  SM = true    TX = true");
                    timerState = 0;
                    SBE41Timer.Change(0, 1600);
                    break;
            }
        }

        private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
        {
            portBytesToRead = SBE41Port.BytesToRead;

            try
            {
                SBE41Port.Read(portReadBytes, 0, portBytesToRead);

            }
            catch
            {
                Debug.Print("SBE41 Read Exception");          //TODO do something smarter here
            }

            for (int i = 0; i < portBytesToRead; i++)
            {
                if (portInByteArrayIndex == -1)    //Check to make sure first character is = LF
                {
                    if (portReadBytes[i] == 10)  //only move on to byte index 1 after we get a LF
                    {
                        portInByteArrayIndex = portInByteArrayIndex + 1;
                    }
                }
                else
                {
                    portInByteArray[portInByteArrayIndex] = portReadBytes[i];
                    if (portInByteArray[portInByteArrayIndex] == 13)
                    {
                        messageNum = messageNum + 1;

                        Array.Copy(portInByteArray, 1, message, header.Length, portInByteArrayIndex);
                        lock (SBE41Lock) { Program.MessageQueue.Enqueue(message); }

                        Array.Clear(portReadBytes, 0, portReadBytes.Length);
                        Array.Clear(portInByteArray, 0, portInByteArray.Length);
                        portInByteArrayIndex = -1;
                    }
                    else
                    {
                        portInByteArrayIndex = portInByteArrayIndex + 1;
                    }
                }
            }
        }

        public static byte[] parse(byte[] inArray)          //TODO This should be it's own class that everyone else can call
        {
            parserJ = -1;
            fieldSign = 1;
            decimalPtIndex = -1;

            inArrayLength = Array.IndexOf(inArray, 0);

            for (parserI = 0; parserI < inArrayLength; parserI++)
            {
                //Find the index of the first token
                if (parserJ < 0)
                {
                    if (inArray[parserI] == token)
                    {
                        parserJ = parserJ + 1;
                        // get rid of any consecutive tokens
                        while (inArray[parserI + 1] == token)
                            parserI = parserI + 1;
                    }
                }
                else
                {
                    //When the second token shows up, turn the bytes into a number
                    if ((inArray[parserI] == token) || (inArray[parserI] == CR) || (inArray[parserI] == comma))
                    {
                        accum = 0.0;
                        lastByte = parserJ;

                        //Handle fields that don't have any decimal points
                        if (decimalPtIndex < 0)
                        {
                            decimalPtIndex = lastByte - 1;
                        }

                        //Turn the field into a number
                        for (parserJ = 0; parserJ < lastByte; parserJ++)
                        {
                            accum = accum + ((parserField[parserJ] - 48) * System.Math.Pow(10, (decimalPtIndex - parserJ)));
                        }
                        accum = fieldSign * accum;
                        pressure = accum;
                        break;

                        //Debug.Print("Pressure = " + pressure.ToString("F5"));
                    }
                    else   // put the number in the field unless it is a decimal point or a negative sign
                    {
                        pressureByteArray[pressureByteArrayIndex] = inArray[parserI];
                        //Look for a decimal point
                        if (inArray[parserI] == decimalPt)
                        {
                            decimalPtIndex = parserJ - 1;
                        }
                        //Look for a negative sign
                        else if (inArray[parserI] == negativeSign)
                        {
                            fieldSign = -1;
                        }
                        else
                        {
                            parserField[parserJ] = inArray[parserI];
                            parserJ = parserJ + 1;
                        }
                    }
                }
            }
            return (buildSBELoggerMessage(pressureByteArray));
        }

        private static byte[] buildSBELoggerMessage(byte[] pressureByteArray)
        {
            Array.Clear(loggerMessage, 0, loggerMessage.Length);
            loggerMessage = Logger.getNewLogArrayHeader(UTF8Encoding.UTF8.GetBytes("SBE Message"));

            tokenCounter = 0;
            loggerMessageIndex = Array.IndexOf(loggerMessage, 0);
            for (int i = 0; i < (int)Logger.ColumnNums.lastColumn; i++)
            {
                //loggerMessage[loggerMessageIndex] = Logger.token;
                loggerMessageIndex = loggerMessageIndex + 1;
                if (tokenCounter == (int)Logger.ColumnNums.pressure)
                {
                    Array.Copy(pressureByteArray, 0, loggerMessage, loggerMessageIndex, (Array.IndexOf(pressureByteArray, 0) + 1));
                    loggerMessageIndex = loggerMessageIndex + Array.IndexOf(pressureByteArray, 0);
                }
            }
            return loggerMessage;
        }

    }
}