#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

// The design structure of my test suite is to test every possible option for the direction of routes, for example:
// - a route where the elevation increases
// - a route where the elevation decreases
// - a route where the elevation is stationary
// - a route where the elevation increases and then decreases
// - a route where the elevation decreases and then increases
// - a route where there is only one point meaning an invalid argument should be thrown
// - a route where the elevation does not change through oyt the route which lead to a flat surface route

//how the check value hasve been worked out:
// in a three dimension where the longitude and latitude are the vertical axies and the elevation is the horizontal axie
// by using pythagoras rule a^2 = b^2 + c^2
// where ( a ) is the gradient
//       ( b ) is the difference between the elevation of two point
//       ( c ) is the  distent between two point using the Law of haversines using the horizantal axies
// the gardient will be zero if the (b) is equal to zero

using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_MinGradient_n0719479 )

const bool isFileName = true;

// uphill route AGM:
// M is Pontianak with Position(0,109.322134,0) the vertical, horizontal units for this grid is 10 and 120000
// as the route increases, the smallest gradient should be the smallest positive value between them
BOOST_AUTO_TEST_CASE( uphill_route )
{
   Route route = Route(LogFiles::GPXRoutesDir + "going_up_a_hill_route_n0719479"+".gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), 169625, 0.1);
}

// downhill route MSY:
// M is citycampus with Position(52.9581383,-1.1542364,53) the vertical, horizontal units for this grid is 10 and 120000
// as the route decreases, the smallest gradient should be the biggest negative value between them
BOOST_AUTO_TEST_CASE( downhill_route )
{
   Route route = Route(LogFiles::GPXRoutesDir + "going_down_a_hill_route_n0719479"+".gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -172816, 0.1 );
}

// uphill then downhill route AGMX:
// M is citycampus with Position(52.9581383,-1.1542364,53) the vertical, horizontal units for this grid is 10 and 120000
// as the route increases then decreases, the smallest gradient should be the biggest negative value between them
BOOST_AUTO_TEST_CASE( up_down_hill_route )
{
   Route route = Route(LogFiles::GPXRoutesDir + "going_up_down_hill_route_n0719479"+".gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -269700, 0.1);
}

// downhill then uphill route MDI:
// M is Pontianak with Position(0,109.322134,0) the vertical, horizontal units for this grid is 10 and 120000
// as the route decreases then increases, the smallest gradient should be the biggest negative value between them
BOOST_AUTO_TEST_CASE( down_up_hill_route )
{
   Route route = Route(LogFiles::GPXRoutesDir + "going_down_up_hill_route_n0719479"+".gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -268,376, 0.1 );
}

// route containing only 1 point:
// as there is only one point and the gradient cannot be calculated, an invalid arugment should be thrown
BOOST_AUTO_TEST_CASE( one_point_location )
{
   Route route = Route(LogFiles::GPXRoutesDir + "one_point_location_n0719479"+".gpx", isFileName);
   BOOST_CHECK_THROW( route.minGradient() , std::invalid_argument );
}

//flat route LQRS:
// M is Pontianak with Position(0,109.322134,0) the vertical, horizontal units for this grid is 10 and 120000
// as the route is flat, the gradient should be zero
BOOST_AUTO_TEST_CASE( flat_surface_route )
{
   Route route = Route(LogFiles::GPXRoutesDir + "flat_route_n0719479"+".gpx", isFileName);
   BOOST_CHECK_EQUAL( route.minGradient(), 0 );
}
BOOST_AUTO_TEST_SUITE_END()


//####### Test module for chi functions #############################

//Define Module name
 #define BOOST_TEST_MODULE "phys/chi_functions"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <picsar_qed/physics/chi_functions.hpp>

#include <picsar_qed/physics/unit_conversion.hpp>
#include <picsar_qed/math/vec_functions.hpp>

using namespace picsar::multi_physics::phys;
using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-6;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

const double me_c = electron_mass<double> * light_speed<double>;
const double lambda = 800.0e-9;
const double omega = 2.0*pi<double>*light_speed<double>/lambda;
const double eref = 2.0*pi<double>*electron_mass<double>*
    light_speed<double>*light_speed<double>/(lambda*elementary_charge<double>);
const double bref = eref/light_speed<double>;

// ------------- Tests --------------

//***Test chi calculation for photons

template<typename RealType, unit_system UnitSystem>
void test_chi_photons(
    vec3<double> t_p, vec3<double> t_em_e, vec3<double> t_em_b,
    double t_exp_res, double t_ref_quant = 1.0)
{
    const auto p =  vec3<RealType>{
        static_cast<RealType>(t_p[0]),
        static_cast<RealType>(t_p[1]),
        static_cast<RealType>(t_p[2])}*
        conv<quantity::momentum, unit_system::SI, UnitSystem, RealType>::fact();

    const auto em_e =  vec3<RealType>{
        static_cast<RealType>(t_em_e[0]),
        static_cast<RealType>(t_em_e[1]),
        static_cast<RealType>(t_em_e[2])}*
        conv<quantity::E, unit_system::SI, UnitSystem, RealType>::fact(1.0, t_ref_quant);

    const auto em_b =  vec3<RealType>{
        static_cast<RealType>(t_em_b[0]),
        static_cast<RealType>(t_em_b[1]),
        static_cast<RealType>(t_em_b[2])}*
        conv<quantity::B, unit_system::SI, UnitSystem, RealType>::fact(1.0, t_ref_quant);

    const auto res = chi_photon<RealType, UnitSystem>(p, em_e, em_b, t_ref_quant);

    const auto exp_res = static_cast<RealType>(t_exp_res);

    if(exp_res != static_cast<RealType>(0.0))
        BOOST_CHECK_SMALL((res-exp_res)/exp_res, tolerance<RealType>());
    else
        BOOST_CHECK_SMALL(res, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( chi_photons_1 )
{
    const double px = 83.759*me_c;
    const double py = 139.311*me_c;
    const double pz = -230.553*me_c;
    const double ex = -166.145*eref;
    const double ey = -78.231*eref;
    const double ez = -278.856*eref;
    const double bx = -279.174*bref;
    const double by = -158.849*bref;
    const double bz = -93.826*bref;

    const double chi_exp = 0.3471118445206898;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_photons_2 )
{
    const double px = 9.2627*me_c;
    const double py = -25.4575*me_c;
    const double pz = -10.2246*me_c;
    const double ex = 2.9271*eref;
    const double ey = 10.4293*eref;
    const double ez = 3.6103*eref;
    const double bx = 1.7439*bref;
    const double by = 1.9778*bref;
    const double bz = 17.8799*bref;

    const double chi_exp = 0.0009041474058668584;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_photons_3 )
{
    const double px = -2314.45*me_c;
    const double py = -2356.30*me_c;
    const double pz = 546.28*me_c;
    const double ex = 1230.11*eref;
    const double ey = 1638.02*eref;
    const double ez = -2911.04*eref;
    const double bx = -2203.66*bref;
    const double by = 1243.79*bref;
    const double bz = -2830.99*bref;

    const double chi_exp = 57.22043983047014;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_photons_4 )
{
    const double px = 0*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;
    const double ex = 1230.11*eref;
    const double ey = 1638.02*eref;
    const double ez = -2911.04*eref;
    const double bx = -2203.66*bref;
    const double by = 1243.79*bref;
    const double bz = -2830.99*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_photons_5 )
{
    const double px = -2314.45*me_c;
    const double py = -2356.30*me_c;
    const double pz = 546.28*me_c;
    const double ex = 0.0*eref;
    const double ey = 0.0*eref;
    const double ez = 0.0*eref;
    const double bx = 0.0*bref;
    const double by = 0.0*bref;
    const double bz = 0.0*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_photons_6 )
{
    const double px = -2314.45*me_c;
    const double py = 0.0*me_c;
    const double pz = 0.0*me_c;
    const double ex = 1230.11*eref;
    const double ey = 0.0*eref;
    const double ez = 0.0*eref;
    const double bx = 0.0*bref;
    const double by = 0.0*bref;
    const double bz = 0.0*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_photons<double, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_photons<float, unit_system::SI>(p, em_e, em_b, chi_exp);
    test_chi_photons<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_photons<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_photons<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

//*******************************

//***Test chi calculation for electrons and positrons

template<typename RealType, unit_system UnitSystem>
void test_chi_ele_pos(
    vec3<double> t_p, vec3<double> t_em_e, vec3<double> t_em_b,
    double t_exp_res, double t_ref_quant = 1.0)
{
    const auto p =  vec3<RealType>{
        static_cast<RealType>(t_p[0]),
        static_cast<RealType>(t_p[1]),
        static_cast<RealType>(t_p[2])}*
        conv<quantity::momentum, unit_system::SI, UnitSystem, RealType>::fact();

    const auto em_e =  vec3<RealType>{
        static_cast<RealType>(t_em_e[0]),
        static_cast<RealType>(t_em_e[1]),
        static_cast<RealType>(t_em_e[2])}*
        conv<quantity::E, unit_system::SI, UnitSystem, RealType>::fact(1.0, t_ref_quant);

    const auto em_b =  vec3<RealType>{
        static_cast<RealType>(t_em_b[0]),
        static_cast<RealType>(t_em_b[1]),
        static_cast<RealType>(t_em_b[2])}*
        conv<quantity::B, unit_system::SI, UnitSystem, RealType>::fact(1.0, t_ref_quant);

    const auto res = chi_ele_pos<RealType, UnitSystem>(p, em_e, em_b, t_ref_quant);

    const auto exp_res = static_cast<RealType>(t_exp_res);

    if(exp_res != static_cast<RealType>(0.0))
        BOOST_CHECK_SMALL((res-exp_res)/exp_res, tolerance<RealType>());
    else
        BOOST_CHECK_SMALL(res, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( chi_ele_pos_1 )
{
    const double px = 24.3752*me_c;
    const double py = -11.5710*me_c;
    const double pz = -10.0841*me_c;
    const double ex = 57.185*eref;
    const double ey = -16.6555*eref;
    const double ez = 22.4340*eref;
    const double bx = 6.6911*bref;
    const double by = -23.8724*bref;
    const double bz = 13.9934*bref;

    const double chi_exp = 0.002167166273468506;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);

}

BOOST_AUTO_TEST_CASE( chi_ele_pos_2 )
{
    const double px = 4.015*me_c;
    const double py = 197.287*me_c;
    const double pz = 141.705*me_c;
    const double ex = 30.287*eref;
    const double ey = 115.740*eref;
    const double ez = 120.891*eref;
    const double bx = -190.161*bref;
    const double by = -129.115*bref;
    const double bz = -57.002*bref;

    const double  chi_exp = 0.16631811297207244;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_ele_pos_3 )
{
    const double px = -2534.83*me_c;
    const double py = 1011.54*me_c;
    const double pz = -793.04*me_c;
    const double ex = 741.67*eref;
    const double ey = -2359.97*eref;
    const double ez = 1463.50*eref;
    const double bx = 1477.19*bref;
    const double by = -1448.33*bref;
    const double bz = 1953.68*bref;

    const double chi_exp = 16.011457274095676;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_ele_pos_4 )
{
    const double px = 0*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;
    const double ex = 741.67*eref;
    const double ey = -2359.97*eref;
    const double ez = 1463.50*eref;
    const double bx = 1477.19*bref;
    const double by = -1448.33*bref;
    const double bz = 1953.68*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_ele_pos_5 )
{
    const double px = -2534.83*me_c;
    const double py = 1011.54*me_c;
    const double pz = -793.04*me_c;
    const double ex = 0*eref;
    const double ey = 0*eref;
    const double ez = 0*eref;
    const double bx = 0*bref;
    const double by = 0*bref;
    const double bz = 0*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( chi_ele_pos_6 )
{
    const double px = -2534.83*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;
    const double ex = 0*eref;
    const double ey = 0*eref;
    const double ez = 0*eref;
    const double bx = 1477.19*bref;
    const double by = 0*bref;
    const double bz = 0*bref;

    const double chi_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};
    const auto em_e = vec3<double>{ex,ey,ez};
    const auto em_b = vec3<double>{bx,by,bz};

    test_chi_ele_pos<double, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<double, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<double, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<double, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::SI>(p, em_e, em_b, chi_exp, 1.0);
    test_chi_ele_pos<float, unit_system::norm_lambda>(p, em_e, em_b, chi_exp, lambda);
    test_chi_ele_pos<float, unit_system::norm_omega>(p, em_e, em_b, chi_exp, omega);
    test_chi_ele_pos<float, unit_system::heaviside_lorentz>(p, em_e, em_b, chi_exp, 1.0);
}

//*******************************


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <fstream>
#include <iostream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;
typedef K::Point_2                                                  Point;

typedef CGAL::Delaunay_triangulation_2<K>                           Triangulation;

typedef boost::graph_traits<Triangulation>::vertex_descriptor       vertex_descriptor;
typedef boost::graph_traits<Triangulation>::vertex_iterator         vertex_iterator;
typedef boost::graph_traits<Triangulation>::edge_descriptor         edge_descriptor;

// The BGL makes use of indices associated to the vertices
// We use a std::map to store the index
typedef std::map<vertex_descriptor,int>                             VertexIndexMap;

// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<VertexIndexMap>             VertexIdPropertyMap;

int main(int argc,char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/points.xy";
  std::ifstream input(filename);
  Triangulation tr;

  Point p;
  while(input >> p)
    tr.insert(p);

  // Associate indices to the vertices
  VertexIndexMap vertex_id_map;
  VertexIdPropertyMap vertex_index_pmap(vertex_id_map);
  int index = 0;

  for(vertex_descriptor vd : vertices(tr))
    vertex_id_map[vd] = index++;

  // We use the default edge weight which is the squared length of the edge
  // This property map is defined in graph_traits_Triangulation_2.h

  // In the function call you can see a named parameter: vertex_index_map
  std::list<edge_descriptor> mst;
  boost::kruskal_minimum_spanning_tree(tr, std::back_inserter(mst),
                                       vertex_index_map(vertex_index_pmap));

  std::cout << "The edges of the Euclidean mimimum spanning tree:" << std::endl;
  for(edge_descriptor ed : mst)
  {
    vertex_descriptor svd = source(ed, tr);
    vertex_descriptor tvd = target(ed, tr);
    Triangulation::Vertex_handle sv = svd;
    Triangulation::Vertex_handle tv = tvd;
    std::cout << "[ " << sv->point() << "  |  " << tv->point() << " ] " << std::endl;
  }

  return EXIT_SUCCESS;
}


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include "test_common.h"
#include "vw/core/vw_math.h"

#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(math_factorial_tests)
{
  BOOST_CHECK_EQUAL(VW::math::factorial(0), 1);
  BOOST_CHECK_EQUAL(VW::math::factorial(1), 1);
  BOOST_CHECK_EQUAL(VW::math::factorial(2), 2);
  BOOST_CHECK_EQUAL(VW::math::factorial(3), 6);
  BOOST_CHECK_EQUAL(VW::math::factorial(4), 24);
  BOOST_CHECK_EQUAL(VW::math::factorial(5), 120);
  BOOST_CHECK_EQUAL(VW::math::factorial(10), 3628800);
}

BOOST_AUTO_TEST_CASE(math_number_of_combinations_with_repetition_tests)
{
  BOOST_CHECK_EQUAL(VW::math::number_of_combinations_with_repetition(1, 1), 1);
  BOOST_CHECK_EQUAL(VW::math::number_of_combinations_with_repetition(5, 1), 5);
  BOOST_CHECK_EQUAL(VW::math::number_of_combinations_with_repetition(1, 5), 1);
  BOOST_CHECK_EQUAL(VW::math::number_of_combinations_with_repetition(5, 5), 126);
  BOOST_CHECK_EQUAL(VW::math::number_of_combinations_with_repetition(10, 2), 55);
}

BOOST_AUTO_TEST_CASE(math_number_of_permutations_with_repetition_tests)
{
  BOOST_CHECK_EQUAL(VW::math::number_of_permutations_with_repetition(1, 1), 1);
  BOOST_CHECK_EQUAL(VW::math::number_of_permutations_with_repetition(5, 1), 5);
  BOOST_CHECK_EQUAL(VW::math::number_of_permutations_with_repetition(1, 5), 1);
  BOOST_CHECK_EQUAL(VW::math::number_of_permutations_with_repetition(5, 5), 3125);
  BOOST_CHECK_EQUAL(VW::math::number_of_permutations_with_repetition(10, 2), 100);
}

BOOST_AUTO_TEST_CASE(math_sign_tests)
{
  BOOST_CHECK_CLOSE(VW::math::sign(1.f), 1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::math::sign(-1.f), -1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::math::sign(0.f), -1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::math::sign(0.1f), 1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::math::sign(1000.f), 1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::math::sign(-999.f), -1.f, FLOAT_TOL);
}

BOOST_AUTO_TEST_CASE(math_choose_tests)
{
  BOOST_CHECK_EQUAL(VW::math::choose(1, 1), 1);
  BOOST_CHECK_EQUAL(VW::math::choose(4, 2), 6);
  BOOST_CHECK_EQUAL(VW::math::choose(2, 4), 0);
  BOOST_CHECK_EQUAL(VW::math::choose(0, 0), 1);
  BOOST_CHECK_EQUAL(VW::math::choose(0, 1), 0);
  BOOST_CHECK_EQUAL(VW::math::choose(1, 0), 1);
}


#include <boost/dynamic_bitset.hpp>
#include <cmath>
#include <iostream>
#include <vector>

/**
 * Generate all prime numbers up to a limit
 * @param limit test for primality up to this limit, not including the limit
 * @return the prime numbers
 */
std::vector<unsigned long> getPrimes( unsigned long limit )
{
  std::vector<unsigned long> primes;

  // special cases
  if ( limit <= 5 )
  {
    if ( limit > 2 )
    {
      primes.push_back( 2 );
      if ( limit > 3 )
      {
	primes.push_back( 3 );
      }
    }
    return primes;
  }

  // multiples of 2 and 3 will not be used, just add them by hand
  primes.push_back( 2 );
  primes.push_back( 3 );

  // crossed out nonprimes
  boost::dynamic_bitset<> crossOut( limit );

  // add primes to result and cross out nonprimes
  unsigned long crossOutLimit = static_cast<unsigned long>( sqrt( limit ) ) + 1;
  unsigned long i = 5;
  for ( ; i < crossOutLimit; i += 6 )
  {
    for ( unsigned long d = 0; d != 4; d += 2 )
    {
      unsigned long num = i + d;
      if ( !crossOut.test( num ) )
      {
	primes.push_back( num );
	for ( unsigned long j = num * num; j < limit; j += num )
	{
	  crossOut.set( j );
	}
      }
    }
  }

  // add extra primes to result
  for ( ; i < limit - 2; i += 6 )
  {
    for ( unsigned long d = 0; d != 4; d += 2 )
    {
      unsigned long num = i + d;
      if ( !crossOut.test( num ) )
      {
	primes.push_back( num );
      }
    }
  }

  // one may be missing
  if ( i < limit && !crossOut.test( i ) )
  {
    primes.push_back( i );
  }

  return primes;
}

int main()
{
  unsigned long limit = 10000000;
  std::vector<unsigned long> primes = getPrimes( limit );
  std::cout << "Trere are " << primes.size() << " primes below " << limit << "." << std::endl;
  return 0;
}


#include <bits/stdc++.h>

// Rational number
// Positive/negative infinity can be allowed.
template <typename T, bool allow_infinity = false>
struct Rational {
 public:
  // #include <boost/multiprecision/cpp_int.hpp>
  // using multip = boost::multiprecision;
  // using BigInt = boost::multiprecision::checked_int128_t;
  using BigInt = __int128_t;

  T nume, deno;

  Rational() : nume(0), deno(1) {}

  Rational(T n) : nume(n), deno(1) {}

  Rational(BigInt n, BigInt d) {
    if constexpr (allow_infinity) {
      assert(d != 0 or n != 0);  // 0/0 is undefined
    } else {
      assert(d != 0);
    }
    if (allow_infinity and d == 0) {
      n = (n < 0) ? -1 : 1;  // infinity
    } else if (n == 0) {
      d = 1;
    } else {
      const int sign = ((n < 0) xor (d < 0)) ? -1 : 1;
      if (n < 0) n = -n;
      if (d < 0) d = -d;
      const auto g = gcd_(n, d);
      if (g > 1) {
        n /= g;
        d /= g;
      }
      n *= sign;
    }
    nume = static_cast<T>(n);
    deno = static_cast<T>(d);
  }

  Rational(const Rational &x) = default;
  Rational(Rational &&x) = default;
  Rational &operator=(const Rational &x) = default;
  Rational &operator=(Rational &&x) = default;

  // Cast to a floating point number.
  template <typename Float>
  explicit operator Float() const {
    static_assert(std::is_floating_point<Float>::value);
    return (Float)nume / (Float)deno;
  }

  Rational operator+() const { return *this; }
  Rational operator-() const {
    Rational ret;
    ret.nume = -nume;
    ret.deno = deno;
    return ret;
  }

  friend bool operator==(const Rational &x, const Rational &y) {
    return (x.nume == y.nume) and (x.deno == y.deno);
  }
  friend bool operator!=(const Rational &x, const Rational &y) {
    return not(x == y);
  }
  friend bool operator<(const Rational &x, const Rational &y) {
    if constexpr (allow_infinity) {
      if (x.deno == 0 and y.deno == 0) return x.nume < y.nume;
    }
    return static_cast<BigInt>(x.nume) * y.deno <
           static_cast<BigInt>(y.nume) * x.deno;
  }
  friend bool operator>(const Rational &x, const Rational &y) { return y < x; }
  friend bool operator<=(const Rational &x, const Rational &y) {
    return not(x > y);
  }
  friend bool operator>=(const Rational &x, const Rational &y) {
    return not(x < y);
  }

  friend Rational operator+(const Rational &x, const Rational &y) {
    if constexpr (allow_infinity) {
      if (x.deno == 0 and y.deno == 0) {
        assert(x.nume == y.nume);  // (infinity - infinity) is undefined
        return x;
      }
      if (x.deno == 0) return x;
      if (y.deno == 0) return y;
    }
    auto g = gcd_(x.deno, y.deno);
    BigInt xd = x.deno / g;
    BigInt yd = y.deno / g;
    BigInt zn = x.nume * yd + y.nume * xd;
    BigInt zd = xd * y.deno;
    return Rational(std::move(zn), std::move(zd));
  }
  Rational &operator+=(const Rational &x) { return (*this = *this + x); }

  friend Rational operator-(const Rational &x, const Rational &y) {
    return x + (-y);
  }
  Rational &operator-=(const Rational &x) { return (*this = *this - x); }

  friend Rational operator*(const Rational &x, const Rational &y) {
    if (x.nume == 0 or y.nume == 0) return Rational(0);
    auto g1 = gcd_(abs_(x.nume), y.deno);
    auto g2 = gcd_(abs_(y.nume), x.deno);
    return Rational(BigInt(x.nume / g1) * BigInt(y.nume / g2),
                    BigInt(x.deno / g2) * BigInt(y.deno / g1));
  }
  Rational &operator*=(const Rational &x) { return (*this = *this * x); }

  Rational inv() const { return Rational(deno, nume); }

  friend Rational operator/(const Rational &x, const Rational &y) {
    return x * y.inv();
  }
  Rational &operator/=(const Rational &x) { return (*this = *this / x); }

  friend std::ostream &operator<<(std::ostream &os, const Rational &x) {
    return os << x.nume << "/" << x.deno;
  }

 private:
  template <typename U>
  static inline U abs_(const U &x) {
    // return std::abs(x);
    // return boost::multiprecision::abs(x);
    return (x < 0) ? -x : x;
  }
  template <typename U>
  static U gcd_(const U &a, const U &b) {
    // return std::gcd(a, b);
    return (b == 0) ? a : gcd_(b, a % b);
  }
};
using Rat = Rational<long long, false>;
// using Rat = Rational<multip::checked_int128_t>; // for testing overflow

// Parse a decimal number into a rational.
// (e.g. "0.2029" => 2029/10000 )
std::istream &operator>>(std::istream &is, Rat &x) {
  long long nume = 0, deno = 1;
  char ch;
  while (is.get(ch)) {
    if (not std::isspace(ch)) break;
  }
  int sgn = 1;
  if (ch == '-') {
    sgn = -1;
    is.get(ch);
  }
  bool in_frac = false;
  while (true) {
    if (not std::isdigit(ch)) {
      is.unget();
      break;
    }
    nume = (nume * 10) + int(ch - '0');
    if (in_frac) deno *= 10;
    if (not(is.get(ch))) break;
    if (ch == '.') {
      in_frac = true;
      if (not(is.get(ch))) break;
    }
  }
  x = Rat(nume * sgn, deno);
  return is;
}


// STL includes
#include <iostream>
#include <vector>
#include <set>
#include <string>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

using namespace std;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
typedef boost::graph_traits<graph>::vertex_descriptor                       vertex_desc;

bool perfect_matching(const graph &G) {
  int n = boost::num_vertices(G);
  vector<vertex_desc> mate_map(n);  // exterior property map

  boost::edmonds_maximum_cardinality_matching(G,
    boost::make_iterator_property_map(mate_map.begin(), boost::get(boost::vertex_index, G)));
  int matching_size = boost::matching_size(G,
    boost::make_iterator_property_map(mate_map.begin(), boost::get(boost::vertex_index, G)));

  return 2 * matching_size == n;
}

void solve() {

  int n; cin >> n;
  int c; cin >> c;
  unsigned int f; cin >> f;

  graph G(n);

  string x;
  vector<set<string>> ch(n, set<string>());
  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < c; ++j) {
      cin >> x;
      ch[i].insert(x);
    }
  }

  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < i; ++j) {
        vector<string> intersection;
        set_intersection(ch[i].begin(), ch[i].end(), ch[j].begin(), ch[j].end(), back_inserter(intersection));
        if (intersection.size() > f) {
            boost::add_edge(i, j, G);
        }
    }
  }

  cout << (perfect_matching(G) ? "not optimal" : "optimal") << endl;
}

int main()
{
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
  return 0;
}


/** @file mvn_cdf.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief
 *
 */
#include "PriorHessian/mvn_cdf.h"

#include <cmath>
#include <limits>

#include <armadillo>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>

namespace {
    const double sqrt2 = sqrt(2.);
    const double inv_sqrt2 = 1./sqrt2;
    const double inv_2pi = 1./(2.*arma::datum::pi);
}

namespace prior_hessian {

/** area of the lower tail of the unit normal curve below t. */
double unit_normal_cdf( double t )
{
    if(t==-INFINITY) return 0;
    if(t==INFINITY) return 1;
    return std::max(std::min(.5*std::erfc(-t*::inv_sqrt2),1.0),0.0);
}

double unit_normal_icdf( double u )
{
    if(u<=0) return -INFINITY;
    if(u>=1) return INFINITY;
    return -::sqrt2*boost::math::erfc_inv(2*u);
}

double bounded(double x)
{
    return std::min(std::max(x,0.),1.);
}

/* Integrates the between y(x)=0 and y(x) = a*x, and right of h, to infinity
 *
 * when h=0 this is the triangle from the x-axis with angle arctan(a)
 * h - location of vertical line to integrate to the right from
 * a - slope of line above the x-axis
 * gh - normcdf(h);
 */
double owen_t_integral(double h, double a, double gh)
{
    //Check pre-conditions
    if(std::isnan(h)) throw ParameterValueError("a is NaN");
    if(!(gh >=0 && gh<=1)) throw ParameterValueError("gh is not in [0,1]");
    //Edge cases
    if(h == INFINITY) return 0;
    if(std::isnan(a)) throw ParameterValueError("a is NaN");
    if(a == 0) return 0;
    if(h == 0) return std::atan(a)*::inv_2pi;
    if(a == INFINITY) return  (h>0) ? .5*(1-gh) : .5*gh;
    if(a==1) return .5*gh*(1-gh); //This formula works for h and -h by symmetry
    //Use owens 2.4 and 2.5 to handle negative h and a
    if(a<0 && h<0) return -owen_t_integral(-h,-a,1-gh);
    if(a<0) return -owen_t_integral(h,-a,gh);
    if(h<0) return owen_t_integral(-h,a,1-gh);

    assert(h>0 && std::isfinite(h));
    assert(a>0 && std::isfinite(a));
    //Use Owens 2.3 to invert a if >1.  This requires an additional normcdf call.
    if(a>1) {
        double ah = a*h;
        double gah = unit_normal_cdf(ah);
        double v1 = owen_t_integral(ah,1/a,gah);
        return .5*(gh+gah) -gh*gah - v1;
    }

    assert(a>0 && a<1);

    const int max_iter = 1000;
    const double eps = 1E-10;
    double theta = std::atan(a);
    double asq = a*a;
    double asq_powj = asq;
    double h2 = h*h/2;
    double e2h = std::exp(-h2);
    if(e2h==0) return 0;
    double Qj = 1;
    double Sj = Qj;
    double s = 1 - e2h; //First term of series.
    for(int j=1; j<max_iter; j++) {
        Qj *= h2/j;
        Sj += Qj;
        double sign = (j%2==0) ? 1 : -1;
        double v = sign/(2*j+1) * (1-e2h*Sj) * asq_powj;
        s += v;
        if(fabs(v/s) < eps) return ::inv_2pi*(theta - a*s);
        asq_powj *= asq;
    }
    std::ostringstream msg;
    msg<<"Power series failed to converge in max_iter="<<max_iter<<" iterations. h="<<h<<" a="<<a<<" sum="<<s;
    throw RuntimeConvergenceError(msg.str());
}

double owen_b_integral(double h,double k, double r)
{
    if(std::isnan(h)) throw ParameterValueError("h is NaN");
    if(std::isnan(k)) throw ParameterValueError("k is NaN");
    if(fabs(r)>1 || !std::isfinite(r)) throw ParameterValueError("r is not in interval [-1,1]");

    if(h==-INFINITY || k==-INFINITY) return 0;
    if(h==INFINITY && k==INFINITY) return 1;
    if(fabs(r)==1) { //Degenerate case.
        if(r>0) return unit_normal_cdf(std::min(h,k));
        else return unit_normal_cdf(h) - unit_normal_cdf(-k);
    }
    assert(fabs(r)<1);
    double sigma = sqrt(1-r*r);
    double gh = unit_normal_cdf(h);
    double ah = (k-r*h)/(h*sigma);
    double bint;
    if(h==k) {
        if(h==0.) return .25+asin(r)*::inv_2pi;
        bint = gh - 2*owen_t_integral(h,ah,gh);
    } else if(h==-k) {
        bint = .5 - 2*owen_t_integral(h,ah,gh);
    } else {
        double gk = unit_normal_cdf(k);
        double ak = (h-r*k)/(k*sigma);
        double t1 = owen_t_integral(h,ah,gh);
        double t2 = owen_t_integral(k,ak,gk);
        bint = .5*(gh+gk) - t1 - t2;
    }
    if( h*k < 0 || (h*k == 0 && (h<0 || k<0))) bint -= .5;
    return std::min(std::max(bint,0.0),1.0);
}

/** compute the bivariate normal cdf integral
 * computes the probability for two normal variates X and Y
 *    whose correlation is R, that AH <= X and AK <= Y.
 *
 * Adapted to modern C++ with efficiency improvements by:
 * Mark Olah (mjo@cs.unm DOT edu)
 * 10/2018
 *
 * Reference:
 *    Thomas Donnelly,
 *    Algorithm 462: Bivariate Normal Distribution,
 *    Communications of the ACM,
 *    October 1973, Volume 16, Number 10, page 638.
 */
double donnelly_bvn_integral( double ah, double ak, double r )
{
    static double eps = 1.0E-15;
    if (r<-1 || r>1) throw ParameterValueError("must have -1<=rho<=1");
    if ((ah==INFINITY) || (ak==INFINITY)) return 0;
    if ((ah==-INFINITY) && (ak==-INFINITY)) return 1;

    double gh = unit_normal_cdf(-ah)/2.0;
    double gk = unit_normal_cdf(-ak)/2.0;

    if(r == 0.0) return bounded(4*gh*gk);

    double b = 0.0; //return value
    double rr = (1+r)*(1-r);
    if(rr==0) { //Degenerate cases r=-1 r=0 r=1
        if(r<0) {
            if(ah+ak<0) b = 2*(gh+gk)-1;
        } else {
            if(ah-ak<0) b = 2*gk;
            else b = 2*gh;
        }
        return bounded(b);
    }

    double sigma_inv = 1./sqrt(rr);
    double con = arma::datum::pi * eps;
    double wh;
    double wk;
    double gw;
    int is;
    if(ah == 0) {
        if(ak == 0) return bounded(0.25+asin(r)*::inv_2pi); // (0,0)
        //(0, !0)
        b = gk;
        wh = -ak;
        wk = (ah/ak - r)*sigma_inv;
        gw = 2*gk;
        is = 1;
    } else {
        //  (!0, 0)
        b = gh;
        wh = -ah;
        wk = (ak/ah - r)*sigma_inv;
        gw = 2*gh;
        is = -1;
        if(ak != 0) { // ( !0, !0)
            b = gh+gk;
            if (ah*ak < 0) b-= 0.5;
        }
    }
//     std::cout<<"NEW ah:"<<ah<<" ak:"<<ak<<" rho:"<<r<<" gh:"<<gh<<" gk:"<<gk<<"\n";
//     std::cout<<"NEW b:"<<b<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";
    for ( ; ; ) {
        double sgn = -1.0;
        double t = 0.0;
//         std::cout<<"n b:"<<b<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";
        if(wk != 0) {
            if(fabs(wk) == 1) {
                t = .5*wk*gw*(1-gw);
                b += sgn*t;
            } else {
                if (fabs(wk) > 1) {
                    sgn = -sgn;
                    wh *= wk;
                    double g2 = unit_normal_cdf(wh);
                    wk = 1./wk;
                    if(wk < 0) b += 0.5;
                    b += -.5*(gw+g2) + gw*g2;
//                     std::cout<<"nn b: "<<b<<" gw:"<<gw<<" g2:"<<g2<<" t:"<<t<<" sgn:"<<sgn<<" wh:"<<wh<<" wk:"<<wk<<"\n";
                }
                double h2 = wh*wh;
                double a2 = wk*wk;
                double h4 = .5*h2;
                double ex = exp(-h4);
                double w2 = h4*ex;
                double ap = 1.0;
                double s2 = ap - ex;
                double sp = ap;
                double s1 = 0.0;
                double sn = s1;
                double conex = fabs(con/wk);

                for ( ; ; ) {
                    double cn = ap*s2/(sn+sp);
                    s1 += cn;
//                     std::cout<<"NEW cn: "<<cn<<" s1:"<<s1<<" conex:"<<conex<<" sn:"<<sn<<" sp:"<<sp<<" s2:"<<s2<<" w2"<<w2<<" ap:"<<ap<<"\n";

                    if(fabs(cn) <= conex) break;

                    sn = sp;
                    sp += 1.0;
                    s2 -= w2;
                    w2 *= h4/sp;
                    ap *= -a2;
                }
                t = (atan(wk) - wk*s1) * ::inv_2pi;
                b += sgn*t;
            }
        }
        //Check for convergence
//         std::cout<<"N b:"<<b<<" is:"<<is<<" ak:"<<ak<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";
        if(0 <= is || ak == 0) return bounded(b);
        wh = -ak;
        wk = (ah/ak - r)*sigma_inv;
        gw = 2*gk;
        is = 1;
    }
    //never get here
}

double donnelly_bvn_integral_orig( double ah, double ak, double r )
{
    using std::max;
    using std::min;
    using std::fabs;
  double a2;
  double ap;
  double b;
  double cn;
  double con;
  double conex;
  double ex;
  double g2;
  double gh;
  double gk;
  double gw;
  double h2;
  double h4;
  int i;
  static int idig = 15;
  int is;
  double rr;
  double s1;
  double s2;
  double sgn;
  double sn;
  double sp;
  double sqr;
  double t;
  static double twopi = 6.283185307179587;
  double w2;
  double wh;
  double wk;

  b = 0.0;

  gh = unit_normal_cdf ( - ah ) / 2.0;
  gk = unit_normal_cdf ( - ak ) / 2.0;

  if ( r == 0.0 )
  {
    b = 4.00 * gh * gk;
    b = max ( b, 0.0 );
    b = min ( b, 1.0 );
    return b;
  }

  rr = ( 1.0 + r ) * ( 1.0 - r );

  if ( rr < 0.0 )
  {
    return -1;
  }

  if ( rr == 0.0 )
  {
    if ( r < 0.0 )
    {
      if ( ah + ak < 0.0 )
      {
        b = 2.0 * ( gh + gk ) - 1.0;
      }
    }
    else
    {
      if ( ah - ak < 0.0 )
      {
        b = 2.0 * gk;
      }
      else
      {
        b = 2.0 * gh;
      }
    }
    b = max ( b, 0.0 );
    b = min ( b, 1.0 );
    return b;
  }

  sqr = sqrt ( rr );

  if ( idig == 15 )
  {
    con = twopi * 1.0E-15 / 2.0;
  }
  else
  {
    con = twopi / 2.0;
    for ( i = 1; i <= idig; i++ )
    {
      con = con / 10.0;
    }
  }
//
//  (0,0)
//
  if ( ah == 0.0 && ak == 0.0 )
  {
    b = 0.25 + asin ( r ) / twopi;
    b = max ( b, 0.0 );
    b = min ( b, 1.0 );
    return b;
  }
//
//  (0,nonzero)
//
  else if ( ah == 0.0 && ak != 0.0 )
  {
    b = gk;
    wh = -ak;
    wk = ( ah / ak - r ) / sqr;
    gw = 2.0 * gk;
    is = 1;
  }
//
//  (nonzero,0)
//
  else if ( ah != 0.0 && ak == 0.0 )
  {
    b = gh;
    wh = -ah;
    wk = ( ak / ah - r ) / sqr;
    gw = 2.0 * gh;
    is = -1;
  }
//
//  (nonzero,nonzero)
//
  else
  {
    b = gh + gk;
    if ( ah * ak < 0.0 )
    {
      b = b - 0.5;
    }
    wh = - ah;
    wk = ( ak / ah - r ) / sqr;
    gw = 2.0 * gh;
    is = -1;
  }
//     std::cout<<"OLD ah:"<<ah<<" ak:"<<ak<<" rho:"<<r<<" gh:"<<gh<<" gk:"<<gk<<"\n";
//     std::cout<<"OLD b:"<<b<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";
  for ( ; ; )
  {
    sgn = -1.0;
    t = 0.0;
//      std::cout<<"O b:"<<b<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";

    if ( wk != 0.0 )
    {
      if ( fabs ( wk ) == 1.0 )
      {
        t = wk * gw * ( 1.0 - gw ) / 2.0;
        b = b + sgn * t;
      }
      else
      {
        if ( 1.0 < fabs ( wk ) )
        {
          sgn = -sgn;
          wh = wh * wk;
          g2 = unit_normal_cdf ( wh );
          wk = 1.0 / wk;

          if ( wk < 0.0 )
          {
            b = b + 0.5;
          }
          b = b - ( gw + g2 ) / 2.0 + gw * g2;
//             std::cout<<"oo b: "<<b<<" gw:"<<gw<<" g2:"<<g2<<" t:"<<t<<" sgn:"<<sgn<<" wh:"<<wh<<" wk:"<<wk<<"\n";

        }
        h2 = wh * wh;
        a2 = wk * wk;
        h4 = h2 / 2.0;
        ex = exp ( - h4 );
        w2 = h4 * ex;
        ap = 1.0;
        s2 = ap - ex;
        sp = ap;
        s1 = 0.0;
        sn = s1;
        conex = fabs ( con / wk );

        for ( ; ; )
        {
          cn = ap * s2 / ( sn + sp );
          s1 = s1 + cn;
//           std::cout<<"OLD cn: "<<cn<<" s1:"<<s1<<" conex:"<<conex<<" sn:"<<sn<<" sp:"<<sp<<" s2:"<<s2<<" w2"<<w2<<" ap:"<<ap<<"\n";
          if ( fabs ( cn ) <= conex )
          {
            break;
          }
          sn = sp;
          sp = sp + 1.0;
          s2 = s2 - w2;
          w2 = w2 * h4 / sp;
          ap = - ap * a2;
        }
        t = ( atan ( wk ) - wk * s1 ) / twopi;
        b = b + sgn * t;
      }
    }
//     std::cout<<"0 b:"<<b<<" is:"<<is<<" ak:"<<ak<<" wh:"<<wh<<" wk:"<<wk<<" gw:"<<gw<<" is:"<<is<<"\n";

    if ( 0 <= is )
    {
      break;
    }
    if ( ak == 0.0 )
    {
      break;
    }
    wh = -ak;
    wk = ( ah / ak - r ) / sqr;
    gw = 2.0 * gk;
    is = 1;
  }

  b = max ( b, 0.0 );
  b = min ( b, 1.0 );

  return b;
}

} /* namespace prior_hessian */


// Compile with:
// clang++ -o demoGaussianMixtureClusterer5D demoGaussianMixtureClusterer5D.cpp -L../build/ -I ../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
//

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "File.h"
#include "EuclideanMetric.h"
#include "GaussianMixtureClusterer.h"
#include "PrincipalComponentProjector.h"

using namespace std;
using namespace Eigen;

int main()
{
    // Open the input file and read the data (synthetic sampling of a 5D parameter space)

    ifstream inputFile;
    File::openInputFile(inputFile, "kmeans_testsample5D.txt");

    unsigned long Nrows;
    int Ncols;

    File::sniffFile(inputFile, Nrows, Ncols);
    ArrayXXd data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    ArrayXXd sample = data.transpose();
    inputFile.close();

    // Set up the K-means clusterer using a Euclidean metric

    EuclideanMetric myMetric;
    int minNclusters = 2;
    int maxNclusters = 15;
    int Ntrials = 20;
    double relTolerance = 1.e-3;

    bool printNdimensions = true;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = false;

    GaussianMixtureClusterer gaussianMixtureClusterer(myMetric, projector, featureProjectionActivated,
                                                      minNclusters, maxNclusters, Ntrials, relTolerance);

    // Do the clustering, and get for each point the index of the cluster it belongs to

    int optimalNclusters;
    vector<int> clusterIndices(Nrows);
    vector<int> clusterSizes;

    optimalNclusters = gaussianMixtureClusterer.cluster(sample, clusterIndices, clusterSizes);

    // Output the results

    cerr << "Input number of clusters: 4" << endl;
    cerr << "Optimal number of clusters: " << optimalNclusters << endl << endl;

    ArrayXXd finalSample(Nrows,Ncols+1);
    finalSample.block(0,0,Nrows,Ncols) = data;

    for (int n = 0; n < Nrows; ++n)
    {
        finalSample(n,Ncols) = clusterIndices[n];
    }

    ofstream outputFile;
    File::openOutputFile(outputFile, "clusterMembershipFromGMM5D.txt");
    outputFile << scientific << setprecision(4);
    File::arrayXXdToFile(outputFile, finalSample);
    outputFile.close();

    // That's it!

    return EXIT_SUCCESS;
}


#include <Eigen/Dense>

#include <chrono>
#include <memory>
#include <random>
#include <vector>

#include "MatrixUtils.h"

void MatrixUtils::initializeRandomWeights(Eigen::MatrixXd& matrix,
    const bool randomSeed) {

    unsigned seed = 0;
    if (randomSeed) {
        seed = std::chrono::system_clock::now().time_since_epoch().count();
    }

    std::default_random_engine generator(seed);
    std::normal_distribution<double> distribution(0.0, 1.0);

    for (int i=0; i < matrix.cols(); ++i) {
        for (int j=0; j < matrix.rows(); ++j) {
            matrix(j, i) = distribution(generator);
        }
    }
}


//#include <cmath>
#include <iostream>
#include <boost/math/special_functions/legendre.hpp>

#include "gauss_quad.hpp"

namespace DGHydro {

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Constructor
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

GaussQuad::GaussQuad(int n) : n(n)
{
  if (n <= 0)
    throw std::runtime_error("Can not create Gaussian Quadrature with a number of point that is not positive");

  FindAbscissae();
  FindWeights();
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Destructor
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

GaussQuad::~GaussQuad()
{
  delete[] x;
  delete[] w;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void GaussQuad::FindAbscissae()
{
  if (x == nullptr) x = new double[n];

  double dx = 2.0/(double) n;
  double tol = 1.0e-12;

  for (int i = 0; i < n; i++) {
    // Divide up interval: each contains a single zero
    double a = -1.0 + i*dx;
    double b = -1.0 + (i + 1)*dx;

    // Find zero by bisection
    while (1) {
      double c = 0.5*(a + b);

      double f = boost::math::legendre_p(n, c);

      if (f == 0.0 || 0.5*(b - a) < tol) {
        x[i] = c;
        break;
      }

      if (sgn(f) == sgn(boost::math::legendre_p(n, a))) {
        a = c;
      } else {
        b = c;
      }
    }

  }

}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void GaussQuad::FindWeights()
{
  if (w == nullptr) w = new double[n];

  for (int i = 0; i < n; i++) {
    // Derivative of Legendre polynomial
    double p_prime = n*(x[i]*boost::math::legendre_p(n, x[i]) -
                        boost::math::legendre_p(n - 1, x[i]))/(x[i]*x[i]-1.0);
    // Gaussian weight
    w[i] = 2.0/(1 - x[i]*x[i])/p_prime/p_prime;
  }
}

}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3d v = Matrix3d::Random();
cout << "The matrix v is:" << endl;
cout << v << endl;

Vector3d v0(1, v(1,0), v(2,0));
cout << "The first Householder vector is: v_0 = " << v0.transpose() << endl;
Vector3d v1(0, 1, v(2,1));
cout << "The second Householder vector is: v_1 = " << v1.transpose()  << endl;
Vector3d v2(0, 0, 1);
cout << "The third Householder vector is: v_2 = " << v2.transpose() << endl;

Vector3d h = Vector3d::Random();
cout << "The Householder coefficients are: h = " << h.transpose() << endl;

Matrix3d H0 = Matrix3d::Identity() - h(0) * v0 * v0.adjoint();
cout << "The first Householder reflection is represented by H_0 = " << endl;
cout << H0 << endl;
Matrix3d H1 = Matrix3d::Identity() - h(1) * v1 * v1.adjoint();
cout << "The second Householder reflection is represented by H_1 = " << endl;
cout << H1 << endl;
Matrix3d H2 = Matrix3d::Identity() - h(2) * v2 * v2.adjoint();
cout << "The third Householder reflection is represented by H_2 = " << endl;
cout << H2 << endl;
cout << "Their product is H_0 H_1 H_2 = " << endl;
cout << H0 * H1 * H2 << endl;

HouseholderSequence<Matrix3d, Vector3d> hhSeq(v, h);
Matrix3d hhSeqAsMatrix(hhSeq);
cout << "If we construct a HouseholderSequence from v and h" << endl;
cout << "and convert it to a matrix, we get:" << endl;
cout << hhSeqAsMatrix << endl;

  return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector3d v(-1,2,-3);
cout << "the absolute values:" << endl << v.array().abs() << endl;
cout << "the absolute values plus one:" << endl << v.array().abs()+1 << endl;
cout << "sum of the squares: " << v.array().square().sum() << endl;

  return 0;
}


//
// $Id: MatrixInverse.hpp 4146 2012-11-26 23:46:34Z pcbrefugee $
//
//
// NB: Variations of this file appear in many open source projects,
// with no copyright claims or license statements made in any of them.
// Assumed to be public domain, or at least as open as the boost license,
// since the farthest back we can seem to trace it is the Boost Wiki at
// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?Effective_UBLAS/Matrix_Inversion
//

#ifndef _MATRIXINVERSE_HPP_
#define _MATRIXINVERSE_HPP_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

#include <iostream>
#include <fstream>
#include <vector>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace toppic {

namespace matrix_inverse {

/* Matrix inversion routine.
Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
bool InvertMatrix(const boost::numeric::ublas::matrix<double>& input,
                  boost::numeric::ublas::matrix<double>& inverse)
{

    using namespace boost::numeric::ublas;
    typedef permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    matrix<double> A(input);
    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A,pm);
    if( res != 0 ) return false;

    // create identity matrix of "inverse"
    inverse = identity_matrix<double>(A.size1());

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);

    return true;
}

/**
* Invert a matrix via gauss-jordan algorithm (PARTIAL PIVOT)
*
* @param m The matrix to invert. Must be square.
* @param singular If the matrix was found to be singular, then this
*        is set to true, else set to false.
* @return If singular is false, then the inverted matrix is returned.
*         Otherwise it contains random values.
*/

//#define T double /// for debug
boost::numeric::ublas::matrix<double>
gjinverse(const boost::numeric::ublas::matrix<double> &m,
          bool &singular)
{
    using namespace boost::numeric::ublas;

    const size_t size = m.size1();

    // Cannot invert if non-square matrix or 0x0 matrix.
    // Report it as singular in these cases, and return
    // a 0x0 matrix.
    if (size != m.size2() || size == 0)
    {
        singular = true;
        matrix<double> A(0,0);
        return A;
    }

    // Handle 1x1 matrix edge case as general purpose
    // inverter below requires 2x2 to function properly.
    if (size == 1)
    {
        matrix<double> A(1, 1);
        if (m(0,0) == 0.0)
        {
            singular = true;
            return A;
        }
        singular = false;
        A(0,0) = 1/m(0,0);
        return A;
    }

    // Create an augmented matrix A to invert. Assign the
    // matrix to be inverted to the left hand side and an
    // identity matrix to the right hand side.
    matrix<double> A(size, 2*size);
    matrix_range<matrix<double> > Aleft(A,
        range(0, size),
        range(0, size));
    Aleft = m;
    matrix_range<matrix<double> > Aright(A,
        range(0, size),
        range(size, 2*size));
    Aright = identity_matrix<double>(size);

    // Doing partial pivot
    for (size_t k = 0; k < size; k++)
    {
        // Swap rows to eliminate zero diagonal elements.
        for (size_t kk = 0; kk < size; kk++)
        {
            if ( A(kk,kk) == 0 ) // XXX: test for "small" instead
            {
                // Find a row(l) to swap with row(k)
                int l = -1;
                for (size_t i = kk+1; i < size; i++)
                {
                    if ( A(i,kk) != 0 )
                    {
                        l = i;
                        break;
                    }
                }

                // Swap the rows if found
                if ( l < 0 )
                {
                    std::cerr << "Error:" <<  __FUNCTION__ << ":"
                        << "Input matrix is singular, because cannot find"
                        << " a row to swap while eliminating zero-diagonal.";
                    singular = true;
                    return Aleft;
                }
                else
                {
                    matrix_row<matrix<double> > rowk(A, kk);
                    matrix_row<matrix<double> > rowl(A, l);
                    rowk.swap(rowl);

/*#if defined(DEBUG) || !defined(NDEBUG)
                    std::cerr << __FUNCTION__ << ":"
                        << "Swapped row " << kk << " with row " << l
                        << ":" << A << "\n";
#endif*/
                }
            }
        }

        /////////////////////////////////////////////////////////////////////////////////////////////////////////
        // normalize the current row
        for (size_t j = k+1; j < 2*size; j++)
            A(k,j) /= A(k,k);
        A(k,k) = 1;

        // normalize other rows
        for (size_t i = 0; i < size; i++)
        {
            if ( i != k )  // other rows  // FIX: PROBLEM HERE
            {
                if ( A(i,k) != 0 )
                {
                    for (size_t j = k+1; j < 2*size; j++)
                        A(i,j) -= A(k,j) * A(i,k);
                    A(i,k) = 0;
                }
            }
        }

/*#if defined(DEBUG) || !defined(NDEBUG)
        std::cerr << __FUNCTION__ << ":"
            << "GJ row " << k << " : " << A << "\n";
#endif*/
    }

    singular = false;
    return Aright;
}

}

}

#endif // _MATRIXINVERSE_HPP_


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

  Distributed under the Boost Software License, Version 1.0. (See accompanying
  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#ifndef SPRIG_CURVE_BEZIER_HPP
#define SPRIG_CURVE_BEZIER_HPP

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/geometry/core/access.hpp>
#include <sprig/detail/pow.hpp>

namespace sprig {
	//
	// bezier
	//
	template<typename T, typename Point>
	SPRIG_INLINE Point
	bezier(T const& t, Point const& p0, Point const& p1, Point const& p2) {
		namespace bg = boost::geometry;
		using sprout::detail::pow2;
		return Point(
			pow2(1 - t) * bg::get<0>(p0)
				+ 2 * t * (1 - t) * bg::get<0>(p1)
				+ pow2(t) * bg::get<0>(p2)
				,
			pow2(1 - t) * bg::get<1>(p0)
				+ 2 * t * (1 - t) * bg::get<1>(p1)
				+ pow2(t) * bg::get<1>(p2)
			);
	}
	template<typename T, typename Point>
	SPRIG_INLINE Point
	bezier(T const& t, Point const& p0, Point const& p1, Point const& p2, Point const& p3) {
		namespace bg = boost::geometry;
		using sprout::detail::pow2;
		using sprout::detail::pow3;
		return Point(
			pow3(1 - t) * bg::get<0>(p0)
				+ 3 * t * pow2(1 - t) * bg::get<0>(p1)
				+ 3 * pow2(t) * (1 - t) * bg::get<0>(p2)
				+ pow3(t) * bg::get<0>(p3)
				,
			pow3(1 - t) * bg::get<1>(p0)
				+ 3 * t * pow2(1 - t) * bg::get<1>(p1)
				+ 3 * pow2(t) * (1 - t) * bg::get<1>(p2)
				+ pow3(t) * bg::get<1>(p3)
			);
	}
	template<typename T, typename Point>
	SPRIG_INLINE Point
	bezier(T const& t, Point const& p0, Point const& p1, Point const& p2, Point const& p3, Point const& p4) {
		namespace bg = boost::geometry;
		using sprout::detail::pow2;
		using sprout::detail::pow3;
		using sprout::detail::pow4;
		return Point(
			pow4(1 - t) * bg::get<0>(p0)
				+ 4 * t * pow3(1 - t) * bg::get<0>(p1)
				+ 6 * pow2(t) * pow2(1 - t) * bg::get<0>(p2)
				+ 4 * pow3(t) * (1 - t) * bg::get<0>(p3)
				+ pow4(t) * bg::get<0>(p4)
				,
			pow4(1 - t) * bg::get<1>(p0)
				+ 4 * t * pow3(1 - t) * bg::get<1>(p1)
				+ 6 * pow2(t) * pow2(1 - t) * bg::get<1>(p2)
				+ 4 * pow3(t) * (1 - t) * bg::get<1>(p3)
				+ pow4(t) * bg::get<1>(p4)
			);
	}
	template<typename T, typename Point>
	SPRIG_INLINE Point
	bezier(T const& t, Point const& p0, Point const& p1, Point const& p2, Point const& p3, Point const& p4, Point const& p5) {
		namespace bg = boost::geometry;
		using sprout::detail::pow2;
		using sprout::detail::pow3;
		using sprout::detail::pow4;
		using sprout::detail::pow5;
		return Point(
			pow5(1 - t) * bg::get<0>(p0)
				+ 5 * t * pow4(1 - t) * bg::get<0>(p1)
				+ 10 * pow2(t) * pow3(1 - t) * bg::get<0>(p2)
				+ 10 * pow3(t) * pow2(1 - t) * bg::get<0>(p3)
				+ 5 * pow4(t) * (1 - t) * bg::get<0>(p4)
				+ pow5(t) * bg::get<0>(p5)
				,
			pow5(1 - t) * bg::get<1>(p0)
				+ 5 * t * pow4(1 - t) * bg::get<1>(p1)
				+ 10 * pow2(t) * pow3(1 - t) * bg::get<1>(p2)
				+ 10 * pow3(t) * pow2(1 - t) * bg::get<1>(p3)
				+ 5 * pow4(t) * (1 - t) * bg::get<1>(p4)
				+ pow5(t) * bg::get<1>(p5)
			);
	}
}	// namespace sprig

#endif	// #ifndef SPRIG_CURVE_BEZIER_HPP


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <time.h>
using namespace std;
using namespace Eigen;

double division(int a, int b)
{
   if( b == 0 )
   {
      throw "Division by zero condition!";
   }
   return (a/b);
}
int main()
{

    MatrixXd m(3, 3);
	m << 0.1, 0, 0,
        0, 0.2, 0,
        0, 0, 0.4;
    RowVector3d x, u;
    x << 0.5, 0.6, 0.7;
    u << 0.3, 0.2, 0.5;
    cout << m.determinant() << endl;
    cout << m.inverse() << endl;
    cout << m.row(0).size() << endl;

    cout << pow(2 * 3.14159, m.row(0).size()/2.) << endl;

    cout << x - u << endl;
    cout << (x - u) * m.inverse() << endl;
	cout << exp(-0.5 * (x - u) * m.inverse() * (x - u).transpose()) << endl;
	/*
	VectorXd r(4);
	r = m.row(1);

	double* data = r;
	cout << data << endl;
	cout << r <<endl;
	cout << m.row(1) * m.row(2).transpose() << endl;
	*/
    srand((unsigned)time(NULL));
    cout << MatrixXd::Zero(3,3) << endl;

    cout << MatrixXd::Random(3,3) << endl;

    cout << MatrixXd::Identity(3,3) << endl;

    MatrixXd a = (x - u).transpose() * (x - u);
    cout << a << endl;

    ArrayXXd b = a.array();
    ArrayXXd c = MatrixXd::Identity(3,3);

    cout << b << endl;
    cout << c << endl;

    cout << b * c << endl;

    MatrixXd* d = new MatrixXd[3];
    for(int i = 0; i < 3; i++)
    {
        d[i] = MatrixXd::Identity(3,3);
    }
    d[0] /= 3;
    cout << d[0] << endl;
    cout << d[1] << endl;
    cout << d[2] << endl;

    cout << "!!!!!!!!!!!!!!!!!!!!" << endl;

    RowVectorXd e = RowVectorXd::Random(5);
    cout << e << endl;
    ArrayXd tmp = e;
    e = tmp * tmp;
    cout << e << endl;
    cout << e.transpose()*e << endl;
	return 0;

}


#pragma once

#include <Eigen/Dense>

namespace scpp::models
{

template <typename T>
void deg2rad(T &deg)
{
    deg *= M_PI / 180.;
}

template <typename T>
void rad2deg(T &rad)
{
    rad *= 180. / M_PI;
}

template <typename T>
Eigen::Matrix<T, 4, 1> quaternionToVector(const Eigen::Quaternion<T> &q)
{
    Eigen::Matrix<T, 4, 1> q_vec;
    q_vec << q.w(), q.vec();
    return q_vec;
}

// sequence x-y'-z'' is XYZ = Z''Y'X
template <typename T>
Eigen::Quaternion<T> eulerToQuaternionXYZ(const Eigen::Matrix<T, 3, 1> &eta)
{
    Eigen::Quaternion<T> q;
    q = Eigen::AngleAxis<T>(eta.x(), Eigen::Matrix<T, 3, 1>::UnitX()) *
        Eigen::AngleAxis<T>(eta.y(), Eigen::Matrix<T, 3, 1>::UnitY()) *
        Eigen::AngleAxis<T>(eta.z(), Eigen::Matrix<T, 3, 1>::UnitZ());

    return q;
}

template <typename T>
Eigen::Matrix<T, 3, 3> eulerRotationMatrixXY(const Eigen::Matrix<T, 2, 1> &eta)
{
    const T phi = eta.x();
    const T theta = eta.y();

    Eigen::Matrix<T, 3, 3> M;
    M.row(0) << cos(theta), T(0.), sin(theta);
    M.row(1) << sin(theta) * sin(phi), cos(phi), -sin(phi) * cos(theta);
    M.row(2) << -sin(theta) * cos(phi), sin(phi), cos(phi) * cos(theta);

    return M;
}

// sequence x-y-z is ZYX
template <typename T>
Eigen::Quaternion<T> eulerToQuaternionZYX(const Eigen::Matrix<T, 3, 1> &eta)
{
    Eigen::Quaternion<T> q;
    q = Eigen::AngleAxis<T>(eta.z(), Eigen::Matrix<T, 3, 1>::UnitZ()) *
        Eigen::AngleAxis<T>(eta.y(), Eigen::Matrix<T, 3, 1>::UnitY()) *
        Eigen::AngleAxis<T>(eta.x(), Eigen::Matrix<T, 3, 1>::UnitX());
    return q;
}

template <typename T>
Eigen::Matrix<T, 3, 1> quaternionToEulerXYZ(const Eigen::Quaternion<T> &q)
{
    const Eigen::Matrix<T, 3, 3> R = q.toRotationMatrix();
    const T phi = atan2(-R(1, 2), R(2, 2));
    const T theta = asin(R(0, 2));
    const T psi = atan2(-R(0, 1), R(0, 0));
    return Eigen::Matrix<T, 3, 1>(phi, theta, psi);
}

template <typename T>
Eigen::Matrix<T, 3, 1> quaternionToEulerZYX(const Eigen::Quaternion<T> &q)
{
    const Eigen::Matrix<T, 3, 3> R = q.toRotationMatrix();
    const T phi = atan2(R(1, 0), R(0, 0));
    const T theta = asin(-R(2, 0));
    const T psi = atan2(R(2, 1), R(2, 2));
    return Eigen::Matrix<T, 3, 1>(psi, theta, phi);
}

template <typename T>
Eigen::Quaternion<T> vectorToQuaternion(const Eigen::Matrix<T, 3, 1> &v)
{
    return Eigen::Quaternion<T>(std::sqrt(1. - v.squaredNorm()), v.x(), v.y(), v.z());
}

template <typename T>
Eigen::Quaternion<T> vectorToQuaternion(const Eigen::Matrix<T, 4, 1> &v)
{
    return Eigen::Quaternion<T>(v(3), v(0), v(1), v(2));
}

template <typename T>
Eigen::Matrix<T, 3, 3> rotationJacobianXYZ(const Eigen::Matrix<T, 3, 1> &eta)
{
    // const T phi = eta.x();
    const T theta = eta.y();
    const T psi = eta.z();

    Eigen::Matrix<T, 3, 3> M;
    M.row(0) << cos(psi), -sin(psi), T(0.);
    M.row(1) << cos(theta) * sin(psi), cos(theta) * cos(psi), T(0.);
    M.row(2) << -sin(theta) * cos(psi), sin(theta) * sin(psi), cos(theta);

    return M / cos(theta);
}

template <typename T>
Eigen::Matrix<T, 2, 2> rotationJacobianXY(const Eigen::Matrix<T, 2, 1> &eta)
{
    const T theta = eta.y();

    Eigen::Matrix<T, 2, 2> M;
    M.row(0) << cos(theta), sin(theta);
    M.row(1) << -sin(theta), cos(theta);

    return M;
}

template <typename T>
Eigen::Matrix<T, 4, 4> omegaMatrix(const Eigen::Matrix<T, 3, 1> &w)
{
    Eigen::Matrix<T, 4, 4> omega;
    omega << T(0.), -w(0), -w(1), -w(2),
        w(0), T(0.), w(2), -w(1),
        w(1), -w(2), T(0.), w(0),
        w(2), w(1), -w(0), T(0.);

    return omega;
}

template <typename T>
Eigen::Matrix<T, 3, 3> omegaMatrixReduced(const Eigen::Matrix<T, 3, 1> &q)
{
    Eigen::Matrix<T, 3, 3> omega;
    const T qw = sqrt(1. - q.squaredNorm());
    omega << qw, -q(2), q(1),
        q(2), qw, -q(0),
        -q(1), q(0), qw;

    return omega;
}

} // namespace scpp::models


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <algorithm>
#include <boost/core/lightweight_test.hpp>
#include <boost/gil/detail/math.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_processing/hough_transform.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/point.hpp>
#include <boost/gil/rasterization/line.hpp>
#include <boost/gil/typedefs.hpp>
#include <cmath>
#include <cstddef>
#include <iostream>
#include <sstream>
#include <vector>

namespace gil = boost::gil;

const std::ptrdiff_t width = 64;

void translate(std::vector<gil::point_t>& points, std::ptrdiff_t intercept)
{
    std::transform(points.begin(), points.end(), points.begin(),
                   [intercept](gil::point_t point)
                   {
                       return gil::point_t{point.x, point.y + intercept};
                   });
}

void hough_line_test(std::ptrdiff_t height, std::ptrdiff_t intercept)
{
    const auto rasterizer = gil::bresenham_line_rasterizer{};
    gil::gray8_image_t image(width, width, gil::gray8_pixel_t(0));
    auto input = gil::view(image);
    std::vector<gil::point_t> line_points(rasterizer.point_count(width, height));
    rasterizer({0, 0}, {width - 1, height - 1}, line_points.begin());
    translate(line_points, intercept);
    for (const auto& p : line_points)
    {
        input(p) = 255;
    }

    double alpha = std::atan2(height, width);
    const double theta = alpha + gil::detail::pi / 2;
    const auto minimum_angle_step = gil::minimum_angle_step({width, height});
    const double expected_alpha = std::round(alpha / minimum_angle_step) * minimum_angle_step;
    const double expected_radius = std::round(intercept * std::cos(expected_alpha));
    const double expected_theta = std::round(theta / minimum_angle_step) * minimum_angle_step;

    const std::size_t half_step_count = 3;
    const std::ptrdiff_t expected_index = 3;
    const std::size_t accumulator_array_dimensions = half_step_count * 3 + 1;
    auto radius_param =
        gil::hough_parameter<std::ptrdiff_t>::from_step_count(expected_radius, 3, half_step_count);
    auto theta_param = gil::make_theta_parameter(
        expected_theta, minimum_angle_step * half_step_count, {width, height});
    gil::gray32_image_t accumulator_array_image(
        accumulator_array_dimensions, accumulator_array_dimensions, gil::gray32_pixel_t(0));
    auto accumulator_array = gil::view(accumulator_array_image);
    gil::hough_line_transform(input, accumulator_array, theta_param, radius_param);

    auto max_element_iterator =
        std::max_element(accumulator_array.begin(), accumulator_array.end());
    gil::point_t candidates[] = {
        {expected_index - 1, expected_index - 1}, {expected_index, expected_index - 1},
        {expected_index + 1, expected_index - 1}, {expected_index - 1, expected_index},
        {expected_index, expected_index},         {expected_index + 1, expected_index},
        {expected_index - 1, expected_index + 1}, {expected_index, expected_index + 1},
        {expected_index + 1, expected_index + 1}};
    bool match_found = false;
    for (std::size_t i = 0; i < 9; ++i)
    {
        if (*max_element_iterator == accumulator_array(candidates[i]))
        {
            match_found = true;
            break;
        }
    }
    BOOST_TEST(match_found);
}

int main()
{
    for (std::ptrdiff_t height = 1; height < width; ++height)
    {
        for (std::ptrdiff_t intercept = 1; intercept < width - height; ++intercept)
        {
            hough_line_test(height, intercept);
        }
    }
    return boost::report_errors();
}


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>

#include <limits>
#include <vector>

namespace gil = boost::gil;

// Demonstrates the use of a rasterizer to generate an image of a line
// The various rasterizers available are defined in include/boost/gil/rasterization/circle.hpp,
// include/boost/gil/rasterization/ellipse.hpp and include/boost/gil/rasterization/line.hpp
// The rasterizer used implements the Bresenham's line algorithm.
// Multiple images are created, all of the same size, but with areas of different sizes passed to the rasterizer, resulting in different lines.
// See also:
// rasterizer_circle.cpp - Demonstrates the use of a rasterizer to generate an image of a circle
// rasterizer_ellipse.cpp - Demonstrates the use of a rasterizer to generate an image of an ellipse

const std::ptrdiff_t size = 256;

void line_bresenham(std::ptrdiff_t width, std::ptrdiff_t height, const std::string& output_name)
{
    const auto rasterizer = gil::bresenham_line_rasterizer{};
    std::vector<gil::point_t> line_points(rasterizer.point_count(width, height));

    gil::gray8_image_t image(size, size);
    auto view = gil::view(image);

    rasterizer({0, 0}, {width - 1, height - 1}, line_points.begin());
    for (const auto& point : line_points)
    {
        view(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    gil::write_view(output_name, view, gil::png_tag{});
}

int main()
{
    line_bresenham(256, 256, "line-bresenham-256-256.png");
    line_bresenham(256, 128, "line-bresenham-256-128.png");
    line_bresenham(256, 1, "line-bresenham-256-1.png");
    line_bresenham(1, 256, "line-bresenham-1-256.png");
}


#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <cmath>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

const double eps = 0.0001;   // 用于浮点数比较大小
const int sampleNum = 5000; // 样本数目,100
const int attriNum = 768;  // 一个样本特征数目,2

/*
// 用于导入数据 [这个函数本应该从LoadData中调用的，但是我没成功...]
double** load(string filename, int m, int n){
    // 初始化数组
    double** ans = (double **)malloc(m * sizeof(double *));
    for(int i=0;i<m;i++)
        ans[i] = (double *)malloc(n * sizeof(double));

    ifstream inFile(filename, ios::in);
	string lineStr; //每一行的结果
    for(int i=0;i<m;i++){
        getline(inFile, lineStr);
        stringstream ss(lineStr);
        string str;
        for(int j=0;j<n;j++){
            getline(ss, str, ',');
            ans[i][j] = atof(str.c_str());
        }
    }
    return ans;
}

// 损失函数
// X_bar sampleNum * (attriNum+1), y sampleNum*1, theta (attriNum+1)
// 但实际上输入的是 X sampleNum * attriNum，在计算的时候手动加入一项 -1
double loss_theta(double ** X_bar,double ** y,double* theta){
    double * tmp = new double[sampleNum]();
    for(int i=0;i<sampleNum;i++){
        double xTheta = 0;
        for(int j=0;j<attriNum;j++){
            xTheta += X_bar[i][j]*theta[j];
        }
        xTheta -= theta[attriNum];
        tmp[i] = y[i][0]*xTheta;
    }
    double sum = 0;
    for(int i=0;i<sampleNum;i++)
        sum += log(1+exp(-tmp[i]));
    return (double)sum/sampleNum;
}

// 用来统计准确率
double score(double* orgLabel,double* newLabel){
    int sum = 0;
    for(int i=0;i<sampleNum;i++){
        if((orgLabel[i]-newLabel[i])<eps)
            sum+=1;
    }
    return (double)sum/sampleNum;
}

// 用来打印一个二维数组(测试用)
void printTest(double** tmp,int m,int n){
    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            cout << tmp[i][j] << " ";
        }
        cout << "\n";
    }
}

*/

// 需要指定数据的行和列
Matrix<double, Dynamic, Dynamic> loadEigen(string filename, int m ,int n){
    Matrix<double, Dynamic, Dynamic> ans = MatrixXd::Zero(m, n);;
    ifstream inFile(filename, ios::in);
	string lineStr; //每一行的结果
    for(int i=0;i<m;i++){
        getline(inFile, lineStr);
        stringstream ss(lineStr);
        string str;
        for(int j=0;j<n;j++){
            getline(ss, str, ',');
            ans(i,j) = atof(str.c_str());
        }
    }
    return ans;
}

/*
def theta_gradient(X_bar,y,beta,alpha=0):
    ans = np.zeros(beta.shape)+alpha*beta
    for i in range(X_bar.shape[0]):
        tmp = np.exp(beta.dot(X_bar[i]))
        ans += X_bar[i]*(y[i]-tmp/(1+tmp))
    return -ans
*/

Matrix<double,attriNum+1,1> theta_gradient(MatrixXd X_bar ,Matrix<double,sampleNum,1> Y,Matrix<double,attriNum+1,1> beta,double alpha){
    Matrix<double,attriNum+1,1> ans = alpha * beta;
    // ans.fill(0);
    for(int i=0;i<sampleNum;i++){
        // cout << X_bar.row(i).rows() << "\t" << X_bar.row(i).cols() <<endl;
        double tmp = exp(X_bar.row(i)*beta);
        // cout << X_bar.row(i).transpose() << endl;
        ans += X_bar.row(i).transpose()*(Y(i,0)-tmp/(1+tmp));
    }
    return -ans;
}

// // 训练
void train(){

}

// // 预测
Matrix<double,sampleNum,1> predict(MatrixXd X_bar,Matrix<double,attriNum+1,1> beta){
    Matrix<double,sampleNum,1> y_pred;
    y_pred.fill(0);
    for(int i=0;i<sampleNum;i++){
        double tmp = 1/(1+exp(-X_bar.row(i)*beta));
        // cout << tmp << endl;
        if(tmp >0.5)
           y_pred(i,0)= 1; // 否则默认是0
    }
    return y_pred;
}

double accuracy(Matrix<double,sampleNum,1> y_pred, Matrix<double,sampleNum,1> y){
    int num = 0;
    for(int i=0;i<sampleNum;i++)
        if((y_pred(i,0)-y(i,0))<eps)
            num+=1;
    cout << num << " correct out of " <<sampleNum << endl;
    return (double)num/sampleNum;
}


#include "Triangle.h"
#include "Ray.h"
#include "Intersection.h"

#include <Eigen/Dense>

std::unique_ptr<Intersection> Triangle::intersect(const Ray& ray) {
    auto p = ray.p;
    auto d = ray.d;

    if (n.dot(d) > 0.0f) {
        return nullptr;
    } else {
        // Use centroid as point on plane
        auto pt = 1/3.0f*a + 1/3.0f*b + 1/3.0f*c;

        // Find intersection t
        float t = (pt-p).dot(n) / d.dot(n);

        // Find intersection point
        auto x = p + d*t;

        // Perform intersection tests on half planes
        bool hp0 = (b-a).cross(x-a).dot(n) >= 0;
        bool hp1 = (c-b).cross(x-b).dot(n) >= 0;
        bool hp2 = (a-c).cross(x-c).dot(n) >= 0;

        // Check if point intersects all three halfplanes
        if (hp0 && hp1 && hp2)
            return std::unique_ptr<Intersection>(new Intersection(this, n, t));

        return nullptr;
    }
}


//
// Dynamics of Differential Drive circle robot.
// Based on the dynamics in: http://arl.cs.utah.edu/pubs/ACC2014.pdf
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

# pragma once

#include <experiments/simulators/simulator_utils.hh>

#include <Eigen/Dense>

#include <array>

namespace simulators
{
namespace diffdrive
{

template<int dim>
using Vector = Eigen::Matrix<double, dim, 1>;

enum State
{
    POS_X = 0,
    POS_Y,
    THETA,
    dTHETA,
    dV_LEFT,
    dV_RIGHT,
    STATE_DIM
};

enum Control
{
    V_LEFT = 0,
    V_RIGHT,
    CONTROL_DIM
};

// Useful for holding the parameters of the Differential Drive robot,
// including integration timestep.
// Wheel distance defaults to 0.258 m, the width for the iRobot Create.
class DiffDrive
{
public:
    DiffDrive(const double dt,
            const std::array<double, 2> &control_lims, // {-min_u, max_u}
            const std::array<double, 4> &world_lims, // {-min_x, max_x, -min_y, max_y}
            const double wheel_dist=0.258);
    // Discrete time dynamics.
    Vector<STATE_DIM> operator()(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u);

    double wheel_dist() { return wheel_dist_; }
    double dt() { return dt_; }
private:
    double dt_;
    std::array<double, 2> control_lims_;
    std::array<double, 4> world_lims_;
    double wheel_dist_;
};

Vector<STATE_DIM> continuous_dynamics(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u,
                                    const double wheel_dist);
} // namespace diffdrive
} // namespace simulators


#define BOOST_TEST_MODULE Gpufit

#include "Gpufit/gpufit.h"

#include <boost/test/included/unit_test.hpp>

#include <array>

template<std::size_t SIZE>
void generate_gauss_2d_elliptic(std::array< float, SIZE>& values)
{
    int const size_x = int(std::sqrt(SIZE));
    int const size_y = size_x;

    float const a = 4;
    float const x0 = (float(size_x) - 1.f) / 2.f;
    float const y0 = (float(size_y) - 1.f) / 2.f;
    float const sx = 0.4f;
    float const sy = 0.6f;
    float const b = 1.f;

    for (int point_index_y = 0; point_index_y < size_y; point_index_y++)
    {
        for (int point_index_x = 0; point_index_x < size_x; point_index_x++)
        {
            int const point_index = point_index_y * size_x + point_index_x;
            float const argx = ((point_index_x - x0)*(point_index_x - x0)) / (2.f * sx * sx);
            float const argy = ((point_index_y - y0)*(point_index_y - y0)) / (2.f* sy * sy);
            float const ex = exp(-argx) * exp(-argy);
            values[point_index] = a * ex + b;
        }
    }
}

BOOST_AUTO_TEST_CASE( Gauss_Fit_2D_Elliptic )
{
    std::size_t const n_fits{ 1 } ;
    std::size_t const n_points{ 25 } ;
    std::array< float, n_points > data{};
    generate_gauss_2d_elliptic(data);
    std::array< float, n_points > weights{};
    std::fill(weights.begin(), weights.end(), 1.f);
    std::array< float, 6 > initial_parameters{ { 2.f, 1.8f, 2.2f, 0.5f, 0.5f, 0.f } };
    float tolerance{ 0.001f };
    int max_n_iterations{ 10 };
    std::array< int, 6 > parameters_to_fit{ { 1, 1, 1, 1, 1, 1 } };
    std::array< float, 6 > output_parameters;
    int output_states;
    float output_chi_square;
    int output_n_iterations;

    int const status
            = gpufit
            (
                n_fits,
                n_points,
                data.data(),
                weights.data(),
                GAUSS_2D_ELLIPTIC,
                initial_parameters.data(),
                tolerance,
                max_n_iterations,
                parameters_to_fit.data(),
                LSE,
                0,
                0,
                output_parameters.data(),
                &output_states,
                &output_chi_square,
                &output_n_iterations
            ) ;

    BOOST_CHECK( status == 0 ) ;
}


/* boost random/triangle_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: triangle_distribution.hpp 49314 2008-10-13 09:00:03Z johnmaddock $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP
#define BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// triangle distribution, with a smallest, b most probable, and c largest
// value.
template<class RealType = double>
class triangle_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

  explicit triangle_distribution(result_type a_arg = result_type(0),
                                 result_type b_arg = result_type(0.5),
                                 result_type c_arg = result_type(1))
    : _a(a_arg), _b(b_arg), _c(c_arg)
  {
    assert(_a <= _b && _b <= _c);
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine
  result_type a() const { return _a; }
  result_type b() const { return _b; }
  result_type c() const { return _c; }

  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    result_type u = eng();
    if( u <= q1 )
      return _a + p1*sqrt(u);
    else
      return _c - d3*sqrt(d2*u-d1);
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const triangle_distribution& td)
  {
    os << td._a << " " << td._b << " " << td._c;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, triangle_distribution& td)
  {
    is >> std::ws >> td._a >> std::ws >> td._b >> std::ws >> td._c;
    td.init();
    return is;
  }
#endif

private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    d1 = _b - _a;
    d2 = _c - _a;
    d3 = sqrt(_c - _b);
    q1 = d1 / d2;
    p1 = sqrt(d1 * d2);
  }

  result_type _a, _b, _c;
  result_type d1, d2, d3, q1, p1;
};

} // namespace boost

#endif // BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP


/**
   Transformation between Polar-Laguerre and Hermite basis
*/

// system includes
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <boost/lexical_cast.hpp>
#include <functional>
#include <iostream>

// own includes
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/polar_to_hermite.hpp"

using namespace std;
using namespace boltzmann;

TEST(spectral, polar2hermite)
{
  std::vector<int> Ks = {4, 10, 20, 25, 36, 50, 80};

  cout << "Testing for K=";
  for_each(Ks.begin(), Ks.end(), [](int x) { cout << x << " "; });
  cout << "\n";

  for (int K : Ks) {
    typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
    hermite_basis_t hermite_basis;
    SpectralBasisFactoryHN::create(hermite_basis, K, 2);

    typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
    polar_basis_t polar_basis;
    SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true /* sorted */);

    Polar2Hermite<polar_basis_t, hermite_basis_t> p2h(polar_basis, hermite_basis);

    Eigen::VectorXd cp(polar_basis.n_dofs());
    cp.setOnes();

    Eigen::VectorXd ch(hermite_basis.n_dofs());
    p2h.to_hermite(ch, cp);

    Eigen::VectorXd cp2(polar_basis.n_dofs());
    p2h.to_polar(cp2, ch);

    double err = (cp2 - cp).cwiseAbs2().maxCoeff();
    cout << "err: " << err << "\n";
    EXPECT_NEAR(err, 0, 1e-14) << "K=" << boost::lexical_cast<string>(K);
  }
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example overlap_counter.cpp \file overlap_counter.cpp

    \brief The most simple application of an interval map:
           Counting the overlaps of added intervals.

    The most basic application of an interval_map is a counter counting
    the number of overlaps of intervals inserted into it.

    On could call an interval_map an aggregate on overlap machine. A very basic
    aggregation is summation of an integer. A interval_map<int,int> maps
    intervals of int to ints.

    If we insert a value pair (discrete_interval<int>(2,6), 1) into the interval_map, it
    increases the content of all value pairs in the map by 1, if their interval
    part overlaps with discrete_interval<int>(2,6).

    \include overlap_counter_/overlap_counter.cpp
*/
//[example_overlap_counter
#include <iostream>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost::icl;

/* The most simple example of an interval_map is an overlap counter.
   If intervals are added that are associated with the value 1,
   all overlaps of added intervals are counted as a result in the
   associated values.
*/
typedef interval_map<int, int> OverlapCounterT;

void print_overlaps(const OverlapCounterT& counter)
{
    for(OverlapCounterT::const_iterator it = counter.begin(); it != counter.end(); it++)
    {
        discrete_interval<int> itv  = (*it).first;
        int overlaps_count = (*it).second;
        if(overlaps_count == 1)
            cout << "in interval " << itv << " intervals do not overlap" << endl;
        else
            cout << "in interval " << itv << ": "<< overlaps_count << " intervals overlap" << endl;
    }
}

void overlap_counter()
{
    OverlapCounterT overlap_counter;
    discrete_interval<int> inter_val;

    inter_val = discrete_interval<int>::right_open(4,8);
    cout << "-- adding   " << inter_val << " -----------------------------------------" << endl;
    overlap_counter += make_pair(inter_val, 1);
    print_overlaps(overlap_counter);
    cout << "-----------------------------------------------------------" << endl;

    inter_val = discrete_interval<int>::right_open(6,9);
    cout << "-- adding   " << inter_val << " -----------------------------------------" << endl;
    overlap_counter += make_pair(inter_val, 1);
    print_overlaps(overlap_counter);
    cout << "-----------------------------------------------------------" << endl;

    inter_val = discrete_interval<int>::right_open(1,9);
    cout << "-- adding   " << inter_val << " -----------------------------------------" << endl;
    overlap_counter += make_pair(inter_val, 1);
    print_overlaps(overlap_counter);
    cout << "-----------------------------------------------------------" << endl;

}

int main()
{
    cout << ">>Interval Container Library: Sample overlap_counter.cpp <<\n";
    cout << "-----------------------------------------------------------\n";
    overlap_counter();
    return 0;
}

// Program output:

// >>Interval Container Library: Sample overlap_counter.cpp <<
// -----------------------------------------------------------
// -- adding   [4,8) -----------------------------------------
// in interval [4,8) intervals do not overlap
// -----------------------------------------------------------
// -- adding   [6,9) -----------------------------------------
// in interval [4,6) intervals do not overlap
// in interval [6,8): 2 intervals overlap
// in interval [8,9) intervals do not overlap
// -----------------------------------------------------------
// -- adding   [1,9) -----------------------------------------
// in interval [1,4) intervals do not overlap
// in interval [4,6): 2 intervals overlap
// in interval [6,8): 3 intervals overlap
// in interval [8,9): 2 intervals overlap
// -----------------------------------------------------------
//]


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/utility.hpp>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

double f(double) { /* cout << "double\n"; */ return 1.0; }
complex<double> f(complex<double>)
{
    //cout << "complex\n";
    return complex<double>(1.0, -1.0);
}

int main(int, char**)
{
    using namespace mtl; using mtl::io::tout;
    unsigned size=4, row= size+1, col=size;

    double tol(0.00001);
    dense_vector<double>                vec(size), vec1(size);
    dense2D<double>                     A(row, col),   Q(row, row),   R(row, col),   A_test(row, col),
	                                A_t(col, row), Q_t(col, col), R_t(col, row), A_t_test(col, row);
    dense2D<complex<double> >                            dz(row, col), Qz(row, row), Rz(row, col);
    dense2D<double, mat::parameters<col_major> >         dc(size, size);
    compressed2D<double>                                 Ac(size, size), Qc(size, size), Rc(size, size), A_testc(size, size) ;
    A= 0;

    A[0][0]=1;    A[0][1]=1;    A[0][2]=1;
    A[1][0]=3;    A[1][1]=-1;   A[1][2]=-2;
    A[2][0]=1;    A[2][1]=7;    A[2][2]=1;
    A[3][3]=-10;  A[4][0]=4;    A[4][2]=3;
    tout<<"A=\n"<< A <<"\n";
    laplacian_setup(Ac, 2,2);

    tout<<"START-----dense2d---------row > col\n";

    dense2D<double> A1(A[iall][iall]), A2(A);
    boost::tie(Q, R)= qr(A1);
    tout<<"R=\n"<< R <<"\n";
    tout<<"Q=\n"<< Q <<"\n";
    A_test= Q*R-A2;
    tout<<"Q*R=\n"<< Q*R <<"\n";

    tout<< "one_norm(Rest A)=" << one_norm(A_test) << "\n";
    MTL_THROW_IF(one_norm(A_test) > tol, mtl::logic_error("wrong QR decomposition of matrix A"));

    tout<<"START------dense2d-------row < col\n";

    A_t= trans(A);
    boost::tie(Q_t, R_t)= qr(A_t);
    tout<<"R_t=\n"<< R_t <<"\n";
    tout<<"Q_t=\n"<< Q_t <<"\n";
    A_t_test= Q_t*R_t-A_t;
    tout<<"Q_t*R_t=\n"<< Q_t*R_t <<"\n";

    tout<< "one_norm(Rest A')=" << one_norm(A_t_test) << "\n";
    MTL_THROW_IF(one_norm(A_t_test) > tol, mtl::logic_error("wrong QR decomposition of matrix trans(A)"));

    tout<<"START-------compressed2d-------row > col\n";
#if 1
    boost::tie(Qc, Rc)= qr(Ac);
    tout<<"R=\n"<< Rc <<"\n";
    tout<<"Q=\n"<< Qc <<"\n";
    A_testc= Qc*Rc-Ac;
    tout<<"Q*R=\n"<< Qc*Rc <<"\n";
    tout<<"A=\n"<< Ac <<"\n";

    tout<< "one_norm(Rest A)=" << one_norm(A_testc) << "\n";
    MTL_THROW_IF(one_norm(A_testc) > tol, mtl::logic_error("wrong QR decomposition of matrix A"));
#endif

#if 0
    dz[0][0]=complex<double>(1.0, 0.0);
    dz[0][1]=complex<double>(1.0, 0.0);
    dz[0][2]=complex<double>(1,0);
    dz[1][0]=complex<double>(1,0);
    dz[1][1]=complex<double>(-1,0);
    dz[1][2]=complex<double>(-2,0);
    dz[2][0]=complex<double>(1,0);
    dz[2][1]=complex<double>(-2,0);
    dz[2][2]=complex<double>(1,0);
    dz[3][3]=complex<double>(-10,0);
    tout<<"MAtrix complex=\n"<< dz <<"\n";

    tout<<"START-----complex---------"<< dz[0][0] << "\n";
    //
    boost::tie(Qz, Rz)= qr(dz);
    // Rz= qr_zerl(dz).second;
    // tout<<"MAtrix  R="<< Rz <<"\n";
    // tout<<"MAtrix  Q="<< Qz <<"\n";
    // tout<<"MAtrix  A=Q*R--outside"<< Qz*Rz <<"\n";
#endif
    return 0;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include "constants.hpp"

namespace vgl {
template <typename T>
void print_quat(Eigen::Quaternion<T> const& quat) {
    std::cout << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z() << std::endl;
}

template <typename T>
Eigen::Matrix<T, 3, 1> quat_to_euler_angles(Eigen::Quaternion<T> const& q) {
    return 180.0 / math::pi<double> *
           Eigen::Matrix<T, 3, 1>(
               std::atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
               std::asin(2.0 * (q.w() * q.y() - q.x() * q.z())),
               std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())));
}

struct Pose {
    Pose() noexcept;
    Pose(Eigen::Quaterniond const& R, Eigen::Vector3d const& t) noexcept;
    Pose(Eigen::Matrix4d const& T) noexcept;
    Pose inverse() const;
    Eigen::Matrix4d to_matrix() const;
    void from_matrix(Eigen::Matrix4d const& T);
    void print();

    Eigen::Quaterniond rotation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
    Eigen::Vector3d translation = Eigen::Vector3d(0.0, 0.0, 0.0);
};
} // namespace vgl


#include <fstream>
#include <iostream>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

int main() {
    double a11, a12, a13, a21, a22, a23, a31, a32, a33;
    Eigen::Matrix3d Im;

    std::cin >> a11 >> a12 >> a13
             >> a21 >> a22 >> a23
             >> a31 >> a32 >> a33;

    if (std::cin.fail()) {
        std::cerr << "Invalid input. Expected 3x3 matrix in the form a11 a12 a13 a21 ... a33, separated by whitespace." << std::endl;
        abort();
    }

    Im << a11, a12, a13,
          a21, a22, a23,
          a31, a32, a33;

    // The inertia tensor should be a real symmetric matrix; real symmetric
    // matrices are self-adjoint.
    // A real symmetric n-dimensional matrix always possesses n real
    // mutually orthogonal eigenvectors/eigenvalues.
    // http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node66.html
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(Im);

    if (eigenSolver.info() != Eigen::Success) {
        std::cerr << "Failed to solve for principal axes of rotation. Invalid inertia tensor?" << std::endl;
        abort();
    }

    // http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node67.html
    Eigen::Vector3d eigenvalues(eigenSolver.eigenvalues());
    std::cout << eigenvalues[0] << " "
              << eigenvalues[1] << " "
              << eigenvalues[2] << std::endl;
    Eigen::Quaterniond q(eigenSolver.eigenvectors());
    q.normalize();
    std::cout << q.w() << " "
              << q.x() << " "
              << q.y() << " "
              << q.z() << " " << std::endl;
}