// 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.

#ifndef MTL_CHOLESKY_INCLUDE
#define MTL_CHOLESKY_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>
#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/operation/dmat_dmat_mult.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/recursion/base_case_cast.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

namespace with_bracket {

    // ============================================================================
    // Generic Cholesky factorization and operands for Cholesky on with submatrices
    // ============================================================================

    template < typename Matrix >
    void cholesky_base (Matrix & matrix)
    {
	vampir_trace<5001> tracer;
	typedef typename  Collection<Matrix>::size_type    size_type;
	for (size_type k = 0; k < matrix.num_cols(); k++) {
	    matrix[k][k] = sqrt (matrix[k][k]);

		for (size_type i = k + 1; i < matrix.num_rows(); i++) {
		    matrix[i][k] /= matrix[k][k];
		    typename Collection<Matrix>::value_type d = matrix[i][k];

		    for (size_type j = k + 1; j <= i; j++)
			matrix[i][j] -= d * matrix[j][k];
		}
	}
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW)
    {
	vampir_trace<5002> tracer;
	typedef typename  Collection<MatrixSW>::size_type    size_type;
	for (size_type k = 0; k < NW.num_rows (); k++) {

	    for (size_type i = 0; i < SW.num_rows (); i++) {
		SW[i][k] /= NW[k][k];
		typename MatrixSW::value_type d = SW[i][k];

		for (size_type j = k + 1; j < SW.num_cols (); j++)
		    SW[i][j] -= d * NW[j][k];
	    }
	}
    }

    // Lower(SE) -= SW * SW^T
    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW)
    {
	vampir_trace<5003> tracer;
	typedef typename  Collection<MatrixSE>::size_type    size_type;
	for (size_type k = 0; k < SW.num_cols (); k++)

	    for (size_type i = 0; i < SE.num_rows (); i++) {
		    typename MatrixSW::value_type d = SW[i][k];
		    for (size_type j = 0; j <= i; j++)
			SE[i][j] -= d * SW[j][k];
	    }
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
    {
	vampir_trace<5004> tracer;
	typedef typename  Collection<MatrixNE>::size_type    size_type;
	for (size_type k = 0; k < NW.num_cols (); k++)
	    for (size_type i = 0; i < NE.num_rows (); i++) {
		typename MatrixNW::value_type d = NW[i][k];
		for (size_type j = 0; j < NE.num_cols (); j++)
		    NE[i][j] -= d * SW[j][k];
	    }
    }

    // ======================
    // Corresponding functors
    // ======================

    struct cholesky_base_t
    {
	template < typename Matrix >
	void operator() (Matrix & matrix)
	{
	    cholesky_base(matrix);
	}
    };

    struct tri_solve_base_t
    {
	template < typename MatrixSW, typename MatrixNW >
	void operator() (MatrixSW & SW, const MatrixNW & NW)
	{
	    tri_solve_base(SW, NW);
	}
    };

    struct tri_schur_base_t
    {
	template < typename MatrixSE, typename MatrixSW >
	void operator() (MatrixSE & SE, const MatrixSW & SW)
	{
	    tri_schur_base(SE, SW);
	}
    };

    struct schur_update_base_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator() (MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    schur_update_base(NE, NW, SW);
	}
    };

} // namespace with_bracket

#if 0
namespace with_iterator {

    // ============================================================================
    // Generic Cholesky factorization and operands for Cholesky on with submatrices
    // ============================================================================
    // CURRENTLY NOT SUPPORTED -- CAUSES SEGFAULT, e.g. with icc 11.0 in r8536 !!!!
    // ============================================================================

    template < typename Matrix >
    void cholesky_base (Matrix& matrix)
    {
	vampir_trace<5001> tracer;
	typedef typename  Collection<Matrix>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all    all_it;

        typedef typename Collection<Matrix>::value_type                         value_type;
        typedef typename range_generator<col, Matrix>::type       cur_type;
        typedef typename range_generator<all_it, cur_type>::type    iter_type;

	typedef typename range_generator<row, Matrix>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type   riter_type;

	size_type k= 0;
	for (cur_type kb= begin<col>(matrix), kend= end<col>(matrix); kb != kend; ++kb, ++k) {

	    iter_type ib= begin<all_it>(kb), iend= end<all_it>(kb);
	    ib+= k; // points now to matrix[k][k]

	    value_type root= sqrt (*ib);
	    *ib= root;

	    ++ib; // points now to matrix[k+1][k]
	    rcur_type rb= begin<row>(matrix); rb+= k+1; // to row k+1
	    for (size_type i= k + 1; ib != iend; ++ib, ++rb, ++i) {
		*ib = *ib / root;
		typename Collection<Matrix>::value_type d = *ib;
		riter_type it1= begin<all_it>(rb);    it1+= k+1;      // matrix[i][k+1]
		riter_type it1end= begin<all_it>(rb); it1end+= i+1;   // matrix[i][i+1]
		iter_type it2= begin<all_it>(kb);     it2+= k+1;      // matrix[k+1][k]
		for (; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
	}
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW)
    {
	vampir_trace<5002> tracer;
	typedef typename  Collection<MatrixSW>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixNW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixSW>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < NW.num_rows (); k++)
	    for (size_type i = 0; i < SW.num_rows (); i++) {

		typename MatrixSW::value_type d = SW[i][k] /= NW[k][k];

		rcur_type sw_i= begin<row>(SW);     sw_i+= i;  // row i
		riter_type it1= begin<all_it>(sw_i);  it1+= k+1; // SW[i][k+1]
		riter_type it1end= end<all_it>(sw_i);

		ccur_type nw_k= begin<col>(NW);     nw_k+= k;  // column k
		citer_type it2= begin<all_cit>(nw_k); it2+= k+1; // NW[k+1][k]

		for(; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
    }

    // Lower(SE) -= SW * SW^T
    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW)
    {
	vampir_trace<5003> tracer;
	typedef typename  Collection<MatrixSE>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixSW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixSE>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < SW.num_cols (); k++)
	    for (size_type i = 0; i < SE.num_rows (); i++) {
		typename MatrixSW::value_type d = SW[i][k];

		rcur_type se_i= begin<row>(SE);       se_i+= i;      // row i
		riter_type it1= begin<all_it>(se_i);                   // SE[i][0]
		riter_type it1end= begin<all_it>(se_i); it1end+= i+1;  // SE[i][i+i]

		ccur_type sw_k= begin<col>(SW);     sw_k+= k;        // column k
		citer_type it2= begin<all_cit>(sw_k);                  // SW[0][k]

		for(; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
    {
	vampir_trace<5004> tracer;
	typedef typename  Collection<MatrixNE>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixSW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixNE>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < NW.num_cols (); k++)
	    for (size_type i = 0; i < NE.num_rows (); i++) {
		typename MatrixNW::value_type d = NW[i][k];
#if 0
		rcur_type ne_i= begin<row>(NE);       ne_i+= i;      // row i
		riter_type it1= begin<all_it>(ne_i);                   // NE[i][0]
		riter_type it1end= end<all_it>(ne_i);                  // NE[i][num_col]

		ccur_type sw_k= begin<col>(SW);     sw_k+= k;        // column k
		citer_type it2= begin<all_cit>(sw_k);                  // SW[0][k]
#endif
		for (size_type j = 0; j < NE.num_cols (); j++)
		    NE[i][j] -= d * SW[j][k];
	    }
    }

    // ======================
    // Corresponding functors
    // ======================

    struct cholesky_base_t
    {
	template < typename Matrix >
	void operator() (Matrix & matrix)
	{
	    cholesky_base(matrix);
	}
    };

    struct tri_solve_base_t
    {
	template < typename MatrixSW, typename MatrixNW >
	void operator() (MatrixSW & SW, const MatrixNW & NW)
	{
	    tri_solve_base(SW, NW);
	}
    };

    struct tri_schur_base_t
    {
	template < typename MatrixSE, typename MatrixSW >
	void operator() (MatrixSE & SE, const MatrixSW & SW)
	{
	    tri_schur_base(SE, SW);
	}
    };

    struct schur_update_base_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator() (MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    schur_update_base(NE, NW, SW);
	}
    };

} // namespace with_iterator
#endif

// ==================================
// Functor types for Cholesky visitor
// ==================================

template <typename BaseTest, typename CholeskyBase, typename TriSolveBase, typename TriSchur, typename SchurUpdate>
struct recursive_cholesky_visitor_t
{
    typedef  BaseTest                   base_test;

    template < typename Recursator >
    bool is_base(const Recursator& recursator) const
    {
	return base_test()(recursator);
    }

    template < typename Matrix >
    void cholesky_base(Matrix & matrix) const
    {
	CholeskyBase()(matrix);
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW) const
    {
	TriSolveBase()(SW, NW);
    }

    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW) const
    {
	TriSchur()(SE, SW);
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW) const
    {
	SchurUpdate()(NE, NW, SW);
    }
};

namespace detail {

    // Compute schur update with external multiplication; must have Assign == minus_mult_assign_t !!!
    template <typename MatrixMult>
    struct mult_schur_update_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator()(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    transposed_view<MatrixSW> trans_sw(const_cast<MatrixSW&>(SW));
	    MatrixMult()(NW, trans_sw, NE);
	}
    };

} // detail

namespace with_bracket {
    typedef recursive_cholesky_visitor_t<recursion::bound_test_static<64>, cholesky_base_t, tri_solve_base_t,
					 tri_schur_base_t, schur_update_base_t >
               recursive_cholesky_base_visitor_t;
}

#if 0
namespace with_iterator {
    typedef recursive_cholesky_visitor_t<recursion::bound_test_static<64>,
					 cholesky_base_t, tri_solve_base_t, tri_schur_base_t, schur_update_base_t>
               recursive_cholesky_base_visitor_t;
}
#endif

typedef with_bracket::recursive_cholesky_base_visitor_t                    recursive_cholesky_default_visitor_t;

namespace with_recursator {

    template <typename Recursator, typename Visitor>
    void schur_update(Recursator E, Recursator W, Recursator N, Visitor vis)
    {
	vampir_trace<5005> tracer;
	using namespace recursion;

	if (E.is_empty() || W.is_empty() || N.is_empty())
	    return;

	if (vis.is_base(E)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_E(base_case_cast<base_test>(E.get_value())),
		         base_W(base_case_cast<base_test>(W.get_value())),
		         base_N(base_case_cast<base_test>(N.get_value()));
	    vis.schur_update_base(base_E, base_W, base_N);
	} else{
	    schur_update(     E.north_east(),W.north_west()     ,N.south_west()     , vis);
	    schur_update(     E.north_east(),     W.north_east(),     N.south_east(), vis);

	    schur_update(E.north_west()     ,     W.north_east(),     N.north_east(), vis);
	    schur_update(E.north_west()     ,W.north_west()     ,N.north_west()     , vis);

	    schur_update(E.south_west()     ,W.south_west()     ,N.north_west()     , vis);
	    schur_update(E.south_west()     ,     W.south_east(),     N.north_east(), vis);

	    schur_update(     E.south_east(),     W.south_east(),     N.south_east(), vis);
	    schur_update(     E.south_east(),W.south_west()     ,N.south_west()     , vis);
	}
    }

    template <typename Recursator, typename Visitor>
    void tri_solve(Recursator S, Recursator N, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5006> tracer;

        if (S.is_empty())
	    return;

        if (vis.is_base(S)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_S(base_case_cast<base_test>(S.get_value())),
		         base_N(base_case_cast<base_test>(N.get_value()));

	    vis.tri_solve_base(base_S, base_N);
        } else{

	    tri_solve(S.north_west()     ,N.north_west(), vis);
	    schur_update(  S.north_east(),S.north_west()     ,N.south_west(), vis);
	    tri_solve(     S.north_east(),     N.south_east(), vis);
	    tri_solve(S.south_west()     ,N.north_west()     , vis);
	    schur_update(  S.south_east(),S.south_west()     ,N.south_west(), vis);
	    tri_solve(     S.south_east(),     N.south_east(), vis);
	}
    }

    template <typename Recursator, typename Visitor>
    void tri_schur(Recursator E, Recursator W, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5007> tracer;

        if (E.is_empty() || W.is_empty())
	    return;

        if (vis.is_base(W)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_E(base_case_cast<base_test>(E.get_value())),
               		 base_W(base_case_cast<base_test>(W.get_value()));
	    vis.tri_schur_base(base_E, base_W);
        } else{

	    schur_update(E.south_west(),     W.south_west(),    W.north_west(), vis);
	    schur_update(E.south_west(),     W.south_east(),    W.north_east(), vis);
	    tri_schur(   E.south_east()     ,     W.south_east(), vis);
	    tri_schur(   E.south_east()     ,W.south_west()     , vis);
	    tri_schur(        E.north_west(),     W.north_east(), vis);
	    tri_schur(        E.north_west(),W.north_west()     , vis);
        }
    }

    template <typename Recursator, typename Visitor>
    void cholesky(Recursator recursator, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5008> tracer;

        if (recursator.is_empty())
	    return;

        if (vis.is_base (recursator)){
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_matrix(base_case_cast<base_test>(recursator.get_value()));
	    vis.cholesky_base (base_matrix);
        } else {
	    cholesky(recursator.north_west(), vis);
	    tri_solve(    recursator.south_west(),       recursator.north_west(), vis);
	    tri_schur(    recursator.south_east(), recursator.south_west(), vis);
	    cholesky(     recursator.south_east(), vis);
        }
    }

} // namespace with_recursator

template <typename Backup= with_bracket::cholesky_base_t>
struct recursive_cholesky_t
{
    template <typename Matrix>
    void operator()(Matrix& matrix)
    {
	(*this)(matrix, recursive_cholesky_default_visitor_t());
    }

    template <typename Matrix, typename Visitor>
    void operator()(Matrix& matrix, Visitor vis)
    {
	apply(matrix, vis, typename mtl::traits::category<Matrix>::type());
    }

private:
    // If the matrix is not sub-divisible then take backup function
    template <typename Matrix, typename Visitor>
    void apply(Matrix& matrix, Visitor, tag::universe)
    {
	Backup()(matrix);
    }

    // Only if matrix is sub-divisible, otherwise backup
    template <typename Matrix, typename Visitor>
    void apply(Matrix& matrix, Visitor vis, tag::qsub_divisible)
    {
	matrix::recursator<Matrix>  recursator(matrix);
	with_recursator::cholesky(recursator, vis);
    }
};

template <typename Matrix, typename Visitor>
inline void recursive_cholesky(Matrix& matrix, Visitor vis)
{
    recursive_cholesky_t<>()(matrix, vis);
}

template <typename Matrix>
inline void recursive_cholesky(Matrix& matrix)
{
    recursive_cholesky(matrix, recursive_cholesky_default_visitor_t());
}

template <typename Matrix>
void fill_matrix_for_cholesky(Matrix& matrix)
{
    vampir_trace<5008> tracer;
    typedef typename Collection<Matrix>::size_type   size_type;
    typedef typename Collection<Matrix>::value_type  value_type;

    value_type   x= 1.0;
    for (size_type i= 0; i < num_rows(matrix); i++)
       for (size_type j= 0; j <= i; j++)
	   if (i != j) {
	       matrix[i][j]= x; matrix[j][i]= x;
	       x+= 1.0;
	   }

    for (size_type i= 0; i < num_rows(matrix); i++) {
	value_type rowsum= 0.0;
	for (size_type j=0; j<matrix.num_cols(); j++)
	    if (i != j)
		rowsum += matrix[i][j];
	matrix[i][i]= rowsum * 2;
    }
}

}} // namespace mtl::matrix

#endif // MTL_CHOLESKY_INCLUDE


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 6/17/2020, 5:57:05 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

int main()
{
  int N;
  cin >> N;
  map<string, int> M;
  for (auto i{0}; i < N; ++i)
  {
    string S;
    cin >> S;
    M[S];
    M[S]++;
  }
  int maxi{0};
  for (auto const &e : M)
  {
    ch_max(maxi, e.second);
  }
  for (auto const &e : M)
  {
    if (e.second == maxi)
    {
      cout << e.first << endl;
    }
  }
}


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class ElasticProblem: public Problem
	{
	public:
		ElasticProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
	};

	class TorsionElasticProblem: public Problem
	{
	public:
		TorsionElasticProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_linear_in_time() const override { return false; }

		void set_parameters(const json &params) override;
	private:
		double n_turns_ = 0.5;
		int coordiante_0_ = 0;
		int coordiante_1_ = 1;
		RowVectorNd trans_;
	};

	class ElasticProblemZeroBC: public Problem
	{
	public:
		ElasticProblemZeroBC(const std::string &name);
		bool is_rhs_zero() const override { return false; }

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
	};

	class ElasticProblemExact: public ProblemWithSolution
	{
	public:
		ElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return false; }
	};

	class CompressionElasticProblemExact: public ProblemWithSolution
	{
	public:
		CompressionElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return false; }
	};

	class QuadraticElasticProblemExact: public ProblemWithSolution
	{
	public:
		QuadraticElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return false; }
	};

	class LinearElasticProblemExact: public ProblemWithSolution
	{
	public:
		LinearElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return false; }
	};

	class GravityProblem: public Problem
	{
	public:
		GravityProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void initial_solution(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_velocity(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_acceleration(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return true; }
	};
}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core
*/

#ifndef openOR_core_math_matrixfunctions_hpp
#define openOR_core_math_matrixfunctions_hpp

#include <boost/numeric/ublas/lu.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Utility/conceptcheck.hpp>
#include <openOR/Math/constants.hpp>
#include <openOR/Math/utilities.hpp>
#include <openOR/Math/matrix.hpp>
#include <openOR/Math/matrixsetaxis.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/vectorfunctions.hpp>
#include <openOR/Math/create.hpp>

#include <openOR/Math/determinant.hpp>
#include <openOR/Math/detail/inverse_impl.hpp>

namespace openOR {
	namespace Math {

		/**
		* @brief Returns the x-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			xAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 0>(mat), get<1, 0>(mat), get<2, 0>(mat));
		}

		/**
		* @brief Returns the y-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			yAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 1>(mat), get<1, 1>(mat), get<2, 1>(mat));
		}

		/**
		* @brief Returns the z-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			zAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 2>(mat), get<1, 2>(mat), get<2, 2>(mat));
		}

		/**
		* @brief Returns the translation vector of a 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			translation(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 3>(mat), get<1, 3>(mat), get<2, 3>(mat));
		}

		namespace Impl {
			template <class Type, class Vec, int I, int Count>
			struct SetScaleHelp {
				void operator()(Type& mat, const Vec& vec) const {
					get<I, I>(mat) = get<I>(vec);
					SetScaleHelp < Type, Vec, I + 1, Count > ()(mat, vec);
				}
			};

			template <class Type, class Vec, int Count>
			struct SetScaleHelp<Type, Vec, Count, Count> {
				void operator()(Type& mat, const Vec& vec) const {}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstVector<Vec>)),
			(void))
			setScale(Type& mat, const Vec& vec) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				Impl::SetScaleHelp<Type, Vec, 0, MatrixTraits<Type>::RowDimension::value>()(mat, vec);
		}

		namespace Impl {
			template <class Type, class Vec, int J, int Count, int Row>
			struct GetRowHelp {
				void operator()(Vec& vec, const Type& mat) const {
					get<J>(vec) = get<Row, J>(mat);
					GetRowHelp < Type, Vec, J + 1, Count, Row > ()(vec, mat);
				}
			};

			template <class Type, class Vec, int Count, int Row>
			struct GetRowHelp<Type, Vec, Count, Count, Row> {
				void operator()(Vec& vec, const Type& mat) const {}
			};

			template <class Type, class Vec, int I, int Count, int Col>
			struct GetColHelp {
				void operator()(Vec& vec, const Type& mat) const {
					get<I>(vec) = get<I, Col>(mat);
					GetColHelp < Type, Vec, I + 1, Count, Col > ()(vec, mat);
				}
			};

			template <class Type, class Vec, int Count, int Col>
			struct GetColHelp<Type, Vec, Count, Count, Col> {
				void operator()(Vec& vec, const Type& mat) const {}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <int I, class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::Vector<typename MatrixTraits<Type>::RowVectorType>)),
			(typename MatrixTraits<Type>::RowVectorType))
			row(const Type& mat) {
				typedef typename MatrixTraits<Type>::RowVectorType VectorType;
				BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<I>, typename MatrixTraits<Type>::RowDimension>));
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename VectorTraits<VectorType>::Dimension, typename MatrixTraits<Type>::RowDimension>));

				VectorType vec;
				Impl::GetRowHelp<Type, VectorType, 0, MatrixTraits<Type>::ColDimension::value, I>()(vec, mat);
				return vec;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <int J, class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::Vector<typename MatrixTraits<Type>::ColVectorType>)),
			(typename MatrixTraits<Type>::ColVectorType))
			col(const Type& mat) {
				typedef typename MatrixTraits<Type>::ColVectorType VectorType;
				BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<J>, typename MatrixTraits<Type>::ColDimension>));
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename VectorTraits<VectorType>::Dimension, typename MatrixTraits<Type>::ColDimension>));

				VectorType vec;
				Impl::GetColHelp<Type, VectorType, 0, MatrixTraits<Type>::RowDimension::value, J>()(vec, mat);
				return vec;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationX(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = 1;
				get<0, 1>(mat) = 0;
				get<0, 2>(mat) = 0;
				get<1, 0>(mat) = 0;
				get<1, 1>(mat) = cos(angle);
				get<1, 2>(mat) = -sin(angle);
				get<2, 0>(mat) = 0;
				get<2, 1>(mat) = sin(angle);
				get<2, 2>(mat) = cos(angle);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationY(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = cos(angle);
				get<0, 1>(mat) = 0;
				get<0, 2>(mat) = sin(angle);
				get<1, 0>(mat) = 0;
				get<1, 1>(mat) = 1;
				get<1, 2>(mat) = 0;
				get<2, 0>(mat) = -sin(angle);
				get<2, 1>(mat) = 0;
				get<2, 2>(mat) = cos(angle);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationZ(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = cos(angle);
				get<0, 1>(mat) = -sin(angle);
				get<0, 2>(mat) = 0;
				get<1, 0>(mat) = sin(angle);
				get<1, 1>(mat) = cos(angle);
				get<1, 2>(mat) = 0;
				get<2, 0>(mat) = 0;
				get<2, 1>(mat) = 0;
				get<2, 2>(mat) = 1;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateX(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationX(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateY(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationY(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateZ(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationZ(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Matrix33Type))
			rotationAsMatrix(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::ColDimension, boost::mpl::int_<3> >));

				typename MatrixTraits<Type>::Matrix33Type matResult;
				get<0, 0>(matResult) = get<0, 0>(mat);
				get<0, 1>(matResult) = get<0, 1>(mat);
				get<0, 2>(matResult) = get<0, 2>(mat);
				get<1, 0>(matResult) = get<1, 0>(mat);
				get<1, 1>(matResult) = get<1, 1>(mat);
				get<1, 2>(matResult) = get<1, 2>(mat);
				get<2, 0>(matResult) = get<2, 0>(mat);
				get<2, 1>(matResult) = get<2, 1>(mat);
				get<2, 2>(matResult) = get<2, 2>(mat);
				return matResult;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type, class Type2>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstMatrix<Type2>)),
			(void))
			setRotation(Type& mat, const Type2& matRotation) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::ColDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = get<0, 0>(matRotation);
				get<0, 1>(mat) = get<0, 1>(matRotation);
				get<0, 2>(mat) = get<0, 2>(matRotation);
				get<1, 0>(mat) = get<1, 0>(matRotation);
				get<1, 1>(mat) = get<1, 1>(matRotation);
				get<1, 2>(mat) = get<1, 2>(matRotation);
				get<2, 0>(mat) = get<2, 0>(matRotation);
				get<2, 1>(mat) = get<2, 1>(matRotation);
				get<2, 2>(mat) = get<2, 2>(matRotation);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Mat, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Mat>))
			((Concept::ConstVector<Vec>)),
			(void))
			setRotation(Mat& mat, const Vec& vecAxis, typename ScalarTraits<typename MatrixTraits<Mat>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Mat>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Mat>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename VectorTraits<Vec>::Dimension, boost::mpl::int_<3> >));

				//assert(std::abs(norm(vecAxis) - 1.0) <= 0.001);

				setXAxis(mat, rotate3(create<Vec>(1, 0, 0), vecAxis, angle));
				setYAxis(mat, rotate3(create<Vec>(0, 1, 0), vecAxis, angle));
				setZAxis(mat, rotate3(create<Vec>(0, 0, 1), vecAxis, angle));
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(boost::math::quaternion<typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType>))
			rotationAsQuaternions(const Type& mat) {

				typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
				typedef boost::math::quaternion<RealType> Quaternions;
				Quaternions q;

				RealType tr = get<0, 0>(mat) + get<1, 1>(mat) + get<2, 2>(mat);
				RealType s;
				RealType w, x, y, z;

				if (tr > 0.0)
				{
					s = sqrt(tr + static_cast<RealType>(1));
					w = s * static_cast<RealType>(0.5);
					s = static_cast<RealType>(0.5) / s;
					x = static_cast<RealType>((get<2, 1>(mat) - get<1, 2>(mat)) * s);
					y = static_cast<RealType>((get<0, 2>(mat) - get<2, 0>(mat)) * s);
					z = static_cast<RealType>((get<1, 0>(mat) - get<0, 1>(mat)) * s);
				}
				else
				{
					if (get<0, 0>(mat) > get<1, 1>(mat) && get<0, 0>(mat) > get<2, 2>(mat))
					{
						RealType s = 2.0 * sqrt(1.0 + get<0, 0>(mat) - get<1, 1>(mat) - get<2, 2>(mat));
						w = (get<2, 1>(mat) - get<1, 2>(mat)) / s;
						x = 0.25 * s;
						y = (get<1, 0>(mat) + get<0, 1>(mat)) / s;
						z = (get<0, 2>(mat) + get<2, 0>(mat)) / s;

					} else if (get<1, 1>(mat) > get<2, 2>(mat)) {
						RealType s = 2.0 * sqrt(1.0 + get<1, 1>(mat) - get<0, 0>(mat) - get<2, 2>(mat));
						w = (get<0, 2>(mat) - get<2, 0>(mat)) / s;
						x = (get<1, 0>(mat) + get<0, 1>(mat)) / s;
						y = 0.25 * s;
						z = (get<2, 1>(mat) + get<1, 2>(mat)) / s;
					} else {
						RealType s = 2.0 * sqrt(1.0 + get<2, 2>(mat) - get<0, 0>(mat) - get<1, 1>(mat));
						w = (get<1, 0>(mat) - get<0, 1>(mat)) / s;
						x = (get<0, 2>(mat) + get<2, 0>(mat)) / s;
						y = (get<2, 1>(mat) + get<1, 2>(mat)) / s;
						z = 0.25 * s;
					}
				}
				return Quaternions(w, x, y, z);
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsEulerAngles(const Type& mat, double& heading, double& attitude, double& bank)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
			// Assuming the angles are in radians.
			if (get<1, 0>(mat) > static_cast<RealType>(0.998)) { // singularity at north pole
				heading = atan2(get<0, 2>(mat), get<2, 2>(mat));
				attitude = PI / 2;
				bank = 0;
				return;
			}
			if (get<1, 0>(mat) < static_cast<RealType>(-0.998)) { // singularity at south pole
				heading = atan2(get<0, 2>(mat), get<2, 2>(mat));
				attitude = -PI / 2;
				bank = 0;
				return;
			}
			heading = atan2(-get<2, 0>(mat), get<0, 0>(mat));
			bank = atan2(-get<1, 2>(mat), get<1, 1>(mat));
			attitude = asin(get<1, 0>(mat));
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotation(Type& mat, const boost::math::quaternion<typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType>& quaternion) {

				typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
				typedef boost::math::quaternion<RealType> Quaternions;

				RealType s = static_cast<RealType>(2) / static_cast<RealType>(square(quaternion.R_component_1()) +
					square(quaternion.R_component_2()) +
					square(quaternion.R_component_3()) +
					square(quaternion.R_component_4()));

				RealType xs = quaternion.R_component_2() * s;
				RealType ys = quaternion.R_component_3() * s;
				RealType zs = quaternion.R_component_4() * s;

				RealType wx = quaternion.R_component_1() * xs;
				RealType wy = quaternion.R_component_1() * ys;
				RealType wz = quaternion.R_component_1() * zs;

				RealType xx = quaternion.R_component_2() * xs;
				RealType xy = quaternion.R_component_2() * ys;
				RealType xz = quaternion.R_component_2() * zs;

				RealType yy = quaternion.R_component_3() * ys;
				RealType yz = quaternion.R_component_3() * zs;

				RealType zz = quaternion.R_component_4() * zs;

				get<0, 0>(mat) = static_cast<RealType>(1) - (yy + zz);
				get<1, 0>(mat) = xy + wz;
				get<2, 0>(mat) = xz - wy;

				get<0, 1>(mat) = xy - wz;
				get<1, 1>(mat) = static_cast<RealType>(1) - (xx + zz);
				get<2, 1>(mat) = yz + wx;

				get<0, 2>(mat) = xz + wy;
				get<1, 2>(mat) = yz - wx;
				get<2, 2>(mat) = static_cast<RealType>(1) - (xx + yy);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationAxis(Type& mat, const Math::Vector3d& axis, const double& angle) {
				typename MatrixTraits<Type>::ValueType c = cos(angle), s = sin(angle);
				typename MatrixTraits<Type>::ValueType cn = 1.0 - cos(angle);
				double nx = axis(0), ny = axis(1), nz = axis(2);
				double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz;
				double nxs = nx * s, nys = ny * s, nzs = nz * s;

				get<0, 0>(mat) = nx * nx * cn + c;
				get<0, 1>(mat) = nxny * cn - nzs;
				get<0, 2>(mat) = nxnz * cn + nys;

				get<1, 0>(mat) = nxny * cn + nzs;
				get<1, 1>(mat) = ny * ny * cn + c;
				get<1, 2>(mat) = nynz * cn - nxs;

				get<2, 0>(mat) = nxnz * cn - nys;
				get<2, 1>(mat) = nynz * cn + nxs;
				get<2, 2>(mat) = nz * nz * cn + c;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationStationaryZYX(Type& mat,
			const typename MatrixTraits<Type>::ValueType& radZ,
			const typename MatrixTraits<Type>::ValueType& radY,
			const typename MatrixTraits<Type>::ValueType& radX)
		{
			// angles must be in radians
			double cx = cos(radX);
			double sx = sin(radX);
			double cy = cos(radY);
			double sy = sin(radY);
			double cz = cos(radZ);
			double sz = sin(radZ);

			// describes the rotation with euler angles around stationary axes
			// is equal to R = Rx * Ry * Rz

			get<0, 0>(mat) = cy * cz;
			get<0, 1>(mat) = -cy * sz;
			get<0, 2>(mat) = -sy;
			get<1, 0>(mat) = -sx * sy * cz + cx * sz;
			get<1, 1>(mat) = sx * sy * sz + cx * cz;
			get<1, 2>(mat) = -sx * cy;
			get<2, 0>(mat) = cx * sy * cz + sx * sz;
			get<2, 1>(mat) = -cx * sy * sz + sx * cz;
			get<2, 2>(mat) = cx * cy;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsStationaryZYX(const Type& mat, double& radZ, double& radY, double& radX)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;

			double sx, sy, sz, cx, cy, cz, rx, ry, rz;

			ry = -asin(get<0, 2>(mat));
			cy = cos(ry);

			if (abs(cy) > 0.00005) {
				sx = -get<1, 2>(mat) / cy;
				cx = get<2, 2>(mat) / cy;

				sz = -get<0, 1>(mat) / cy;
				cz = get<0, 0>(mat) / cy;

				rx = atan2(sx, cx);
				rz = atan2(sz, cz);
			} else {
				rx = 0;

				sz = get<1, 0>(mat);
				cz = get<2, 0>(mat);
				rz = atan2(sz, cz);
			}

			radZ = rz; radY = ry; radX = rx;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotation(Type& mat,
			const typename MatrixTraits<Type>::ValueType& heading,
			const typename MatrixTraits<Type>::ValueType& attitude,
			const typename MatrixTraits<Type>::ValueType& bank)
		{
			// angles must be in radians
			double ch = cos(heading);
			double sh = sin(heading);
			double ca = cos(attitude);
			double sa = sin(attitude);
			double cb = cos(bank);
			double sb = sin(bank);

			get<0, 0>(mat) = ch * ca;
			get<0, 1>(mat) = sh * sb - ch * sa * cb;
			get<0, 2>(mat) = ch * sa * sb + sh * cb;
			get<1, 0>(mat) = sa;
			get<1, 1>(mat) = ca * cb;
			get<1, 2>(mat) = -ca * sb;
			get<2, 0>(mat) = -sh * ca;
			get<2, 1>(mat) = sh * sa * cb + ch * sb;
			get<2, 2>(mat) = -sh * sa * sb + ch * cb;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationYprDeg(Type& mat,
			const typename MatrixTraits<Type>::ValueType& degYaw,
			const typename MatrixTraits<Type>::ValueType& degPitch,
			const typename MatrixTraits<Type>::ValueType& degRoll)
		{
			typename MatrixTraits<Type>::ValueType cy, sy;
			typename MatrixTraits<Type>::ValueType cp = cos(degPitch * M_PI / 180.0), sp = sin(degPitch * M_PI / 180.0);
			typename MatrixTraits<Type>::ValueType cr = cos(degRoll * M_PI / 180.0), sr = sin(degRoll * M_PI / 180.0);

			cy = cos(degYaw * M_PI / 180.0); sy = sin(degYaw * M_PI / 180.0);

			// correct cos
			if (degYaw == 90.0 || degYaw == 270.0) cy = 0.0;
			if (degPitch == 90.0 || degPitch == 270.0) cp = 0.0;
			if (degRoll == 90.0 || degRoll == 270.0) cr = 0.0;

			// correct sin
			if (degYaw == 0.0 || degYaw == 180.0) sy = 0.0;
			if (degPitch == 0.0 || degPitch == 180.0) sp = 0.0;
			if (degRoll == 0.0 || degRoll == 180.0) sr = 0.0;

			get<0, 0>(mat) = cr * cp;
			get<0, 1>(mat) = cr * sp * sy - sr * cy;
			get<0, 2>(mat) = cr * sp * cy + sr * sy;
			get<1, 0>(mat) = sr * cp;
			get<1, 1>(mat) = sr * sp * sy + cr * cy;
			get<1, 2>(mat) = sr * sp * cy - cr * sy;
			get<2, 0>(mat) = -sp;
			get<2, 1>(mat) = cp * sy;
			get<2, 2>(mat) = cp * cy;

			//get<0, 0>(mat) = cp * cy;
			//get<0, 1>(mat) = cp * sy;
			//get<0, 2>(mat) = -sp;
			//get<1, 0>(mat) = sr * sp * cy - sr * cy;
			//get<1, 1>(mat) = sr * sp * sy + cr * cy;
			//get<1, 2>(mat) = sr * cp;
			//get<2, 0>(mat) = cr * sp * cy + sr * sy;
			//get<2, 1>(mat) = cr * sp * sy - sr * cy;
			//get<2, 2>(mat) = cr * cp;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsYpr(const Type& mat, double& radYaw, double& radPitch, double& radRoll)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;

			double sy, sp, sr, cy, cp, cr, ry, rp, rr;

			rp = -asin(get<2, 0>(mat));
			cp = cos(rp);
			if (abs(cp) > 0.0005) {
				sy = get<2, 1>(mat) / cp;
				cy = get<2, 2>(mat) / cp;

				sr = get<1, 0>(mat) / cp;
				cr = get<0, 0>(mat) / cp;

				ry = atan2(sy, cy);
				rr = atan2(sr, cr);
			} else {
				cy = get<0, 1>(mat);
				sy = get<1, 1>(mat);

				rr = M_PI * -0.5;
				ry = atan2(sy, cy);
			}

			radYaw = ry; radPitch = rp; radRoll = rr;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			orthoInverse(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				//assert(isHomogeneous(mat));
				Type matResult;
				get<0, 0>(matResult) = get<0, 0>(mat);
				get<0, 1>(matResult) = get<1, 0>(mat);
				get<0, 2>(matResult) = get<2, 0>(mat);
				get<1, 0>(matResult) = get<0, 1>(mat);
				get<1, 1>(matResult) = get<1, 1>(mat);
				get<1, 2>(matResult) = get<2, 1>(mat);
				get<2, 0>(matResult) = get<0, 2>(mat);
				get<2, 1>(matResult) = get<1, 2>(mat);
				get<2, 2>(matResult) = get<2, 2>(mat);
				get<3, 0>(matResult) = 0;
				get<3, 1>(matResult) = 0;
				get<3, 2>(matResult) = 0;
				get<3, 3>(matResult) = 1;
				get<0, 3>(matResult) = -(get<0, 0>(mat) * get<0, 3>(mat) + get<1, 0>(mat) * get<1, 3>(mat) + get<2, 0>(mat) * get<2, 3>(mat));
				get<1, 3>(matResult) = -(get<0, 1>(mat) * get<0, 3>(mat) + get<1, 1>(mat) * get<1, 3>(mat) + get<2, 1>(mat) * get<2, 3>(mat));
				get<2, 3>(matResult) = -(get<0, 2>(mat) * get<0, 3>(mat) + get<1, 2>(mat) * get<1, 3>(mat) + get<2, 2>(mat) * get<2, 3>(mat));
				return matResult;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type&))
			orthoInvert(Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				mat = orthoInverse(mat);
				return mat;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			inverse(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				return Impl::Inverse<Type, MatrixTraits<Type>::RowDimension::value>::get(mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			invert(Type& mat) {
				mat = inverse(mat);
		}

		namespace Impl {
			template <class Mat, class MatSrc, int I, int J>
			struct TransposeOp {
				inline void operator()(Mat& mat, const MatSrc& matSrc) const {
					Math::get<I, J>(mat) = Math::get<J, I>(matSrc);
				}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			transposed(const Type& mat) {
				typedef typename MatrixTraits<Type>::RowDimension RowDimension;
				typedef typename MatrixTraits<Type>::ColDimension ColDimension;
				BOOST_MPL_ASSERT((boost::mpl::equal_to<RowDimension, ColDimension>));
				Type matTransposed;
				Impl::MatrixCompileTimeIterator<Type, 0, RowDimension::value, 0, ColDimension::value>().template apply<Impl::TransposeOp, Type>(matTransposed, mat);
				return matTransposed;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type&))
			transpose(Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				mat = transposed(mat);
				return mat;
		}

		/**
		* @brief
		* @ingroup openOR_core_math
		*/
		template<class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::ConstVector<Vec>))
			((Concept::Vector<typename MatrixTraits<Type>::Vector3Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			prod44x3(const Type& mat, const Vec& vec) {
				typename MatrixTraits<Type>::Vector3Type result;
				get<0>(result) = get<0, 0>(mat) * get<0>(vec) + get<0, 1>(mat) * get<1>(vec) + get<0, 2>(mat) * get<2>(vec) + get<0, 3>(mat);
				get<1>(result) = get<1, 0>(mat) * get<0>(vec) + get<1, 1>(mat) * get<1>(vec) + get<1, 2>(mat) * get<2>(vec) + get<1, 3>(mat);
				get<2>(result) = get<2, 0>(mat) * get<0>(vec) + get<2, 1>(mat) * get<1>(vec) + get<2, 2>(mat) * get<2>(vec) + get<2, 3>(mat);
				return result;
		}

		/**
		* @brief
		* @ingroup openOR_core_math
		*/
		template<class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::ConstVector<Vec>))
			((Concept::Vector<typename MatrixTraits<Type>::Vector2Type>)),
			(typename MatrixTraits<Type>::Vector2Type))
			prod33x2(const Type& mat, const Vec& vec) {
				typename MatrixTraits<Type>::Vector2Type result;
				get<0>(result) = get<0, 0>(mat) * get<0>(vec) + get<0, 1>(mat) * get<1>(vec) + get<0, 2>(mat);
				get<1>(result) = get<1, 0>(mat) * get<0>(vec) + get<1, 1>(mat) * get<1>(vec) + get<1, 2>(mat);
				return result;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			makeHomogeneous(Type& mat, int unmodifiedAxis = 2) // 0: xAxis; 1: yAxis; 2: zAxis
		{
			get<3, 0>(mat) = 0;
			get<3, 1>(mat) = 0;
			get<3, 2>(mat) = 0;
			get<3, 3>(mat) = 1;
			typename MatrixTraits<Type>::Vector3Type vecXAxis = xAxis(mat);
			typename MatrixTraits<Type>::Vector3Type vecYAxis = yAxis(mat);
			typename MatrixTraits<Type>::Vector3Type vecZAxis = zAxis(mat);

			switch (unmodifiedAxis)
			{
			case 0:
				normalize(vecXAxis);
				vecZAxis = cross(vecXAxis, vecYAxis);
				normalize(vecZAxis);
				vecYAxis = cross(vecZAxis, vecXAxis);
				break;
			case 1:
				normalize(vecYAxis);
				vecZAxis = cross(vecXAxis, vecYAxis);
				normalize(vecZAxis);
				vecXAxis = cross(vecYAxis, vecZAxis);
				break;
			default:
				normalize(vecZAxis);
				vecXAxis = cross(vecYAxis, vecZAxis);
				normalize(vecXAxis);
				vecYAxis = cross(vecZAxis, vecXAxis);
				break;
			}
			setXAxis(mat, vecXAxis);
			setYAxis(mat, vecYAxis);
			setZAxis(mat, vecZAxis);
		}

	}
}

#endif


// BLAS level 2

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;

using std::cout;
using std::endl;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double, ublas::row_major> rm_t;
typedef ublas::matrix<double, ublas::column_major> cm_t;

int main() {

  cout << endl;

  vct_t vx (2);
  vct_t vy (4);

  // row major matrix
  rm_t rm (2, 4);
  init_m (rm, const_val<double> (0));
  print_m (rm, "row major matrix m");
  cout << endl;

  vx(0) = 1.;
  vy(1) = 1.;
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  blas::ger (1.0, vx, vy, rm);
  print_m (rm, "m += x y^T");
  cout << endl << endl;

  init_m (rm, const_val<double> (1));
  print_m (rm, "m");
  cout << endl;

  blas::set (1., vx);
  blas::set (1., vy);
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += 2 x y^T
  blas::ger (2., vx, vy, rm);
  print_m (rm, "m += 2 x y^T");
  cout << endl << endl;

  init_v (vx, iplus1());
  init_v (vy, iplus1());
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  blas::ger ( 1.0, vx, vy, rm);
  print_m (rm, "m += x y^T");
  cout << endl << endl;

  // column major matrix
  cm_t cm (2, 4);
  init_m (cm, const_val<double> (0));
  print_m (cm, "column major matrix m");
  cout << endl;

  vx(0) = 1.;
  vy(1) = 1.;
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  blas::ger ( 1.0, vx, vy, cm);
  print_m (cm, "m += x y^T");
  cout << endl << endl;

  init_m (cm, const_val<double> (1));
  print_m (cm, "m");
  cout << endl;

  blas::set (1., vx);
  blas::set (1., vy);
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += 2 x y^T
  blas::ger (2., vx, vy, cm);
  print_m (cm, "m += 2 x y^T");
  cout << endl << endl;

  init_v (vx, iplus1());
  init_v (vy, iplus1());
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  blas::ger (1.0, vx, vy, cm);
  print_m (cm, "m += x y^T");
  cout << endl << endl;

}


/**
 * @file boundarywave_test.cc
 * @brief NPDE homework "BoundaryWave" code
 * @author Philipp Lindenberger
 * @date 03.04.2020
 * @copyright Developed at ETH Zurich
 */

#include <memory>
#include <utility>
// Eigen includes
#include <gtest/gtest.h>

#include <Eigen/Core>
// Lehrfem++ includes
#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

#include "../boundarywave.h"

namespace BoundaryWave::test {

auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
const lf::io::GmshReader reader(std::move(mesh_factory),
                                CURRENT_SOURCE_DIR "/../../meshes/simple.msh");
auto mesh_p = reader.mesh();

TEST(BoundaryWave, buildM) {
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  lf::assemble::COOMatrix M_coo = buildM(fe_space_p);

  Eigen::MatrixXd M = M_coo.makeDense();

  // std::cout << M;
  double eps = 1.0e-5;
  Eigen::Matrix<double, 5, 5> reference_M;

  reference_M << 0.666667, 0.166667, 0, 0.166667, 0, 0.166667, 0.666667,
      0.166667, 0, 0, 0, 0.166667, 0.666667, 0.166667, 0, 0.166667, 0, 0.166667,
      0.666667, 0, 0, 0, 0, 0, 0;
  ASSERT_EQ(reference_M.size(), M.size());
  ASSERT_NEAR((reference_M - M).lpNorm<Eigen::Infinity>(), 0.0, eps);
}

TEST(BoundaryWave, buildA) {
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  lf::assemble::COOMatrix A_coo = buildA(fe_space_p);
  Eigen::MatrixXd A = A_coo.makeDense();

  double eps = 1.0e-5;
  Eigen::Matrix<double, 5, 5> reference_A;

  reference_A << 1.33333, 0, 0, 0, -1.33333, 0, 1.66667, 0, 0, -1.66667, 0, 0,
      2, 0, -2, 0, 0, 0, 1.66667, -1.66667, -1.33333, -1.66667, -2, -1.66667,
      6.66667;
  ASSERT_EQ(reference_A.size(), A.size());
  ASSERT_NEAR((reference_A - A).lpNorm<Eigen::Infinity>(), 0.0, eps);
}

TEST(BoundaryWave, InterpolateInitialData) {
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0 = [](const Eigen::Vector2d &x) -> double { return x[0]; };
  auto v0 = [](const Eigen::Vector2d &x) -> double { return x[1]; };

  std::pair<Eigen::VectorXd, Eigen::VectorXd> initialData =
      interpolateInitialData(fe_space_p, std::move(u0), std::move(v0));

  Eigen::VectorXd reference_u0(5);
  reference_u0 << 0, 1, 1, 0, 0.5;

  Eigen::VectorXd reference_v0(5);
  reference_v0 << 0, 0, 1, 1, 0.5;

  double eps = 1.0e-5;
  ASSERT_TRUE(initialData.first.size() == reference_u0.size());
  ASSERT_TRUE(initialData.second.size() == reference_v0.size());
  for (int i = 0; i < 5; i++) {
    ASSERT_NEAR(reference_u0(i), initialData.first(i), eps);
    ASSERT_NEAR(reference_v0(i), initialData.second(i), eps);
  }
}

TEST(BoundaryWave, solveBoundaryWave) {
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0 = [](const Eigen::Vector2d &x) -> double { return x[0]; };
  auto v0 = [](const Eigen::Vector2d &x) -> double { return x[1]; };

  Eigen::VectorXd discrete_solution =
      solveBoundaryWave(fe_space_p, u0, v0, 1.0, 100);

  Eigen::VectorXd reference_solution(5);

  reference_solution << 0.584002, 0.767904, 1.23746, 1.41064, 0.982673;

  double eps = 1.0e-5;
  ASSERT_EQ(reference_solution.rows(), discrete_solution.rows());
  ASSERT_NEAR(
      (reference_solution - discrete_solution).lpNorm<Eigen::Infinity>(), 0.0,
      eps);
}
}  // namespace BoundaryWave::test


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <Eigen/Dense>
#include <localization_error_monitor/node.hpp>

LocalizationErrorMonitor::LocalizationErrorMonitor()
{
  pnh_.param<double>("scale", scale_, 3);
  pnh_.param<double>("error_ellipse_size", error_ellipse_size_, 1.0);
  pnh_.param<double>("warn_ellipse_size", warn_ellipse_size_, 0.8);

  pose_with_cov_sub_ =
    pnh_.subscribe("input/pose_with_cov", 1, &LocalizationErrorMonitor::onPoseWithCovariance, this);
  ellipse_marker_pub_ = pnh_.advertise<visualization_msgs::Marker>("debug/ellipse_marker", 1, true);

  updater_.setHardwareID("localization_error_monitor");
  updater_.add(
    "localization_accuracy",
    boost::bind(&LocalizationErrorMonitor::checkLocalizationAccuracy, this, _1));

  timer_ = pnh_.createTimer(ros::Duration(0.1), &LocalizationErrorMonitor::onTimer, this);
}

void LocalizationErrorMonitor::onTimer(const ros::TimerEvent & event) { updater_.force_update(); }

void LocalizationErrorMonitor::checkLocalizationAccuracy(
  diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  stat.add("localization_accuracy", ellipse_.long_radius);
  int8_t diag_level = diagnostic_msgs::DiagnosticStatus::OK;
  std::string diag_message = "ellipse size is within the expected range";
  if (warn_ellipse_size_ <= ellipse_.long_radius) {
    diag_level = diagnostic_msgs::DiagnosticStatus::WARN;
    diag_message = "ellipse size is too large";
  }
  if (error_ellipse_size_ <= ellipse_.long_radius) {
    diag_level = diagnostic_msgs::DiagnosticStatus::ERROR;
    diag_message = "ellipse size is over the expected range";
  }
  stat.summary(diag_level, diag_message);
}

visualization_msgs::Marker LocalizationErrorMonitor::createEllipseMarker(
  const Ellipse & ellipse, const geometry_msgs::PoseWithCovarianceStamped & pose_with_cov)
{
  tf2::Quaternion quat;
  quat.setEuler(0, 0, ellipse.yaw);

  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
  visualization_msgs::Marker marker;
  marker.header = pose_with_cov.header;
  marker.header.stamp = ros::Time();
  marker.ns = "error_ellipse";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose = pose_with_cov.pose.pose;
  marker.pose.orientation = tf2::toMsg(quat);
  marker.scale.x =  ellipse_long_radius * 2;
  marker.scale.y = ellipse_short_radius * 2;
  marker.scale.z = 0.01;
  marker.color.a = 0.1;
  marker.color.r = 0.0;
  marker.color.g = 0.0;
  marker.color.b = 1.0;
  return marker;
}

void LocalizationErrorMonitor::onPoseWithCovariance(
  const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & input_msg)
{
  // create xy covariance (2x2 matrix)
  // input geometry_msgs::PoseWithCovariance containe 6x6 matrix
  Eigen::Matrix2d xy_covariance;
  const auto cov = input_msg->pose.covariance;
  xy_covariance(0, 0) = cov[0 * 6 + 0];
  xy_covariance(0, 1) = cov[0 * 6 + 1];
  xy_covariance(1, 0) = cov[1 * 6 + 0];
  xy_covariance(1, 1) = cov[1 * 6 + 1];

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);

  // eigen values and vectors are sorted in ascending order
  ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1));
  ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0));

  // principal component vector
  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
  ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x());

  const auto ellipse_marker = createEllipseMarker(ellipse_, *input_msg);
  ellipse_marker_pub_.publish(ellipse_marker);
}


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>
#define MAX 100000000

using namespace std;

int N;

int main()
{
    while (cin >> N)
    {
        int A[N + 1], B[N];
        for (int i = 0; i < N + 1; i++)
        {
            cin >> A[i];
        }
        for (int i = 0; i < N; i++)
        {
            cin >> B[i];
        }
        long long int sum = 0;
        for (int i = N; i > 0; i--)
        {
            sum += min(A[i], B[i - 1]);
            B[i - 1] -= min(A[i], B[i - 1]);
            sum += min(A[i - 1], B[i - 1]);
            A[i - 1] -= min(A[i - 1], B[i - 1]);
        }
        cout << sum << endl;
    }

    return 0;
}


#ifndef BLOCK_TRIDIAGONAL_MATRICE_H_
#define BLOCK_TRIDIAGONAL_MATRICE_H_

#include "tensor.hpp"
#include <Eigen/Dense>

class BlockTridiagonalMatrice {
public:
  BlockTridiagonalMatrice(int size_block, int num_block);
  Eigen::MatrixXd inverse();
  void setA(const Eigen::Ref<const Eigen::MatrixXd> &block, int index);
  void setB(const Eigen::Ref<const Eigen::MatrixXd> &block, int index);

private:
  const int sb_, nb_, ndim_;
  Tensor tA_; // diagonal blocks
  Tensor tB_; // upper off-diagonal blocks
};

#endif


#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include <math.h>
#include <time.h>
#include <random>
#include <sys/time.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/kdtree.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <quadrotor_msgs/PositionCommand.h>

using namespace std;

pcl::search::KdTree<pcl::PointXYZ> kdtreeLocalMap;
vector<int>     pointIdxRadiusSearch;
vector<float>   pointRadiusSquaredDistance;

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double>  rand_x;
uniform_real_distribution<double>  rand_y;
uniform_real_distribution<double>  rand_w;
uniform_real_distribution<double>  rand_h;

ros::Publisher _all_map_pub, _local_map_pub;
ros::Subscriber _map_sub, _odom_sub, _cmd_sub;

vector<double> _state;
pcl::PointXYZ LstMapCenter;

int _obsNum;
double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h;
double _resolution, _sense_rate, _sensing_range, _field_of_view_vertical;
double _init_x, _init_y, _end_x, _end_y;

bool _is_map_ok   = false;
bool _is_has_odom = false;

sensor_msgs::PointCloud2 globalMap_pcd, localMap_pcd, localMap_nofloor_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

void RandomMapGeneration()
{
   pcl::PointXYZ pt_random;

   // Generate a random map, with vertical obstacles
   rand_x = uniform_real_distribution<double>(_x_l, _x_h );
   rand_y = uniform_real_distribution<double>(_y_l, _y_h );
   rand_w = uniform_real_distribution<double>(_w_l, _w_h);
   rand_h = uniform_real_distribution<double>(_h_l, _h_h);

   for(int i = 0; i < _obsNum; i ++){
      double x, y;
      x    = rand_x(eng);
      y    = rand_y(eng);

      double w, h;
      w    = rand_w(eng);

      int widNum = ceil(w/_resolution);

      for(int r = -widNum / 2; r < widNum / 2; r ++ )
         for(int s = -widNum / 2; s < widNum / 2; s ++ ){
            h    = rand_h(eng);
            //if(h < 1.0) continue;
            int heiNum = ceil(h/_resolution);
            for(int t = 0; t < heiNum; t ++ ){
               pt_random.x = x + r * _resolution;
               pt_random.y = y + s * _resolution;
               pt_random.z = t * _resolution;

               if(sqrt( pow(pt_random.x - _init_x, 2) + pow(pt_random.y - _init_y, 2) ) < 2.0
               || sqrt( pow(pt_random.x - _end_x,  2) + pow(pt_random.y - _end_y,  2) ) < 2.0 )
                  continue;

               cloudMap.points.push_back( pt_random );
            }
         }
   }

   cloudMap.width = cloudMap.points.size();
   cloudMap.height = 1;
   cloudMap.is_dense = true;

   ROS_WARN("[Map Generator] Finished generate random map ");
   //cout<<cloudMap.size()<<endl;
   kdtreeLocalMap.setInputCloud( cloudMap.makeShared() );

   _is_map_ok = true;
}

void rcvOdometryCallbck(const nav_msgs::Odometry odom)
{
   if (odom.child_frame_id == "X" || odom.child_frame_id == "O") return ;
   _is_has_odom = true;

   _state = {
      odom.pose.pose.position.x,
      odom.pose.pose.position.y,
      odom.pose.pose.position.z,
      odom.twist.twist.linear.x,
      odom.twist.twist.linear.y,
      odom.twist.twist.linear.z,
      0.0, 0.0, 0.0
   };
}

int global_map_vis_cnt = 0;
void pubSensedPoints()
{
   pcl::toROSMsg(cloudMap, globalMap_pcd);
   globalMap_pcd.header.frame_id = "map";

   if(global_map_vis_cnt < 20 ){
      _all_map_pub.publish(globalMap_pcd);
      sleep(0.01);
   }

   global_map_vis_cnt ++;
   if(!_is_map_ok || !_is_has_odom)
      return;

   pcl::PointCloud<pcl::PointXYZ>::Ptr localMap(new pcl::PointCloud<pcl::PointXYZ>());
   pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]);

   pointIdxRadiusSearch.clear();
   pointRadiusSquaredDistance.clear();

   pcl::PointXYZ ptInNoflation;

   if ( kdtreeLocalMap.radiusSearch (searchPoint, _sensing_range, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
      for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
         ptInNoflation = cloudMap.points[pointIdxRadiusSearch[i]];
         if( abs(ptInNoflation.z - searchPoint.z ) >  _sensing_range * sin(_field_of_view_vertical/ 2.0 / 180.0 * M_PI) )
            continue;

         localMap->points.push_back(ptInNoflation);

         if( sqrt(pow(ptInNoflation.x - LstMapCenter.x, 2) + pow(ptInNoflation.y - LstMapCenter.y, 2) + pow(ptInNoflation.z - LstMapCenter.z, 2) ) < _sensing_range )
            continue;
      }

   }

   localMap->width = localMap->points.size();
   localMap->height = 1;
   localMap->is_dense = true;

   pcl::toROSMsg(*localMap, localMap_pcd);
   localMap_pcd.header.frame_id = "map";
   _local_map_pub.publish(localMap_pcd);

   LstMapCenter = pcl::PointXYZ(_state[0], _state[1], _state[2]);
}

int main (int argc, char** argv)
{
   ros::init (argc, argv, "random map generator");
   ros::NodeHandle nodehandle( "~" );

   _local_map_pub =
         nodehandle.advertise<sensor_msgs::PointCloud2>("RandomMap", 1);

   _all_map_pub   =
         nodehandle.advertise<sensor_msgs::PointCloud2>("all_map", 1);

   _odom_sub      =
         nodehandle.subscribe( "odometry", 50, rcvOdometryCallbck );

   nodehandle.param("mapBoundary/lower_x", _x_l,       0.0);
   nodehandle.param("mapBoundary/upper_x", _x_h,     100.0);
   nodehandle.param("mapBoundary/lower_y", _y_l,       0.0);
   nodehandle.param("mapBoundary/upper_y", _y_h,     100.0);
   nodehandle.param("ObstacleShape/lower_rad", _w_l,   0.3);
   nodehandle.param("ObstacleShape/upper_rad", _w_h,   0.8);
   nodehandle.param("ObstacleShape/lower_hei", _h_l,   3.0);
   nodehandle.param("ObstacleShape/upper_hei", _h_h,   7.0);

   nodehandle.param("sensing_radius", _sensing_range,          10.0);
   nodehandle.param("ObstacleNum",    _obsNum,                 30  );
   nodehandle.param("Resolution",     _resolution,             0.2 );
   nodehandle.param("SensingRate",    _sense_rate,             10.0);
   nodehandle.param("fov_vertical",   _field_of_view_vertical, 30.0);

   nodehandle.param("init_x", _init_x,  0.0);
   nodehandle.param("init_y", _init_y,  0.0);
   nodehandle.param("end_x",  _end_x,   0.0);
   nodehandle.param("end_y",  _end_y,   0.0);

   RandomMapGeneration();
   ros::Rate loop_rate(_sense_rate);
   while (ros::ok())
   {
        pubSensedPoints();
        ros::spinOnce();
        loop_rate.sleep();
   }
}


//============================================================================
// Name        : predict-radar-measurement.cpp
// Author      : ddigges
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include "ukf.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

int main() {

	//Create a UKF instance
	UKF ukf;

/*******************************************************************************
* Programming assignment calls
*******************************************************************************/

    VectorXd z_out = VectorXd(3);
    MatrixXd S_out = MatrixXd(3, 3);
    ukf.PredictRadarMeasurement(&z_out, &S_out);

	return 0;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <cmath>
#include <iomanip>
#include <limits>
#include <sstream>

#include <Eigen/Core>

#include "utils/cnpy.hpp"
#include "cavity/Element.hpp"
#include "cavity/GePolCavity.hpp"
#include "bi_operators/IntegratorHelperFunctions.hpp"
#include "utils/MathUtils.hpp"
#include "TestingMolecules.hpp"

using integrator::integrateS;
using integrator::integrateD;

double constant(const Eigen::Vector3d & /* s */, const Eigen::Vector3d & /* p */);
double one_over_r(double r, const Eigen::Vector3d & /* s */, const Eigen::Vector3d & /* p */);

SCENARIO("Numerical quadrature of functions", "[numerical_quadrature]")
{
    GIVEN("A function on the unit sphere")
    {
        double radius = 1.55;
        double area = 0.4;
        Molecule point = dummy<0>(radius);
        GePolCavity cavity(point, area, 0.0, 100.0, "");
        Eigen::VectorXd results = Eigen::VectorXd::Zero(cavity.size());
        /*! \class NumericalQuadrature
         *  \test \b NumericalQuadrature_sphere tests numerical quadrature on a sphere for integrand = 1.0
         */
        WHEN("The function is a constant")
        {
            THEN("the integrals are the finite element areas")
            {
                for (int i = 0; i < cavity.size(); ++i) {
                    results(i) = integrateS<32, 16>(pcm::bind(constant, pcm::_1, pcm::_2), cavity.elements(i));
                    double diff = results(i) - cavity.elementArea(i);
                    if (std::abs(diff) > 1.0e-12) {
                        WARN("Test versus area for single sphere");
                        WARN("Tessera n. " << i+1);
                        WARN("diff = " << results(i) - cavity.elementArea(i));
                    }
                }
                for (int i = 0; i < cavity.size(); ++i) {
                    REQUIRE(results(i) == Approx(cavity.elementArea(i)));
                }
            }
        }

        /*! \class NumericalQuadrature
         *  \test \b NumericalQuadrature_sphere_1r tests numerical quadrature on a sphere for integrand 1.0/r
         */
        WHEN("The function is 1.0/r")
        {
            THEN("the integrals are the finite element areas divided by the sphere radius")
            {
                for (int i = 0; i < cavity.size(); ++i) {
                    results(i) = integrateS<32, 16>(pcm::bind(one_over_r, 1.55, pcm::_1, pcm::_2), cavity.elements(i));
                    double diff = results(i) - (cavity.elementArea(i)/radius);
                    if (std::abs(diff) > 1.0e-11) {
                        WARN("Test versus area divided by radius for single sphere");
                        WARN("Tessera n. " << i+1);
                        WARN("diff = " << results(i) - (cavity.elementArea(i)/radius) );
                    }
                }
                for (int i = 0; i < cavity.size(); ++i) {
                    REQUIRE(results(i) == Approx(cavity.elementArea(i)/radius));
                }
            }
        }
    }

    GIVEN("A function on a molecular surface")
    {
        double area = 0.2;
        double probeRadius = 1.385;
        double minRadius = 0.2;
        Molecule molecule = H2();
        GePolCavity cavity(molecule, area, probeRadius, minRadius, "");
        Eigen::VectorXd results = Eigen::VectorXd::Zero(cavity.size());
        Eigen::VectorXd reference = Eigen::VectorXd::Zero(cavity.size());

        /*! \class NumericalQuadrature
         *  \test \b NumericalQuadrature_molecule tests numerical quadrature function on H2 cavity
         */
        WHEN("The function is a constant")
        {
            THEN("the integrals are the finite element areas")
            {
                for (int i = 0; i < cavity.size(); ++i) {
                    results(i) = integrateS<64, 16>(pcm::bind(constant, pcm::_1, pcm::_2), cavity.elements(i));
                    double diff = results(i) - cavity.elementArea(i);
                    if (std::abs(diff) > 1.0e-11) {
                        WARN("Test versus area for H2 molecule");
                        WARN("Tessera n. " << i+1);
                        WARN("diff = " << results(i) - cavity.elementArea(i));
                    }
                }
                /*
                // In case you need to update the reference files...
                cnpy::custom::npy_save("molecule.npy", results);
                */
                reference = cnpy::custom::npy_load<double>("molecule.npy");

                for (int i = 0; i < cavity.size(); ++i) {
                    REQUIRE(results(i) == Approx(reference(i)));
                }
            }
        }

        /*! \class NumericalQuadrature
         *  \test \b NumericalQuadrature_molecule_1r tests numerical quadrature function on H2 cavity
         */
        WHEN("The function is 1.0/r")
        {
            THEN("the integrals are the finite element areas divided by the sphere radius")
            {
                for (int i = 0; i < cavity.size(); ++i) {
                    results(i) = integrateS<64, 16>(pcm::bind(one_over_r, 1.20, pcm::_1, pcm::_2), cavity.elements(i));
                    double diff = results(i) - (cavity.elementArea(i)/molecule.spheres(0).radius);
                    if (std::abs(diff) > 1.0e-11) {
                        WARN("Test versus area divided by radius for H2 molecule");
                        WARN("Tessera n. " << i+1);
                        WARN("diff = " << results(i) - (cavity.elementArea(i)/molecule.spheres(0).radius) );
                    }
                }
                /*
                // In case you need to update the reference files...
                cnpy::custom::npy_save("molecule_1r.npy", results);
                */
                reference = cnpy::custom::npy_load<double>("molecule_1r.npy");

                for (int i = 0; i < cavity.size(); ++i) {
                    REQUIRE(results(i) == Approx(reference(i)));
                }
            }
        }
    }
}

double constant(const Eigen::Vector3d & /* s */, const Eigen::Vector3d & /* p */) { return 1.0; }
double one_over_r(double r, const Eigen::Vector3d & /* s */, const Eigen::Vector3d & /* p */) { return 1.0 / r; }


#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <math.h>
#include <time.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

#include "origarm_ros/Command_Position.h"
#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/modenumber.h"
#include "origarm_ros/segnumber.h"
#include "myGlobalData.h"

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))

using namespace std;

ifstream inFile;
const int ms = 1000;  //1ms
int ts = 50*ms;

int mode_in;
float x_in, y_in, z_in;
float a_in[6], b_in[6], l_in[6];

int flag = 1;
int period = 100;

vector<vector<float>> s_A;
vector<vector<float>> s_B;
vector<vector<float>> s_L;

vector<vector<float>> state_A;
vector<vector<float>> state_B;
vector<vector<float>> state_L;

//generate single point by defining starting and ending point
static float lineardiffgenetraj(float ps, float pe, int step, int tstep)
{
	float pm = ps + step*(pe-ps)/(tstep-1);
	return pm;
}

//read state information from File
static void readFromFile()
{
	vector<float> s_a;
	vector<float> s_b;
	vector<float> s_l;
	vector<float> s_a1;
	vector<float> s_b1;
	vector<float> s_l1;

	inFile.open("/home/ubuntu/catkin_ws/src/origarm_ros/predefined_param/trajp_2seg_1.txt", ios::in);

	if (!inFile)
	{
		printf("%s\n", "unable to open the file.");
		exit(1);
	}
	else
	{
		while (true)
		{
			inFile>>mode_in>>x_in>>y_in>>z_in>>a_in[0]>>b_in[0]>>l_in[0]>>a_in[1]>>b_in[1]>>l_in[1]>>a_in[2]>>b_in[2]>>l_in[2]>>a_in[3]>>b_in[3]>>l_in[3]>>a_in[4]>>b_in[4]>>l_in[4]>>a_in[5]>>b_in[5]>>l_in[5];

			if ( inFile.eof() )
			{
				break;
			}

			for (int i = 0; i < 6; i++)
			{
				s_a.push_back(a_in[i]);
				s_b.push_back(b_in[i]);
				s_l.push_back(l_in[i]);
			}

			if (s_a.size() < 6)
			{
				printf("%s\n", ".............");
			}
			else
			{
				s_a1.assign(s_a.end()-6, s_a.end());
				s_b1.assign(s_b.end()-6, s_b.end());
				s_l1.assign(s_l.end()-6, s_l.end());

				s_A.push_back(s_a1);
				s_B.push_back(s_b1);
				s_L.push_back(s_l1);
			}
		}
	}

	inFile.close();
}

static vector<vector<float>> TrajGeneration(vector<vector<float>> & vect1, int tstep)
{
	int vectsize = vect1.size(); // return row size of 2d vector, size of timestamp read from file

	vector< vector<float> > state;
	vector<float> state1;
	vector<float> state_1;

	int t = 0;

	for (int i = 0; i < 6; i++)
	{
		for (int j = 0; j < vectsize-1; j++)
		{
			float ps = vect1[j][i];
			float pe = vect1[j+1][i];
			// printf("ps: %f, pe: %f\n", ps, pe);

			for (int k = 0; k < tstep-1; k++)
			{
				float pm = lineardiffgenetraj(ps, pe, k, tstep);
				state1.push_back(pm);
				// printf("state1[%d]: %f\n", t, state1[t]);
				t ++;
			}
		}

		int newvectorsize = (tstep-1)*(vectsize-1);

		if (state1.size() < newvectorsize)
		{
			printf("%s\n", "!.............");
		}
		else
		{
			state_1.assign(state1.end() - newvectorsize, state1.end());
			state.push_back(state_1);
		}

	}

	return state;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "demo_trajectory_regenerate");
	ros::NodeHandle nh;
	ros::Rate r(100);     //Hz

	ros::Publisher  pub1 = nh.advertise<origarm_ros::Command_ABL>("Cmd_ABL_joy", 100);

	readFromFile();

	// for (int i = 0; i < s_A.size(); i++)
	// {
	// 	for (int j = 0; j < s_A[0].size(); j++)
	// 	{
	// 		printf("state_A[%d][%d]: %f\n", i, j, s_A[i][j]);
	// 	}
	// }

	state_A = TrajGeneration(s_A, period);
	state_B = TrajGeneration(s_B, period);
	state_L = TrajGeneration(s_L, period);

	// for (int i = 0; i < state_A.size(); i++)
	// {
	// 	for (int j = 0; j < state_A[0].size(); j++)
	// 	{
	// 		printf("state_A[%d][%d]: %f\n", i, j, state_A[i][j]);
	// 	}
	// }

	while (ros::ok())
	{
		origarm_ros::Command_ABL Command_ABL_demo;

		if (flag == 1)
		{
			for (int t = 0; t < 100; t++)
			{
				for (int i = 0; i < 6; i++)
				{
					Command_ABL_demo.segment[i].A = state_A[i][0];
					Command_ABL_demo.segment[i].B = state_B[i][0];
					Command_ABL_demo.segment[i].L = state_L[i][0];
				}
				for (int i = 6; i < 9; i++)
				{
					Command_ABL_demo.segment[i].A = 0;
					Command_ABL_demo.segment[i].B = 0;
					Command_ABL_demo.segment[i].L = g_length0;
				}

				pub1.publish(Command_ABL_demo);
				usleep(ts);
			}

			flag = 2;
		}
		else if (flag == 2)
		{
			for (int j = 0; j < state_A[0].size(); j++)
			{
				for (int i = 0; i < 6; i++)
				{
					Command_ABL_demo.segment[i].A = state_A[i][j];
					Command_ABL_demo.segment[i].B = state_B[i][j];
					Command_ABL_demo.segment[i].L = state_L[i][j];
				}
				for (int i = 6; i < 9; i++)
				{
					Command_ABL_demo.segment[i].A = 0;
					Command_ABL_demo.segment[i].B = 0;
					Command_ABL_demo.segment[i].L = g_length0;
				}

				pub1.publish(Command_ABL_demo);
				usleep(ts);
			}

			flag = 0;
		}
		else
		{
			for (int i = 0; i < 9; i++)
			{
				Command_ABL_demo.segment[i].A = 0;
				Command_ABL_demo.segment[i].B = 0;
				Command_ABL_demo.segment[i].L = g_length0;
			}

			pub1.publish(Command_ABL_demo);
			usleep(ts);
		}

		r.sleep();
	}

	inFile.close();
	return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// 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/.

// discard stack allocation as that too bypasses malloc
#define EIGEN_STACK_ALLOCATION_LIMIT 0
#define EIGEN_RUNTIME_NO_MALLOC
#include "main.h"
#include <Eigen/SVD>

#define SVD_DEFAULT(M) JacobiSVD<M>
#define SVD_FOR_MIN_NORM(M) JacobiSVD<M,ColPivHouseholderQRPreconditioner>
#include "svd_common.h"

// Check all variants of JacobiSVD
template<typename MatrixType>
void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
{
  MatrixType m = a;
  if(pickrandom)
    svd_fill_random(m);

  CALL_SUBTEST(( svd_test_all_computation_options<JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner> >(m, true)  )); // check full only
  CALL_SUBTEST(( svd_test_all_computation_options<JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>  >(m, false) ));
  CALL_SUBTEST(( svd_test_all_computation_options<JacobiSVD<MatrixType, HouseholderQRPreconditioner>        >(m, false) ));
  if(m.rows()==m.cols())
    CALL_SUBTEST(( svd_test_all_computation_options<JacobiSVD<MatrixType, NoQRPreconditioner>               >(m, false) ));
}

template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
{
  svd_verify_assert<JacobiSVD<MatrixType> >(m);
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  MatrixType a = MatrixType::Zero(rows, cols);
  a.setZero();

  if (ColsAtCompileTime == Dynamic)
  {
    JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner> svd_fullqr;
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeFullU|ComputeThinV))
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeThinV))
    VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeFullV))
  }
}

template<typename MatrixType>
void jacobisvd_method()
{
  enum { Size = MatrixType::RowsAtCompileTime };
  typedef typename MatrixType::RealScalar RealScalar;
  typedef Matrix<RealScalar, Size, 1> RealVecType;
  MatrixType m = MatrixType::Identity();
  VERIFY_IS_APPROX(m.jacobiSvd().singularValues(), RealVecType::Ones());
  VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixU());
  VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixV());
  VERIFY_IS_APPROX(m.jacobiSvd(ComputeFullU|ComputeFullV).solve(m), m);
}

void test_jacobisvd()
{
  CALL_SUBTEST_3(( jacobisvd_verify_assert(Matrix3f()) ));
  CALL_SUBTEST_4(( jacobisvd_verify_assert(Matrix4d()) ));
  CALL_SUBTEST_7(( jacobisvd_verify_assert(MatrixXf(10,12)) ));
  CALL_SUBTEST_8(( jacobisvd_verify_assert(MatrixXcd(7,5)) ));

  CALL_SUBTEST_11(svd_all_trivial_2x2(jacobisvd<Matrix2cd>));
  CALL_SUBTEST_12(svd_all_trivial_2x2(jacobisvd<Matrix2d>));

  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_3(( jacobisvd<Matrix3f>() ));
    CALL_SUBTEST_4(( jacobisvd<Matrix4d>() ));
    CALL_SUBTEST_5(( jacobisvd<Matrix<float,3,5> >() ));
    CALL_SUBTEST_6(( jacobisvd<Matrix<double,Dynamic,2> >(Matrix<double,Dynamic,2>(10,2)) ));

    int r = internal::random<int>(1, 30),
        c = internal::random<int>(1, 30);

    TEST_SET_BUT_UNUSED_VARIABLE(r)
    TEST_SET_BUT_UNUSED_VARIABLE(c)

    CALL_SUBTEST_10(( jacobisvd<MatrixXd>(MatrixXd(r,c)) ));
    CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(r,c)) ));
    CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(r,c)) ));
    (void) r;
    (void) c;

    // Test on inf/nan matrix
    CALL_SUBTEST_7(  (svd_inf_nan<JacobiSVD<MatrixXf>, MatrixXf>()) );
    CALL_SUBTEST_10( (svd_inf_nan<JacobiSVD<MatrixXd>, MatrixXd>()) );

    // bug1395 test compile-time vectors as input
    CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix<double,6,1>()) ));
    CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix<double,1,6>()) ));
    CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix<double,Dynamic,1>(r)) ));
    CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix<double,1,Dynamic>(c)) ));
  }

  CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
  CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3))) ));

  // test matrixbase method
  CALL_SUBTEST_1(( jacobisvd_method<Matrix2cd>() ));
  CALL_SUBTEST_3(( jacobisvd_method<Matrix3f>() ));

  // Test problem size constructors
  CALL_SUBTEST_7( JacobiSVD<MatrixXf>(10,10) );

  // Check that preallocation avoids subsequent mallocs
  CALL_SUBTEST_9( svd_preallocate<void>() );

  CALL_SUBTEST_2( svd_underoverflow<void>() );
}


#ifndef DPP_H
#define DPP_H

#include <stdlib.h>
#include <cstdlib>
#include <opencv2/imgproc.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include "utils.hpp"

using namespace cv;
using namespace std;
using namespace Eigen;

class DPP
{
public:
	DPP();
	vector<MyTarget> run(vector<MyTarget> raw_detections, VectorXd &weights, MatrixXd &features, double epsilon = 0.1, double mu = 0.7, double lambda = 0.1);
	vector<MyTarget> run(vector<MyTarget> tracks, double epsilon, double mu, double lambda);
	vector<MyTarget> run(vector<MyTarget> tracks, double epsilon, Size img_size);
	//vector<MyTarget> run(vector<MyTarget> tracks, double epsilon);
private:
	VectorXd getQualityTerm(VectorXd &weights, VectorXd &nPenalty);
	MatrixXd getSimilarityTerm(MatrixXd &features, MatrixXd &intersection, MatrixXd &sqrtArea, double mu);
	MatrixXd affinity_kernel(vector<MyTarget> tracks, Size img_size);
	MatrixXd squared_exponential_kernel(MatrixXd X, double nu, double sigma_f);
	vector<int> solve(VectorXd &qualityTerm, MatrixXd &similarityTerm, double epsilon);
};

#endif


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::::unnormalized_pdf::uniform.hpp     //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_UNIFORM_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_UNIFORM_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{
namespace math{

    template<typename T,typename P>
    T log_unnormalized_pdf(
        const boost::math::uniform_distribution<T,P>& dist,
        const T& x
    )
    {
        T lower = dist.lower();
        T upper = dist.upper();
        T result; // of checks.
        if(false == boost::math::detail::check_uniform(
            "boost::math::pdf(const uniform_distribution<%1%>&, %1%)",
            lower, upper, &result, P()))
        {
            return result;
        }
        if(false == boost::math::detail::check_uniform_x(
            "boost::math::pdf(const uniform_distribution<%1%>&, %1%)",
             x, &result, P()))
        {
            return result;
        }

        if((x < lower) || (x > upper) )
        {
            static T inf = std::numeric_limits<T>::infinity();
            return (- inf);
        }
        else
        {
            return static_cast<T>(0);
        }
    }

}// math
}// boost

#endif