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[] messageBytes = 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 token = space;

        private static int pressureBytesIndex = 0;
        private static int portBytesToRead;
        private static int messageBytesIndex = -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, configFile.SBE41SamplePeriod);
            //Stop the timer by setting 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;
            }
        }

        public static double pressureTimestamp;
        private static int readCounter = 0;
        private static int messageBytesLength = 0;

        private static byte ETX = LF;

        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);
                Array.Copy(portReadBytes, 0, messageBytes, messageBytesLength, portBytesToRead);
                messageBytesLength = messageBytesLength + portBytesToRead;
                readCounter = readCounter + 1;

                //sbInArray.Clear();
                //sbInArray.Append(UTF8Encoding.UTF8.GetChars(portReadBytes));
                //            Debug.Print(sbInArray.ToString());
            }
            catch
            {
                Debug.Print("SBE41 Read Exception");          //TODO do something smarter here
            }

            if (messageBytes[messageBytesLength - 1] == ETX)       //process the message when we get the ETX character
            {
                if (messageBytesLength >= 8)
                {
                    pressureTimestamp = (double)Utility.GetMachineTime().Ticks / (double)TimeSpan.TicksPerSecond;
                    //sbTemp.Clear();
                    //sbTemp.Append(UTF8Encoding.UTF8.GetChars(messageBytes));
                    //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() +
                    //            "  SBE41 Message = " + sbTemp.ToString());
                    messageNum = messageNum + 1;

                    Array.Clear(message, 0, message.Length);
                    Array.Copy(header, message, header.Length);
                    Array.Copy(messageBytes, 0, message, header.Length, messageBytesLength);
                    lock (SBE41Lock) { Program.MessageQueue.Enqueue(message); }

                    Array.Clear(portReadBytes, 0, portReadBytes.Length);
                    Array.Clear(messageBytes, 0, messageBytes.Length);
                    messageBytesLength = 0;
                }
                else
                {
                    messageBytesLength = 0;
                    Array.Clear(messageBytes, 0, messageBytes.Length);
                }
            }

            //endTime = Utility.GetMachineTime();
            //diffTime = endTime - startTime;
            //Debug.Print("SBE41 Event Handler time diff = " + diffTime.ToString());
        }

        private static double parsedPressure = double.NaN;
        private static StringBuilder sbInArray = new StringBuilder(64);

        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));
            parsedPressure = double.Parse(sbTemp.ToString());
            //Debug.Print(parsedPressure.ToString("f2"));
            Array.Clear(pressureBytes, 0, pressureBytes.Length);
            pressureBytesIndex = 0;

            return (parsedPressure);
        }

        private static bool initializeFilter = true;

        //Designed with Matlab fdesign.differentiator('N,Fp,Fst,Ast', 6, .4, .45, 20)
        private static double[] filterCoef = {  0.090171043318574,   0.151330004225777,   0.093823854938812,
                                                0.0,                -0.093823854938812,  -0.151330004225777,
                                               -0.090171043318574};

        //Designed with Matlab fdesign.differentiator('N,Fp,Fst',20,.25,.45)
        //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 LPFPressure;
            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;
                }
            }

            //TODO Eventually need a LPF for pressure
            SBE41LPFResults.LPFPressure = 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 positively down but velocity in the down direction needs to be negative
            SBE41LPFResults.diffVel = (velFilterDelayLine[1] - velFilterDelayLine[0]) / dt;  //Don't need the -1.0 because we're subtracting  ( (t-1) - t0 )

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

            //TODO Check for bottom contact
            if (SBE41LPFResults.LPFAcc > configFile.hitBottomAccThreshold)
            {
                Elmo.stopMotor();
                Program.hitBottom = true;
            }
            else
            {
                Program.hitBottom = false;
            }
            return (SBE41LPFResults);
        }


        //private static void GetPressure()
        //{
        //    double pressure;          // in Decibars

        //    _port.Close();
        //    _serialTX = new OutputPort(Pin.IO6, true);
        //    AssertDataRequest();
        //    AssertSpace_P_OnlyMark_CTD();
        //    Thread.Sleep(50);
        //    NegateDataRequest();
        //    Thread.Sleep(150);
        //    _serialTX.Write(false);
        //    Thread.Sleep(150);
        //    NegateSpace_P_OnlyMark_CTD();
        //    _serialTX.Write(true);
        //    Thread.Sleep(50);
        //    _serialTX.Dispose();
        //    _port.Open();
        //    GetResponse();
        //    Thread.Sleep(100);
        //    //string s = new string(System.Text.Encoding.UTF8.GetChars(_response));
        //    //Debug.Print(s);

        //    bool parsed = UTILS.NumberParser.TryParseDouble(_response, out pressure);
        //    Assert.IsTrue(parsed, "Parsing CTD data Failed");
        //    if (pressure < -0.25)
        //    {
        //        Debug.Print("Parse issue");
        //    }
        //    filterPressure(pressure);                           //Added by GM
        //    PlatformVelocityController.Pressure = pressure;
        //}
    }
}



//private static void SBE41TimerCallback(object state)
//{
//    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;
//    Thread.Sleep(50);

//    DR.Write(false);
//    // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 1  DR = false  SM = false   TX = true");
//    timerState = 2;
//    Thread.Sleep(150);

//    serialTX.Write(false);
//    // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 2  DR = false  SM = false   TX = false");
//    timerState = 3;
//    Thread.Sleep(150);

//    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;
//    Thread.Sleep(50);

//    serialTX.Dispose();
//    SBE41Port.Open();
//    //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State 3  DR = false  SM = true    TX = true");
//    timerState = 0;
//}