#include <catch2/catch.hpp>
#include <iostream>

#include "../utils.hpp"
#include "utils_test.hpp"
#include "../FlyByWire.hpp"

#define print(x) PRINT_VAR(x);
#define printvec(x) PRINT_VEC(x);
#define printstr(x) PRINT_STR(x);

const Real deg = M_PI/180.;
const Real tol = 1e-12;

TEST_CASE( "Basic FlyByWire functions", "[BasicFBW]" )
{
    std::cout.precision(16);

    SECTION( "Heading error computation" ) {
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(180, 45), 135., tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(45, 180), -135., tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(10, 270), 100., tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(270, 10), -100., tol));

        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(M_PI, 0.7853981633974483), 2.356194490192345, tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(0.7853981633974483, 3.141592653589793), -2.356194490192345, tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(0.17453292519943295, 4.71238898038469), 1.7453292519943295, tol));
        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(4.71238898038469, 0.17453292519943295), -1.7453292519943295, tol));
    }

    SECTION( "Vote" ) {
        Real a = 1.5, b = -1., c = 3.;
        REQUIRE(FlyByWire::Vote(a,b,c) == a);
        REQUIRE(FlyByWire::Vote(a,c,b) == a);
        REQUIRE(FlyByWire::Vote(b,a,c) == a);
        REQUIRE(FlyByWire::Vote(c,b,a) == a);
        REQUIRE(FlyByWire::Vote(c,a,b) == a);
    }

    SECTION( "Deadzone" ) {
        REQUIRE(FlyByWire::Deadzone(1.5, 0.1) == 1.5);
        REQUIRE(FlyByWire::Deadzone(-1.5, 0.1) == -1.5);
        REQUIRE(FlyByWire::Deadzone(0.1, 0.1) == 0.1);
        REQUIRE(FlyByWire::Deadzone(-0.1, 0.1) == -0.1);
        REQUIRE(FlyByWire::Deadzone(0.05, 0.1) == 0.);
    }

    SECTION( "Sat1" ) {
        REQUIRE(FlyByWire::Sat1(1.5, -0.2, 0.1) == 0.1);
        REQUIRE(FlyByWire::Sat1(-1.5, -0.2, 0.1) == -0.2);
        REQUIRE(FlyByWire::Sat1(0.1, -0.2, 0.1) == 0.1);
        REQUIRE(FlyByWire::Sat1(-0.2, -0.2, 0.1) == -0.2);
        REQUIRE(FlyByWire::Sat1(0.05, -0.2, 0.1) == 0.05);
        REQUIRE(FlyByWire::Sat1(-0.05, -0.2, 0.1) == -0.05);
    }

    SECTION( "Ratelim" ) {
        Real    y_n_1 = 10.,
                dy_dt_min = -1.5,
                dy_dt_max = 2.5,
                dt = 0.1;
        // REQUIRE(Ratelim(Real x_n, Real y_n_1, Real dy_dt_min, Real dy_dt_max, Real dt) == );
        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_max/2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_max/2.*dt);// Within bounds positive
        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_min/2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_min/2.*dt);// Within bounds negative
        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_max*2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_max*dt);// Outside bounds positive
        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_min*2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_min*dt);// Outside bounds negative
    }

    // REQUIRE(true);
    // CHECK(check_almost_equalV(v1, v2, 0.001));
}

TEST_CASE( "Integrator1", "[Integrator1]" )
{
    std::cout.precision(16);

    SECTION( "Integral of a known function" )
    {
        Real dt = 0.01, t = 0., v_int;
        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0.);

        for(int i = 0 ; i <= (int)(1./dt) ; i++)
        {
            t = i*dt;
            v_int = int1.Filter(t*t);
        }

        REQUIRE(check_almost_equal(v_int, 1./3., 1e-3));
    }

    SECTION( "Upper limit of integrator" )
    {
        Real dt = 0.1, lower_bound = -5., upper_bound = 2.5, v_int;
        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0., lower_bound, upper_bound);

        for(int i = 0 ; i <= (int)(1./dt) ; i++)
            v_int = int1.Filter(1000.);

        REQUIRE(check_almost_equal(v_int, upper_bound, tol));
    }

    SECTION( "Lower limit of integrator" )
    {
        Real dt = 0.1, lower_bound = -5., upper_bound = 2.5, v_int;
        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0., lower_bound, upper_bound);

        for(int i = 0 ; i <= (int)(1./dt) ; i++)
            v_int = int1.Filter(-1000.);

        REQUIRE(check_almost_equal(v_int, lower_bound, tol));
    }
}