using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using GHI.Premium.Hardware;

namespace CPF
{
    public static class BuoyancyControl
    {
        private struct PlatVel
        {
            //PID constants
            public const double K = configFile.KPlatVel,            // Constant for P term of PID
                                    Ti = configFile.TiPlatVel,          // Time constant for I term of PID (in denominator, bigger values less I term in output
                                    Td = configFile.TdPlatVel,          // Time constant for D term of PID
                                    Tf = configFile.TfPlatVel,          // Derivative filter time constant
                                    Tt = configFile.TtPlatVel,          // Anti-windup time constant
                                    B = configFile.BPlatVel,            // Setpoint weight
                                    uLow = configFile.uLowPlatVel,      // minimum possible output (RPS)
                                    uHigh = configFile.uHighPlatVel,    // maximum possible output (RPS)
                                    h = configFile.hPlatVel;            // 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
        }

        private struct Depth
        {
            //PID constants
            public const double K = configFile.KDepth,            // Constant for P term of PID
                                    Ti = configFile.TiDepth,          // Time constant for I term of PID (in denominator, bigger values less I term in output
                                    Td = configFile.TdDepth,          // Time constant for D term of PID
                                    Tf = configFile.TfDepth,          // Derivative filter time constant
                                    Tt = configFile.TtDepth,          // Anti-windup time constant
                                    B = configFile.BDepth,            // Setpoint weight
                                    uLow = configFile.uLowDepth,      // minimum possible output (RPS)
                                    uHigh = configFile.uHighDepth,    // maximum possible output (RPS)
                                    h = configFile.hDepth;            // 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;
        }
        private static PIDLogValues BCPIDLogValues;

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

        public static void resetDepthPID()
        {
            Depth.I = 0;
            Depth.y1 = 0;
            Depth.y2 = 0;
            Depth.v = 0.0;
            Depth.u = 0.0;
            Depth.uCPS = 0;
        }

        public static PIDLogValues PIDPlatformVelocity(double velocitySetPoint, double currentPlatVel)
        {
            //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 * configFile.COUNTSperREV);

            if (PlatVel.uCPS >= 0)
            {
                Elmo.extendBellows(PlatVel.uCPS);      //Note: extend bellows takes the absolute value of the passed in speed
            }
            else
            {
                Elmo.retractBellows(PlatVel.uCPS);     //Note: retract bellows takes the absolute value of the passed in speed
            }

            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;
            return (BCPIDLogValues);
        }

        public static PIDLogValues PIDDepth(double depthSetPoint, double measuredDepth)
        {
            //First correct the input depths from pressure increasing with depth to pressure decreasing with depth
            depthSetPoint = -1.0 * System.Math.Abs(depthSetPoint);
            measuredDepth = -1.0 * System.Math.Abs(measuredDepth);
           
            //Astrom & Hagglund PID Algorithm
            Depth.y2 = (Depth.p1 * Depth.y2) + (Depth.p2 * (measuredDepth - Depth.y1));     // update filter state y2
            Depth.y1 = Depth.y1 + Depth.y2;                                               // update filter state y1
            Depth.v = Depth.K * ((Depth.B * depthSetPoint) - Depth.y1) -             // compute nominal output
                    (Depth.p4 * Depth.y2) + Depth.I;

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

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

            //Convert Rev per second to Counts per second
            Depth.uCPS = (int)(Depth.u * configFile.COUNTSperREV);
          
            if (Depth.uCPS >= 0)
            {
                Elmo.extendBellows(Depth.uCPS);      //Note: extend bellows takes the absolute value of the passed in speed
            }
            else
            {
                Elmo.retractBellows(Depth.uCPS);     //Note: retract bellows takes the absolute value of the passed in speed
            }

            BCPIDLogValues.I = Depth.I;
            BCPIDLogValues.PV = measuredDepth;
            BCPIDLogValues.setPoint = depthSetPoint;
            BCPIDLogValues.u = Depth.u;
            BCPIDLogValues.uCPS = Depth.uCPS;
            BCPIDLogValues.v = Depth.v;
            BCPIDLogValues.y1 = Depth.y1;
            BCPIDLogValues.y2 = Depth.y2;
            return (BCPIDLogValues);
        }
    }
}