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

}// namespace FlyByWire