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 SerialPort SBE41Port = new SerialPort("COM4", 9600, Parity.None, 8, StopBits.One);

        private static Timer SBE41Timer = null;

        private static OutputPort DR = new OutputPort(Pin.IO15, false);
        private static OutputPort SPM = new OutputPort(Pin.IO46, true);
        private static OutputPort serialTX;

        private static byte[] pressureBytes = new byte[16];
        private static byte[] portReadBytes = new byte[1024];
        private static byte[] portInBytes = new byte[240];
        private static byte[] header = UTF8Encoding.UTF8.GetBytes("CTD ");
        private static byte[] message = new byte[256];

        private static byte space = 32;
        private static byte CR = 13;
        private static byte LF = 10;
        private static byte comma = 44;
        private static byte STX = LF;
        private static byte ETX = CR;
        private static byte token = space;

        private static int pressureBytesIndex = 0;
        private static int portBytesToRead;
        private static int portInBytesIndex = -1;
        private static int messageNum;
        private static int timerState = 0;
        private static int parserI = 0;
        private static int inArrayLength = 0;

        private static Object SBE41Lock = new Object();

        private static StringBuilder sbTemp = new StringBuilder(256);

        //private static TimeSpan startTime;
        //private static TimeSpan endTime;
        //private static TimeSpan diffTime;

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

            dt = configFile.SBE41SamplePeriod / 1000;  //dt needs to be in seconds SBE41SamplePeriod is in milliSeconds

            //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
        }

        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, (configFile.SBE41SamplePeriod - 400));
                    break;
            }
        }

        private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
        {
            //       startTime = Utility.GetMachineTime();

            //TODO might be able to do this in a way that if we start in the middle of a message, 
            // we can resynch so subsequent reads get the whole message.
            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 (portInBytesIndex == -1)    //Check to make sure first character is = LF
                {
                    if (portReadBytes[i] == STX)  //only move on to byte index 1 after we get the STX character
                    {
                        portInBytesIndex = portInBytesIndex + 1;
                    }
                }
                else
                {
                    portInBytes[portInBytesIndex] = portReadBytes[i];
                    if (portInBytes[portInBytesIndex] == ETX)       //process the message when we get the ETX character
                    {
                        messageNum = messageNum + 1;

                        Array.Clear(message, 0, message.Length);
                        Array.Copy(header, message, header.Length);
                        Array.Copy(portInBytes, 1, message, header.Length, portInBytesIndex);
                        lock (SBE41Lock) { Program.MessageQueue.Enqueue(message); }

                        Array.Clear(portReadBytes, 0, portReadBytes.Length);
                        Array.Clear(portInBytes, 0, portInBytes.Length);
                        portInBytesIndex = -1;
                    }
                    else
                    {
                        portInBytesIndex = portInBytesIndex + 1;
                    }
                }
            }
            //endTime = Utility.GetMachineTime();
            //diffTime = endTime - startTime;
            //Debug.Print("SBE41 Event Handler time diff = " + diffTime.ToString());
        }

        public static double parse(byte[] inArray)
        {
            inArrayLength = Array.IndexOf(inArray, 0);

            parserI = Array.IndexOf(inArray, token);

            while ((inArray[parserI] != CR) && (inArray[parserI] != comma) && (parserI < inArrayLength))
            {
                pressureBytes[pressureBytesIndex] = inArray[parserI];
                pressureBytesIndex = pressureBytesIndex + 1;
                parserI = parserI + 1;
            }
            sbTemp.Clear();
            sbTemp.Append(UTF8Encoding.UTF8.GetChars(pressureBytes));
            Array.Clear(pressureBytes, 0, pressureBytes.Length);
            pressureBytesIndex = 0;
            return (double.Parse(sbTemp.ToString()));
        }

        private static bool initializeFilter = true;

        private static double[] filterCoef = {  -0.010316320303577,   0.002862341441897,   0.026430189271868,   0.007331989949475,  -0.025727381182904,
                                                -0.054710187198367,  -0.007726535812123,   0.094047649266387,   0.174529230732090,   0.139815454232672,                            
                                                0.000000000000000,  -0.139815454232672,  -0.174529230732090,  -0.094047649266387,   0.007726535812123,
                                                0.054710187198367,   0.025727381182904,  -0.007331989949475,  -0.026430189271868,  -0.002862341441897,
                                                0.010316320303577};
        private static int numCoef = filterCoef.Length;

        private static double accum = 0.0;
        private static double dt;
        private static double[] velFilterDelayLine = new double[numCoef];
        private static double[] accFilterDelayLine = new double[numCoef];

        private static int filterI;

        public struct LPFResults
        {
            public double LPFVel;
            public double LPFAcc;
            public double diffVel;
        }
        private static LPFResults SBE41LPFResults;
        
        public static LPFResults LPFPressure(double pressureSample)
        {
            if (initializeFilter)
            {
                for (filterI = 0; filterI < numCoef; filterI++)
                {
                    velFilterDelayLine[filterI] = pressureSample;
                }
            }
            accum = 0.0;
            velFilterDelayLine[0] = pressureSample;
            for (filterI = 0; filterI < numCoef; filterI++)
            {
                accum = accum + (filterCoef[filterI] * velFilterDelayLine[filterI]);
            }
            SBE41LPFResults.LPFVel = -1.0 * accum / dt;         //-1.0 because pressure increases down but velocity down needs to be negative
            SBE41LPFResults.diffVel = (velFilterDelayLine[1] - velFilterDelayLine[0]) / dt;

            if (initializeFilter)
            {
                for (filterI = 0; filterI < numCoef; filterI++)
                {
                    accFilterDelayLine[filterI] = SBE41LPFResults.LPFVel;
                    initializeFilter = false;
                }
            }
            accum = 0.0;
            accFilterDelayLine[0] = SBE41LPFResults.LPFVel;
            for (filterI = 0; filterI < numCoef; filterI++)
            {
                accum = accum + (filterCoef[filterI] * accFilterDelayLine[filterI]);
            }
            SBE41LPFResults.LPFAcc = accum / dt;
            //Debug.Print("Pressure = " + pressureSample.ToString("F6") +
            //            "   Difference Velocity = " + platformDiffVel.ToString("f4") +
            //            "   Filtered Velocity = " + platformLPFVel.ToString("f4") +
            //            "   Filtered Accel = " + platformLPFAcc.ToString("f4"));

            for (filterI = numCoef - 2; filterI >= 0; filterI--)
            {
                velFilterDelayLine[filterI + 1] = velFilterDelayLine[filterI];
                accFilterDelayLine[filterI + 1] = accFilterDelayLine[filterI];
            }

            return (SBE41LPFResults);
        }

    }
}
