using System;
using System.Threading;

using Microsoft.SPOT;

namespace MFConsoleApplication1
{
    public static class BuoyancyControl
    {
        private static double pressureDBar = Double.NaN;

        private static AutoResetEvent gotDGHPressureEvent = new AutoResetEvent(false);

        private struct DepthPIDParam
        {
            public const double K = 12.0,       // Constant for P term of PID
                                 Ti = 200.0,     // Time constant for I term of PID 
                                 Td = 120.0,     // Time constant for D term of PID
                                 Tf = 2.0,       // Derivative filter time constant
                                 Tt = 1.0,       // Anti-windup time constant
                                 B = 1.0,        // Setpoint weight
                                 uLow = -3000.0, // minimum possible output (RPM)
                                 uHigh = 3000.0,   // maximum possible output (RPM)
                                 h = 1.0;        // Sample period (seconds)
            //Calculated 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
        }

        private struct VelocityPIDParam
        {
            public const double K = 100.0,        // Constant for P term of PID
                                 Ti = 5000.0,    // Time constant for I term of PID 
                                 Td = 0.1,       // Time constant for D term of PID
                                 Tf = 2.0,       // Derivative filter time constant
                                 Tt = 1.0,       // Anti-windup time constant
                                 B = 1.0,        // Setpoint weight
                                 uLow = -3000.0, // minimum possible output (RPM)
                                 uHigh = 3000.0, // maximum possible output (RPM)
                                 h = 1.0;        // Sample period (seconds)
            //Calculated 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 static int GotoDepth(double depthSetPoint)
        {
            bool atDepth = false;
            bool timedOut = false;

            string engrDataLogString = "";

            const double RPM2CPS = 500.0 / 60.0;      //500 Counts per Rev divided by 60 seconds per minu

            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

            DateTime descendStartTime = DateTime.Now;

            EngrLog.writeEvent(Program.DateTimeNowMilliSec() + " ,Starting DGH read pressure timer");
            System.Threading.Timer DGHReadPressureTimer =
                new System.Threading.Timer(new TimerCallback(DGHReadPressureTimerCallback), null, 0, (int)(DepthPIDParam.h * 1000.0));

            while (!atDepth && !timedOut)
            {
                if (gotDGHPressureEvent.WaitOne(1000, false)) //waiting for notification, 1000 mSec timeout
                {
                    EngrLog.writeEvent(Program.DateTimeNowMilliSec() + " ,Buoyancy control got pressure from DGH" + pressureDBar.ToString());
                }
                else
                {
                    pressureDBar = Double.NaN;
                    timedOut = true;
                    EngrLog.writeEvent(Program.DateTimeNowMilliSec() + " ,Buoancy control GotoDepth timed out waiting for DGH Pressure");
                }

                //PID Algorithm
                if (!timedOut)
                {
                    y2 = (DepthPIDParam.p1 * y2) + (DepthPIDParam.p2 * (pressureDBar - y1));    // update filter state x2
                    y1 = y1 + y2;                                                               // update filter state x1
                    v = DepthPIDParam.K * ((DepthPIDParam.B * depthSetPoint) - y1) -            // compute nominal output
                            (DepthPIDParam.p4 * y2) + I;

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

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

                    uCPS = u * RPM2CPS;     //Convert RPM to counts per second
                }
                if ((pressureDBar < depthSetPoint + 0.5) && (pressureDBar > depthSetPoint - 0.5))
                    atDepth = true;             /* really need to be at depth for a while before calling it good or
                                                 * maybe in the band && JV < some small number
                                                 */
                else
                    Elmo.sendJV(uCPS);

                engrDataLogString = Program.DateTimeNowMilliSec() + "PID"
                                    + " Press. (dBar): " + pressureDBar.ToString("F3") + ","
                                    + " y1: " + y1.ToString("F3") + ","
                                    + " y2: " + y2.ToString("F3") + ","
                                    + " v: " + v.ToString("F3") + ","
                                    + " u: " + u.ToString("F3") + ","
                                    + " uCPS: " + uCPS.ToString("F3") + ","
                                    + " I: " + I.ToString();
                EngrLog.writeData(engrDataLogString);
            }

            if (timedOut)
                return (-1);   //eventually return a meaningful error code
            else
            {
                Program.wakeStateMachineEvent.Set();
                Thread.Sleep(1);  //Don't know if we need this or not
                return (0);
            }
        }

        public static void GotoVelocity(double velocity)
        {
            //same as GotoDepth but with VelocityPIDParam
        }

        private static void DGHReadPressureTimerCallback(object state)
        {
            pressureDBar = DGHPressure.readPressure();
            gotDGHPressureEvent.Set();
        }
    }
}