using System;
using System.IO.Ports;
using System.Text;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SWModules;
using HWModules;

namespace SensorModules
{
    public sealed class BuoyancyEngine
    {
        private MotherBoard mb;
        private ADC2485 bellowsPositionADC;
        private ElmoTwitter pumpMotor;
        private LheBoard lheBoard;
        
        private BuoyancyEngine()
        {
            mb = MotherBoard.Instance;

            bellowsPositionADC = new ADC2485(new I2CDevice.Configuration(0x26, 20), 6);
            
            pumpMotor = new ElmoTwitter("PumpMotor", "COM2", 115200, Parity.None, 8, StopBits.One, 5000, 20, false, (byte)';', (byte)';');

            emHotel = EnergyMonitorHotel.Instance;
            emPump = EnergyMonitorPump.Instance;

            lheBoard = LheBoard.Instance;
        }

        private static BuoyancyEngine instance;

        public static BuoyancyEngine Instance
        {
            get
            {
                instance = instance ?? (new BuoyancyEngine());
                return instance;
            }
        }

        public static StringBuilder sbTemp = new StringBuilder(128);

        public static double IB { get; private set; }
        public static double OB { get; private set; }

        public void initPumpMotor()
        {
            pumpMotor.init();
        }
 
        public void init()
        {
            EngrLogger.Comment("BE::init: Initializing Buoyancy Engine");
            IB = 0.75;
            OB = 2.0;

            initPumpMotor();

            mb.EnableChannelPower(MotherBoard.ChannelNames.MotorizedValve);
            readyMV();
            
            stopMotor();
            closeMV();

            bellowsPosition = double.NaN;


            mb.EnableChannelPower((int)MotherBoard.ChannelNames.PositionSensorChannel);

            EngrLogger.writeToColumns("BE::init: Getting bellows position 5X");

            for (var i = 0; i < 5; i++)
            {
                bellowsPosition = getBellowsPositionStringPot();
                sbTemp.Clear();
                sbTemp.Append("Try ");
                sbTemp.Append(i);
                sbTemp.Append(" BP = ");
                sbTemp.Append(bellowsPosition);
                EngrLogger.writeToColumns(sbTemp);
                Thread.Sleep(500);
            }
        }

        public void EnableBEPumping()
        {
            lheBoard.EnablePumpPower(true);

            mb.EnableChannelPower(MotherBoard.ChannelNames.MotorizedValve);
            mb.EnableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);
            mb.EnableChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel);

            //TODO might not have to do either or both these
            mb.Enable12V();
            mb.Enable5V();
        }

        public void DisableBEPumping()
        {
            lheBoard.EnablePumpPower(true);

            mb.DisableChannelPower(MotherBoard.ChannelNames.MotorizedValve);
            mb.DisableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);
            mb.DisableChannelPower(MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
        }

        public void getPumpMotorStats()
        {
            pumpMotor.getStats();
        }

        public void setPumpMotorAccDeacc1e9()
        {
            pumpMotor.setAccDeacc1e9();
        }

        //private const double sensorLength = 250.1;

        private double bellowsPosition = double.NaN; 

        /*
        public double getBellowsPositionMTS()
        {
            int value = 0;
            double volts = double.NaN;
            double voltage = double.NaN;

            value = bellowsPositionADC.readValue();

            if (value != int.MaxValue)
            {
                value = (int)(value ^ 0x80000000);  //Need to invert the msb per 2485 data sheet
                //Debug.Print("ADC Value = " + value.ToString());
                volts = (float)value;
                voltage = 2.5 + (5.0 * (double)volts / 2147483648.0);
                //Debug.Print("Voltage = " + voltage.ToString() + "\r\n");
                bellowsPosition = (voltage - 0.1) * (sensorLength / 4.8);
                //Debug.Print("Position = " + position.ToString());
            }

            if (MissionConfig.EngrLoggerVerbosityLevel > 5)
                EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, DateTime.Now,
                                            EngrLiterals.ColumnNums.state, SV.CurrentState,
                                            EngrLiterals.ColumnNums.comment, "Bellows Position",
                                            EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition);

            return (bellowsPosition);
        }*/

        public int getBellowsADCValue()
        {
            return bellowsPositionADC.readValue();
        }

        private static short _debugCounter = 1000;
        public double getBellowsPositionStringPot(bool writeToLog = false)
        {
            //const double fullScaleStringOut = 500.0;  //mm
            //const double fullScaleVolts = 2.5;
            //const double voltageOffset = -2.5;
            //const double MMperVolt = fullScaleStringOut / (fullScaleVolts - voltageOffset);

            bellowsPosition = double.NaN;

            var value = getBellowsADCValue();

            if (value != int.MaxValue)
            {
                //Convert ADC value to volts per 2485 data sheet
                value = (int)(value ^ 0x80000000);
                bellowsPosition = MissionConfig.StringPotSlope * value + MissionConfig.StringPotIntercept;

                if ( (MissionConfig.EngrLoggerVerbosityLevel > 5) || writeToLog)
                    EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, DateTime.Now,
                                                EngrLiterals.ColumnNums.state, SV.CurrentState,
                                                EngrLiterals.ColumnNums.comment, "Bellows Position",
                                                EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition);
                
                if (MissionConfig.DebuggerVerbosityLevel > 2 && ++_debugCounter == 1000)
                {
                    _debugCounter = 0;
                    sbTemp.Clear().Append(DateTime.Now.ToString());
                    sbTemp.Append(",Bellows Position,").Append(bellowsPosition.ToString("d2"));
                    sbTemp.Append(",ADC,").Append(value.ToString());
                    Debug.Print(sbTemp.ToString());
                }
            }
            else
            {
                bellowsPosition = double.NaN;
                sbTemp.Clear();
                sbTemp.Append("[ERROR] converting ADC value, raw value = ");
                sbTemp.Append(value);
                EngrLogger.writeToColumns(sbTemp);
            }

            SV.BellowsPosition = bellowsPosition;

            return (bellowsPosition);
        }

        public bool motorStopped;

        public void stopMotor()
        {
            pumpMotor.stopMotor();

            increasingBuoyancy = false;
            decreasingBuoyancy = false;
            motorStopped = true;

        }

        private static int exerciseBellowsJV = 100000;

        EnergyMonitorHotel emHotel;
        EnergyMonitorPump emPump;

        public int exerciseBellows()
        {
            bellowsPosition = double.NaN;
            stopMotor();

            Thread.Sleep(2000);

            sbTemp.Clear();
            sbTemp.Append("Decreasing Buoyancy");
            for (int i = 0; i < 10; i++)
            {
                //Debug.Print("Pump stats: ");
                //Debug.Print(emPump.getVICE().ToString());
                //Debug.Print("Hotel stats: ");
                //Debug.Print(emHotel.getVICE().ToString());
                decreaseBuoyancy(exerciseBellowsJV);
                //CN0254Volts = CN0254.scanAll();
                //bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                bellowsPosition = getBellowsPositionStringPot();
                EngrLogger.writeToColumns(1, EngrLiterals.ColumnNums.dateTime, DateTime.Now,
                                                EngrLiterals.ColumnNums.state, SV.CurrentState,
                                                EngrLiterals.ColumnNums.comment, sbTemp,
                                                EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(2000);
            }
            stopMotor();
            Thread.Sleep(2000);

            sbTemp.Clear();
            sbTemp.Append("Increasing Buoyancy");
            for (int i = 0; i < 10; i++)
            {
                //Debug.Print("Pump stats: ");
                //Debug.Print(emPump.getVICE().ToString());
                //Debug.Print("Hotel stats: ");
                //Debug.Print(emHotel.getVICE().ToString());
                increaseBuoyancy(exerciseBellowsJV);
                bellowsPosition = getBellowsPositionStringPot();
                EngrLogger.writeToColumns(1, EngrLiterals.ColumnNums.dateTime, DateTime.Now,
                                                EngrLiterals.ColumnNums.state, SV.CurrentState,
                                                EngrLiterals.ColumnNums.comment, sbTemp,
                                                EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(2000);
            }
            stopMotor();

            return (1);
        }

        private static int moveBellowsInsideLimitsJV = 100000;
            
        public int moveBellowsInsideLimits()
        {
            //Just in case the bellows was outside the limits, put it inside the limits
            bellowsPosition = getBellowsPositionStringPot();

            if (bellowsPosition > (MissionConfig.IBFillUpperLimit - 1.0))
            {
                sbTemp.Clear();
                sbTemp.Append("Inner bellows exceeds upper limit, increasing buoyancy BP = ");
                sbTemp.Append(bellowsPosition).Append(" limit: ").Append(MissionConfig.IBFillUpperLimit);
                EngrLogger.writeToColumns(sbTemp);

                increaseBuoyancy(moveBellowsInsideLimitsJV);

                return (0);     //action not complete, return 0
            }
            else if (bellowsPosition < (MissionConfig.IBFillLowerLimit + 1.0))
            {
                sbTemp.Clear();
                sbTemp.Append("Inner bellows below lower limit, decreasing buoyancy BP = ");
                sbTemp.Append(bellowsPosition).Append(" limit: ").Append(MissionConfig.IBFillLowerLimit);
                EngrLogger.writeToColumns(sbTemp);

                decreaseBuoyancy(moveBellowsInsideLimitsJV);

                return (0);        //action not complete, return 0
            }
            else
            {
                stopMotor();

                sbTemp.Clear();
                sbTemp.Append("Bellows inside limits: ").Append(bellowsPosition);
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();

                return (1);
            }
        }

        public bool increasingBuoyancy;

        public bool increaseBuoyancy(int jogVelocityCPS, bool checkBellowsPosition = true)
        {
            if (checkBellowsPosition)
            {
                bellowsPosition = getBellowsPositionStringPot();

                if (bellowsPosition < MissionConfig.IBFillLowerLimit)
                {
                    pumpMotor.stopMotor();
                    EngrLogger.writeToColumns("[ERROR] Elmo.increasingBuoyancy: Bellows at lower limit");
                    return false;
                }
            }

            if (!isValveOpen()) {
                EngrLogger.Comment("Increase Buoyancy is opening valve");
                openMV();
                Thread.Sleep(1500);
            }
            pumpMotor.sendJV(-1 * System.Math.Abs(jogVelocityCPS));
            motorStopped = false;
            increasingBuoyancy = true;
            decreasingBuoyancy = false;
            return true;

        }

        public bool decreasingBuoyancy;

        public bool decreaseBuoyancy(int jogVelocityCPS)
        {

            bellowsPosition = getBellowsPositionStringPot();

            if (bellowsPosition > MissionConfig.IBFillUpperLimit)
            {
                pumpMotor.stopMotor();
                EngrLogger.writeToColumns("[ERROR] Elmo.decreaseBuoyancy: Bellows at upper limit");
                return (false);
            }

            if (!isValveOpen()) {
                EngrLogger.Comment("Decrease buoyancy is opening valve");
                openMV();
                Thread.Sleep(1500);
            }

            pumpMotor.sendJV((1 * System.Math.Abs(jogVelocityCPS)));
            motorStopped = false;
            decreasingBuoyancy = true;
            increasingBuoyancy = false;
            return (true);

        }

        /// <summary>
        /// Reads the digital input on the ready line from
        /// the motorized valve controller.
        /// </summary>
        /// <returns>True is the valve is ready for commands</returns>
        public bool isValveReady()
        {
            return !mb.getMVReadyStatus();
        }

        /// <summary>
        /// Reads the digital input on the open line from
        /// the motorized valve controller.
        /// </summary>
        /// <returns>True if the motor is driven to the valve
        /// open position.</returns>
        public bool isValveOpen()
        {
            return !mb.getMVOpenedStatus();
        }

        public static bool valveOpen;

        /// <summary>
        /// Waits for motorize valve control to read ready
        /// </summary>
        /// <returns>The final state read from the motorized
        /// valve controller.</returns>
        public bool readyMV()
        {
            if (isValveReady()) return true;

            var attempts = 10;
            do
            {
                EngrLogger.Comment("BE::readyMV: Valve not ready, attempts remaining: "+attempts);
                Thread.Sleep(3000);
                if (isValveReady())
                    break;
            } while (--attempts > 0);

            if (attempts > 0)
                return true;

            EngrLogger.Comment("BE::readyMV: ERROR Valve not ready after 20 seconds");
            return false;
        }

        /// <summary>
        /// Opens the motorized valve
        /// </summary>
        public void openMV()
        {
            EngrLogger.Comment("BE::openMV: Starting valve open");

            mb.openMV();

            if (waitForOpen(true))
                valveOpen = true;
            else
            {
                EngrLogger.Comment("BE::openMV: ERROR valve not reading open");
                valveOpen = false;
            }

        }

        /// <summary>
        /// Waits up to 3 seconds for the valve to either open or close
        /// </summary>
        /// <param name="waitForOpen">boolean, true if you're waiting for
        /// the valve to open, false if you're waiting for it to close
        /// </param>
        /// <returns></returns>
        private bool waitForOpen(bool waitForOpen)
        {
            var attempts = 30;
            do
            {
                if (isValveOpen() == waitForOpen)
                    break;
                Thread.Sleep(100);
            } while (--attempts > 0);

            return attempts > 0;
        }

        /// <summary>
        /// Closes the motorized valve.
        /// </summary>
        public void closeMV()
        {
            EngrLogger.writeToColumns("BE::closeMV: Starting valve close");

            mb.closeMV();

            if (waitForOpen(false))
                valveOpen = true;
            else
            {
                EngrLogger.Comment("BE::closeMV: ERROR valve not reading closed");
                valveOpen = false;
            }
        }

        /// <summary>
        /// Stops the motor and closes the motorized valve
        /// </summary>
        public void stopMotorAndCloseValve()
        {
            stopMotor();
            Thread.Sleep(500);
            closeMV();
        }

        // Below is legacy CPF code
        public struct PlatVel
        {
            //PID constants
            //Constants based on Matlab analysis 2018 Aug 31 for 0.156 cm^3/rev pump
            public const double     K = 80.0,               // Constant for P term of PID
                                    Ti = 50000.0,           // Time constant for I term of PID (in denominator, bigger values less I term in output
                                    Td = 10.0,              // Time constant for D term of PID
                                    Tf = 10.0,              // Derivative filter time constant
                                    Tt = 100.0,             // Anti-windup time constant
                                    B = 1.0,                // Setpoint weight
                                    uLow = -3000.0 / 60.0,  // minimum possible output (RPS)
                                    uHigh = 3000.0 / 60.0;  // maximum possible output (RPS)

            public static double h = MissionConfig.SBE41CommandModeSamplePeriod / 1000.0;            // Sample period (seconds)

            //Precomputed constants
            public static double    den = (Tf * Tf) + (2 * h * Tf) + (2 * h * h),   // denominator of measured signal filter terms
                                    p1 = Tf * Tf / den,                             // first part of state variable x2 equation
                                    p2 = 2 * h * h / den,                           // second part of state variable x1 equation
                                    p3 = K * h / Ti,                                // integral gain
                                    p4 = K * Td / h,                                // derivative gain
                                    p5 = h / Tt;                                    // anti-windup gain

            //Loop variables
            public static double    I,      // Integrator part of PID calculation
                                    y1,     // first state variable
                                    y2,     // second state variable
                                    v,    // unclipped PID output
                                    u;    // clipped PID output
            public static int uCPS;  // u value in counts per second
        }

        public struct PIDLogValues
        {
            public double setPoint;
            public double PV;
            public double y1;
            public double y2;
            public double I;
            public double v;
            public double u;
            public double uCPS;
            public double h;
            public DateTime timeStamp;
        }
        private static PIDLogValues BCPIDLogValues;

        public void resetPVPID()
        {
            PlatVel.I = 0;
            PlatVel.y1 = 0;
            PlatVel.y2 = 0;
            PlatVel.v = 0.0;
            PlatVel.u = 0.0;
            PlatVel.uCPS = 0;
        }

        public void calcPIDConstants(int samplePeriod)
        {
            PlatVel.h = samplePeriod / 1000.0;
            
            PlatVel.den = (PlatVel.Tf * PlatVel.Tf) + (2 * PlatVel.h * PlatVel.Tf) + (2 * PlatVel.h * PlatVel.h);   // denominator of measured signal filter terms
            PlatVel.p1 = PlatVel.Tf * PlatVel.Tf / PlatVel.den;                             // first part of state variable x2 equation
            PlatVel.p2 = 2 * PlatVel.h * PlatVel.h / PlatVel.den;                           // second part of state variable x1 equation
            PlatVel.p3 = PlatVel.K * PlatVel.h / PlatVel.Ti;                                // integral gain
            PlatVel.p4 = PlatVel.K * PlatVel.Td / PlatVel.h;                                // derivative gain
            PlatVel.p5 = PlatVel.h / PlatVel.Tt;                                            // anti-windup gain

            resetPVPID();
            
            sbTemp.Clear();
            sbTemp.Append("Calculated new PID Constant, h = ");
            sbTemp.Append(PlatVel.h.ToString());
            EngrLogger.writeToColumns(sbTemp);
        }

        //Pump motor encoder counts per revolution
        public const int COUNTSperREV = 2048 * 4;      //2048  Counts per Revolution Encoder *4 for quadrature interpolation
 
        public PIDLogValues runPvPID(double velocitySetPoint, double currentPlatVel)
        {
            //sbTemp.Clear();
            //sbTemp.Append("runPvPID got velocitySetPoint: ");
            //sbTemp.Append(velocitySetPoint.ToString());
            //sbTemp.Append(" and currentPlatVel:  ");
            //sbTemp.Append(currentPlatVel.ToString());
            //Debug.Print(sbTemp.ToString());

            //Astrom & Hagglund PID Algorithm
            PlatVel.y2 = (PlatVel.p1 * PlatVel.y2) + (PlatVel.p2 * (currentPlatVel - PlatVel.y1));     // update filter state y2
            PlatVel.y1 = PlatVel.y1 + PlatVel.y2;                                               // update filter state y1
            PlatVel.v = PlatVel.K * ((PlatVel.B * velocitySetPoint) - PlatVel.y1) -             // compute nominal output
                    (PlatVel.p4 * PlatVel.y2) + PlatVel.I;

            // limit the nominal output to desired range
            if (PlatVel.v < PlatVel.uLow)
                PlatVel.u = PlatVel.uLow;
            else if (PlatVel.v > PlatVel.uHigh)
                PlatVel.u = PlatVel.uHigh;
            else
                PlatVel.u = PlatVel.v;

            // update the integral
            PlatVel.I = PlatVel.I + (PlatVel.p3 * (velocitySetPoint - PlatVel.y1)) + (PlatVel.p5 * (PlatVel.u - PlatVel.v));

            //Convert Rev per second to Counts per second
            PlatVel.uCPS = (int)(PlatVel.u * COUNTSperREV);

            if (PlatVel.uCPS >= 0)
            {
                increaseBuoyancy(PlatVel.uCPS);      
            }
            else
            {
                decreaseBuoyancy(PlatVel.uCPS);     
            }

            BCPIDLogValues.timeStamp = DateTime.Now;
            BCPIDLogValues.I = PlatVel.I;
            BCPIDLogValues.PV = currentPlatVel;
            BCPIDLogValues.setPoint = velocitySetPoint;
            BCPIDLogValues.u = PlatVel.u;
            BCPIDLogValues.uCPS = PlatVel.uCPS;
            BCPIDLogValues.v = PlatVel.v;
            BCPIDLogValues.y1 = PlatVel.y1;
            BCPIDLogValues.y2 = PlatVel.y2;
            BCPIDLogValues.h = PlatVel.h;
            
            return (BCPIDLogValues);
        }

        private const double currentErrorGain = 29.07;
        private const double prevErrorGain = -56.78;
        private const double prevPrevErrorGain = 27.71;
        private const double prevPumpSpeedGain = 1.238;
        private const double prevPrevPumpSpeedGain = -0.3829;

        private const double pumpSpeedUpperLimit = 50.0;     // [rev/sec]
        private const double pumpSpeedLowerLimit = -50.0;    // [rev/sec]

        private static double currentPressureError = double.NaN;
        private static double prevPressureError;
        private static double prevPrevPressureError;

        private static double nextPumpSpeedCmd = double.NaN;
        private static double prevPumpSpeedCmd;
        private static double prevPrevPumpSpeedCmd;

        private const double maxSetPointIncrement = 1.2;   // [decibar]
        private const double maxSetPointDecrement = -1.2;  // [decibar]

        private static double governedPressureSetPoint = double.NaN; // [decibar]


        /// <summary>
        /// Depth Controller using a dual lead model
        /// </summary>
        /// <param name="desiredPressureSetPoint"></param>
        /// <param name="currentPlatformPressure">pressure in dbar</param>
        /// <returns>pump speed in counts/second</returns>
        /// 
        public void InitializeReferenceGovernor(double desiredPressureSetPoint, double currentPlatformPressure)
        {
            //governedPressureSetPoint = (desiredPressureSetPoint - currentPlatformPressure);
            //if (governedPressureSetPoint > maxSetPointIncrement)
            //    governedPressureSetPoint = maxSetPointIncrement;
            //else if (governedPressureSetPoint < maxSetPointDecrement)
            //    governedPressureSetPoint = maxSetPointDecrement;

            //governedPressureSetPoint = governedPressureSetPoint + currentPlatformPressure;

            governedPressureSetPoint = currentPlatformPressure;
        }

        public double ReferenceGovernor(double desiredPressureSetPoint, double currentPlatformPressure)
        {
            // AUTHOR: Brian Ha (2018 Summer Intern)

            //
            double setPointDelta;   // [decibar]
            setPointDelta = desiredPressureSetPoint - governedPressureSetPoint;
            //EngrLogger.writeToColumns("REF GOV:  setPointDelta = " + setPointDelta.ToString());

            
            //
            if (setPointDelta > maxSetPointIncrement)
            {
                setPointDelta = maxSetPointIncrement;
            }
            else if (setPointDelta < maxSetPointDecrement)
            {
                setPointDelta = maxSetPointDecrement;
            }

            //
            governedPressureSetPoint = governedPressureSetPoint + setPointDelta;

            if (System.Math.Abs(desiredPressureSetPoint - governedPressureSetPoint) <= maxSetPointIncrement)
            {

                governedPressureSetPoint = desiredPressureSetPoint;
            }

            //EngrLogger.writeToColumns("governedPressureSetPoint = " + governedPressureSetPoint.ToString());

            return governedPressureSetPoint;
        }


        public int DualLeadDepthController(double governedPressureSP, double currentPlatformPressure)
        {
            // AUTHOR: Brian Ha (2018 Summer Intern)

            // Calculate current pressure error
            currentPressureError = governedPressureSP - currentPlatformPressure;
            //EngrLogger.writeToColumns("ERROR = " + currentPressureError.ToString());


            // Calculate the next pump speed command.
            nextPumpSpeedCmd = (currentErrorGain * currentPressureError) + (prevErrorGain * prevPressureError) + (prevPrevErrorGain * prevPrevPressureError) + (prevPumpSpeedGain * prevPumpSpeedCmd) + (prevPrevPumpSpeedGain * prevPrevPumpSpeedCmd);
            //EngrLogger.writeToColumns("Pre threshold nextPumpSpeedCmd: " + nextPumpSpeedCmd.ToString());

            // Limit pump speed command magnitude to 50 revs/sec.
            if (nextPumpSpeedCmd > pumpSpeedUpperLimit)
            {
                nextPumpSpeedCmd = pumpSpeedUpperLimit;
            }
            else if (nextPumpSpeedCmd < pumpSpeedLowerLimit)
            {
                nextPumpSpeedCmd = pumpSpeedLowerLimit;
            }

            // Deadband
            //if (nextPumpSpeedCmd < 0.05 && nextPumpSpeedCmd > -0.05)
            //{
            //    nextPumpSpeedCmd = 0.0;
            //}

            // Set historical values for next function call
            prevPrevPumpSpeedCmd = prevPumpSpeedCmd;
            prevPrevPressureError = prevPressureError;
            prevPumpSpeedCmd = nextPumpSpeedCmd;
            prevPressureError = currentPressureError;

            EngrLogger.writeToColumns("TARGET: " + governedPressureSP.ToString() + "; CURRENT DEPTH = " + currentPlatformPressure.ToString() + "; ERROR: " + currentPressureError.ToString() + "; CMD = " + nextPumpSpeedCmd.ToString() + " [revs/sec];");

            // Convert from rev/sec to counts/sec
            nextPumpSpeedCmd = nextPumpSpeedCmd * COUNTSperREV;

            // Return next pump speed command [counts/sec].
            //EngrLogger.writeToColumns("Final nextPumpSpeedCmd:" + ((int)nextPumpSpeedCmd).ToString());
            return ((int)nextPumpSpeedCmd);
        }
        //public int DualLeadDepthController(double pressureSetPoint, double currentPlatformPressure)
        //{
        //    // AUTHOR: Brian Ha (2018 Summer Intern)

        //    // Calculate current pressure error
        //    currentPressureError = pressureSetPoint - currentPlatformPressure;

			
        //    // Error Governor - This acts as a saturation on the error signal.
        //    if (currentPressureError > errorGovernorUpperLimit)
        //    {
        //        currentPressureError = errorGovernorUpperLimit;
        //    }
        //    else if (currentPressureError < errorGovernorLowerLimit)
        //    {
        //        currentPressureError = errorGovernorLowerLimit;
        //    }
			
        //    Debug.Print("Current Pressure Error: "+ currentPressureError);

        //    // Calculate the next pump speed command.
        //    nextPumpSpeedCmd = (currentErrorGain * currentPressureError) +
        //                       (prevErrorGain * prevPressureError) + (prevPrevErrorGain * prevPrevPressureError) +
        //                       (prevPumpSpeedGain * prevPumpSpeedCmd) + (prevPrevPumpSpeedGain * prevPrevPumpSpeedCmd);

			
        //    // Limit pump speed command magnitude to 50 revs/sec.
        //    if (nextPumpSpeedCmd > pumpSpeedUpperLimit)
        //    {
        //        nextPumpSpeedCmd = pumpSpeedUpperLimit;
        //    }
        //    else if(nextPumpSpeedCmd < pumpSpeedLowerLimit)
        //    {
        //        nextPumpSpeedCmd = pumpSpeedLowerLimit;
        //    }
			
        //    // Set values for next function call
        //    prevPrevPumpSpeedCmd = prevPumpSpeedCmd;
        //    prevPrevPressureError = prevPressureError;
        //    prevPumpSpeedCmd = nextPumpSpeedCmd;
        //    prevPressureError = currentPressureError;

        //    // Convert from rev/sec to counts/sec
        //    nextPumpSpeedCmd = nextPumpSpeedCmd * COUNTSperREV;

        //    // Return next pump speed command [counts/sec].
        //    return (int)(nextPumpSpeedCmd);
        //}
    }
}