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

using FlyByWire::Real;
using FlyByWire::Complex;

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

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

    SECTION( "Filter coefficient computation" )
    {
        Real    b1 = 1, b0 = 2,
                a1 = 3, a0 = 4,
                dt = 0.1;
        FlyByWire::Filter1 filt1(b1, b0, a1, a0, dt);
        REQUIRE(check_almost_equal(filt1.cx_n, 0.343750000000000, tol));
        REQUIRE(check_almost_equal(filt1.cx_n_1, -0.281250000000000, tol));
        REQUIRE(check_almost_equal(filt1.cy_n_1, 0.875000000000000, tol));
    }
}

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

    SECTION( "Filter coefficient computation" )
    {
        Real    b2 = 1., b1 = 2., b0 = 3.,
                a2 = 4., a1 = 5., a0 = 6.,
                dt = 0.1;
        FlyByWire::Filter2 filt2(b2, b1, b0, a2, a1, a0, dt);
        REQUIRE(check_almost_equal(filt2.cx_n, 0.259671746776084, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_1, -0.465416178194607, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_2, 0.212778429073857, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_1, -1.868698710433763, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_2, 0.882766705744432, tol));
    }

    SECTION( "Filter coefficient computation from poles and zeros" )
    {
        Real    a = 1.5,
                dt = 0.1;
        Complex p1(-1,2), p2(-1,-2), z1(-2,1), z2(-2,-1);
        FlyByWire::Filter2 filt2 = FlyByWire::Filter2::FromPolesAndZeros(a, p1, p2, z1, z2, dt);
        REQUIRE(check_almost_equal(filt2.cx_n, 1.634831460674157, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_1, -2.6629213483146064, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_2, 1.095505617977528, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_1, -1.775280898876405, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_2, 0.820224719101123, tol));

    }

    SECTION( "Filter coefficient computation from poles" )
    {
        Real    a = 1.5,
                dt = 0.1;
        Complex p1(-1,2), p2(-1,-2);
        FlyByWire::Filter2 filt2 = FlyByWire::Filter2::FromPoles(a, p1, p2, dt);
        REQUIRE(check_almost_equal(filt2.cx_n, 0.016853932584270, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_1, 0.033707865168539, tol));
        REQUIRE(check_almost_equal(filt2.cx_n_2, 0.016853932584270, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_1, -1.775280898876405, tol));
        REQUIRE(check_almost_equal(filt2.cy_n_2, 0.820224719101123, tol));

    }
}