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

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

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

        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 init()
        {
            IB = 0.75;
            OB = 2.0;

            pumpMotor.init();

            stopMotor();
            closeMV();
        }

        private const double sensorLength = 250.1;

        public double bellowsPosition { get; private set; }

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

            value = bellowsPositionADC.readValue();

            if (bellowsPosition != 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.verbosityLevel > 5)
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now,
                                            EngrLogger.ColumnNums.state, SV.CurrentState,
                                            EngrLogger.ColumnNums.comment, "Bellows Position",
                                            EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);

            return (bellowsPosition);
        }
 
        public double getBellowsPositionStringPot()
        {
            int value = 0;
            double volts = double.NaN;
            double voltage = double.NaN;

            const double fullScaleStringOut = 500.0;  //mm
            const double fullScaleVolts = 2.5;
            const double voltageOffset = -2.5;
            const double MMperVolt = fullScaleStringOut  / (fullScaleVolts - voltageOffset);

            value = bellowsPositionADC.readValue();

            if (bellowsPosition != int.MaxValue)
            {
                //Convert ADC value to volts per 2485 data sheet
                value = (int)(value ^ 0x80000000);  
                //Debug.Print("ADC Value = " + value.ToString());
                volts = (float)value;
                voltage = 5.0 * (double)volts / 2147483648.0;
                //Debug.Print("Voltage = " + voltage.ToString() + "\r\n");
                
                //Convert voltage to position
                bellowsPosition = (voltage - voltageOffset) * MMperVolt;
                //Debug.Print("Position = " + bellowsPosition.ToString());

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

            }

            SV.BellowsPosition = bellowsPosition;

            return (bellowsPosition);
        }

        public void processMessage(QRecord qRecord)
        { }

        public bool motorStopped = false;

        public void stopMotor()
        {
            pumpMotor.stopMotor();
            
            increasingBuoyancy = false;
            decreasingBuoyancy = false;
            motorStopped = true;
        }

        private static int exerciseBellowsJV = 30000;

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

            sbTemp.Clear();
            sbTemp.Append("Increasing bellows position");
            Thread.Sleep(1000);
            decreaseBuoyancy(exerciseBellowsJV);
            for (int i = 0; i < 10; i++)
            {
                //CN0254Volts = CN0254.scanAll();
                //bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                bellowsPosition = getBellowsPositionStringPot();
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now,
                                                EngrLogger.ColumnNums.state, SV.CurrentState,
                                                EngrLogger.ColumnNums.comment, sbTemp,
                                                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(2000);
            }
            pumpMotor.stopMotor();
            Thread.Sleep(200);

            sbTemp.Clear();
            sbTemp.Append("Decreasing bellows position");
            increaseBuoyancy(exerciseBellowsJV);
            for (int i = 0; i < 10; i++)
            {
                bellowsPosition = getBellowsPositionStringPot();
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, DateTime.Now,
                                                EngrLogger.ColumnNums.state, SV.CurrentState,
                                                EngrLogger.ColumnNums.comment, sbTemp,
                                                EngrLogger.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.bellowsUpperLimit - 1.0))
            {
                sbTemp.Clear();
                sbTemp.Append("Inner bellows exceeds upper limit, decreasing bellows position");
                EngrLogger.writeToColumns(sbTemp);

                if (!decreasingBuoyancy)
                    increaseBuoyancy(moveBellowsInsideLimitsJV);

                return (0);     //action not complete, return 0
            }
            else if (bellowsPosition < (missionConfig.bellowsLowerLimit + 1.0))
            {
                sbTemp.Clear();
                sbTemp.Append("Inner bellows below lower limit, increasing bellows position");
                EngrLogger.writeToColumns(sbTemp);

                if (!increasingBuoyancy)
                    decreaseBuoyancy(moveBellowsInsideLimitsJV);

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

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

                return (1);
            }
        }

        public bool increasingBuoyancy = false;

        public bool increaseBuoyancy(int jogVelocityCPS)
        {
            if (!SV.BellowsAtLowerLimit)
            {
                if (!isValveOpen())
                {
                    openMV();
                    Thread.Sleep(1500);
                }
                pumpMotor.sendJV(System.Math.Abs(jogVelocityCPS));
                motorStopped = false;
                increasingBuoyancy = true;
                decreasingBuoyancy = false;
                return (true);
            }
            else
            {
                EngrLogger.writeToColumns("ERROR: Elmo.increasingBuoyancy: Bellows at lower limit");
                return (false);
            }
        }

        public bool decreasingBuoyancy = false;

        public bool decreaseBuoyancy(int jogVelocityCPS)
        {

            if (!SV.BellowsAtUpperLimit)
            {
                if (!isValveOpen())
                {
                    openMV();
                    Thread.Sleep(1500);
                }
                pumpMotor.sendJV((-1 * System.Math.Abs(jogVelocityCPS)));
                motorStopped = false;
                decreasingBuoyancy = true;
                increasingBuoyancy = false;
                return (true);
            }
            else
            {
                EngrLogger.writeToColumns("ERROR: Elmo.decreaseBuoyancy: Bellows at uppper limit");
                return (false);
            }
        }

        public bool isValveOpen()
        {
            return (!mb.getMVOpenedStatus());
        }

        public static bool valveOpen;

        public void openMV()
        {
            EngrLogger.writeToColumns("Starting valve open");

            mb.openMV();

            valveOpen = true;
        }

        public void closeMV()
        {
            EngrLogger.writeToColumns("Starting valve close");

            mb.closeMV();

            valveOpen = false;
        }

        public int stopMotorAndCloseValve()
        {
            stopMotor();
            Thread.Sleep(500);
            closeMV();

            return (0);
        }

        public int getMotorStatus()
        {
            pumpMotor.sendSR();

            return (1);
        }

        // 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.SBE41SamplePeriod / 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 = 0,      // Integrator part of PID calculation
                                    y1 = 0,     // first state variable
                                    y2 = 0,     // second state variable
                                    v = 0.0,    // unclipped PID output
                                    u = 0.0;    // clipped PID output
            public static int uCPS = 0;  // 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;
        }
        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()
        {
            PlatVel.h = missionConfig.SBE41SamplePeriod;
            
            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 PIDPlatformVelocity(double velocitySetPoint, double currentPlatVel)
        {
            PlatVel.h = SV.CtdSamplePeriod;

            //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.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 = 0.0;
        private static double prevPrevPressureError = 0.0;

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

        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="pressureSetPoint">pressure in dbar</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);
        //}
    }
}