using System;
using Microsoft.SPOT;

namespace CPF
{
    class SavedStuff
    {
                    case (CPFStates.Descend):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            parked = false;
                            depthTableNum = 0;
                            //Make sure the PV set point is negative regardless of typos etc.
                            PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;
                        }

                        //A positive park time means a transition to park

                        if (!parked)
                        {
                            if (pressure >= MissionConfigFile.descendTable[depthTableNum].depth)
                            {
                                //check to see if this is a park station
                                //if it is, take a sample and set park = true
                                if (MissionConfigFile.descendTable[depthTableNum].parkTime > TimeSpan.Zero)
                                {
                                    PVSetPoint = 0.0;
                                    //DepthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth;
                                    //runPVPID = false;
                                    //BuoyancyControl.resetDepthPID();
                                    //runDepthPID = true;

                                    //used as reference for exiting park mode
                                    mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    //used as reference for when to sample during park mode
                                    lastParkSampleTime = mtParkStartTime;
                                    sbTemp.Clear();
                                    sbTemp.Append("Starting park period = ");
                                    sbTemp.Append(MissionConfigFile.descendTable[depthTableNum].parkTime.ToString());
                                    EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString()));
                                    //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); }
                                    InstrumentHandler.sampleInstruments(depthTableNum);         //TODO Probably needs to be done in it's own thread or with a event
                                    parked = true;
                                }
                                //else just take a sample
                                else
                                {
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                    depthTableNum = depthTableNum + 1;
                                    if (depthTableNum < MissionConfigFile.descendTable.Length)
                                        PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                }
                            }
                        }
                        else //If we are parked
                        {
                            mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                            timeDiff = mtTimeNow - mtParkStartTime;

                            //check if it is time to take a sample based on time while parked
                            if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                            {
                                lastParkSampleTime = mtTimeNow;
                                InstrumentHandler.sampleInstruments(depthTableNum);
                            }
                            //check to see if park time has expired
                            if (timeDiff >= MissionConfigFile.descendTable[depthTableNum].parkTime)
                            {
                                depthTableNum = depthTableNum + 1;
                                if (depthTableNum < MissionConfigFile.descendTable.Length)
                                    PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                //runDepthPID = false;
                                //BuoyancyControl.resetPVPID();
                                runPVPID = true;
                                parked = false;
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString()));
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); }
                            }
                        }

                        //check to see if this is that last descend station
                        if (depthTableNum >= MissionConfigFile.descendTable.Length)
                        {
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            PVSetPoint = 0.0;
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                        }
                        break;
                    case (CPFStates.Ascend):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            parked = false;
                            depthTableNum = 0;
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;

                            //Check to see if we need to descend to first ascend depth
                            if (pressure > MissionConfigFile.ascendTable[depthTableNum].depth)
                            {
                                //Make sure the PV set point is positive regardless of typos etc.
                                PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                descendToFirstAscend = false;
                            }
                            else
                            {
                                //need to descend to first ascend station
                                PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                descendToFirstAscend = true;
                            }
                        }

                        //Do all this if were not at the last depth in the ascend table if we are, just check for the surface
                        if (depthTableNum < MissionConfigFile.ascendTable.Length - 1)
                        {
                            //First deal with descend to first ascend station
                            if (descendToFirstAscend)
                            {
                                if (pressure >= MissionConfigFile.ascendTable[depthTableNum].depth)
                                {
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                    depthTableNum = depthTableNum + 1;
                                    if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                        PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                    descendToFirstAscend = false;
                                }
                            }
                            else
                            {
                                if (!parked)
                                {
                                    //check to see if we've passed the target depth
                                    if (pressure <= MissionConfigFile.ascendTable[depthTableNum].depth)
                                    {
                                        //First check to see if it is a park depth
                                        if (MissionConfigFile.ascendTable[depthTableNum].parkTime > TimeSpan.Zero)
                                        {
                                            PVSetPoint = 0.0;
                                            //DepthSetPoint = MissionConfigFile.ascendTable[depthTableNum].depth;
                                            //runPVPID = false;
                                            //BuoyancyControl.resetDepthPID();
                                            //runDepthPID = true;

                                            //used as reference for exiting park mode
                                            mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                            //used as reference for when to sample during park mode
                                            lastParkSampleTime = mtParkStartTime;

                                            sbTemp.Clear();
                                            sbTemp.Append("Starting park period = ");
                                            sbTemp.Append(MissionConfigFile.ascendTable[depthTableNum].parkTime.ToString());
                                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString()));
                                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); }
                                            parked = true;

                                            //Now take a sample but don't move on to next sample depth until park period is over
                                            InstrumentHandler.sampleInstruments(depthTableNum);
                                        }
                                        //If it isn't a park depth, take a sample and increment the depth table number
                                        else
                                        {
                                            InstrumentHandler.sampleInstruments(depthTableNum);
                                            depthTableNum = depthTableNum + 1;
                                            if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                                PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                        }
                                    }
                                }
                                else
                                {
                                    //If we are parked, check the time for sampling and to see if park time has ended
                                    mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    timeDiff = mtTimeNow - mtParkStartTime;

                                    //check if it is time to take a sample based on time while parked
                                    if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                                    {
                                        lastParkSampleTime = mtTimeNow;
                                        InstrumentHandler.sampleInstruments(depthTableNum);
                                    }
                                    //check to see if park time has expired
                                    if (timeDiff >= MissionConfigFile.ascendTable[depthTableNum].parkTime)
                                    {
                                        depthTableNum = depthTableNum + 1;
                                        //runDepthPID = false;
                                        //BuoyancyControl.resetPVPID();
                                        runPVPID = true;
                                        if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                            PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                        parked = false;
                                        EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString()));
                                        //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); }
                                    }
                                }
                            } //At this point we're aimed at the next sample depth
                        }
                        else
                        {
                            //check to see if we've hit the surface
                            if (pressure < configFile.surfacePressure)
                            {
                                Elmo.stopMotor();
                                runPVPID = false;
                                runDepthPID = false;
                                stateEntry = true;
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                                CPFState = CPFStates.Initialize;
                                stateTimedout = false;
                            }
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.Anchor):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            initialAnchorBP = bellowsPosition;
                        }

                        //normal state exit criteria goes here
                        //if (true)
                        //{
                        //    stateEntry = true;
                        //    stateTimedout = false;
                        //   EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                        //    //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                        //    CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod);
                        //    CPFState = CPFStates.DeAnchor;
                        //}

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.DeAnchor;
                        }
                        break;
    }
}
