//#define NO_OUTPUTtoELMO

using System;
using System.Text;

using Microsoft.SPOT;
using Microsoft.SPOT.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
        }

        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;

        private static StringBuilder sbTemp = new StringBuilder(512);

        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 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(!NO_OUTPUTtoELMO)
            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
            }
#else
            Debug.Print("Output to Elmo is disabled in Buoyancy Control");
            sbTemp.Clear();
            sbTemp.Append("Output to Elmo is disabled in Buoyancy Control");
            Program.btConsole.SendLine(sbTemp);
#endif

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