using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using GHI.Premium.Hardware;

namespace miniCPF
{
    public static class BuoyancyControl
    {
        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 (RPS)
                                    uHigh = configFile.uHighVel,    // maximum possible output (RPS)
                                    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
        }

        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;

        //Loop variables
        private 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
        private static int      uCPS = 0;  // u value in counts per second

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

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

            // update the integral
            I = I + (Vel.p3 * (velocitySetPoint - y1)) + (Vel.p5 * (u - v));    
            
            //Convert Rev per second to Counts per second
            uCPS = (int)(u * configFile.COUNTSperREV);     

            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 = I;
            BCPIDLogValues.PV = currentVelocity;
            BCPIDLogValues.setPoint = velocitySetPoint;
            BCPIDLogValues.u = u;
            BCPIDLogValues.uCPS = uCPS;
            BCPIDLogValues.v = v;
            BCPIDLogValues.y1 = y1;
            BCPIDLogValues.y2 = y2;
            return(BCPIDLogValues);
        }
    }
}