//  (C) Copyright Jeremy Murphy 2015.
//  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/config.hpp>
#define BOOST_TEST_MAIN
#include <boost/array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/integer/common_factor_rt.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <utility>
#include <array>
#include <list>

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3)
#  define TEST1
#  define TEST2
#  define TEST3
#endif

using namespace boost::math;
using boost::integer::gcd;
using namespace boost::math::tools;
using namespace std;
using boost::integer::gcd_detail::Euclid_gcd;
using boost::math::tools::subresultant_gcd;

template <typename T>
struct answer
{
    answer(std::pair< polynomial<T>, polynomial<T> > const &x) :
    quotient(x.first), remainder(x.second) {}

    polynomial<T> quotient;
    polynomial<T> remainder;
};

std::array<double, 4> const d3a = {{10, -6, -4, 3}};
std::array<double, 4> const d3b = {{-7, 5, 6, 1}};

std::array<double, 2> const d1a = {{-2, 1}};
std::array<double, 1> const d0a = {{6}};
std::array<double, 2> const d0a1 = {{0, 6}};
std::array<double, 6> const d0a5 = {{0, 0, 0, 0, 0, 6}};

std::array<int, 9> const d8 = {{-5, 2, 8, -3, -3, 0, 1, 0, 1}};
std::array<int, 9> const d8b = {{0, 2, 8, -3, -3, 0, 1, 0, 1}};

BOOST_AUTO_TEST_CASE(trivial)
{
   /* We have one empty test case here, so that there is always something for Boost.Test to do even if the tests below are #if'ed out */
}

#ifdef TEST1

std::array<double, 4> const d3c = {{10.0/3.0, -2.0, -4.0/3.0, 1.0}};
std::array<double, 3> const d2a = {{-2, 2, 3}};
std::array<double, 3> const d2b = {{-7, 5, 6}};
std::array<double, 3> const d2c = {{31, -21, -22}};
std::array<double, 1> const d0b = {{3}};
std::array<int, 7> const d6 = {{21, -9, -4, 0, 5, 0, 3}};
std::array<int, 3> const d2 = {{-6, 0, 9}};
std::array<int, 6> const d5 = {{-9, 0, 3, 0, -15}};

BOOST_AUTO_TEST_CASE( test_construction )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d3a.begin(), 3);
    BOOST_CHECK_EQUAL(a, b);
}

#ifdef BOOST_MATH_HAS_IS_CONST_ITERABLE

#include <list>
#include <array>

BOOST_AUTO_TEST_CASE(test_range_construction)
{
   std::list<double> l{ 1, 2, 3, 4 };
   std::array<double, 4> a{ 3, 4, 5, 6 };
   polynomial<double> p1{ 1, 2, 3, 4 };
   polynomial<double> p2{ 3, 4, 5, 6 };

   polynomial<double> p3(l);
   polynomial<double> p4(a);

   BOOST_CHECK_EQUAL(p1, p3);
   BOOST_CHECK_EQUAL(p2, p4);
}
#endif

#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !BOOST_WORKAROUND(BOOST_GCC_VERSION, < 40500)
BOOST_AUTO_TEST_CASE( test_initializer_list_construction )
{
    polynomial<double> a(begin(d3a), end(d3a));
    polynomial<double> b = {10, -6, -4, 3};
    polynomial<double> c{10, -6, -4, 3};
    polynomial<double> d{10, -6, -4, 3, 0, 0};
    BOOST_CHECK_EQUAL(a, b);
    BOOST_CHECK_EQUAL(b, c);
    BOOST_CHECK_EQUAL(d.degree(), 3u);
}

BOOST_AUTO_TEST_CASE( test_initializer_list_assignment )
{
    polynomial<double> a(begin(d3a), end(d3a));
    polynomial<double> b;
    b = {10, -6, -4, 3, 0, 0};
    BOOST_CHECK_EQUAL(b.degree(), 3u);
    BOOST_CHECK_EQUAL(a, b);
}
#endif

BOOST_AUTO_TEST_CASE( test_degree )
{
    polynomial<double> const zero;
    polynomial<double> const a(d3a.begin(), d3a.end());
    BOOST_CHECK_THROW(zero.degree(), std::logic_error);
    BOOST_CHECK_EQUAL(a.degree(), 3u);
}

BOOST_AUTO_TEST_CASE( test_division_over_field )
{
    polynomial<double> const a(d3a.begin(), d3a.end());
    polynomial<double> const b(d1a.begin(), d1a.end());
    polynomial<double> const q(d2a.begin(), d2a.end());
    polynomial<double> const r(d0a.begin(), d0a.end());
    polynomial<double> const c(d3b.begin(), d3b.end());
    polynomial<double> const d(d2b.begin(), d2b.end());
    polynomial<double> const e(d2c.begin(), d2c.end());
    polynomial<double> const f(d0b.begin(), d0b.end());
    polynomial<double> const g(d3c.begin(), d3c.end());
    polynomial<double> const zero;
    polynomial<double> const one(1.0);

    answer<double> result = quotient_remainder(a, b);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);
    BOOST_CHECK_EQUAL(a, q * b + r); // Sanity check.

    result = quotient_remainder(a, c);
    BOOST_CHECK_EQUAL(result.quotient, f);
    BOOST_CHECK_EQUAL(result.remainder, e);
    BOOST_CHECK_EQUAL(a, f * c + e); // Sanity check.

    result = quotient_remainder(a, f);
    BOOST_CHECK_EQUAL(result.quotient, g);
    BOOST_CHECK_EQUAL(result.remainder, zero);
    BOOST_CHECK_EQUAL(a, g * f + zero); // Sanity check.
    // Check that division by a regular number gives the same result.
    BOOST_CHECK_EQUAL(a / 3.0, g);
    BOOST_CHECK_EQUAL(a % 3.0, zero);

    // Sanity checks.
    BOOST_CHECK_EQUAL(a / a, one);
    BOOST_CHECK_EQUAL(a % a, zero);
    // BOOST_CHECK_EQUAL(zero / zero, zero); // TODO
}

BOOST_AUTO_TEST_CASE( test_division_over_ufd )
{
    polynomial<int> const zero;
    polynomial<int> const one(1);
    polynomial<int> const aa(d8.begin(), d8.end());
    polynomial<int> const bb(d6.begin(), d6.end());
    polynomial<int> const q(d2.begin(), d2.end());
    polynomial<int> const r(d5.begin(), d5.end());

    answer<int> result = quotient_remainder(aa, bb);
    BOOST_CHECK_EQUAL(result.quotient, q);
    BOOST_CHECK_EQUAL(result.remainder, r);

    // Sanity checks.
    BOOST_CHECK_EQUAL(aa / aa, one);
    BOOST_CHECK_EQUAL(aa % aa, zero);
}

#endif

template <typename T>
struct FM2GP_Ex_8_3__1
{
    polynomial<T> x;
    polynomial<T> y;
    polynomial<T> z;

    FM2GP_Ex_8_3__1()
    {
        std::array<T, 5> const x_data = {{105, 278, -88, -56, 16}};
        std::array<T, 5> const y_data = {{70, 232, -44, -64, 16}};
        std::array<T, 3> const z_data = {{35, -24, 4}};
        x = polynomial<T>(x_data.begin(), x_data.end());
        y = polynomial<T>(y_data.begin(), y_data.end());
        z = polynomial<T>(z_data.begin(), z_data.end());
    }
};

template <typename T>
struct FM2GP_Ex_8_3__2
{
    polynomial<T> x;
    polynomial<T> y;
    polynomial<T> z;

    FM2GP_Ex_8_3__2()
    {
        std::array<T, 5> const x_data = {{1, -6, -8, 6, 7}};
        std::array<T, 5> const y_data = {{1, -5, -2, 15, 11}};
        std::array<T, 3> const z_data = {{1, 2, 1}};
        x = polynomial<T>(x_data.begin(), x_data.end());
        y = polynomial<T>(y_data.begin(), y_data.end());
        z = polynomial<T>(z_data.begin(), z_data.end());
    }
};

template <typename T>
struct FM2GP_mixed
{
    polynomial<T> x;
    polynomial<T> y;
    polynomial<T> z;

    FM2GP_mixed()
    {
        std::array<T, 4> const x_data = {{-2.2, -3.3, 0, 1}};
        std::array<T, 3> const y_data = {{-4.4, 0, 1}};
        std::array<T, 2> const z_data= {{-2, 1}};
        x = polynomial<T>(x_data.begin(), x_data.end());
        y = polynomial<T>(y_data.begin(), y_data.end());
        z = polynomial<T>(z_data.begin(), z_data.end());
    }
};

template <typename T>
struct FM2GP_trivial
{
    polynomial<T> x;
    polynomial<T> y;
    polynomial<T> z;

    FM2GP_trivial()
    {
        std::array<T, 4> const x_data = {{-2, -3, 0, 1}};
        std::array<T, 3> const y_data = {{-4, 0, 1}};
        std::array<T, 2> const z_data= {{-2, 1}};
        x = polynomial<T>(x_data.begin(), x_data.end());
        y = polynomial<T>(y_data.begin(), y_data.end());
        z = polynomial<T>(z_data.begin(), z_data.end());
    }
};

// Sanity checks to make sure I didn't break it.
#ifdef TEST1
typedef boost::mpl::list<signed char, short, int, long> integral_test_types;
typedef boost::mpl::list<int, long> large_integral_test_types;
typedef boost::mpl::list<> mp_integral_test_types;
#elif defined(TEST2)
typedef boost::mpl::list<
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   boost::multiprecision::cpp_int
#endif
> integral_test_types;
typedef integral_test_types large_integral_test_types;
typedef large_integral_test_types mp_integral_test_types;
#elif defined(TEST3)
typedef boost::mpl::list<> large_integral_test_types;
typedef boost::mpl::list<> integral_test_types;
typedef large_integral_test_types mp_integral_test_types;
#endif

#ifdef TEST1
typedef boost::mpl::list<double, long double> non_integral_test_types;
#elif defined(TEST2)
typedef boost::mpl::list<
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   boost::multiprecision::cpp_rational
#endif
> non_integral_test_types;
#elif defined(TEST3)
typedef boost::mpl::list<
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   boost::multiprecision::cpp_bin_float_single, boost::multiprecision::cpp_dec_float_50
#endif
> non_integral_test_types;
#endif

typedef boost::mpl::joint_view<integral_test_types, non_integral_test_types> all_test_types;

template <typename T>
void normalize(polynomial<T> &p)
{
    if (leading_coefficient(p) < T(0))
        std::transform(p.data().begin(), p.data().end(), p.data().begin(), std::negate<T>());
}

/**
 * Note that we do not expect 'pure' gcd algorithms to normalize the result.
 * However, the usual public interface function gcd() will do that.
 */

BOOST_AUTO_TEST_SUITE(test_subresultant_gcd)

// This test is just to show that gcd<polynomial<T>>(u, v) is defined (and works) when T is integral and multiprecision.
BOOST_FIXTURE_TEST_CASE_TEMPLATE( gcd_interface, T, mp_integral_test_types, FM2GP_Ex_8_3__1<T> )
{
    typedef FM2GP_Ex_8_3__1<T> fixture_type;
    polynomial<T> w;
    w = gcd(fixture_type::x, fixture_type::y);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
    w = gcd(fixture_type::y, fixture_type::x);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
}

// This test is just to show that gcd<polynomial<T>>(u, v) is defined (and works) when T is floating point.
BOOST_FIXTURE_TEST_CASE_TEMPLATE( gcd_float_interface, T, non_integral_test_types, FM2GP_Ex_8_3__1<T> )
{
    typedef FM2GP_Ex_8_3__1<T> fixture_type;
    polynomial<T> w;
    w = gcd(fixture_type::x, fixture_type::y);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
    w = gcd(fixture_type::y, fixture_type::x);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
}

// The following tests call subresultant_gcd explicitly to remove any ambiguity
// and to permit testing on single-precision integral types.
BOOST_FIXTURE_TEST_CASE_TEMPLATE( Ex_8_3__1, T, large_integral_test_types, FM2GP_Ex_8_3__1<T> )
{
    typedef FM2GP_Ex_8_3__1<T> fixture_type;
    polynomial<T> w;
    w = subresultant_gcd(fixture_type::x, fixture_type::y);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
    w = subresultant_gcd(fixture_type::y, fixture_type::x);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( Ex_8_3__2, T, large_integral_test_types, FM2GP_Ex_8_3__2<T> )
{
    typedef FM2GP_Ex_8_3__2<T> fixture_type;
    polynomial<T> w;
    w = subresultant_gcd(fixture_type::x, fixture_type::y);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
    w = subresultant_gcd(fixture_type::y, fixture_type::x);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( trivial_int, T, large_integral_test_types, FM2GP_trivial<T> )
{
    typedef FM2GP_trivial<T> fixture_type;
    polynomial<T> w;
    w = subresultant_gcd(fixture_type::x, fixture_type::y);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
    w = subresultant_gcd(fixture_type::y, fixture_type::x);
    normalize(w);
    BOOST_CHECK_EQUAL(w, fixture_type::z);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_CASE_TEMPLATE( test_addition, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero;

    polynomial<T> result = a + b; // different degree
    std::array<T, 4> tmp = {{8, -5, -4, 3}};
    polynomial<T> expected(tmp.begin(), tmp.end());
    BOOST_CHECK_EQUAL(result, expected);
    BOOST_CHECK_EQUAL(a + zero, a);
    BOOST_CHECK_EQUAL(a + b, b + a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_subtraction, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const zero;

    BOOST_CHECK_EQUAL(a - T(0), a);
    BOOST_CHECK_EQUAL(T(0) - a, -a);
    BOOST_CHECK_EQUAL(a - zero, a);
    BOOST_CHECK_EQUAL(zero - a, -a);
    BOOST_CHECK_EQUAL(a - a, zero);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_multiplication, T, all_test_types )
{
    polynomial<T> const a(d3a.begin(), d3a.end());
    polynomial<T> const b(d1a.begin(), d1a.end());
    polynomial<T> const zero;
    std::array<T, 7> const d3a_sq = {{100, -120, -44, 108, -20, -24, 9}};
    polynomial<T> const a_sq(d3a_sq.begin(), d3a_sq.end());

    BOOST_CHECK_EQUAL(a * T(0), zero);
    BOOST_CHECK_EQUAL(a * zero, zero);
    BOOST_CHECK_EQUAL(zero * T(0), zero);
    BOOST_CHECK_EQUAL(zero * zero, zero);
    BOOST_CHECK_EQUAL(a * b, b * a);
    polynomial<T> aa(a);
    aa *= aa;
    BOOST_CHECK_EQUAL(aa, a_sq);
    BOOST_CHECK_EQUAL(aa, a * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_arithmetic_relations, T, all_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(2), a + a);
    BOOST_CHECK_EQUAL(a - b, -b + a);
    BOOST_CHECK_EQUAL(a, (a * a) / a);
    BOOST_CHECK_EQUAL(a, (a / a) * a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_non_integral_arithmetic_relations, T, non_integral_test_types )
{
    polynomial<T> const a(d8b.begin(), d8b.end());
    polynomial<T> const b(d1a.begin(), d1a.end());

    BOOST_CHECK_EQUAL(a * T(0.5), a / T(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_cont_and_pp, T, integral_test_types)
{
    std::array<polynomial<T>, 4> const q={{
        polynomial<T>(d8.begin(), d8.end()),
        polynomial<T>(d8b.begin(), d8b.end()),
        polynomial<T>(d3a.begin(), d3a.end()),
        polynomial<T>(d3b.begin(), d3b.end())
    }};
    for (std::size_t i = 0; i < q.size(); i++)
    {
        BOOST_CHECK_EQUAL(q[i], content(q[i]) * primitive_part(q[i]));
        BOOST_CHECK_EQUAL(primitive_part(q[i]), primitive_part(q[i], content(q[i])));
    }

    polynomial<T> const zero;
    BOOST_CHECK_EQUAL(primitive_part(zero), zero);
    BOOST_CHECK_EQUAL(content(zero), T(0));
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_self_multiply_assign, T, all_test_types )
{
    polynomial<T> a(d3a.begin(), d3a.end());
    polynomial<T> const b(a);
    std::array<double, 7> const d3a_sq = {{100, -120, -44, 108, -20, -24, 9}};
    polynomial<T> const asq(d3a_sq.begin(), d3a_sq.end());

    a *= a;

    BOOST_CHECK_EQUAL(a, b*b);
    BOOST_CHECK_EQUAL(a, asq);

    a *= a;

    BOOST_CHECK_EQUAL(a, b*b*b*b);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_right_shift, T, all_test_types )
{
    polynomial<T> a(d8b.begin(), d8b.end());
    polynomial<T> const aa(a);
    polynomial<T> const b(d8b.begin() + 1, d8b.end());
    polynomial<T> const c(d8b.begin() + 5, d8b.end());
    a >>= 0u;
    BOOST_CHECK_EQUAL(a, aa);
    a >>= 1u;
    BOOST_CHECK_EQUAL(a, b);
    a = a >> 4u;
    BOOST_CHECK_EQUAL(a, c);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_left_shift, T, all_test_types )
{
    polynomial<T> a(d0a.begin(), d0a.end());
    polynomial<T> const aa(a);
    polynomial<T> const b(d0a1.begin(), d0a1.end());
    polynomial<T> const c(d0a5.begin(), d0a5.end());
    a <<= 0u;
    BOOST_CHECK_EQUAL(a, aa);
    a <<= 1u;
    BOOST_CHECK_EQUAL(a, b);
    a = a << 4u;
    BOOST_CHECK_EQUAL(a, c);
    polynomial<T> zero;
    // Multiplying zero by x should still be zero.
    zero <<= 1u;
    BOOST_CHECK_EQUAL(zero, zero_element(multiplies< polynomial<T> >()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_odd_even, T, all_test_types)
{
    polynomial<T> const zero;
    BOOST_CHECK_EQUAL(odd(zero), false);
    BOOST_CHECK_EQUAL(even(zero), true);
    polynomial<T> const a(d0a.begin(), d0a.end());
    BOOST_CHECK_EQUAL(odd(a), true);
    BOOST_CHECK_EQUAL(even(a), false);
    polynomial<T> const b(d0a1.begin(), d0a1.end());
    BOOST_CHECK_EQUAL(odd(b), false);
    BOOST_CHECK_EQUAL(even(b), true);
}

// NOTE: Slightly unexpected: this unit test passes even when T = char.
BOOST_AUTO_TEST_CASE_TEMPLATE( test_pow, T, all_test_types )
{
   if (std::numeric_limits<T>::digits < 32)
      return;   // Invokes undefined behaviour
    polynomial<T> a(d3a.begin(), d3a.end());
    polynomial<T> const one(T(1));
    std::array<double, 7> const d3a_sqr = {{100, -120, -44, 108, -20, -24, 9}};
    std::array<double, 10> const d3a_cub =
        {{1000, -1800, -120, 2124, -1032, -684, 638, -18, -108, 27}};
    polynomial<T> const asqr(d3a_sqr.begin(), d3a_sqr.end());
    polynomial<T> const acub(d3a_cub.begin(), d3a_cub.end());

    BOOST_CHECK_EQUAL(pow(a, 0), one);
    BOOST_CHECK_EQUAL(pow(a, 1), a);
    BOOST_CHECK_EQUAL(pow(a, 2), asqr);
    BOOST_CHECK_EQUAL(pow(a, 3), acub);
    BOOST_CHECK_EQUAL(pow(a, 4), pow(asqr, 2));
    BOOST_CHECK_EQUAL(pow(a, 5), asqr * acub);
    BOOST_CHECK_EQUAL(pow(a, 6), pow(acub, 2));
    BOOST_CHECK_EQUAL(pow(a, 7), acub * acub * a);

    BOOST_CHECK_THROW(pow(a, -1), std::domain_error);
    BOOST_CHECK_EQUAL(pow(one, 137), one);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_bool, T, all_test_types)
{
    polynomial<T> const zero;
    polynomial<T> const a(d0a.begin(), d0a.end());
    BOOST_CHECK_EQUAL(bool(zero), false);
    BOOST_CHECK_EQUAL(bool(a), true);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_set_zero, T, all_test_types)
{
    polynomial<T> const zero;
    polynomial<T> a(d0a.begin(), d0a.end());
    a.set_zero();
    BOOST_CHECK_EQUAL(a, zero);
    a.set_zero(); // Ensure that setting zero to zero is a no-op.
    BOOST_CHECK_EQUAL(a, zero);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_leading_coefficient, T, all_test_types)
{
    polynomial<T> const zero;
    BOOST_CHECK_EQUAL(leading_coefficient(zero), T(0));
    polynomial<T> a(d0a.begin(), d0a.end());
    BOOST_CHECK_EQUAL(leading_coefficient(a), T(d0a.back()));
}

#if !defined(BOOST_NO_CXX11_RVALUE_REFERENCES) && !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)
BOOST_AUTO_TEST_CASE_TEMPLATE(test_prime, T, all_test_types)
{
    std::vector<T> d{1,1,1,1,1};
    polynomial<T> p(std::move(d));
    polynomial<T> q = p.prime();
    BOOST_CHECK_EQUAL(q(0), T(1));

    for (size_t i = 0; i < q.size(); ++i)
    {
        BOOST_CHECK_EQUAL(q[i], i+1);
    }

    polynomial<T> P = p.integrate();
    BOOST_CHECK_EQUAL(P(0), T(0));
    for (size_t i = 1; i < P.size(); ++i)
    {
        BOOST_CHECK_EQUAL(P[i], 1/static_cast<T>(i));
    }

    polynomial<T> empty;
    q = empty.prime();
    BOOST_CHECK_EQUAL(q.size(), 0);

}
#endif


#pragma once

#include <Eigen/Core>
#include <vector>

#include "../common.hpp"

namespace Discregrid
{

/**
 * \brief Computes smallest enclosing spheres of pointsets using Welzl's algorithm
 * \Author: Tassilo Kugelstadt
 */
class BoundingSphere
{

public:

	/**
	 * \brief default constructor sets the center and radius to zero.
	 */
	BoundingSphere() : m_x(Vector3r::Zero()), m_r(0.0) {}

	/**
	 * \brief constructor which sets the center and radius
	 *
	 * \param x	3d coordinates of the center point
	 * \param r radius of the sphere
	 */
	BoundingSphere(const Vector3r& x, Real r) : m_x(x), m_r(r) {}

	/**
	 * \brief	constructs a sphere for one point (with radius 0)
	 *
	 * \param a	3d coordinates of point a
	 */
	BoundingSphere(const Vector3r& a)
	{
		m_x = a;
		m_r = 0.0;
	}

	/**
	 * \brief	constructs the smallest enclosing sphere for two points
	 *
	 * \param a 3d coordinates of point a
	 * \param b 3d coordinates of point b
	 */
	BoundingSphere(const Vector3r& a, const Vector3r& b)
	{
		const Vector3r ba = b - a;

		m_x = (a + b) * 0.5;
		m_r = 0.5 * ba.norm();
	}

	/**
	 * \brief	constructs the smallest enclosing sphere for three points
	 *
	 * \param a 3d coordinates of point a
	 * \param b 3d coordinates of point b
	 * \param c 3d coordinates of point c
	 */
	BoundingSphere(const Vector3r& a, const Vector3r& b, const Vector3r& c)
	{
		const Vector3r ba = b - a;
		const Vector3r ca = c - a;
		const Vector3r baxca = ba.cross(ca);
		Vector3r r;
		Matrix3r T;
		T << ba[0], ba[1], ba[2],
			ca[0], ca[1], ca[2],
			baxca[0], baxca[1], baxca[2];

		r[0] = 0.5 * ba.squaredNorm();
		r[1] = 0.5 * ca.squaredNorm();
		r[2] = 0.0;

		m_x = T.inverse() * r;
		m_r = m_x.norm();
		m_x += a;
	}

	/**
	 * \brief constructs the smallest enclosing sphere for four points
	 *
	 * \param a 3d coordinates of point a
	 * \param b 3d coordinates of point b
	 * \param c 3d coordinates of point c
	 * \param d 3d coordinates of point d
	 */
	BoundingSphere(const Vector3r& a, const Vector3r& b, const Vector3r& c, const Vector3r& d)
	{
		const Vector3r ba = b - a;
		const Vector3r ca = c - a;
		const Vector3r da = d - a;
		Vector3r r;
		Matrix3r T;
		T << ba[0], ba[1], ba[2],
			ca[0], ca[1], ca[2],
			da[0], da[1], da[2];

		r[0] = 0.5 * ba.squaredNorm();
		r[1] = 0.5 * ca.squaredNorm();
		r[2] = 0.5 * da.squaredNorm();
		m_x = T.inverse() * r;
		m_r = m_x.norm();
		m_x += a;
	}

	/**
	 * \brief	constructs the smallest enclosing sphere a given pointset
	 *
	 * \param p vertices of the points
	 */
	BoundingSphere(const std::vector<Vector3r>& p)
	{
		m_r = 0;
		m_x.setZero();
		setPoints(p);
	}

	/**
	 * \brief	Getter for the center of the sphere
	 *
	 * \return	const reference of the sphere center
	 */
	Vector3r const& x() const { return m_x; }

	/**
	 * \brief	Access function for center of the sphere
	 *
	 * \return	reference of the sphere center
	 */
	Vector3r& x() { return m_x; }

	/**
	 * \brief	Getter for the radius
	 *
	 * \return	Radius of the sphere
	 */
	Real r() const { return m_r; }

	/**
	 * \brief	Access function for the radius
	 *
	 * \return	Reference to the radius of the sphere
	 */
	Real& r() { return m_r; }

	/**
	 * \brief	constructs the smallest enclosing sphere a given pointset
	 *
	 * \param p vertices of the points
	 */
	void setPoints(const std::vector<Vector3r>& p)
	{
		//remove duplicates
		std::vector<Vector3r> v(p);
		std::sort(v.begin(), v.end(), [](const Vector3r& a, const Vector3r& b)
			{
				if (a[0] < b[0]) return true;
				if (a[0] > b[0]) return false;
				if (a[1] < b[1]) return true;
				if (a[1] > b[1]) return false;
				return (a[2] < b[2]);
			});
		v.erase(std::unique(v.begin(), v.end(), [](Vector3r& a, Vector3r& b) { return a.isApprox(b); }), v.end());

		Vector3r d;
		const int n = int(v.size());

		//generate random permutation of the points and perturb the points by epsilon to avoid corner cases
		const Real epsilon = 1.0e-6;
		for (int i = n - 1; i > 0; i--)
		{
			const Vector3r epsilon_vec = epsilon * Vector3r::Random();
			const int j = static_cast<int>(floor(i * Real(rand()) / RAND_MAX));
			d = v[i] + epsilon_vec;
			v[i] = v[j] - epsilon_vec;
			v[j] = d;
		}

		BoundingSphere S = BoundingSphere(v[0], v[1]);

		for (int i = 2; i < n; i++)
		{
			//SES0
			d = v[i] - S.x();
			if (d.squaredNorm() > S.r()* S.r())
				S = ses1(i, v, v[i]);
		}

		m_x = S.m_x;
		m_r = S.m_r + epsilon;	//add epsilon to make sure that all non-perturbed points are inside the sphere
	}

	/**
	 * \brief		intersection test for two spheres
	 *
	 * \param other other sphere to be tested for intersection
	 * \return		returns true when this sphere and the other sphere are intersecting
	 */
	bool overlaps(BoundingSphere const& other) const
	{
		const Real rr = m_r + other.m_r;
		return (m_x - other.m_x).squaredNorm() < rr * rr;
	}

	/**
	 * \brief		tests whether the given sphere other is contained in the sphere
	 *
	 * \param		other bounding sphere
	 * \return		returns true when the other is contained in this sphere or vice versa
	 */
	bool contains(BoundingSphere const& other) const
	{
		const Real rr = r() - other.r();
		return (x() - other.x()).squaredNorm() < rr * rr;
	}

	/**
	 * \brief		tests whether the given point other is contained in the sphere
	 *
	 * \param		other 3d coordinates of a point
	 * \return		returns true when the point is contained in the sphere
	 */
	bool contains(Vector3r const& other) const
	{
		return (x() - other).squaredNorm() < m_r * m_r;
	}

private:

	/**
	 * \brief		constructs the smallest enclosing sphere for n points with the points q1, q2, and q3 on the surface of the sphere
	 *
	 * \param n		number of points
	 * \param p		vertices of the points
	 * \param q1	3d coordinates of a point on the surface
	 * \param q2	3d coordinates of a second point on the surface
	 * \param q3	3d coordinates of a third point on the surface
	 * \return		smallest enclosing sphere
	 */
	BoundingSphere ses3(int n, std::vector<Vector3r>& p, Vector3r& q1, Vector3r& q2, Vector3r& q3)
	{
		BoundingSphere S(q1, q2, q3);

		for (int i = 0; i < n; i++)
		{
			Vector3r d = p[i] - S.x();
			if (d.squaredNorm() > S.r()* S.r())
				S = BoundingSphere(q1, q2, q3, p[i]);
		}
		return S;
	}

	/**
	 * \brief		constructs the smallest enclosing sphere for n points with the points q1 and q2 on the surface of the sphere
	 *
	 * \param n		number of points
	 * \param p		vertices of the points
	 * \param q1	3d coordinates of a point on the surface
	 * \param q2	3d coordinates of a second point on the surface
	 * \return		smallest enclosing sphere
	 */
	BoundingSphere ses2(int n, std::vector<Vector3r>& p, Vector3r& q1, Vector3r& q2)
	{
		BoundingSphere S(q1, q2);

		for (int i = 0; i < n; i++)
		{
			Vector3r d = p[i] - S.x();
			if (d.squaredNorm() > S.r()* S.r())
				S = ses3(i, p, q1, q2, p[i]);
		}
		return S;
	}
	/**
	 * \brief		constructs the smallest enclosing sphere for n points with the point q1 on the surface of the sphere
	 *
	 * \param n		number of points
	 * \param p		vertices of the points
	 * \param q1	3d coordinates of a point on the surface
	 * \return		smallest enclosing sphere
	 */
	BoundingSphere ses1(int n, std::vector<Vector3r>& p, Vector3r& q1)
	{
		BoundingSphere S(p[0], q1);

		for (int i = 1; i < n; i++)
		{
			Vector3r d = p[i] - S.x();
			if (d.squaredNorm() > S.r()* S.r())
				S = ses2(i, p, q1, p[i]);
		}
		return S;
	}

	Vector3r m_x;
	Real m_r;
};

}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>
#include <iostream>
#include <ctime>
#include <stdio.h>

using namespace boost;
typedef adjacency_list<> Graph;
// The length of cycle of rand48 is 2^48-1, which is much longer than
// minstd_rand.
// http://www.boost.org/doc/libs/1_55_0/doc/html/boost_random/reference.html
typedef rand48 rand_gen_t;
typedef rmat_iterator<rand_gen_t, Graph> RMATGen;
typedef graph_traits<Graph>::vertex_iterator vertex_iter;
typedef property_map<Graph, vertex_index_t>::type IndexMap;

int main(int argc, char* argv[])
{
	if (argc < 3) {
		fprintf(stderr, "usage: make_graph #vertices, #numedges [output]\n");
		return EXIT_FAILURE;
	}
	typedef boost::graph_traits<Graph>::vertices_size_type vertices_size_type;
	typedef boost::graph_traits<Graph>::edges_size_type edges_size_type;
	vertices_size_type n = atol(argv[1]);
	edges_size_type m = atol(argv[2]);

	FILE *f = stdout;
	std::string out_file;
	if (argc >= 4) {
		out_file = argv[3];
		f = fopen(argv[3], "w");
		assert(f);
	}

	fprintf(stderr, "Vertices = %ld\n", n);
	fprintf(stderr, "Edges = %ld\n", m);

	std::clock_t start;
	start = std::clock();
	rand_gen_t gen(time(NULL));
	RMATGen gen_it(gen, n, m, 0.57, 0.19, 0.19, 0.05, true);
	RMATGen gen_end;
	for (; gen_it != gen_end; ++gen_it) {
		fprintf(f, "%ld %ld\n", gen_it->first, gen_it->second);
	}
#if 0
	// Create graph with 100 nodes and 400 edges
	Graph g(RMATGen(gen, n, m, 0.57, 0.19, 0.19, 0.05, true), RMATGen(), n);

	IndexMap index = get(vertex_index, g);

	// Get vertex set
#if 0
	std::pair<vertex_iter, vertex_iter> vp;
	for (vp = vertices(g); vp.first != vp.second; ++vp.first)
		std::cout << index[*vp.first] <<  " ";
	std::cout << std::endl;
#endif

	// Get edge set
	graph_traits<Graph>::edge_iterator ei, ei_end;
	for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
		std::cout << index[source(*ei, g)]<< " " << index[target(*ei, g)] << "\n";
#endif
	if (f != stdout) {
		printf("close %s\n", out_file.c_str());
		fclose(f);
	}

	std::cerr << "The time to build the graph = " << (std::clock()-start)/(double)CLOCKS_PER_SEC << std::endl;
	return 0;
}


#include <iostream>

#include <El.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

const int m = 50000;
const int n = 500;

template<typename MatrixType, typename RhsType, typename SolType>
void check_solution(const MatrixType &A, const RhsType &b, const SolType &x,
    const RhsType &r0,
    double &res, double &resAtr, double &resFac) {
    RhsType r(b);
    skylark::base::Gemv(El::NORMAL, -1.0, A, x, 1.0, r);
    res = skylark::base::Nrm2(r);

    SolType Atr(x.Height(), x.Width(), x.Grid());
    skylark::base::Gemv(El::TRANSPOSE, 1.0, A, r, 0.0, Atr);
    resAtr = skylark::base::Nrm2(Atr);

    skylark::base::Axpy(-1.0, r0, r);
    RhsType dr(b);
    skylark::base::Axpy(-1.0, r0, dr);
    resFac = skylark::base::Nrm2(r) / skylark::base::Nrm2(dr);
}

template<typename MatrixType, typename RhsType, typename SolType>
void experiment() {
    typedef MatrixType matrix_type;
    typedef RhsType rhs_type;
    typedef SolType sol_type;

    double res, resAtr, resFac;

    boost::mpi::communicator world;
    int rank = world.rank();

    skylark::base::context_t context(23234);

    // Setup problem and righthand side
    // Using Skylark's uniform generator (as opposed to Elemental's)
    // will insure the same A and b are generated regardless of the number
    // of processors.
    matrix_type A, b;
    skylark::base::UniformMatrix(A, m, n, context);
    skylark::base::UniformMatrix(b, m, 1, context);

    sol_type x(n,1);
    rhs_type r(b);

    boost::mpi::timer timer;
    double telp;

    // Solve using Elemental. Note: Elemental only supports [MC,MR]...
    El::DistMatrix<double> A1 = A, b1 = b, x1;
    timer.restart();
    El::LeastSquares(El::NORMAL, A1, b1, x1);
    telp = timer.elapsed();
    x = x1;
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Elemental:\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    double res_opt = res;

    // The following computes the optimal residual (r^\star in the logs)
    skylark::base::Gemv(El::NORMAL, -1.0, A, x, 1.0, r);

#if SKYLARK_HAVE_FFTW || SKYLARK_HAVE_FFTWF || SKYLARK_HAVE_KISSFFT
    // Solve using Sylark
    timer.restart();
    skylark::nla::FasterLeastSquares(El::NORMAL, A, b, x, context);
    telp = timer.elapsed();
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Skylark:\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

    // Approximately solve using Sylark
    timer.restart();
    skylark::nla::ApproximateLeastSquares(El::NORMAL, A, b, x, context);
    telp = timer.elapsed();
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Skylark (approximate):\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
#else
    std::cout << "You need to have Skylark supporting FFTW or FFTWF "
              << "to solve with skylark least_squares.cpp"
              << std::endl;
#endif
}

int main(int argc, char** argv) {

    El::Initialize(argc, argv);

    boost::mpi::communicator world;
    int rank = world.rank();

    if (rank == 0)
        std::cout << "Matrix: [VC,STAR], Rhs: [VC,STAR], Sol: [STAR,STAR]\n\n";
    experiment<El::DistMatrix<double, El::VC, El::STAR>,
               El::DistMatrix<double, El::VC, El::STAR>,
               El::DistMatrix<double, El::STAR, El::STAR> > ();

    if (rank == 0)
        std::cout << "\nMatrix: [MC,MR], Rhs: [MC,MR], Sol: [MC,MR]\n\n";
    experiment<El::DistMatrix<double>, El::DistMatrix<double>,
               El::DistMatrix<double> >();

    return 0;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>

using real = double;
using uint = unsigned int;

using Mat = Eigen::MatrixXd;
using Vec = Eigen::VectorXd;
using Vec3 = Eigen::Vector3d;
using Vec3i = Eigen::Vector3i;
using Vec4i = Eigen::Vector4i;
using SpMat = Eigen::SparseMatrix<double, Eigen::ColMajor>;
using SpVec = Eigen::SparseVector<double, Eigen::ColMajor>;


/*!
 * \file hnf.hpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the 2-clause BSD License.
 * \date August, 2020: created
 */

#pragma once

#include <valarray>
#include <tuple>
#include <cmath>
#include <Eigen/Dense>

#include "utils.hpp"
#include "hnf_impl_lll.hpp"

//* For debug
#include "debug/debug.hpp"
// */

namespace khover {

/*!
 * Computing the Hermite normal form of a given matrix.
 * The original "pseudo-code" is found in the paper
 *   > George Havas, Bohdan S. Majewski & Keith R. Matthews (1998) Extended GCD and Hermite Normal Form Algorithms via Lattice Basis Reduction, Experimental Mathematics, 7:2, 125-136, DOI: 10.1080/10586458.1998.10504362
 * This function also applies the transformation on the target matrix and its adjoint transformation to given matrices.
 * Usage:
 * \code
 *   // Compute a row echelon form.
 *   auto u0 = u; auto m0 = m; auto v0 = v;
 *   hnf_LLL<typename khover::rowops>(m,std::tie(u),std::tie(v));
 *   assert(is_rowhnf(m));
 *   assert(u*m == u0*m0);
 *   assert(u*v == u0*v0);
 * \endcode
 *
 * \code
 *   // Compute a column echelon form.
 *   auto u0 = u; auto m0 = m; auto v0 = v;
 *   hnf_LLL<typename khover::colops>(m,std::tie(u),std::tie(v));
 *   assert(is_colhnf(m));
 *   assert(m*u == m0*u0);
 *   assert(v*u == v0*u0);
 * \endcode
 * \tparam Ops A collection of elementary operations; \see{khover::rowops}, \see{khover::colops}.
 * \param m The target matrix to be transformed into its Hermite normal form.
 * \param us A tuple of matrices subject to the adjoint transformation.
 * \param vs A tuple of matrices subject to the transformation.
 * \return If success, the rank of the given matrix over Q (the field of rationals).
 */
template<
    class Ops,
    class MT,int MR, int MC, int MOpt, int MRMax, int MCMax,
    class...UTs, class...VTs
    >
std::optional<std::size_t> hnf_LLL(
    Eigen::Matrix<MT,MR,MC,MOpt,MRMax,MCMax> &m,
    std::tuple<UTs&...> us,
    std::tuple<VTs&...> vs
    ) noexcept
{
    static_assert(
        std::conjunction<typename khover::is_pubbase_of_template<Eigen::MatrixBase,UTs>...>::value,
        "Matrices U contain a class not derived from Eigen::MatrixBase");
    static_assert(
        std::conjunction<typename khover::is_pubbase_of_template<Eigen::MatrixBase,VTs>...>::value,
        "Matices V contain a class not derived from Eigen::MatrixBase");
    static_assert(
        std::conjunction<std::bool_constant<(UTs::Flags & Eigen::LvalueBit) != 0>...>::value,
        "Matrices U contain read-only variables");
    static_assert(
        std::conjunction<std::bool_constant<(VTs::Flags & Eigen::LvalueBit) != 0>...>::value,
        "Matrices V contain read-only variables");

    std::size_t nvecs = Ops::dual_t::size(m);

    if (!foldl_tuple(true, us, [nvecs](bool b, auto& u) { return b && Ops::size(u) >= nvecs; })) {
        ERR_MSG("Matricies U with invalid sizes.");
        return std::nullopt;
    }

    if (!foldl_tuple(true, vs, [nvecs](bool b, auto& v) { return b && Ops::dual_t::size(v) >= nvecs; })) {
        ERR_MSG("Matricies V with invalid sizes.");
        return std::nullopt;
    }

    // Nothing to do on empty matrices
    if (nvecs == 0 || Ops::size(m) == 0) {
        return std::make_optional(0);
    }

    // Ensure the pivot of the last vector to be non-negative.
    std::size_t l = Ops::find_nonzero(
        m, nvecs-1,
        [&m,&us,&vs](std::size_t l, auto x) {
            if (std::signbit(x)) {
                Ops::scalar(m,0,-1);
                for_each_tuple(us, [](auto& u){ Ops::dual_t::scalar(u,0,-1); });
                for_each_tuple(vs, [](auto& v){ Ops::scalar(v,0,-1); });
            }
        });

    // If the given matrix consists of a single vector, then all the step is finished.
    if (nvecs == 1) {
        return l < Ops::size(m) ? 1 : 0;
    }

    _impl_LLL::Lambda_t lambda(nvecs);

    // The index of the vector that we currently focus on.
    std::size_t cur = nvecs - 1;
    // The rank of the span of vectors below cursor.
    std::size_t rk = 0;
    // Flag whether the vector just below the cursor is non-zero or not.
    bool is_below_nz = false;

    // Proceed the algorithm on the first k rows.
    while (cur > 0) {
        //DBG_MSG("cur=" << cur << "\n" << "rk = " << rk << "\n" << m);

        auto howswap = _impl_LLL::reduce<Ops,false>(cur-1, cur, m, us, vs, lambda);

        if (howswap & _impl_LLL::HowSwap::ShouldSwap) {
            _impl_LLL::swap<Ops>(cur-1, m, us, vs, lambda);
            if (cur+1 < nvecs) {
                ++cur;
                if (is_below_nz) {
                    --rk;
                    is_below_nz = rk > 0;
                }
            }
        }
        else if (howswap & _impl_LLL::HowSwap::ZeroReducer) {
            --cur;
            is_below_nz = false;
        }
        else {
            for (std::size_t i = cur+1; i < nvecs; ++i)
                _impl_LLL::reduce<Ops,true>(cur-1, i, m, us, vs, lambda);
            --cur;
            ++rk;
            is_below_nz = true;
        }
    }

    if (rk > 0) {
        return rk+1;
    }
    else {
        return Ops::find_nonzero(m, 0, [](auto,auto){})
            < Ops::size(m)
              ? 1 : 0;
    }
}

}


/**
 *  testAbst.cpp
 *
 *  Test abstraction by using a car kinematics model.
 *
 *  Created by Yinan Li on Nov. 14, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <cmath>
#include <sys/stat.h>
#include <boost/numeric/odeint.hpp>

#include "src/grid.h"
#include "src/definitions.h"
#include "src/abstraction.hpp"
#include "src/hdf5io.h"

/* user defined dynamics */
struct car_ode {
    rocs::Rn u;
    car_ode (const rocs::Rn param): u (param) {}
    /**
     * ODE model
     * @param x system state: [x,y,theta], n=3
     * @param dxdt vector field
     * @param t time
     */
    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = u[0]*std::cos(x[2]);
	dxdt[1] = u[0]*std::sin(x[2]);
	dxdt[2] = u[1];
    }
};

const double h = 0.3;  // sampling time
const double dt = 0.001; //integration step size for odeint

struct carde { // discrete-time model (difference equation)
    static const int n = 3;  // system dimension
    static const int m = 2;
    /**
     * Discrete-time dynamics
     * @param h sampling time
     * @param x system state: [x,y,theta], n=3
     * @param u control array (size of 2, velocity and steering angle)
     * @param nu the number of different control values
     */
    template<typename S>
    carde(S &dx, const S &x, rocs::Rn u) {
	if (std::fabs(u[0]) < 1e-6) { //v=0
	    dx[0] = x[0];
	    dx[1] = x[1];
	    dx[2] = x[2] + u[1] * h;
	} else if (std::fabs(u[1]) < 1e-6) { //w=0
	    dx[0] = x[0] + u[0]* cos(x[2])*h;
	    dx[1] = x[1] + u[0]* sin(x[2])*h;
	    dx[2] = x[2];
	} else { //v,w not 0
	    dx[0] = x[0] + u[0]/u[1]*2*sin(u[1]*h/2.)*cos(x[2]+u[1]*h/2.);
	    dx[1] = x[1] + u[0]/u[1]*2*sin(u[1]*h/2.)*sin(x[2]+u[1]*h/2.);
	    dx[2] = x[2] + u[1] * h;
	}
    }

}; // struct carde

struct twoagent {
    static const int n = 3;  // system dimension
    static const int nu = 2;  // control dimension
    rocs::ivec d{rocs::interval(-0.8, 0.8),
		 rocs::interval(-0.8, 0.8)};

    /* template constructor
     * @param[out] dx
     * @param[in] x = [xr, yr, psir]
     * @param u = [v, w]
     * @param d = [v', w']
     */
    template<typename S>
    twoagent(S *dx, const S *x, rocs::Rn u) {
	dx[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dx[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dx[2] = d[1] - u[1];
    }
};

int main()
{
    /* Config */
    clock_t tb, te;
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;

    /**
     * Case I
     */
    /* Set the state and control space */
    const int xdim = 3;
    const int udim = 2;

    double xlb[] = {-3, -3, -M_PI};
    double xub[] = {3, 3, M_PI};
    double eta[] = {0.2, 0.2, 0.2};

    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};

    /**
     * Define the two-agent system
     */
    double t = 0.3;
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 5;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);
    rocs::CTCntlSys<twoagent> safety("collision-free", t,
    				     twoagent::n, twoagent::nu,
    				     delta, &controlparams);

    safety.init_workspace(xlb, xub);
    safety.init_inputset(mu, ulb, uub);
    safety.allocate_flows();

    rocs::abstraction< rocs::CTCntlSys<twoagent> > abst(&safety);
    abst.init_state(eta, xlb, xub);
    std::cout << "# of in-domain nodes: " << abst._x._nv << '\n';
    /**
     * Assign 1 to the target invariant set and 0 to others.
     * Mark 0 for any box intersect or inside the cylinder: x^2+y^2<=rmin^2, any phi.
     * The invariant set is the region outside of the cylinder.
     */
    auto inv_set = [&abst, &eta](size_t i) {
    		       const double rmin = 1.21;
    		       std::vector<double> x(abst._x._dim);
    		       abst._x.id_to_val(x, i);
    		       double xl = x[0] - eta[0]/2.;
    		       double xr = x[0] + eta[0]/2.;
    		       double yl = x[1] - eta[1]/2.;
    		       double yr = x[1] + eta[1]/2.;
    		       double xsqr = (xr*xr) > (xl*xl) ? (xl*xl) : (xr*xr);
    		       double ysqr = (yr*yr) > (yl*yl) ? (yl*yl) : (yr*yr);
    		       if(xsqr + ysqr < rmin*rmin)
    			   return 0;
    		       else
    			   return 1;
    		   };
    abst.assign_labels(inv_set);
    abst.assign_label_outofdomain(1); //out of domain is safe

    std::string transfile = "abstca_0.2-0.2-0.2.h5";
    struct stat buffer;
    if(stat(transfile.c_str(), &buffer) == 0) {
    	/* Read from a file */
    	std::cout << "Reading transitions...\n";
    	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
    	tb = clock();
    	transRdr.read_transitions(abst._ts);
    	te = clock();
    } else {
    	std::cout << "No transition file found. Computing transitions...\n";
    	/* Robustness margins */
    	double e1[] = {0,0,0};
    	double e2[] = {0,0,0};
    	tb = clock();
    	abst.assign_transitions(e1, e2);
    	te = clock();
    	/* Write transitions to file */
    	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
    	transWtr.write_transitions(abst._ts);
    }
    float time = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of reading/computing abstraction: " << time << '\n';
    std::cout << "# of all nodes: " << abst._ts._nx << '\n';
    std::cout << "# of actions: " << abst._ts._nu << '\n';
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    /**
     * Case II
     */
    // /* Set the state space */
    // const int xdim = 3;
    // const int udim = 2;
    // const double theta = 3.5;
    // double xlb[] = {0, 0, -theta};
    // double xub[] = {10, 10, theta};
    // double eta[] = {0.2, 0.2, 0.2};
    // /* Set the control values */
    // double ulb[] = {-1.0, -1.0};
    // double uub[] = {1.0, 1.0};
    // double mu[] = {0.3, 0.3};
    // /* Define the control system */
    // rocs::DTCntlSys<carde> car("DBA", h, carde::n, carde::m);
    // car.init_workspace(xlb, xub);
    // car.init_inputset(mu, ulb, uub);

    // rocs::abstraction< rocs::DTCntlSys<carde> > abst(&car);
    // abst.init_state(eta, xlb, xub);
    // std::cout << "# of in-domain nodes: " << abst._x._nv << '\n';
    // /* Assign the label of avoid area to -1 */
    // rocs::UintSmall nAvoid = 4;
    // double obs[4][4] = {
    // 	{1.6, 5.7, 4.0, 5.0},
    // 	{3.0, 5.0, 5.0, 8.0},
    // 	{4.3, 5.7, 1.8, 4.0},
    // 	{5.7, 8.5, 1.8, 2.5}
    // };
    // auto label_avoid = [&obs, &nAvoid, &abst, &eta](size_t i) {
    // 		     std::vector<double> x(abst._x._dim);
    // 		     abst._x.id_to_val(x, i);
    // 		     double c1= eta[0]/2.0+1e-10;
    // 		     double c2= eta[1]/2.0+1e-10;
    // 		     for(size_t i = 0; i < nAvoid; ++i) {
    // 			 if ((obs[i][0]-c1) <= x[0] && x[0] <= (obs[i][1]+c1) &&
    // 			     (obs[i][2]-c2) <= x[1] && x[1] <= (obs[i][3]+c2))
    // 			     return -1;
    // 		     }
    // 		     return 0;
    // 		 };
    // abst.assign_labels(label_avoid);
    // abst.assign_label_outofdomain(-1);
    // std::vector<size_t> obstacles;
    // for (size_t i = 0; i < abst._x._nv; ++i) {
    // 	if (abst._labels[i] < 0)
    // 	    obstacles.push_back(i);
    // }

    // /* Compute/Read abstraction */
    // float tabst;
    // std::string transfile = "abstfull_0.2-0.2-0.2.h5";
    // struct stat buffer;
    // if(stat(transfile.c_str(), &buffer) == 0) {
    // 	/* Read from a file */
    // 	std::cout << "Reading transitions...\n";
    // 	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
    // 	tb = clock();
    // 	transRdr.read_transitions(abst._ts);
    // 	te = clock();
    // } else {
    // 	std::cout << "No transition file found. Computing transitions...\n";
    // 	/* Robustness margins */
    // 	double e1[] = {0,0,0};
    // 	double e2[] = {0,0,0};
    // 	tb = clock();
    // 	abst.assign_transitions(e1, e2);
    // 	te = clock();

    // 	/* Write abstraction to file */
    // 	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
    // 	transWtr.write_transitions(abst._ts);
    // }
    // tabst = (float)(te - tb)/CLOCKS_PER_SEC;
    // std::cout << "Time of reading/computing abstraction: " << tabst << '\n';
    // std::cout << "# of all nodes: " << abst._ts._nx << '\n';
    // std::cout << "# of actions: " << abst._ts._nu << '\n';
    // std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    /* Test */
    size_t na = abst._ts._nu;
    size_t nx = abst._ts._nx;
    size_t si, sk, k;
    bool suc = 0;
    int np;

    /* Test post-pre consistency */
    std::cout << "Checking post-pre consistency...\n";
    for(size_t i = 0; i < nx; ++i) {
	for(size_t j = 0; j < na; ++j) {
	    si = abst._ts._ptrpost[i*na+j];
	    for(size_t p=si; p<si+abst._ts._npost[i*na+j]; ++p) {
		k = abst._ts._idpost[p];
		/* Test if the pre of post by j contains i */
		suc = 0;
		sk = abst._ts._ptrpre[k*na+j];
		// /********** logging **********/
		// if(i == 0 && j == 16 && k == 0) {
		//     std::cout << "The predecessors of " << k << " with " << j << ": ";
		// }
		// /********** logging **********/
		for(size_t pp=sk; pp<sk+abst._ts._npre[k*na+j]; ++pp) {
		    // /********** logging **********/
		    // if(i == 0 && j == 16 && k == 0) {
		    // 	std::cout << "idpre["<< pp << "]="
		    // 		  << abst._ts._idpre[pp] << '\n';
		    // }
		    // /********** logging **********/
		    if(abst._ts._idpre[pp] == i) {
			suc = 1;
			break;
		    }
		}
		if(i == 0 && j == 16 && k == 0) {
		    std::cout << '\n';
		}
		if(!suc) {//two cases: npre(k,j)=0 or no i in npre(k, j)
		    std::cout << "Post and pre transitions are inconsistent "
			      << i << "->(" << j << ")->" << k << '\n';
		    return -1;
		}

	    }
	}
    }
    std::cout << "Every post transition has its corresponding pre transition.\n";

    for(size_t i = 0; i < nx; ++i) {
	for(size_t j = 0; j < na; ++j) {
	    si = abst._ts._ptrpre[i*na+j];
	    for(size_t p=si; p<si+abst._ts._npre[i*na+j]; ++p) {
		k = abst._ts._idpre[p];
		/* Test if the post of pre by j contains i */
		suc = 0;
		sk = abst._ts._ptrpost[k*na+j];
		for(size_t pp=sk; pp<sk+abst._ts._npost[k*na+j]; ++pp) {
		    if(abst._ts._idpost[pp] == i) {
			suc = 1;
			break;
		    }
		}
		if(!suc) {//two cases: npost(k,j)=0 or no i in npost(k, j)
		    std::cout << "Post and pre transitions are inconsistent "
			      << k << "->(" << j << ")->" << i << '\n';
		    return -1;
		}

	    }
	}
    }
    std::cout << "Every pre transition has its corresponding post transition.\n";

    // /* Test reachable set computation */
    // std::cout << "Checking post transitions by rechable set computation...\n";
    // rocs::Rn x(xdim);
    // rocs::Rn u(udim);
    // rocs::Rn xpost(xdim);
    // rocs::ivec box(xdim);
    // std::vector<rocs::ivec> reachset(na, rocs::ivec(xdim));
    // // std::vector<rocs::Rn> corners(std::pow(2, xdim), rocs::Rn(xdim));
    // rocs::Rn corner(xdim);
    // int quo, rem;
    // rocs::ivec margin{rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL),
    // 		      rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL),
    // 		      rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL)};
    // rocs::ivec yt(xdim);
    // for(size_t i = 0; i < nx; ++i) {
    // 	// std::cout << "State x= " << '(' << x[0] << ',' << x[1] << ',' << x[2] << "):\n";
    // 	if(i < abst._x._nv) { //belongs to xgrid
    // 	    /* Compute the reachable set */
    // 	    abst._x.id_to_val(x, i); //x is the center of the box i
    // 	    for(int d = 0; d < xdim; ++d)
    // 		box.setval(d, rocs::interval(x[d]-eta[d]/2., x[d]+eta[d]/2.));
    // 	    car.get_reach_set(reachset, box);

    // 	    /* Test valid control inputs */
    // 	    for(size_t j = 0; j < na; ++j) {
    // 		if(abst._ts._npost[i*na+j] > 0) {
    // 		    car._ugrid.id_to_val(u, j); //get control values
    // 		    /* Test if the reachable set covers ode solutions of all corners */
    // 		    for(int k = 0; k < std::pow(2, xdim); ++k) {
    // 			quo = k;
    // 			for(int d = 0; d < xdim; ++d) {
    // 			    if(quo % 2) {
    // 				corner[d] = x[d]+eta[d]/2.; //upper bound
    // 			    } else {
    // 				corner[d] = x[d]-eta[d]/2.; //lower bound
    // 			    }
    // 			    quo /= 2;
    // 			}
    // 			// std::cout << "Corner "
    // 			// 	  << '(' << corner[0] << ',' << corner[1] << ',' << corner[2] << ")\n";
    // 			boost::numeric::odeint::integrate_const(rk45, car_ode(u), corner, 0.0, h, dt);
    // 			yt = reachset[j] + margin;
    // 			if(!yt.isin(corner)) {
    // 			    std::cout << "The reachable set is incorrect with u="
    // 				      << '(' << u[0] << ',' << u[1] << "):"
    // 				      << '(' << corner[0] << ',' << corner[1] << ',' << corner[2] << ')'
    // 				      << " is not in " << yt << '\n'
    // 				      << "Test terminates.\n";
    // 			    return -1;
    // 			}
    // 		    }
    // 		    /* Test if all post nodes are in the reachable set (soundness) */
    // 		    si = abst._ts._ptrpost[i*na+j];
    // 		    for(size_t p = si; p<si+abst._ts._npost[i*na+j]; ++p) {
    // 			abst._x.id_to_val(xpost, abst._ts._idpost[p]); //xpost: post node center
    // 			for(int d = 0; d < xdim; ++d) //box: post interval centered at xpost
    // 			    box.setval(d, rocs::interval(xpost[d]-eta[d]/2., xpost[d]+eta[d]/2.));
    // 			if(reachset[j].isout(box)) { //box and reachset[j] should intersect
    // 			    std::cout << "Post transition for xid,uid=" << i << ',' << j
    // 				      << " is incorrect.\n"
    // 				      << "Test terminates.\n";
    // 			    return -1;
    // 			}
    // 		    }
    // 		}
    // 	    }//end for control values
    // 	} else { //out-of-domain node
    // 	    std::cout << "Checking the out-of-domain node...\n";
    // 	}
    // }

    return 0;
}


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2015 Chris Sweeney (cmsweeney@cs.ucsb.edu)
// Copyright (c) 2016 Pierre Moulon

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_NUMERIC_L1_SOLVER_ADMM_HPP
#define OPENMVG_NUMERIC_L1_SOLVER_ADMM_HPP

#include <Eigen/Core>
#ifdef EIGEN_MPL2_ONLY
#include <Eigen/SparseLU>
#else
#include <Eigen/Cholesky>
#include <Eigen/SparseCholesky>
#endif

#include <algorithm>
#include <iostream>
#include <string>

namespace openMVG {

// These are template overrides that allow the sparse linear solvers to work
// with sparse or dense matrices. The sparseView() method is not implemented for
// Eigen::SparseMatrix.
namespace l1_solver_internal {

template <typename Linear_SolverT>
inline void Compute
(
  const Eigen::SparseMatrix<double>& spd_mat,
  Linear_SolverT * linear_solver
)
{
  linear_solver->compute(spd_mat);
}

template <typename Linear_SolverT>
inline void Compute
(
  const Eigen::MatrixXd& spd_mat,
  Linear_SolverT * linear_solver
)
{
  linear_solver->compute(spd_mat.sparseView());
}

}  // namespace l1_solver_internal

// A L1 norm approximation solver. This class will attempt to solve the
// problem: || A * x - b || under L1-norm (as opposed to L2 i.e. "least-squares"
// norm). This problem can be solved with the alternating direction method of
// multipliers (ADMM) as a least unsquared deviations minimizer. A full
// description of the method, including how to use ADMM for L1 minimization can
// be found in "Distributed Optimization and Statistical Learning via the
// Alternating Direction Method of Multipliers" by Boyd et al, Foundations and
// Trends in Machine Learning (2012). The paper can be found at:
//   https://web.stanford.edu/~boyd/papers/pdf/admm_distr_stats.pdf
//
// ADMM can be much faster than interior point methods but convergence may be
// slower. Generally speaking, ADMM solvers converge to good solutions in only a
// few number of iterations, but can spend many iterations subsequently refining
// the solution to obtain the global optimum. The speed improvements are because
// the matrix A only needs to be factorized (by Cholesky decomposition) once, as
// opposed to every iteration.
//
// This implementation is based off of the code found at:
//   https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html
template <class MatrixType>
class L1Solver {
 public:
  struct Options {
    int max_num_iterations = 1000;
    // Rho is the augmented Lagrangian parameter.
    double rho = 1.0;
    // Alpha is the over-relaxation parameter (typically between 1.0 and 1.8).
    double alpha = 1.0;

    double absolute_tolerance = 1e-4;
    double relative_tolerance = 1e-2;
  };

  L1Solver
  (
    const Options& options,
    const MatrixType& mat
  )
  : options_(options), a_(mat)
  {
    // Analyze the sparsity pattern once. Only the values of the entries will be
    // changed with each iteration.
    const MatrixType spd_mat = a_.transpose() * a_;
    l1_solver_internal::Compute(spd_mat, &linear_solver_);
  }

  void SetMaxIterations
  (
    const int max_iterations
  )
  {
    options_.max_num_iterations = max_iterations;
  }

  bool Status() const
  {
    return linear_solver_.info() == Eigen::Success;
  }

  // Solves ||Ax - b||_1 for the optimal L1 solution given an initial guess for
  // x. To solve this we introduce an auxiliary variable y such that the
  // solution to:
  //        min   1 * y
  //   s.t. [  A   -I ] [ x ] < [  b ]
  //        [ -A   -I ] [ y ]   [ -b ]
  // which is an equivalent linear program.
  bool Solve
  (
    const Eigen::VectorXd& rhs,
    Eigen::VectorXd* solution
  )
  {
    // Since constructor was called before we check Compute status
    if (linear_solver_.info() != Eigen::Success)
    {
      std::cerr << "Cannot compute the matrix factorization" << std::endl;
      return false;
    }

    Eigen::VectorXd& x = *solution;
    Eigen::VectorXd z(a_.rows()), u(a_.rows());
    z.setZero();
    u.setZero();

    Eigen::VectorXd a_times_x(a_.rows()), z_old(z.size()), ax_hat(a_.rows());
    // Precompute some convergence terms.
    const double rhs_norm = rhs.norm();
    const double primal_abs_tolerance_eps =
      std::sqrt(a_.rows()) * options_.absolute_tolerance;
    const double dual_abs_tolerance_eps =
      std::sqrt(a_.cols()) * options_.absolute_tolerance;

    for (int i = 0; i < options_.max_num_iterations; ++i)
    {
      // Update x.
      x.noalias() = linear_solver_.solve(a_.transpose() * (rhs + z - u));
      a_times_x.noalias() = a_ * x;
      ax_hat.noalias() = options_.alpha * a_times_x;
      ax_hat.noalias() += (1.0 - options_.alpha) * (z + rhs);

      // Update z and set z_old.
      std::swap(z, z_old);
      z.noalias() = Shrinkage(ax_hat - rhs + u, 1.0 / options_.rho);

      // Update u.
      u.noalias() += ax_hat - z - rhs;

      // Compute the convergence terms.
      const double r_norm = (a_times_x - z - rhs).norm();
      const double s_norm =
        (-options_.rho * a_.transpose() * (z - z_old)).norm();
      const double max_norm =
        std::max({a_times_x.norm(), z.norm(), rhs_norm});
      const double primal_eps =
        primal_abs_tolerance_eps + options_.relative_tolerance * max_norm;
      const double dual_eps =
        dual_abs_tolerance_eps +
        options_.relative_tolerance *
          (options_.rho * a_.transpose() * u).norm();

      // Log the result to the screen.
      // std::ostringstream os;
      // os << "Iteration: " << i << "\n"
      //   << "R norm: " << r_norm << "\n"
      //   << "S norm: " << s_norm << "\n"
      //   << "Primal eps: " << primal_eps << "\n"
      //   << "Dual eps: " << dual_eps << std::endl;
      // std::cout << os.str() << std::endl;

      // Determine if the minimizer has converged.
      if (r_norm < primal_eps && s_norm < dual_eps)
      {
        return true;
      }
    }
    return false;
  }

 private:
  Options options_;

  // Matrix A where || Ax - b ||_1 is the problem we are solving.
  MatrixType a_;

  // Cholesky linear solver.
#ifdef EIGEN_MPL2_ONLY
  using Linear_Solver_T = Eigen::SparseLU<Eigen::SparseMatrix<double>>;
#else
  // Since our linear system will be a SPD matrix we can
  // utilize the Cholesky factorization.
  using Linear_Solver_T = Eigen::SimplicialLLT<Eigen::SparseMatrix<double>>;
#endif
  Linear_Solver_T linear_solver_;

  Eigen::VectorXd Shrinkage
  (
    const Eigen::VectorXd& vec, const double kappa
  ) const
  {
    Eigen::ArrayXd zero_vec(vec.size());
    zero_vec.setZero();
    return zero_vec.max( vec.array() - kappa) -
           zero_vec.max(-vec.array() - kappa);
  }
};

}  // namespace openMVG

#endif  // OPENMVG_NUMERIC_L1_SOLVER_ADMM_HPP


/**
 * @file radauthreetimestepping_test.cc
 * @brief NPDE homework "RadauThreeTimestepping" code
 * @author Tobias Rohner, edited by Oliver Rietmann
 * @date 16.03.2020
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>

#include <gtest/gtest.h>

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include "../radauthreetimestepping.h"

namespace RadauThreeTimestepping::test {

TEST(RadauThreeTimestepping, TrapRuleLinFEElemVecProvider) {
  // Get some triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  // Define some easy functions to test the provider with
  auto f1 = [](const Eigen::Vector2d &x) { return 0.0; };
  auto f2 = [](const Eigen::Vector2d &x) { return 1.0; };
  auto f3 = [](const Eigen::Vector2d &x) { return x[0]; };
  // Check the element vector for each triangle
  RadauThreeTimestepping::TrapRuleLinFEElemVecProvider f1p(f1);
  RadauThreeTimestepping::TrapRuleLinFEElemVecProvider f2p(f2);
  RadauThreeTimestepping::TrapRuleLinFEElemVecProvider f3p(f3);
  for (const auto tria : mesh_p->Entities(0)) {
    const auto geom = tria->Geometry();
    auto ev1 = f1p.Eval(*tria);
    auto ev2 = f2p.Eval(*tria);
    auto ev3 = f3p.Eval(*tria);
    ASSERT_TRUE(ev1.isApprox(Eigen::Vector3d::Zero()));
    ASSERT_TRUE(ev2.isApprox(
        Eigen::Vector3d::Constant(lf::geometry::Volume(*geom) / 3)));
    Eigen::Vector3d e3_correct =
        lf::geometry::Volume(*geom) / 3 * lf::geometry::Corners(*geom).row(0);
    ASSERT_TRUE(ev3.isApprox(e3_correct));
  }
}

TEST(RadauThreeTimestepping, rhsVectorheatSource) {
  // Generate a triangular test mesh on [0,1]^2
  const auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1. / 3);
  // Create a DOF handler
  const lf::uscalfe::FeSpaceLagrangeO1<double> fespace(mesh_p);
  const auto &dofh = fespace.LocGlobMap();
  // Assemble the vectors for t=0 and t=0.5
  const Eigen::VectorXd rhs0 =
      RadauThreeTimestepping::rhsVectorheatSource(dofh, 0.0);
  const Eigen::VectorXd rhs1 =
      RadauThreeTimestepping::rhsVectorheatSource(dofh, 0.5);
  // Get the DOFs on the boundary
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2);

  // Create a functional for time t=0 and t=0.5
  auto f0 = [](const Eigen::Vector2d &x) {
    return ((x[0] - 0.5) * (x[0] - 0.5) + x[1] * x[1] < 0.25) ? 1.0 : 0.0;
  };
  auto f1 = [](const Eigen::Vector2d &x) {
    return (x[0] * x[0] + (x[1] - 0.5) * (x[1] - 0.5) < 0.25) ? 1.0 : 0.0;
  };

  // Assume TrapRuleLinFEElemVecProvider works correctly,
  // as it is tested above
  RadauThreeTimestepping::TrapRuleLinFEElemVecProvider provider0(f0);
  RadauThreeTimestepping::TrapRuleLinFEElemVecProvider provider1(f1);
  Eigen::VectorXd rhs0_test = Eigen::VectorXd::Zero(dofh.NumDofs());
  Eigen::VectorXd rhs1_test = Eigen::VectorXd::Zero(dofh.NumDofs());
  lf::assemble::AssembleVectorLocally(0, dofh, provider0, rhs0_test);
  lf::assemble::AssembleVectorLocally(0, dofh, provider1, rhs1_test);

  double tol = 1e-10;

  // Make sure the TrapRuleLinFEElemVecProvider is indeed implemented already
  ASSERT_TRUE(rhs0_test.norm() > tol);
  ASSERT_TRUE(rhs1_test.norm() > tol);

  for (int i = 0; i < dofh.NumDofs(); ++i) {
    if (boundary(dofh.Entity(i))) {
      // Check whether boundary DOFs are set to zero
      ASSERT_DOUBLE_EQ(rhs0[i], 0)
          << "Have you forgotten to set the boundary values to zero?";
      ASSERT_DOUBLE_EQ(rhs1[i], 0)
          << "Have you forgotten to set the boundary values to zero?";
    } else {
      // Check whether the value coincides with the one
      // computed previously
      ASSERT_NEAR(rhs0[i], rhs0_test[i], tol);
      ASSERT_NEAR(rhs1[i], rhs1_test[i], tol);
    }
  }
}

TEST(RadauThreeTimestepping, solveHeatEvolution) {
  // Generate a triangular test mesh on [0,1]^2
  const auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1. / 3);
  // Create a DOF handler
  const lf::uscalfe::FeSpaceLagrangeO1<double> fespace(mesh_p);
  const auto &dofh = fespace.LocGlobMap();

  // Solve heat evolution with zero initial and boundary conditions
  double final_time = 1.0;
  unsigned int m = 50;

  Eigen::VectorXd sol =
      RadauThreeTimestepping::solveHeatEvolution(dofh, m, final_time);

  Eigen::VectorXd ref_sol(13);
  ref_sol << 0, 0, 0, 1.47965e-06, 1.46476e-06, 0, 0, 1.78839e-06, 1.36475e-06,
      0, 0, 0, 0;

  double tol = 1.e-4;

  ASSERT_NEAR((ref_sol - sol).norm(), 0.0, tol);
}

TEST(RadauThreeTimestepping, dropMatrixRowsColumns) {
  // Generate a triangular test mesh on [0,1]^2
  const auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1. / 3);
  // Create a DOF handler
  const lf::uscalfe::FeSpaceLagrangeO1<double> fespace(mesh_p);
  const auto &dofh = fespace.LocGlobMap();
  const lf::base::size_type N_dofs = dofh.NumDofs();

  // Obtain an array of boolean flags for the vertices of the mesh: 'true'
  // indicates that the vertex lies on the boundary.
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2)};
  // Index predicate for the selectvals FUNCTOR of dropMatrixRowsColumns
  auto bdy_vertices_selector = [&bd_flags, &dofh](unsigned int idx) -> bool {
    return bd_flags(dofh.Entity(idx));
  };

  lf::assemble::COOMatrix<double> A_COO(N_dofs, N_dofs);
  lf::uscalfe::LinearFELaplaceElementMatrix elLapMat_builder;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elLapMat_builder, A_COO);

  RadauThreeTimestepping::dropMatrixRowsColumns(bdy_vertices_selector, A_COO);

  Eigen::SparseMatrix<double> A_sps = A_COO.makeSparse();

  Eigen::MatrixXd A(A_sps);

  Eigen::MatrixXd A_ref = Eigen::MatrixXd::Zero(N_dofs, N_dofs);
  A_ref(0, 0) = 1;
  A_ref(1, 1) = 1;
  A_ref(2, 2) = 1;
  A_ref(3, 3) = 3.625;
  A_ref(3, 4) = -0.625;
  A_ref(3, 7) = -0.25;
  A_ref(3, 8) = -0.75;
  A_ref(4, 3) = -0.625;
  A_ref(4, 4) = 4.375;
  A_ref(4, 7) = -2;
  A_ref(5, 5) = 1;
  A_ref(6, 6) = 1;
  A_ref(7, 3) = -0.25;
  A_ref(7, 4) = -2;
  A_ref(7, 7) = 4.5;
  A_ref(7, 8) = -0.75;
  A_ref(8, 3) = -0.75;
  A_ref(8, 7) = -0.75;
  A_ref(8, 8) = 3.75;
  A_ref(9, 9) = 1;
  A_ref(10, 10) = 1;
  A_ref(11, 11) = 1;
  A_ref(12, 12) = 1;

  double tol = 1.e-4;

  ASSERT_NEAR((A - A_ref).norm(), 0.0, tol);
}

TEST(RadauThreeTimestepping, LinFEMassMatrixProvider) {
  // Generate a triangular test mesh on [0,1]^2
  const auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1. / 3);
  // Create a DOF handler
  const lf::uscalfe::FeSpaceLagrangeO1<double> fespace(mesh_p);
  const auto &dofh = fespace.LocGlobMap();

  // Compare the element matrices for every cell
  RadauThreeTimestepping::LinFEMassMatrixProvider provider;
  for (const auto cell : mesh_p->Entities(0)) {
    const auto geom = cell->Geometry();
    // Compute the correct element matrix
    Eigen::Matrix3d em_correct;
    em_correct.setConstant(lf::geometry::Volume(*geom) / 12);
    em_correct.diagonal() *= 2;
    // Get the element matrix from the element matrix provider
    Eigen::Matrix3d em_prov = provider.Eval(*cell);
    // Compare the matrices
    ASSERT_TRUE(em_correct.isApprox(em_prov));
  }
}

TEST(RadauThreeTimestepping, discreteEvolutionOperator) {
  // Generate a triangular test mesh on [0,1]^2
  const auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1. / 3);
  // Create a DOF handler
  const lf::uscalfe::FeSpaceLagrangeO1<double> fespace(mesh_p);
  const auto &dofh = fespace.LocGlobMap();

  // Compute A and M
  lf::uscalfe::LinearFELaplaceElementMatrix A_provider;
  RadauThreeTimestepping::LinFEMassMatrixProvider M_provider;
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  lf::assemble::COOMatrix<double> M_COO(dofh.NumDofs(), dofh.NumDofs());
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, A_provider, A_COO);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, M_provider, M_COO);
  const auto bd_flags = lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2);
  const auto selector = [&](unsigned idx) {
    return bd_flags(dofh.Entity(idx));
  };
  RadauThreeTimestepping::dropMatrixRowsColumns(selector, A_COO);
  RadauThreeTimestepping::dropMatrixRowsColumns(selector, M_COO);
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SparseMatrix<double> M = M_COO.makeSparse();

  const double dt = 0.0001;
  const Eigen::VectorXd phi =
      RadauThreeTimestepping::rhsVectorheatSource(dofh, dt);
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver(M + dt * A);
  RadauThreeTimestepping::Radau3MOLTimestepper timestepper(dofh);
  // Test for different mu
  for (unsigned i = 0; i < dofh.NumDofs(); ++i) {
    Eigen::VectorXd mu0(dofh.NumDofs());
    mu0.setZero();
    mu0[i] = 1;
    // Compute mu at the next timestep using discreteEvolutionOperator
    const Eigen::VectorXd mu_dEO =
        timestepper.discreteEvolutionOperator(0, dt, mu0);
    // Compute mu at the next timestep using implicit euler
    const Eigen::VectorXd mu_iE = solver.solve(M * mu0 + dt * phi);
    // Compare the two mu
    ASSERT_TRUE((mu_dEO - mu_iE).array().abs().maxCoeff() < 1e-4);
  }
}

}  // end namespace RadauThreeTimestepping::test


/*

	krandmptest01.cpp

	Written by Koji Yamamoto
	Copyright (C) 2020 Koji Yamamoto

	Testing krandmp Ver.k09.00

	Use compilation options:
	> cl krandmptest01.cpp -EHsc -openmp -Ox
	> g++ krandmptest01.cpp -std=c++17 -fopenmp -O3

*/

/* ********** Preprocessor Directives ********** */

#include <iostream>
#include <chrono>

#include <boost/math/tools/bivariate_statistics.hpp>

#include <k09/krand00.cpp>
#include <k09/krandmp00.cpp>

/* ********** Namespace Declarations/Directives ********** */

using namespace std;

/* ********** Class Declarations ********** */

/* ********** Enum Definitions ********** */

/* ********** Function Declarations ********** */

int main( int, char *[]);
double corr( const std::vector <double> &, const std::vector <double> &);

/* ********** Class Definitions ********** */

/* ********** Global Variables ********** */

/* ********** Definitions of Static Member Variables ********** */

/* ********** Function Definitions ********** */

int main( int argc, char *argv[])
{

	using time_point = std::chrono::system_clock::time_point;

	int npoints = 50000000;

	int nmaxthreads = getNMaxThreads();
	std::cout << "N Max Threads: " << nmaxthreads << std::endl << std::endl;

	int nusedthreads = getNUsedThreads();
	std::cout << "N Used Threads: " << nusedthreads << std::endl << std::endl;

	// マルチスレッド
	{

		std::cout << "Trial 1: Multi-Threaded" << std::endl;

		time_point startt = std::chrono::system_clock::now();

		RandomNumberEngineMP xrnemp( 123);
		RandomNumberEngineMP yrnemp( 456);
		// below instances will be initialized by moved objects
		std::vector <double> xvec = xrnemp.getRealUniformSeq( npoints, -1.0, 1.0);
		std::vector <double> yvec = yrnemp.getRealUniformSeq( npoints, -1.0, 1.0);

		time_point endt = std::chrono::system_clock::now();
		auto millisec = std::chrono::duration_cast <chrono::milliseconds> ( endt - startt);
		std::cout << millisec.count() << " milliseconds passed." << std::endl;
		std::cout << "Corr: " << corr( xvec, yvec) << std::endl;
		std::cout << std::endl;

	}

	// シングルスレッドでの比較用
	{

		std::cout << "Trial 2: Single-Threaded" << std::endl;

		time_point startt = std::chrono::system_clock::now();

		RandomNumberEngine xrne( 123);
		RandomNumberEngine yrne( 456);
		// below instances will be initialized by moved objects
		vector <double> xvec = xrne.getRealUniformSeq( npoints, -1.0, 1.0);
		vector <double> yvec = yrne.getRealUniformSeq( npoints, -1.0, 1.0);

		time_point endt = std::chrono::system_clock::now();
		auto millisec = std::chrono::duration_cast <chrono::milliseconds> ( endt - startt);
		std::cout << millisec.count() << " milliseconds passed." << std::endl;
		std::cout << "Corr: " << corr( xvec, yvec) << std::endl;
		std::cout << std::endl;

	}

	return 0;

}

double corr( const std::vector <double> &xvec, const std::vector <double> &yvec)
{

	double ret = boost::math::tools::correlation_coefficient( xvec, yvec);
	return ret;

}

/* ********** Definitions of Member Functions ********** */