using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using GHI.Premium.Hardware;

namespace miniCPF
{
    public static class BuoyancyControl
    {
        //engrDataLogString = Program.DateTimeNowMilliSeconds() + "Type\tData"
        //                    + "\tComment\t" + Program.phaseNames[(int)Program.MissionState.missionPhase]
        //                    + "\tvelocitySetPoint\t" + velocitySetPoint.ToString("F6") + " \tpressureSetPoint\t" + pressureSetPoint.ToString("F6")
        //                    + "\tpressureDBar\t" + pressureDBar.ToString("F6")
        //                    + "\tlastPressure\t" + lastPressure.ToString("F6")
        //                    + "\tvelocity\t" + velocity.ToString("F6")
        //                    + "\ty1\t" + y1.ToString("F6")
        //                    + "\ty2\t" + y2.ToString("F6")
        //                    + "\tv\t" + v.ToString("F6")
        //                    + "\tu\t" + u.ToString("F6")
        //                    + "\tuCPS\t" + uCPS.ToString("F6")
        //                    + "\tI\t" + I.ToString("F6")
        //                    + "\tbellowsPosition\t" + bellowsPosition.ToString("F6");



        private struct Vel
        {
            //PID constants
            public const double K = configFile.KVel,            // Constant for P term of PID
                                    Ti = configFile.TiVel,          // Time constant for I term of PID (in denominator, bigger values less I term in output
                                    Td = configFile.TdVel,          // Time constant for D term of PID
                                    Tf = configFile.TfVel,          // Derivative filter time constant
                                    Tt = configFile.TtVel,          // Anti-windup time constant
                                    B = configFile.BVel,            // Setpoint weight
                                    uLow = configFile.uLowVel,      // minimum possible output (RPM)
                                    uHigh = configFile.uHighVel,    // maximum possible output (RPM)
                                    h = configFile.hVel;            // Sample period (seconds)

            //Precomputed constants
            public const 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
                                    uCPS = 0.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 int uCPS = 0;

        public static PIDLogValues PIDPlatformVelocity(double velocitySetPoint, double currentVelocity)
        {
            //Astrom & Hagglund PID Algorithm
            Vel.y2 = (Vel.p1 * Vel.y2) + (Vel.p2 * (currentVelocity - Vel.y1));      // update filter state y2
            Vel.y1 = Vel.y1 + Vel.y2;                                                                   // update filter state y1
            Vel.v = Vel.K * ((Vel.B * velocitySetPoint) - Vel.y1) -       // compute nominal output
                    (Vel.p4 * Vel.y2) + Vel.I;

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

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

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

            BCPIDLogValues.I = Vel.I;
            BCPIDLogValues.PV = currentVelocity;
            BCPIDLogValues.setPoint = velocitySetPoint;
            BCPIDLogValues.u = Vel.u;
            BCPIDLogValues.uCPS = Vel.uCPS;
            BCPIDLogValues.v = Vel.v;
            BCPIDLogValues.y1 = Vel.y1;
            BCPIDLogValues.y2 = Vel.y2;
            return(BCPIDLogValues);
        }
    }
}