#include "FlyByWire.hpp"

// =============================================================================
// General fly-by-wire helper functions
// =============================================================================

namespace FlyByWire
{

Real Hdg_err(Real tgt, Real hdg)
{
    Real err = tgt - hdg;
    if(err > Real(M_PI))
        return err - Real(M_PI*Real(2.));
    else if(err < Real(-M_PI))
        return err + Real(M_PI*Real(2.));
    else
        return err;
}

Real Hdg_err_deg(Real tgt, Real hdg)
{
    Real err = tgt - hdg;
    if(err > Real(180.))
        return err - Real(360.);
    else if(err < Real(-180.))
        return err + Real(360.);
    else
        return err;
}

Real Vote(Real a, Real b, Real c)
{
    Real tmp;
    if(c < a)
    {
        tmp = c;
        c = a;
        a = tmp;
    }
    if(b < a)
    {
        tmp = b;
        b = a;
        a = tmp;
    }
    if(c < b)
    {
        tmp = c;
        c = b;
        b = tmp;
    }

    return b;
}

Real Deadzone(Real x, Real a)
{
    if(fabs(x) >= a)
        return x;
    else
        return Real(0.);
}

Real Sat1(Real x, Real a, Real b)
{
    if(x < a)
        return a;
    else if(x > b)
        return b;
    else
        return x;
}

Real Ratelim(Real x_n, Real y_n_1, Real dy_dt_min, Real dy_dt_max, Real dt)
{
    Real dydt = (x_n - y_n_1)/dt;       // candidate derivative

    if(dydt < dy_dt_min)                // minimum derivative
        return y_n_1 + dy_dt_min*dt;
    else if(dydt > dy_dt_max)           // maximum derivative
        return y_n_1 + dy_dt_max*dt;
    else                                // derivative within bounds
        return x_n;
}

RateLimiter::RateLimiter(Real dy_dt_min_, Real dy_dt_max_, Real dt_, Real y_)
    : dy_dt_min(dy_dt_min_),
      dy_dt_max(dy_dt_max_),
      dt(dt_),
      y_n_1(y_)
{}

Real RateLimiter::Filter(Real x_n)
{
    Real y_n = Ratelim(x_n, y_n_1, dy_dt_min, dy_dt_max, dt);
    y_n_1 = y_n;
    return y_n;
}

Real Heaviside(Real x)
{
    return (x >= Real(0.)) ? Real(1.) : Real(0.);
}

// =============================================================================
// IRR Filters from the continuous Laplace transfer function.
// =============================================================================

Integrator1::Integrator1(Real Ts_, Real y_, Real lower_bound_, Real upper_bound_)
    :   Ts(Ts_),
        lower_bound(lower_bound_),
        upper_bound(upper_bound_)
{
    SetState(y_, Real(0.));
}

void Integrator1::SetState(Real y_, Real x_)
{
    y_n_1 = y_;
    x_n_1 = x_;
}

Real Integrator1::Filter(Real x_n)
{
    Real y_n = y_n_1 + (x_n + x_n_1)*Ts/Real(2.);
    y_n = Sat1(y_n, lower_bound, upper_bound);
    x_n_1 = x_n;
    y_n_1 = y_n;
    return y_n;
}

Filter1::Filter1()
{
    SetState(Real(0.));
    ComputeCoeffsFromContinuousTF(Real(0.), Real(1.), Real(0.), Real(1.), Real(1.));
}

Filter1::Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0)
{
    SetState(y0);
    ComputeCoeffsFromContinuousTF(b1, b0, a1, a0, Ts);
}

void Filter1::ComputeCoeffsFromContinuousTF(Real b1, Real b0, Real a1, Real a0, Real Ts)
{
    cx_n = (2*b1 + b0*Ts)/(2*a1 + a0*Ts);           // C(x[n])
    cx_n_1 = (-2*b1 + b0*Ts)/(2*a1 + a0*Ts);        // C(x[n-1])
    cy_n_1 = (2*a1 - a0*Ts)/(2*a1 + a0*Ts);         // C(y[n-1])
}

void Filter1::SetState(Real y_, Real x_)
{
    y_n_1 = y_;
    x_n_1 = x_;
}

Real Filter1::Filter(Real x_n)
{
    Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cy_n_1*y_n_1;
    x_n_1 = x_n;
    y_n_1 = y_n;
    return y_n;
}

Filter1 Filter1::FilteredDerivative(Real Ts, Real tau)
{
    return Filter1(Real(1.), Real(0.), tau, Real(1.), Ts, Real(0.));
}

Filter2::Filter2(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts, Real y0)
{
    SetState(y0);
    ComputeCoeffsFromContinuousTF(b2, b1, b0, a2, a1, a0, Ts);
}

void Filter2::ComputeCoeffsFromContinuousTF(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts)
{
    Real Tssqr = Ts*Ts;
    cx_n   = (Real(4.)*b2 + Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr);       // C(x[n])
    cx_n_1 = (-Real(8.)*b2 + Real(2.)*b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr);              // C(x[n-1])
    cx_n_2 = (Real(4.)*b2 - Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr);       // C(x[n-2])
    cy_n_1 = (-Real(8.)*a2 + Real(2.)*a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr);              // C(y[n-1])
    cy_n_2 = (Real(4.)*a2 - Real(2.)*a1*Ts + a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr);       // C(y[n-2])
}

void Filter2::SetState(Real y_, Real x_)
{
    y_n_1 = y_;
    y_n_2 = y_;
    x_n_1 = x_;
    x_n_2 = x_;
}

Real Filter2::Filter(Real x_n)
{
    Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cx_n_2*x_n_2 - cy_n_1*y_n_1 - cy_n_2*y_n_2;
    x_n_2 = x_n_1;
    x_n_1 = x_n;
    y_n_2 = y_n_1;
    y_n_1 = y_n;
    return y_n;
}

Filter2 Filter2::FromPolesAndZeros(Real a, Complex p1, Complex p2, Complex z1, Complex z2, Real Ts)
{
    Complex b2(a), b1(a*(-z1 - z2)), b0(a*z1*z2),
                a2(Real(1.)), a1(-p1 - p2), a0(p1*p2);

    return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts);
}

Filter2 Filter2::FromPoles(Real a, Complex p1, Complex p2, Real Ts)
{
    Complex b2(Real(0.)), b1(Real(0.)), b0(a*p1*p2),
                a2(Real(1.)), a1(-p1 - p2), a0(p1*p2);
    return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts);
}

Filter2 Filter2::Lowpass(Real omega, Real Q, Real Ts)
{
    Real omegasqr = omega*omega;
    return Filter2(Real(0.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts);
}

Filter2 Filter2::Highpass(Real omega, Real Q, Real Ts)
{
    return Filter2(Real(1.), Real(0.), Real(0.), Real(1.), omega/Q, omega*omega, Ts);
}

Filter2 Filter2::Bandpass(Real omega, Real Q, Real Ts)
{
    return Filter2(Real(0.), omega/Q, Real(0.), Real(1.), omega/Q, omega*omega, Ts);
}

Filter2 Filter2::Bandstop(Real omega, Real Q, Real Ts)
{
    Real omegasqr = omega*omega;
    return Filter2(Real(1.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts);
}

// =============================================================================
// PID controller
// =============================================================================

PID::PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_, Real output_upper_bound_, Real tau_filtered_derivative, Real integrator_lower_bound_, Real integrator_upper_bound_, bool auto_anti_windup_, Real auto_anti_windup_margin_)
    :   Kp(Kp_),
        Ki(Ki_),
        Kd(Kd_),
        Ts(Ts_),
        output_lower_bound(output_lower_bound_),
        output_upper_bound(output_upper_bound_),
        auto_anti_windup_margin(auto_anti_windup_margin_),
        u_n_1(Real(0.)),
        auto_anti_windup(auto_anti_windup_),
        freeze_integration(false)
{
    integrator = Integrator1(Ts, integrator_lower_bound_, integrator_upper_bound_);
    differentiator = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);
}

Real PID::Filter(Real e_n)
{
    Real inte = integrator.Filter(Real(!ComputeFreezeIntegrator())*e_n);
    Real dere = differentiator.Filter(e_n);
    Real u = Kp*e_n + Ki*inte + Kd*dere;
    u = Sat1(u, output_lower_bound, output_upper_bound);
    u_n_1 = u;
    return u;
}

void PID::SetFreezeIntegration(bool freeze_integration_)
{
    freeze_integration = freeze_integration_;
}

void PID::SetFilteredDerivativeTimeConstant(Real tau_filtered_derivative)
{
    Filter1 differentiator2 = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);// Recompute filter coefficients
    differentiator2.x_n_1 = differentiator.x_n_1;
    differentiator2.y_n_1 = differentiator.y_n_1;// copy current state of filter for continuity
    differentiator = differentiator2;
}

void PID::SetIntegratorLimits(Real integrator_lower_bound, Real integrator_upper_bound)
{
    integrator.lower_bound = integrator_lower_bound;
    integrator.upper_bound = integrator_upper_bound;
}

void PID::SetIntegratorValue(Real integrator_value)
{
    integrator.SetState(integrator_value, integrator_value);
}

bool PID::ComputeFreezeIntegrator()
{
    // automatic anti-windup freezes the integrator when the option is activated and the input is past either bound minus the margin
    bool anti_windup_active = auto_anti_windup && ((u_n_1 <= output_lower_bound + auto_anti_windup_margin) || (u_n_1 >= output_upper_bound - auto_anti_windup_margin));

    // Final order is computed from the user-forced inhibition and the automatic inhibition
    return anti_windup_active || freeze_integration;
}

}// namespace FlyByWire