using System;
using Microsoft.SPOT;

namespace CPF
{
    class oldStuff
    {
    }
}
//public static byte[] addPair(ColumnNums columnNum, byte[] valueByteArray, byte[] inMessage)
//{
//    tabIndex = 0;
//    LFIndex = -1;
//    tabCounter = 0;

//    //find the index of the desired token
//    while (tabCounter != (int)columnNum)
//    {
//        if (inMessage[tabIndex] == token)
//            tabCounter = tabCounter + 1;
//        tabIndex = tabIndex + 1;
//    }
//    tabIndex = tabIndex + 1;    //Column numbers start at 0 and don't include the token between data and time

//    //find the index of the terminating LF
//    LFIndex = Array.IndexOf(inMessage, LF, 0);
//    numValueArrayBytes = Array.IndexOf(valueByteArray, 0, 0);

//    //Clear the out message
//    Array.Clear(outMessage, 0, outMessage.Length);

//    //Copy everything in the in message up to the desired token into out message
//    Array.Copy(inMessage, 0, outMessage, 0, tabIndex);
//    sb.Clear();
//    sb.Append(UTF8Encoding.UTF8.GetChars(outMessage));
//    Debug.Print(sb.ToString());

//    //Copy the value into out message after the desired token
//    Array.Copy(valueByteArray, 0, outMessage, tabIndex, numValueArrayBytes);
//    sb.Clear();
//    sb.Append(UTF8Encoding.UTF8.GetChars(outMessage));
//    Debug.Print(sb.ToString());

//    //Copy everything in the in message after the desired token into out message after the value byte array
//    Array.Copy(inMessage, tabIndex + 1, outMessage, tabIndex + numValueArrayBytes, LFIndex - tabIndex);
//    sb.Clear();
//    sb.Append(UTF8Encoding.UTF8.GetChars(outMessage));
//    Debug.Print(sb.ToString());

//    return outMessage;
//}



        //Old version that works on the single channel ADuC data message
        //private static void ADuCPortDataReceived(object sender, SerialDataReceivedEventArgs e)
        //{
        //    ADuCPortBytesToRead = ADuCPort.BytesToRead;
        //    try
        //    {
        //        ADuCPort.Read(ADuCPortReadBytes, 0, ADuCPortBytesToRead);
        //    }
        //    catch
        //    {
        //        Debug.Print("ADuC Read Exception");          //TODO do something smarter here
        //    }

        //    for (int i = 0; i < ADuCPortBytesToRead; i++)
        //    {
        //        ADuCPortInByteArray[ADuCPortInByteArrayIndex] = ADuCPortReadBytes[i];
        //        if (ADuCPortInByteArrayIndex == 0)    //Check to make sure first character is = LF
        //        {
        //            if (ADuCPortInByteArray[0] == 10)  //only move on to byte index 1 after we get a LF
        //            {
        //                ADuCPortInByteArrayIndex = ADuCPortInByteArrayIndex + 1;
        //            }
        //        }
        //        else
        //        {
        //            if (ADuCPortInByteArray[ADuCPortInByteArrayIndex] == 13)
        //            {
        //                lock (ADuCLock)
        //                {
        //                    bellowsPosition = ((ADuCPortInByteArray[1] - 48.0) * 10.0) + (ADuCPortInByteArray[2] - 48.0) +
        //                        ((ADuCPortInByteArray[4] - 48.0) * 0.1) + ((ADuCPortInByteArray[5] - 48.0) * 0.01);
        //                    Debug.Print(" ");
        //                    Debug.Print("Bellows Position = " + bellowsPosition.ToString("F2"));
        //                    Debug.Print(" ");
        //                }
        //                ADuCPortInByteArrayIndex = 0;
        //            }
        //            else
        //            {
        //                ADuCPortInByteArrayIndex = ADuCPortInByteArrayIndex + 1;
        //            }
        //        }
        //    }
        //}

        //public static byte[] getNewLogArrayHeader(byte[] comment)
        //{
        //    timeNow = DateTime.Now;
        //    Array.Clear(tempQueueArray, 0, tempQueueArray.Length);

        //    sb.Clear();
        //    sb.Append(timeNow.ToString());

        //    // Year/Month/Day token
        //    for (int i = 6; i < 10; i++)
        //        tempQueueArray[i - 6] = (byte)sb[i];
        //    tempQueueArray[4] = (byte)'/';
        //    for (i = 0; i < 2; i++)
        //        tempQueueArray[i + 5] = (byte)sb[i];
        //    tempQueueArray[7] = (byte)'/';
        //    for (i = 3; i < 5; i++)
        //        tempQueueArray[i + 5] = (byte)sb[i];
        //    tempQueueArray[10] = token;

        //    // Hour:Minute:Second token
        //    for (i = 11; i < 19; i++)
        //        tempQueueArray[i] = (byte)sb[i];
        //    tempQueueArray[19] = token;

        //    // Add in comment
        //    //trim off the trailing empty array elements  
        //    //Note: comment array must be (byte)characters for this to work
        //    endOfCommentIndex = Array.IndexOf(comment, 0);
        //    if (endOfCommentIndex >= 0)
        //    {
        //        Array.Copy(comment, 0, tempQueueArray, 20, endOfCommentIndex);
        //        tempQueueArray[20 + endOfCommentIndex] = token;
        //        Array.Copy(Program.stateName[(int)Program.CPFState], 0, tempQueueArray, (21 + endOfCommentIndex), Program.stateName[(int)Program.CPFState].Length);
        //    }
        //    else
        //    {
        //        Array.Copy(comment, 0, tempQueueArray, 20, comment.Length);
        //        tempQueueArray[20 + comment.Length] = token;
        //        Array.Copy(Program.stateName[(int)Program.CPFState], 0, tempQueueArray, (21 + comment.Length), Program.stateName[(int)Program.CPFState].Length);
        //    }
        //    //tempCharArray = UTF8Encoding.UTF8.GetChars(tempQueueArray);

        //    return tempQueueArray;
        //}



        //private static byte[] buildSBELoggerMessage(byte[] pressureBytes)
        //{
        //    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(pressureBytes, 0, loggerMessage, loggerMessageIndex, (Array.IndexOf(pressureBytes, 0) + 1));
        //            loggerMessageIndex = loggerMessageIndex + Array.IndexOf(pressureBytes, 0);
        //        }
        //    }
        //    return loggerMessage;
        //}


//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;
//                pressure = double.Parse(UTF8Encoding.UTF8.GetChars(pressureByteArray).ToString());
//                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];
//                pressureByteArrayIndex = pressureByteArrayIndex + 1;
//                //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 void setStateNames()
      //  {
      //      //Need to make sure names are properly assigned to correct value every time states are changed
      //      stateName[(int)CPFStates.Initialize] = UTF8Encoding.UTF8.GetBytes("Initialize          ");
      //      sbStateNames[(int)CPFStates.Initialize] = new StringBuilder("Initialize          ");
      //      stateName[(int)CPFStates.PreMissionDelay] = UTF8Encoding.UTF8.GetBytes("PreMissionDelay     ");
      //      sbStateNames[(int)CPFStates.PreMissionDelay] = new StringBuilder("PreMissionDelay     ");
      //      stateName[(int)CPFStates.StartSurfaceOps] = UTF8Encoding.UTF8.GetBytes("StartSurfaceOps     ");
      //      stateName[(int)CPFStates.WaitingForTimeSynch] = UTF8Encoding.UTF8.GetBytes("WaitingForTimeSynch ");
      //      stateName[(int)CPFStates.SurfaceOps] = UTF8Encoding.UTF8.GetBytes("SurfaceOps          ");
      //      stateName[(int)CPFStates.StartAutoballast] = UTF8Encoding.UTF8.GetBytes("StartAutoBallast    ");
      //      stateName[(int)CPFStates.Autoballast] = UTF8Encoding.UTF8.GetBytes("Autoballast         ");
      //      stateName[(int)CPFStates.StartDescend] = UTF8Encoding.UTF8.GetBytes("StartDescend        ");
      //      stateName[(int)CPFStates.Descend] = UTF8Encoding.UTF8.GetBytes("Descend             ");
      //      stateName[(int)CPFStates.StartDescendPark] = UTF8Encoding.UTF8.GetBytes("StartDescendPark    ");
      //      stateName[(int)CPFStates.DescendPark] = UTF8Encoding.UTF8.GetBytes("DescendPark         ");
      //      stateName[(int)CPFStates.StartAscend] = UTF8Encoding.UTF8.GetBytes("StartAscend         ");
      //      stateName[(int)CPFStates.Ascend] = UTF8Encoding.UTF8.GetBytes("Ascend              ");
      //      stateName[(int)CPFStates.StartAscendPark] = UTF8Encoding.UTF8.GetBytes("StartAscendPark     ");
      //      stateName[(int)CPFStates.AscendPark] = UTF8Encoding.UTF8.GetBytes("AscendPark          ");
      //      stateName[(int)CPFStates.StartEmergencyAscend] = UTF8Encoding.UTF8.GetBytes("StartEmergencyAscend");
      //      stateName[(int)CPFStates.EmergencyAscend] = UTF8Encoding.UTF8.GetBytes("EmergencyAscend     ");
      //      stateName[(int)CPFStates.StartRecovery] = UTF8Encoding.UTF8.GetBytes("StartRecovery       ");
      //      stateName[(int)CPFStates.Recovery] = UTF8Encoding.UTF8.GetBytes("Recovery            ");
      //      stateName[(int)CPFStates.ProcessErrors] = UTF8Encoding.UTF8.GetBytes("ProcessErrors       ");
      //      stateName[(int)CPFStates.Exit] = UTF8Encoding.UTF8.GetBytes("Exit                ");
      //  }

//public static bool uploadDataFile(StringBuilder uploadFileName)
//{
//    uploadFile = new FileStream(uploadFileName.ToString(), FileMode.Open, FileAccess.Read);
//    uploadFileLength = uploadFile.Length;

//    //TODO the uploadStartComment and uploadEndComment should actually get written to the log file on SD as well as being uploaded
//    uploadStartComment.Clear();
//    uploadStartComment.Append("Start of Data File Upload = ");
//    uploadStartComment.Append(uploadFileLength.ToString());
//    uploadStartComment.AppendLine(" Bytes");
//    uploadStartPrefix = Logger.getPrefix(uploadStartComment);

//    uploadFile.Seek(0, SeekOrigin.Begin);

//    //send the file up in blocks
//    while ((uploadFile.Position + blockSize) <= uploadFileLength)
//    {
//        Array.Clear(blockToSend, 0, blockToSend.Length);
//        uploadFile.Read(blockToSend, 0, blockSize);
//        consolePort.Write(blockToSend, 0, blockSize);
//        consolePort.Flush();
//        while (consolePort.BytesToWrite > 128)  //TODO really need to get rid of this
//        {
//        }
//    }
//    //send the last bytes if any left
//    if (uploadFile.Position != uploadFileLength)
//    {
//        numBytesToSend = (int)(uploadFileLength - uploadFile.Position);
//        Array.Clear(blockToSend, 0, blockToSend.Length);
//        uploadFile.Read(blockToSend, 0, numBytesToSend);
//        consolePort.Write(blockToSend, 0, numBytesToSend);
//        consolePort.Flush();
//    }

//    //send the upload start and stop times
//    Array.Clear(blockToSend, 0, blockToSend.Length);
//    blockToSend = UTF8Encoding.UTF8.GetBytes(uploadStartPrefix.ToString());
//    consolePort.Write(blockToSend, 0, blockToSend.Length);
//    Array.Clear(blockToSend, 0, blockToSend.Length);
//    blockToSend = UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(uploadEndComment).ToString());
//    consolePort.Write(blockToSend, 0, blockToSend.Length);
//    consolePort.Flush();
//    while (consolePort.BytesToWrite > 0)  //TODO really need to get rid of this
//    {
//    }

//    uploadFile.Close();

//    return true;
//}


            //else if ((dequeueMessage[0] == (byte)'$') && (dequeueMessage[1] == (byte)'A') && (dequeueMessage[2] == (byte)'D') &&
            //            (dequeueMessage[3] == (byte)'U'))
            //    {
            //        ADuCVolts = ADuC.parseADuC(dequeueMessage);

            //        bellowsPosition = ADuCVolts[0];
            //        //Check bellows position, unless we're emergency ascending
            //        if (CPFState != CPFStates.EmergencyAscend)
            //        {
            //            if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition))
            //            {
            //                Elmo.stopMotor();
            //                CPFStateTimer = null;
            //                runPVPID = false;
            //                runDepthPID = false;
            //                Logger.write(Logger.ColumnNums.comment,
            //                            Logger.getPrefix(BPError));
            //                CPFState = CPFStates.EmergencyAscend;
            //            }
            //        }

            //        batteryBusVolts = (ADuCVolts[1] * configFile.aducCH1Scale) + configFile.aducCH1Offset;
            //        if (batteryBusVolts < configFile.minBatteryBusVolts)
            //        {
            //            Elmo.stopMotor();
            //            CPFStateTimer = null;
            //            runPVPID = false;
            //            runDepthPID = false;
            //            Logger.write(Logger.ColumnNums.comment,
            //                        Logger.getPrefix(BBVError));
            //            CPFState = CPFStates.EmergencyAscend;
            //        }

            //        Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(ADuCHeader),                       //TODO Should do this in a way that the message is time stamped at the source
            //                        Logger.ColumnNums.bellowsPosition, bellowsPosition,
            //                        Logger.ColumnNums.batteryBusVolts, batteryBusVolts,
            //                        Logger.ColumnNums.batteryBusAmps, ADuCVolts[2], Logger.ColumnNums.Amps12V, ADuCVolts[3],
            //                        Logger.ColumnNums.Amps33V, ADuCVolts[4]);
            //    }

//public enum ColumnNums : int
//{
//    //After the date and time and current state goes the comment
//    comment = 0,

//    //Then the buoyancy control stuff
//    pressureTimestamp,
//    pressure,
//    platformLPFVel,
//    platformDiffVel,
//    platformLPFAcc,
//    pressureAverage,
//    pressureVariance,
//    bellowsPosition,

//    //PID stuff
//    PID_setPoint,
//    PID_PV,
//    PID_y1,
//    PID_y2,
//    PID_I,
//    PID_v,
//    PID_u,
//    PID_uCPS,

//    //ADuC Stuff
//    batteryBusVolts,
//    batteryBusAmps,
//    Amps12V,
//    Amps33V,

//    //GPS Stuff
//    lat,
//    lon,
//    lastColumn,
//}


//EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(CN0254Header),                       //TODO Should do this in a way that the message is time stamped at the source
//                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
//                EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts,
//                EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps,
//                EngrLogger.ColumnNums.Amps12V, amps12V,
//                EngrLogger.ColumnNums.Amps33V, amps33V);



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


////It might be more consistent if the ADC results were returned on the queue
//if (readADC)
//{
//    CN0254Volts = CN0254.scanAll();
//    bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
//    batteryBusVolts = (CN0254Volts[1] * configFile.cn0254CH1Scale) + configFile.cn0254CH1Offset;
//    batteryBusAmps = (CN0254Volts[2] * configFile.cn0254CH2Scale) + configFile.cn0254CH2Offset;
//    amps12V = (CN0254Volts[3] * configFile.cn0254CH3Scale) + configFile.cn0254CH3Offset;
//    amps33V = (CN0254Volts[4] * configFile.cn0254CH4Scale) + configFile.cn0254CH4Offset;
//    if (verbosityLevel > 2)
//    {
//        EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(CN0254Header),                       //TODO Should do this in a way that the message is time stamped at the source
//                        EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
//                        EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts,
//                        EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps,
//                        EngrLogger.ColumnNums.Amps12V, amps12V,
//                        EngrLogger.ColumnNums.Amps33V, amps33V);
//    }
//    if (CPFState != CPFStates.EmergencyAscend)
//    {
//        //if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition))
//        //{
//        //    Elmo.stopMotor();
//        //    CPFStateTimer = null;
//        //    runPVPID = false;
//        //    runDepthPID = false;
//        //    EngrLogger.write(EngrLogger.ColumnNums.comment,
//        //                EngrLogger.getPrefix(BPError));
//        //    CPFState = CPFStates.EmergencyAscend;
//        //}

//        if (batteryBusVolts < configFile.minBatteryBusVolts)
//        {
//            Elmo.stopMotor();
//            CPFStateTimer = null;
//            runPVPID = false;
//            runDepthPID = false;
//            EngrLogger.write(EngrLogger.ColumnNums.comment,
//                        EngrLogger.getPrefix(BBVError));
//            CPFState = CPFStates.EmergencyAscend;
//        }
//    }
//    readADC = false;
//}

                        //if (stateEntry)
                        //{
                        //    Elmo.stopMotor();
                        //    runPVPID = false;
                        //    runDepthPID = false;
                        //    lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); }
                        //    stateEntry = false;
                        //    firstEngrTest = true;
                        //    stateTimedout = false;
                        //}