// Copyright 2008-2010 Gordon Woodhull
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/msm/mpl_graph/breadth_first_search.hpp>
#include <boost/msm/mpl_graph/adjacency_list_graph.hpp>
#include <boost/msm/mpl_graph/incidence_list_graph.hpp>

#include <iostream>

namespace mpl_graph = boost::msm::mpl_graph;
namespace mpl = boost::mpl;

// vertices
struct A{}; struct B{}; struct C{}; struct D{}; struct E{}; struct F{}; struct G{};

// edges
struct A_B{}; struct B_C{}; struct C_D{}; struct C_E{}; struct C_F{}; struct B_F{};

/*
    incidence list test graph:
    A -> B -> C -\--> D
           \     |--> E
            \    \--> F
             \-----/
*/

typedef mpl::vector<mpl::vector<A_B,A,B>,
               mpl::vector<B_C,B,C>,
               mpl::vector<C_D,C,D>,
               mpl::vector<C_E,C,E>,
               mpl::vector<C_F,C,F>,
               mpl::vector<B_F,B,F> >
    some_incidence_list;
typedef mpl_graph::incidence_list_graph<some_incidence_list> some_incidence_list_graph;

/*
    adjacency list test graph:
    A -> B -> C -\--> D
           \     |--> E
            \    \--> F
             \-----/
    G
*/

typedef mpl::vector<
            mpl::pair<A, mpl::vector<mpl::pair<A_B, B> > >,
            mpl::pair<B, mpl::vector<mpl::pair<B_C, C>,
                                     mpl::pair<B_F, F> > >,
            mpl::pair<C, mpl::vector<mpl::pair<C_D, D>,
                                     mpl::pair<C_E, E>,
                                     mpl::pair<C_F, F> > >,
            mpl::pair<G, mpl::vector<> > >
    some_adjacency_list;
typedef mpl_graph::adjacency_list_graph<some_adjacency_list> some_adjacency_list_graph;

struct preordering_visitor : mpl_graph::bfs_default_visitor_operations {
    template<typename Vertex, typename Graph, typename State>
    struct discover_vertex :
        mpl::push_back<State, Vertex>
    {};
};

struct postordering_visitor : mpl_graph::bfs_default_visitor_operations {
    template<typename Vertex, typename Graph, typename State>
    struct finish_vertex :
        mpl::push_back<State, Vertex>
    {};
};

struct examine_edge_visitor : mpl_graph::bfs_default_visitor_operations {
    template<typename Edge, typename Graph, typename State>
    struct examine_edge :
        mpl::push_back<State, Edge>
    {};
};

struct tree_edge_visitor : mpl_graph::bfs_default_visitor_operations {
    template<typename Edge, typename Graph, typename State>
    struct tree_edge :
        mpl::push_back<State, Edge>
    {};
};

// adjacency list tests

// preordering, start from A
typedef mpl::first<mpl_graph::
    breadth_first_search<some_adjacency_list_graph,
                         preordering_visitor,
                         mpl::vector<>,
                         A>::type>::type
                preorder_adj_a;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj_a::type, mpl::vector<A,B,C,F,D,E> > ));

// examine edges, start from A
typedef mpl::first<mpl_graph::
    breadth_first_search<some_adjacency_list_graph,
                         examine_edge_visitor,
                         mpl::vector<>,
                         A>::type>::type
                ex_edges_adj_a;
BOOST_MPL_ASSERT(( mpl::equal<ex_edges_adj_a::type, mpl::vector<A_B,B_C,B_F,C_D,C_E,C_F> > ));

// tree edges, start from A
typedef mpl::first<mpl_graph::
    breadth_first_search<some_adjacency_list_graph,
                         tree_edge_visitor,
                         mpl::vector<>,
                         A>::type>::type
                tree_edges_adj_a;
BOOST_MPL_ASSERT(( mpl::equal<tree_edges_adj_a::type, mpl::vector<A_B,B_C,B_F,C_D,C_E> > ));

// preordering, search all, default start node (first)
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_adjacency_list_graph,
                             preordering_visitor,
                             mpl::vector<> >::type>::type
                preorder_adj;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj::type, mpl::vector<A,B,C,F,D,E,G> > ));

// postordering, starting at A (same as preordering because BFS fully processes one vertex b4 moving to next)
typedef mpl::first<mpl_graph::
    breadth_first_search<some_adjacency_list_graph,
                         postordering_visitor,
                         mpl::vector<>,
                         A>::type>::type
                postorder_adj_a;
BOOST_MPL_ASSERT(( mpl::equal<postorder_adj_a::type, mpl::vector<A,B,C,F,D,E> > ));

// postordering, default start node (same as preordering because BFS fully processes one vertex b4 moving to next)
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_adjacency_list_graph,
                             postordering_visitor,
                             mpl::vector<> >::type>::type
                postorder_adj;
BOOST_MPL_ASSERT(( mpl::equal<postorder_adj::type, mpl::vector<A,B,C,F,D,E,G> > ));

// preordering starting at C
typedef mpl::first<mpl_graph::
    breadth_first_search<some_adjacency_list_graph,
                         preordering_visitor,
                         mpl::vector<>,
                         C>::type>::type
                preorder_adj_from_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj_from_c::type, mpl::vector<C,D,E,F> > ));

// preordering, search all, starting at C
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_adjacency_list_graph,
                             preordering_visitor,
                             mpl::vector<>,
                             C>::type>::type
                preorder_adj_from_c_all;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj_from_c_all::type, mpl::vector<C,D,E,F,A,B,G> > ));

// incidence list tests

// preordering, start from A
typedef mpl::first<mpl_graph::
    breadth_first_search<some_incidence_list_graph,
                         preordering_visitor,
                         mpl::vector<>,
                         A>::type>::type
                preorder_inc_a;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc_a::type, mpl::vector<A,B,C,F,D,E> > ));

// preordering, start from C
typedef mpl::first<mpl_graph::
    breadth_first_search<some_incidence_list_graph,
                         preordering_visitor,
                         mpl::vector<>,
                         C>::type>::type
                preorder_inc_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc_c::type, mpl::vector<C,D,E,F> > ));

// preordering, default start node (first)
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_incidence_list_graph,
                             preordering_visitor,
                             mpl::vector<> >::type>::type
                preorder_inc;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc::type, mpl::vector<A,B,C,F,D,E> > ));

// postordering, default start node
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_incidence_list_graph,
                             postordering_visitor,
                             mpl::vector<> >::type>::type
                postorder_inc;
BOOST_MPL_ASSERT(( mpl::equal<postorder_inc::type, mpl::vector<A,B,C,F,D,E> > ));

// preordering, search all, starting at C
typedef mpl::first<mpl_graph::
    breadth_first_search_all<some_incidence_list_graph,
                             preordering_visitor,
                             mpl::vector<>,
                             C>::type>::type
                preorder_inc_from_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc_from_c::type, mpl::vector<C,D,E,F,A,B> > ));

int main() {
    return 0;
}


//  (C) Copyright Matt Borland 2021.
//  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_MATH_CCMATH_CEIL_HPP
#define BOOST_MATH_CCMATH_CEIL_HPP

#include <cmath>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/floor.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T ceil_impl(T arg) noexcept
{
    T result = boost::math::ccmath::floor(arg);

    if(result == arg)
    {
        return result;
    }
    else
    {
        return result + 1;
    }
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real ceil(Real arg) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? arg :
               boost::math::ccmath::isinf(arg) ? arg :
               boost::math::ccmath::isnan(arg) ? arg :
               boost::math::ccmath::detail::ceil_impl(arg);
    }
    else
    {
        using std::ceil;
        return ceil(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double ceil(Z arg) noexcept
{
    return boost::math::ccmath::ceil(static_cast<double>(arg));
}

inline constexpr float ceilf(float arg) noexcept
{
    return boost::math::ccmath::ceil(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double ceill(long double arg) noexcept
{
    return boost::math::ccmath::ceil(arg);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_CEIL_HPP


#include <exception>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

#include <nit/eval/card.h>
#include <nit/eval/card_set.h>
#include <nit/util/combinations.h>

#include "guard.h"

namespace po = boost::program_options;

namespace {

int runColex(int argc, char** argv) {
  // set up the program options, handle the help case, and extract the values.
  po::options_description desc(
      "nit-colex, a utility which prints all combinations "
      "of poker hands, using canonical suits, or only ranks");

  // clang-format off
  desc.add_options()
      ("help,?", "produce help message")
      ("num-cards,n", po::value<std::size_t>()->default_value(2),
       "number of cards in hands")
      ("ranks", "print the set of rank values");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
  } catch (const std::exception& err) {
    std::cerr << "Option error: " << err.what() << "\n\n" << desc << std::endl;
    return 1;
  }
  po::notify(vm);

  // check for help
  if (vm.count("help") || argc == 1) {
    std::cout << desc << std::endl;
    return 1;
  }

  // extract the options
  std::size_t num_cards = vm["num-cards"].as<std::size_t>();

  std::set<nit::CardSet> canonicalHands;
  std::map<std::string, std::size_t> rankHands;
  nit::combinations cards(52, num_cards);
  do {
    nit::CardSet hand;
    for (std::size_t i = 0; i < num_cards; i++) {
      hand.insert(nit::Card(cards[i]));
    }
    canonicalHands.insert(hand.canonize());
    rankHands[hand.rankstr()] = hand.rankColex();
  } while (cards.next());

  if (vm.count("ranks") > 0) {
    for (auto& rankHand : rankHands)
      std::cout << rankHand.first << ": " << rankHand.second << '\n';
  } else {
    for (const auto& canonicalHand : canonicalHands) {
      std::cout << canonicalHand.str() << ": " << canonicalHand.colex() << '\n';
    }
  }

  return 0;
}

}  // namespace

int main(int argc, char** argv) {
  return guard([&argc, &argv] { return runColex(argc, argv); });
}


//  (C) Copyright John Maddock 2008.
//  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_TR1_CMATH_HPP_INCLUDED
#  define BOOST_TR1_CMATH_HPP_INCLUDED
#  include <boost/tr1/detail/config.hpp>

#ifdef BOOST_HAS_TR1_CMATH

#  if defined(BOOST_HAS_INCLUDE_NEXT) && !defined(BOOST_TR1_DISABLE_INCLUDE_NEXT)
#     include_next BOOST_TR1_HEADER(cmath)
#  else
#     include <boost/tr1/detail/config_all.hpp>
#     include BOOST_TR1_HEADER(cmath)
#  endif

#else

#include <boost/math/tr1.hpp>

namespace std{ namespace tr1{

using boost::math::tr1::assoc_laguerre;
using boost::math::tr1::assoc_laguerref;
using boost::math::tr1::assoc_laguerrel;
// [5.2.1.2] associated Legendre functions:
using boost::math::tr1::assoc_legendre;
using boost::math::tr1::assoc_legendref;
using boost::math::tr1::assoc_legendrel;
// [5.2.1.3] beta function:
using boost::math::tr1::beta;
using boost::math::tr1::betaf;
using boost::math::tr1::betal;
// [5.2.1.4] (complete) elliptic integral of the first kind:
using boost::math::tr1::comp_ellint_1;
using boost::math::tr1::comp_ellint_1f;
using boost::math::tr1::comp_ellint_1l;
// [5.2.1.5] (complete) elliptic integral of the second kind:
using boost::math::tr1::comp_ellint_2;
using boost::math::tr1::comp_ellint_2f;
using boost::math::tr1::comp_ellint_2l;
// [5.2.1.6] (complete) elliptic integral of the third kind:
using boost::math::tr1::comp_ellint_3;
using boost::math::tr1::comp_ellint_3f;
using boost::math::tr1::comp_ellint_3l;
#if 0
// [5.2.1.7] confluent hypergeometric functions:
using boost::math::tr1::conf_hyperg;
using boost::math::tr1::conf_hypergf;
using boost::math::tr1::conf_hypergl;
#endif
// [5.2.1.8] regular modified cylindrical Bessel functions:
using boost::math::tr1::cyl_bessel_i;
using boost::math::tr1::cyl_bessel_if;
using boost::math::tr1::cyl_bessel_il;
// [5.2.1.9] cylindrical Bessel functions (of the first kind):
using boost::math::tr1::cyl_bessel_j;
using boost::math::tr1::cyl_bessel_jf;
using boost::math::tr1::cyl_bessel_jl;
// [5.2.1.10] irregular modified cylindrical Bessel functions:
using boost::math::tr1::cyl_bessel_k;
using boost::math::tr1::cyl_bessel_kf;
using boost::math::tr1::cyl_bessel_kl;
// [5.2.1.11] cylindrical Neumann functions;
// cylindrical Bessel functions (of the second kind):
using boost::math::tr1::cyl_neumann;
using boost::math::tr1::cyl_neumannf;
using boost::math::tr1::cyl_neumannl;
// [5.2.1.12] (incomplete) elliptic integral of the first kind:
using boost::math::tr1::ellint_1;
using boost::math::tr1::ellint_1f;
using boost::math::tr1::ellint_1l;
// [5.2.1.13] (incomplete) elliptic integral of the second kind:
using boost::math::tr1::ellint_2;
using boost::math::tr1::ellint_2f;
using boost::math::tr1::ellint_2l;
// [5.2.1.14] (incomplete) elliptic integral of the third kind:
using boost::math::tr1::ellint_3;
using boost::math::tr1::ellint_3f;
using boost::math::tr1::ellint_3l;
// [5.2.1.15] exponential integral:
using boost::math::tr1::expint;
using boost::math::tr1::expintf;
using boost::math::tr1::expintl;
// [5.2.1.16] Hermite polynomials:
using boost::math::tr1::hermite;
using boost::math::tr1::hermitef;
using boost::math::tr1::hermitel;
#if 0
// [5.2.1.17] hypergeometric functions:
using boost::math::tr1::hyperg;
using boost::math::tr1::hypergf;
using boost::math::tr1::hypergl;
#endif
// [5.2.1.18] Laguerre polynomials:
using boost::math::tr1::laguerre;
using boost::math::tr1::laguerref;
using boost::math::tr1::laguerrel;
// [5.2.1.19] Legendre polynomials:
using boost::math::tr1::legendre;
using boost::math::tr1::legendref;
using boost::math::tr1::legendrel;
// [5.2.1.20] Riemann zeta function:
using boost::math::tr1::riemann_zeta;
using boost::math::tr1::riemann_zetaf;
using boost::math::tr1::riemann_zetal;
// [5.2.1.21] spherical Bessel functions (of the first kind):
using boost::math::tr1::sph_bessel;
using boost::math::tr1::sph_besself;
using boost::math::tr1::sph_bessell;
// [5.2.1.22] spherical associated Legendre functions:
using boost::math::tr1::sph_legendre;
using boost::math::tr1::sph_legendref;
using boost::math::tr1::sph_legendrel;
// [5.2.1.23] spherical Neumann functions;
// spherical Bessel functions (of the second kind):
using boost::math::tr1::sph_neumann;
using boost::math::tr1::sph_neumannf;
using boost::math::tr1::sph_neumannl;

// types
using boost::math::tr1::double_t;
using boost::math::tr1::float_t;
// functions
using boost::math::tr1::acosh;
using boost::math::tr1::acoshf;
using boost::math::tr1::acoshl;
using boost::math::tr1::asinh;
using boost::math::tr1::asinhf;
using boost::math::tr1::asinhl;
using boost::math::tr1::atanh;
using boost::math::tr1::atanhf;
using boost::math::tr1::atanhl;
using boost::math::tr1::cbrt;
using boost::math::tr1::cbrtf;
using boost::math::tr1::cbrtl;
using boost::math::tr1::copysign;
using boost::math::tr1::copysignf;
using boost::math::tr1::copysignl;
using boost::math::tr1::erf;
using boost::math::tr1::erff;
using boost::math::tr1::erfl;
using boost::math::tr1::erfc;
using boost::math::tr1::erfcf;
using boost::math::tr1::erfcl;
#if 0
using boost::math::tr1::exp2;
using boost::math::tr1::exp2f;
using boost::math::tr1::exp2l;
#endif
using boost::math::tr1::expm1;
using boost::math::tr1::expm1f;
using boost::math::tr1::expm1l;
#if 0
using boost::math::tr1::fdim;
using boost::math::tr1::fdimf;
using boost::math::tr1::fdiml;
using boost::math::tr1::fma;
using boost::math::tr1::fmaf;
using boost::math::tr1::fmal;
#endif
using boost::math::tr1::fmax;
using boost::math::tr1::fmaxf;
using boost::math::tr1::fmaxl;
using boost::math::tr1::fmin;
using boost::math::tr1::fminf;
using boost::math::tr1::fminl;
using boost::math::tr1::hypot;
using boost::math::tr1::hypotf;
using boost::math::tr1::hypotl;
#if 0
using boost::math::tr1::ilogb;
using boost::math::tr1::ilogbf;
using boost::math::tr1::ilogbl;
#endif
using boost::math::tr1::lgamma;
using boost::math::tr1::lgammaf;
using boost::math::tr1::lgammal;
#if 0
using boost::math::tr1::llrint;
using boost::math::tr1::llrintf;
using boost::math::tr1::llrintl;
#endif
using boost::math::tr1::llround;
using boost::math::tr1::llroundf;
using boost::math::tr1::llroundl;
using boost::math::tr1::log1p;
using boost::math::tr1::log1pf;
using boost::math::tr1::log1pl;
#if 0
using boost::math::tr1::log2;
using boost::math::tr1::log2f;
using boost::math::tr1::log2l;
using boost::math::tr1::logb;
using boost::math::tr1::logbf;
using boost::math::tr1::logbl;
using boost::math::tr1::lrint;
using boost::math::tr1::lrintf;
using boost::math::tr1::lrintl;
#endif
using boost::math::tr1::lround;
using boost::math::tr1::lroundf;
using boost::math::tr1::lroundl;
#if 0
using boost::math::tr1::nan;
using boost::math::tr1::nanf;
using boost::math::tr1::nanl;
using boost::math::tr1::nearbyint;
using boost::math::tr1::nearbyintf;
using boost::math::tr1::nearbyintl;
#endif
using boost::math::tr1::nextafter;
using boost::math::tr1::nextafterf;
using boost::math::tr1::nextafterl;
using boost::math::tr1::nexttoward;
using boost::math::tr1::nexttowardf;
using boost::math::tr1::nexttowardl;
#if 0
using boost::math::tr1::remainder;
using boost::math::tr1::remainderf;
using boost::math::tr1::remainderl;
using boost::math::tr1::remquo;
using boost::math::tr1::remquof;
using boost::math::tr1::remquol;
using boost::math::tr1::rint;
using boost::math::tr1::rintf;
using boost::math::tr1::rintl;
#endif
using boost::math::tr1::round;
using boost::math::tr1::roundf;
using boost::math::tr1::roundl;
#if 0
using boost::math::tr1::scalbln;
using boost::math::tr1::scalblnf;
using boost::math::tr1::scalblnl;
using boost::math::tr1::scalbn;
using boost::math::tr1::scalbnf;
using boost::math::tr1::scalbnl;
#endif
using boost::math::tr1::tgamma;
using boost::math::tr1::tgammaf;
using boost::math::tr1::tgammal;
using boost::math::tr1::trunc;
using boost::math::tr1::truncf;
using boost::math::tr1::truncl;
// C99 macros defined as C++ templates
using boost::math::tr1::signbit;
using boost::math::tr1::fpclassify;
using boost::math::tr1::isfinite;
using boost::math::tr1::isinf;
using boost::math::tr1::isnan;
using boost::math::tr1::isnormal;
#if 0
using boost::math::tr1::isgreater;
using boost::math::tr1::isgreaterequal;
using boost::math::tr1::isless;
using boost::math::tr1::islessequal;
using boost::math::tr1::islessgreater;
using boost::math::tr1::isunordered;
#endif
} } // namespaces

#endif // BOOST_HAS_TR1_CMATH

#endif // BOOST_TR1_CMATH_HPP_INCLUDED


/**
 * @file
 * @brief Solution of general second-order elliptic boundary value problem with
 * linear finite elements from a Gmsh generated mesh
 * @author Simon Meierhans
 * @date   January 2019
 * @copyright MIT License
 */

#include <fstream>
#include <iomanip>

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <boost/filesystem.hpp>

int main() {
  // abbreviations for types
  using size_type = lf::base::size_type;
  using glb_idx_t = lf::assemble::glb_idx_t;
  using coord_t = Eigen::Vector2d;

  // find path to mesh
  boost::filesystem::path here = __FILE__;
  auto mesh_path = here.parent_path() / "meshes/square.msh";

  // load mesh of square computational domain
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(std::move(mesh_factory), mesh_path.string());

  // get pointer to mesh
  auto mesh = reader.mesh();

  // Count the number of edges with von Neumann boundary condition
  auto physical_entity_nr_neu = reader.PhysicalEntityName2Nr("neu");
  int num_neumann = 0;
  for (auto e : mesh->Entities(1)) {
    if (reader.IsPhysicalEntity(*e, physical_entity_nr_neu)) {
      ++num_neumann;
    }
  }
  std::cout << "boundary edges with von Neumann boundary condition: "
            << num_neumann << "\n";

  // Count the number of edges with Dirichlet boundary condition
  auto physical_entity_nr_dir = reader.PhysicalEntityName2Nr("dir");
  int num_dirichlet = 0;
  for (auto e : mesh->Entities(1)) {
    if (reader.IsPhysicalEntity(*e, physical_entity_nr_dir)) {
      ++num_dirichlet;
    }
  }
  std::cout << "boundary edges with Dirichlet boundary condition: "
            << num_dirichlet << "\n";

  // set up finite element space
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);

  // set up dof handler
  const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};

  // Dimension of finite element space`
  const size_type N_dofs(dofh.NumDofs());

  // identity mesh function for very simple problem
  lf::uscalfe::MeshFunctionConstant mf_identity(1.0);

  auto zero = [](const Eigen::Vector2d & /*x*/) -> double { return 0.; };
  lf::uscalfe::MeshFunctionGlobal mf_zero{zero};

  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);

  // Obtain an object that computes the element matrix for the
  // volumne part of the bilinear form
  lf::uscalfe::ReactionDiffusionElementMatrixProvider elmat_builder(
      fe_space, mf_identity, mf_identity);

  // Invoke assembly on cells (co-dimension = 0)
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);

  // Right-hand side vector; has to be set to zero initially
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  // Initialize object taking care of local computations on all cells for the
  // source f. The source is the identity function
  lf::uscalfe::ScalarLoadElementVectorProvider elvec_builder(fe_space,
                                                             mf_identity);
  // Invoke assembly on cells (codim == 0)
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  if (num_neumann > 0) {
    // Select von Neumann edges
    auto edge_sel_neu = [&reader, physical_entity_nr_neu](
                            const lf::mesh::Entity& edge) -> bool {
      return reader.IsPhysicalEntity(edge, physical_entity_nr_neu);
    };

    // Add contributions of von Neumann boundary conditions
    lf::uscalfe::ScalarLoadEdgeVectorProvider<double, decltype(mf_identity),
                                              decltype(edge_sel_neu)>
        elvec_builder_neu(fe_space, mf_identity, edge_sel_neu);
    AssembleVectorLocally(1, dofh, elvec_builder_neu, phi);
  }

  if (num_dirichlet > 0) {
    // Select Dirichlet edges
    auto edge_sel_dir = [&reader, physical_entity_nr_dir](
                            const lf::mesh::Entity& edge) -> bool {
      return reader.IsPhysicalEntity(edge, physical_entity_nr_dir);
    };

    // Obtain specification for shape functions on edges
    std::shared_ptr<const lf::uscalfe::ScalarReferenceFiniteElement<double>>
        rsf_edge_p = fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());
    LF_ASSERT_MSG(rsf_edge_p != nullptr, "FE specification for edges missing");

    // Fetch flags and values for degrees of freedom located on Dirichlet
    // edges.
    // bd_flags strictly speaking would not be necessary here since only
    // boundary edges are flagged as 'dir' anyway. In other cases this might
    // however be necessary.
    auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 1)};
    auto ess_bdc_flags_values{lf::uscalfe::InitEssentialConditionFromFunction(
        dofh, *rsf_edge_p,
        [&edge_sel_dir, &bd_flags](const lf::mesh::Entity& edge) -> bool {
          return (bd_flags(edge) && edge_sel_dir(edge));
        },
        mf_zero)};
    // Eliminate Dirichlet dofs from linear system
    lf::assemble::FixFlaggedSolutionComponents<double>(
        [&ess_bdc_flags_values](glb_idx_t gdof_idx) {
          return ess_bdc_flags_values[gdof_idx];
        },
        A, phi);
  }
  // Assembly completed: Convert COO matrix into CRS format using Eigen's
  // internal conversion routines.
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  Eigen::VectorXd sol_vec = solver.solve(phi);

  // Compute H1 Norm
  if (N_dofs > 0) {
    // Version 1: Using Mesh Functions
    auto mf_FE = lf::uscalfe::MeshFunctionFE(fe_space, sol_vec);
    auto mf_GradFe = lf::uscalfe::MeshFunctionGradFE(fe_space, sol_vec);
    auto h1_norm = std::sqrt(IntegrateMeshFunction(
        *mesh, squaredNorm(mf_FE) + squaredNorm(mf_GradFe), 2));

    std::cout << "Computed H1 Norm: " << h1_norm << std::endl;

    // Version 2: Compute Energy by assembling Stiffness matrix/mass matrix
    lf::assemble::COOMatrix<double> Stiffness(N_dofs, N_dofs);
    lf::uscalfe::ReactionDiffusionElementMatrixProvider stiffness_mat_builder(
        fe_space, mf_identity, mf_zero);
    lf::assemble::AssembleMatrixLocally(0, dofh, dofh, stiffness_mat_builder,
                                        Stiffness);
    Eigen::SparseMatrix<double> Stiffness_crs = Stiffness.makeSparse();

    // Matrix in triplet format holding Mass matrix.
    lf::assemble::COOMatrix<double> Mass(N_dofs, N_dofs);
    lf::uscalfe::ReactionDiffusionElementMatrixProvider mass_mat_builder(
        fe_space, mf_zero, mf_identity);
    lf::assemble::AssembleMatrixLocally(0, dofh, dofh, mass_mat_builder, Mass);
    Eigen::SparseMatrix<double> Mass_crs = Mass.makeSparse();

    // h1_seminorm_sq = \mu' A \mu
    double h1_semi2 = sol_vec.transpose() * (Stiffness_crs * sol_vec);
    // l2_norm_sq = \mu' M \mu
    double l22 = sol_vec.transpose() * Mass_crs * sol_vec;

    std::cout << "Computed H1 Norm: " << std::sqrt(h1_semi2 + l22) << "\n";
  }
}


/* boost random/triangle_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: triangle_distribution.hpp,v 1.9 2002/12/22 22:03:11 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP
#define BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP

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

namespace boost {

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

  explicit triangle_distribution(base_type & rng,
                                 result_type a = result_type(0),
                                 result_type b = result_type(0.5),
                                 result_type c = result_type(1))
    : _rng(rng), _a(a), _b(b), _c(c)
  {
    assert(_a <= _b && _b <= _c);
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    result_type u = _rng();
    if( u <= q1 )
      return _a + p1*sqrt(u);
    else
      return _c - d3*sqrt(d2*u-d1);
  }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const triangle_distribution& x,
                         const triangle_distribution& y)
  { return x._a == y._a && x._b == y._b && x._c == y._c && x._rng == y._rng; }

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

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

#else
  // Use a member function
  bool operator==(const triangle_distribution& rhs) const
  { return _a == rhs._a && _b == rhs._b && _c == rhs._c && _rng == rhs._rng;  }
#endif

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

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

} // namespace boost

#endif // BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP


#ifndef COMP_EIG_HH
#define COMP_EIG_HH

#include <armadillo>
#include <cassert>

// sort and compare eigenvectors and eigenvalues
static double comp_eig(arma::vec Avalues, arma::mat Avectors, arma::vec Bvalues, arma::mat Bvectors) {
  assert(size(Avalues) == size(Bvalues));
  assert(size(Avectors) == size(Bvectors));
  assert(Avalues.n_elem == Avectors.n_rows && Avectors.is_square());

  size_t M = Avalues.n_elem;

  // normalize and sort A eigenvalues (quick and dirty algorithm)
  for(size_t i = 0; i < M; i++) {
    // Avectors.col(i) = arma::normalise(Avectors.col(i));

    size_t k = i;
    for(size_t j = i + 1; j < M; j++) {
      if(Avalues(j) < Avalues(k))
        k = j;
    }
    if(i == k) continue;
    Avalues.swap_rows(i, k);
    Avectors.swap_cols(i, k);
  }

  // sort B eigenvalues (quick and dirty algorithm)
  for(size_t i = 0; i < M; i++) {
    // Bvectors.col(i) = arma::normalise(Bvectors.col(i));

    size_t k = i;
    for(size_t j = i + 1; j < M; j++) {
      if(Bvalues(j) < Bvalues(k))
        k = j;
    }
    if(i == k) continue;
    Bvalues.swap_rows(i, k);
    Bvectors.swap_cols(i, k);
  }

  // fix antiparallell eigenvectors in A and B
  for(size_t i = 0; i < M; i++) {
    if(arma::dot(Avectors.col(i), Bvectors.col(i)) < 0)
      Bvectors.col(i) = - Bvectors.col(i);
  }

  // fix maximal difference between eigenvalues or eigenvectors
  const double values_error = arma::abs(Avalues - Bvalues).max();
  const double vectors_error = arma::abs(Avectors - Bvectors).max();

  // return error metric
  return std::max(values_error, vectors_error);
}

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <iostream>

#include <boost/bind.hpp>
#include <memory>
#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h"
#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/rotationRepresentations.h"

namespace tudat
{

namespace reference_frames
{

//! Function to get a string representing a 'named identification' of a reference frame.
std::string getAerodynamicFrameName( const AerodynamicsReferenceFrames frame )
{
    std::string frameName;
    switch( frame )
    {
    case inertial_frame:
        frameName = "inertial frame ";
        break;
    case corotating_frame:
        frameName = "corotating frame ";
        break;
    case vertical_frame:
        frameName = "vertical frame ";
        break;
    case trajectory_frame:
        frameName = "trajectory frame ";
        break;
    case aerodynamic_frame:
        frameName = "aerodynamic frame ";
        break;
    case body_frame:
        frameName = "body frame ";
        break;
    default:
        std::string errorMessage = "Error, aerodynamic frame type " +
                std::to_string( frame ) +
                "not found when retrieving frame name ";
        throw std::runtime_error( errorMessage );
    }
    return frameName;
}

//! Function to get a string representing a 'named identification' of an aerodynamic angle
std::string getAerodynamicAngleName( const AerodynamicsReferenceFrameAngles angle )
{
    std::string angleName;
    switch( angle )
    {
    case latitude_angle:
        angleName = "latitude angle ";
        break;
    case longitude_angle:
        angleName = "longitude angle ";
        break;
    case heading_angle:
        angleName = "heading angle ";
        break;
    case flight_path_angle:
        angleName = "flight path angle ";
        break;
    case angle_of_attack:
        angleName = "angle of attack ";
        break;
    case angle_of_sideslip:
        angleName = "sideslip angle ";
        break;
    case bank_angle:
        angleName = "bank angle ";
        break;
    default:
        std::string errorMessage = "Error, aerodynamic angle type " +
                std::to_string( angle ) +
                "not found when retrieving angle name ";
        throw std::runtime_error( errorMessage );
    }
    return angleName;
}

//! Function to update the orientation angles to the current state.
void AerodynamicAngleCalculator::update( const double currentTime, const bool updateBodyOrientation )
{
    // Clear all current rotation matrices.
    currentRotationMatrices_.clear( );

    // Get current body-fixed state.
    if( !( currentTime == currentTime_ ) )
    {
        currentBodyFixedGroundSpeedBasedState_ = bodyFixedStateFunction_( );
        currentRotationFromCorotatingToInertialFrame_ = rotationFromCorotatingToInertialFrame_( );

        Eigen::Vector3d sphericalCoordinates = coordinate_conversions::convertCartesianToSpherical< double >(
                    currentBodyFixedGroundSpeedBasedState_.segment( 0, 3 ) );

        // Calculate latitude and longitude.
        currentAerodynamicAngles_[ latitude_angle ] =
                mathematical_constants::PI / 2.0 - sphericalCoordinates( 1 );
        currentAerodynamicAngles_[ longitude_angle ] = sphericalCoordinates( 2 );

        // Compute wind velocity vector
        Eigen::Vector3d localWindVelocity = Eigen::Vector3d::Zero( );
        if( windModel_ != nullptr )
        {
            localWindVelocity = windModel_->getCurrentWindVelocity(
                        shapeModel_->getAltitude( currentBodyFixedGroundSpeedBasedState_.segment( 0, 3 ) ),
                        currentAerodynamicAngles_[ longitude_angle ],
                        currentAerodynamicAngles_[ latitude_angle ],
                        currentTime );
        }

        // Compute airspeed-based velocity vector
        currentBodyFixedAirspeedBasedState_ = currentBodyFixedGroundSpeedBasedState_;
        currentBodyFixedAirspeedBasedState_.segment( 3, 3 ) += localWindVelocity;

        // Calculate vertical <-> aerodynamic <-> body-fixed angles if neede.
        if( calculateVerticalToAerodynamicFrame_ )
        {
            Eigen::Vector3d verticalFrameVelocity =
                    getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                        currentAerodynamicAngles_.at( longitude_angle ),
                        currentAerodynamicAngles_.at( latitude_angle ) ) *
                    currentBodyFixedAirspeedBasedState_.segment( 3, 3 );

            currentAerodynamicAngles_[ heading_angle ] = calculateHeadingAngle( verticalFrameVelocity );
            currentAerodynamicAngles_[ flight_path_angle ] =
                    calculateFlightPathAngle( verticalFrameVelocity );
        }

        currentTime_ = currentTime;
    }

    if( updateBodyOrientation  && !( currentBodyAngleTime_ == currentTime ) )
    {
        if( !( angleUpdateFunction_ == nullptr ) )
        {
            angleUpdateFunction_( currentTime );
        }

        if( !( angleOfAttackFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ angle_of_attack ] = angleOfAttackFunction_( );
        }

        if( !( angleOfSideslipFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ angle_of_sideslip ] = angleOfSideslipFunction_( );
        }

        if( !( bankAngleFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ bank_angle ] = bankAngleFunction_( );
        }

        currentBodyAngleTime_ = currentTime;
    }
    else if( !( currentBodyAngleTime_ == currentTime ) )
    {
        currentAerodynamicAngles_[ angle_of_attack ] = 0.0;
        currentAerodynamicAngles_[ angle_of_sideslip ] = 0.0;
        currentAerodynamicAngles_[ bank_angle ] = 0.0;
    }
}

//! Function to get the rotation quaternion between two frames
Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames(
        const AerodynamicsReferenceFrames originalFrame,
        const AerodynamicsReferenceFrames targetFrame )
{
    // Initialize rotation to identity matrix.
    Eigen::Quaterniond rotationToFrame = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );

    // Check if update settings are consistent with requested frames.
    if( !calculateVerticalToAerodynamicFrame_ &&
            ( originalFrame > vertical_frame || targetFrame > vertical_frame ) )
    {
        throw std::runtime_error( "Error in AerodynamicAngleCalculator, instance ends at vertical frame" );
    }

    // Set current frame pair.
    std::pair< AerodynamicsReferenceFrames, AerodynamicsReferenceFrames > currentRotationPair =
            std::make_pair( originalFrame, targetFrame );

    // Calculate rotation matrix if current rotation is not yet calculated.
    if( currentRotationMatrices_.count( currentRotationPair ) == 0 )
    {
        // Get indices of required frames.
        int currentFrameIndex = static_cast< int >( originalFrame );
        int targetFrameIndex = static_cast< int >( targetFrame );

        // Check if any rotation is needed.
        if( currentFrameIndex != targetFrameIndex )
        {
            // Check 'direction' of transformation through AerodynamicsReferenceFrames list.
            bool isTargetFrameUp;
            if( targetFrameIndex > currentFrameIndex )
            {
                isTargetFrameUp = 1;
            }
            else if( targetFrameIndex < currentFrameIndex )
            {
                isTargetFrameUp = 0;
            }
            else
            {
                throw std::runtime_error(
                            "Error when identifying target frame direction in AerodynamicAngleCalculator." );
            }

            // Add rotation sequence until final frame is reached.
            while( currentFrameIndex != targetFrameIndex )
            {
                switch( currentFrameIndex )
                {
                case static_cast< int >( inertial_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame = currentRotationFromCorotatingToInertialFrame_.inverse( ) *
                                rotationToFrame;
                    }
                    else
                    {
                        throw std::runtime_error(
                                    "Error, inertial_frame is end frame in AerodynamicAngleCalculator" );
                    }
                    break;
                case static_cast< int >( corotating_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame = currentRotationFromCorotatingToInertialFrame_ *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( vertical_frame ):
                    if( isTargetFrameUp )
                    {

                        rotationToFrame =
                                getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) * rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( trajectory_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getTrajectoryToAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( aerodynamic_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getAerodynamicToTrajectoryFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( body_frame ):
                    if( isTargetFrameUp )
                    {
                        throw std::runtime_error(
                                    "Error, body frame is end frame in AerodynamicAngleCalculator." );
                    }
                    else
                    {
                        rotationToFrame =
                                getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    break;
                default:
                    throw std::runtime_error(
                                "Error, index " + std::to_string( currentFrameIndex ) +
                                "not found in AerodynamicAngleCalculator." );
                }

                // Increment/decrement current frame.
                if( isTargetFrameUp )
                {
                    currentFrameIndex++;
                }
                else
                {
                    currentFrameIndex--;
                }
            }
        }

        // Set current rotation (as well as inverse).
        currentRotationMatrices_[ currentRotationPair ] = rotationToFrame;
        currentRotationMatrices_[ std::make_pair( targetFrame, originalFrame ) ] =
                rotationToFrame.inverse( );
    }
    else
    {
        rotationToFrame = currentRotationMatrices_.at( currentRotationPair );
    }

    return rotationToFrame;
}

//! Function to get a single orientation angle.
double AerodynamicAngleCalculator::getAerodynamicAngle(
        const AerodynamicsReferenceFrameAngles angleId )
{
    double angleValue = TUDAT_NAN;
    if( currentAerodynamicAngles_.count( angleId ) == 0 )
    {
        throw std::runtime_error( "Error in AerodynamicAngleCalculator, angle " +
                                  std::to_string( angleId ) + " not found" );
    }
    else
    {
        angleValue = currentAerodynamicAngles_.at( angleId );
    }
    return angleValue;
}

//! Function to set the trajectory<->body-fixed orientation angles.
void AerodynamicAngleCalculator::setOrientationAngleFunctions(
        const std::function< double( ) > angleOfAttackFunction,
        const std::function< double( ) > angleOfSideslipFunction,
        const std::function< double( ) > bankAngleFunction,
        const std::function< void( const double ) > angleUpdateFunction )
{
    if( !( angleOfAttackFunction == nullptr ) )
    {
        if( !( angleOfAttackFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing angle of attack function in AerodynamicAngleCalculator" << std::endl;
        }
        angleOfAttackFunction_ = angleOfAttackFunction;
    }

    if( !( angleOfSideslipFunction == nullptr ) )
    {
        if( !( angleOfSideslipFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing angle of sideslip function in AerodynamicAngleCalculator" << std::endl;
        }
        angleOfSideslipFunction_ = angleOfSideslipFunction;
    }

    if( !( bankAngleFunction == nullptr ) )
    {
        if( !( bankAngleFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing bank angle function in AerodynamicAngleCalculator" << std::endl;
        }
        bankAngleFunction_ = bankAngleFunction;
    }

    if( !( angleUpdateFunction == nullptr ) )
    {
        if( !( angleUpdateFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing aerodynamic angle update function in AerodynamicAngleCalculator" << std::endl;
        }
        angleUpdateFunction_ = angleUpdateFunction;
    }
}

//! Function to set constant trajectory<->body-fixed orientation angles.
void AerodynamicAngleCalculator::setOrientationAngleFunctions(
        const double angleOfAttack,
        const double angleOfSideslip,
        const double bankAngle )
{
    std::function< double( ) > angleOfAttackFunction =
            ( ( angleOfAttack == angleOfAttack ) ? [ = ]( ){ return angleOfAttack; } : std::function< double( ) >( ) );
    std::function< double( ) > angleOfSideslipFunction =
            ( ( angleOfSideslip == angleOfSideslip ) ? [ = ]( ){ return angleOfSideslip; } : std::function< double( ) >( ) );
    std::function< double( ) > bankAngleFunction =
            ( ( bankAngle == bankAngle ) ? [ = ]( ){ return bankAngle; }: std::function< double( ) >( ) );
    setOrientationAngleFunctions( angleOfAttackFunction, angleOfSideslipFunction, bankAngleFunction );
}

//! Get a function to transform aerodynamic force from local to propagation frame.
std::function< Eigen::Vector3d( const Eigen::Vector3d& ) >
getAerodynamicForceTransformationFunction(
        const std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator,
        const AerodynamicsReferenceFrames accelerationFrame,
        const std::function< Eigen::Quaterniond( ) > bodyFixedToInertialFrameFunction,
        const AerodynamicsReferenceFrames propagationFrame )
{
    std::function< Eigen::Vector3d( const Eigen::Vector3d& ) > transformationFunction;

    // If propagation frame is the inertial frame, use bodyFixedToInertialFrameFunction.
    if( propagationFrame == inertial_frame )
    {
        std::vector< std::function< Eigen::Vector3d( const Eigen::Vector3d& ) > > rotationsList;

        // Get accelerationFrame to corotating frame transformation.
        std::function< Eigen::Quaterniond( ) > firstRotation =
                std::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, corotating_frame );
        rotationsList.push_back(
                    std::bind( &transformVectorFromQuaternionFunction,
                                 std::placeholders::_1, firstRotation ) );

        // Add corotating to inertial frame.
        rotationsList.push_back(
                    std::bind( &transformVectorFromQuaternionFunction,
                                 std::placeholders::_1, bodyFixedToInertialFrameFunction ) );

        // Create transformation function.
        transformationFunction = std::bind( &transformVectorFromVectorFunctions,
                                              std::placeholders::_1, rotationsList );
    }
    else
    {
        // Get accelerationFrame to propagationFrame frame transformation directly.
        std::function< Eigen::Quaterniond( ) > rotationFunction =
                std::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, propagationFrame );

        // Create transformation function.
        transformationFunction = std::bind( &transformVectorFromQuaternionFunction, std::placeholders::_1,
                                              rotationFunction );
    }

    return transformationFunction;
}

//! Function to update the aerodynamic angles to current time.
void AerodynamicAnglesClosure::updateAngles( const double currentTime )
{
    // Retrieve rotation matrix that is to be converted to orientation angles.
    currentRotationFromBodyToTrajectoryFrame_ =
            ( ( imposedRotationFromInertialToBodyFixedFrame_( currentTime ).toRotationMatrix( ) *
                aerodynamicAngleCalculator_->getRotationQuaternionBetweenFrames(
                    trajectory_frame, inertial_frame ).toRotationMatrix( ) ) ).transpose( );

    // Compute associated Euler angles and set as orientation angles.
    Eigen::Vector3d eulerAngles = basic_mathematics::get132EulerAnglesFromRotationMatrix(
                currentRotationFromBodyToTrajectoryFrame_ );
    currentBankAngle_ = eulerAngles( 0 );
    currentAngleOfSideslip_ = eulerAngles( 1 );
    currentAngleOfAttack_ = -eulerAngles( 2 );
}

//! Function to make aerodynamic angle computation consistent with imposed body-fixed to inertial rotation.
void setAerodynamicDependentOrientationCalculatorClosure(
        const std::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    std::shared_ptr< AerodynamicAnglesClosure > aerodynamicAnglesClosure =
            std::make_shared< AerodynamicAnglesClosure >(
                imposedRotationFromInertialToBodyFixedFrame, aerodynamicAngleCalculator );
    aerodynamicAngleCalculator->setOrientationAngleFunctions(
                std::bind( &AerodynamicAnglesClosure::getCurrentAngleOfAttack, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::getCurrentAngleOfSideslip, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::getCurrentBankAngle, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::updateAngles, aerodynamicAnglesClosure, std::placeholders::_1 ) );
}

//! Function to make aerodynamic angle computation consistent with existing DependentOrientationCalculator
void setAerodynamicDependentOrientationCalculatorClosure(
        std::shared_ptr< DependentOrientationCalculator > dependentOrientationCalculator,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    std::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame =
            std::bind( &DependentOrientationCalculator::computeAndGetRotationToLocalFrame, dependentOrientationCalculator, std::placeholders::_1 );
    setAerodynamicDependentOrientationCalculatorClosure(
                imposedRotationFromInertialToBodyFixedFrame,
                aerodynamicAngleCalculator );
}

//! Function to make aerodynamic angle computation consistent with existing rotational ephemeris
void setAerodynamicDependentOrientationCalculatorClosure(
        std::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    setAerodynamicDependentOrientationCalculatorClosure(
                std::bind( &ephemerides::RotationalEphemeris::getRotationToTargetFrame,
                             rotationalEphemeris, std::placeholders::_1 ),
                aerodynamicAngleCalculator );
}

} // namespace reference_frames

} // namespace tudat


#include "refill/measurement_models/linear_measurement_model.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

namespace refill {

class LinearMeasurementModelTest : public ::testing::Test {
 public:
  LinearMeasurementModelTest()
      : measurement_noise_{2},
        measurement_mapping_{Eigen::Matrix2d::Identity()},
        noise_mapping_{Eigen::Matrix2d::Identity()} {}

  GaussianDistribution measurement_noise_;

  Eigen::Matrix2d measurement_mapping_;
  Eigen::Matrix2d noise_mapping_;
};

TEST_F(LinearMeasurementModelTest, ConstructorTest) {
  LinearMeasurementModel measurement_model_1;

  EXPECT_EQ(0, measurement_model_1.getStateDim());
  EXPECT_EQ(0, measurement_model_1.getMeasurementDim());
  EXPECT_EQ(0, measurement_model_1.getNoiseDim());

  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearMeasurementModel measurement_model_2(Eigen::Matrix2d::Identity(),
                                             measurement_noise);

  EXPECT_EQ(2, measurement_model_2.getStateDim());
  EXPECT_EQ(2, measurement_model_2.getMeasurementDim());
  EXPECT_EQ(2, measurement_model_2.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model_2.getMeasurementMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), measurement_model_2.getNoiseMapping());

  LinearMeasurementModel measurement_model_3(Eigen::Matrix2d::Identity(),
                                             measurement_noise,
                                             Eigen::Matrix2d::Ones());

  EXPECT_EQ(2, measurement_model_3.getStateDim());
  EXPECT_EQ(2, measurement_model_3.getMeasurementDim());
  EXPECT_EQ(2, measurement_model_3.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model_3.getMeasurementMapping());
  EXPECT_EQ(Eigen::Matrix2d::Ones(), measurement_model_3.getNoiseMapping());
}

TEST_F(LinearMeasurementModelTest, SetterTest) {
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearMeasurementModel measurement_model;

  measurement_model.setModelParameters(Eigen::Matrix2d::Identity(),
                                             measurement_noise);

  EXPECT_EQ(2, measurement_model.getStateDim());
  EXPECT_EQ(2, measurement_model.getMeasurementDim());
  EXPECT_EQ(2, measurement_model.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model.getMeasurementMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), measurement_model.getNoiseMapping());

  measurement_noise.setDistributionParameters(Eigen::Vector3d::Zero(),
                                              Eigen::Matrix3d::Identity());

  measurement_model.setModelParameters(Eigen::Matrix3d::Identity(),
                                             measurement_noise,
                                             Eigen::Matrix3d::Ones());

  EXPECT_EQ(3, measurement_model.getStateDim());
  EXPECT_EQ(3, measurement_model.getMeasurementDim());
  EXPECT_EQ(3, measurement_model.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix3d::Identity(),
            measurement_model.getMeasurementMapping());
  EXPECT_EQ(Eigen::Matrix3d::Ones(), measurement_model.getNoiseMapping());
}

TEST_F(LinearMeasurementModelTest, GetterTest) {
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearMeasurementModel measurement_model(Eigen::Matrix2d::Identity(),
                                           measurement_noise,
                                           Eigen::Matrix2d::Ones());

  EXPECT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model.getMeasurementMapping());
  EXPECT_EQ(Eigen::Matrix2d::Ones(), measurement_model.getNoiseMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model.getMeasurementJacobian(Eigen::Vector2d::Zero()));
  EXPECT_EQ(Eigen::Matrix2d::Ones(),
            measurement_model.getNoiseJacobian(Eigen::Vector2d::Zero()));
}

TEST_F(LinearMeasurementModelTest, ObservationTest) {
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearMeasurementModel measurement_model(Eigen::Matrix2d::Identity(),
                                           measurement_noise,
                                           Eigen::Matrix2d::Ones());

  EXPECT_EQ(
      Eigen::Vector2d::Constant(3.0),
      measurement_model.observe(Eigen::Vector2d::Ones(),
                                Eigen::Vector2d::Ones()));
}

TEST_F(LinearMeasurementModelTest, GetLikelihoodTest) {
  LinearMeasurementModel measurement_model(measurement_mapping_,
                                           measurement_noise_,
                                           noise_mapping_);

  Eigen::Vector2d measurement{Eigen::Vector2d::Constant(1)};
  Eigen::Vector2d state{Eigen::Vector2d::Constant(1)};

  double expected_likelihood{1.0 / (2.0 * M_PI)};

  EXPECT_EQ(expected_likelihood,
            measurement_model.getLikelihood(state, measurement));
}

TEST_F(LinearMeasurementModelTest, GetLikelihoodVectorizedTest) {
  LinearMeasurementModel measurement_model(measurement_mapping_,
                                           measurement_noise_,
                                           noise_mapping_);

  Eigen::Vector2d measurement{Eigen::Vector2d::Constant(1)};
  Eigen::Matrix2d sampled_state{Eigen::Matrix2d::Constant(1)};

  Eigen::Vector2d expected_likelihood{
      Eigen::Vector2d::Constant(1.0 / (2 * M_PI))};

  EXPECT_EQ(expected_likelihood, measurement_model.getLikelihoodVectorized(
                                     sampled_state, measurement));
}

}  // namespace refill


// Copyright (c) 2005-2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL$
// $Id$
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Sebastien Loriot, Sylvain Pion

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <CGAL/Regular_triangulation_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>
#include <boost/format.hpp>

namespace CGAL_alpha_shapes{

typedef CGAL::Exact_predicates_inexact_constructions_kernel       Kernel;
typedef CGAL::Regular_triangulation_vertex_base_2<Kernel>         Rvb;
typedef CGAL::Alpha_shape_vertex_base_2<Kernel,Rvb>               Vb;
typedef CGAL::Regular_triangulation_face_base_2<Kernel>           Rf;
typedef CGAL::Alpha_shape_face_base_2<Kernel,Rf>                  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>               Tds;
typedef CGAL::Regular_triangulation_2<Kernel,Tds>                 Regular;
typedef CGAL::Alpha_shape_2<Regular>                              Alpha_shape_2;

const std::string  sublabel[] = {
  "k-th Alpha-shape", "Help"
};

const std::string  helpmsg[] = {
  "Draw alpha-shape for the k-th critical alpha value"
};

class ASphapeIpelet
  : public CGAL::Ipelet_base<Kernel,2> {
public:
  ASphapeIpelet()
    : CGAL::Ipelet_base<Kernel,2>("Alpha-shapes",sublabel,helpmsg){}
  void protected_run(int);
};

void ASphapeIpelet::protected_run(int fn)
{
  if (fn==1) {
    show_help();
    return;
  }

  std::list<Weighted_point_2> LWP;

  std::list<Circle_2> cir_list;
  std::list<Point_2> pt_list;

  read_active_objects(
    CGAL::dispatch_or_drop_output<Point_2,Circle_2>(std::back_inserter(pt_list),
                                                    std::back_inserter(cir_list))
  );

  if (pt_list.empty() && cir_list.empty()) {
    print_error_message("No circle nor point selected");
    return;
  }

  for (std::list<Point_2>::iterator it=pt_list.begin();it!=pt_list.end();++it)
    LWP.push_back(Weighted_point_2(*it,0));
  for (std::list<Circle_2>::iterator it=cir_list.begin();it!=cir_list.end();++it)
    LWP.push_back(Weighted_point_2(it->center(),it->squared_radius()));

  Alpha_shape_2 A(LWP.begin(),LWP.end());
  int alpha=-1;
  int nb_ret;
  boost::tie(nb_ret,alpha)=request_value_from_user<int>((boost::format("# Spectral critical value (0-%d)") % A.number_of_alphas()).str() );
  if (nb_ret == -1) return;

  if(alpha<0 || (std::size_t) alpha>A.number_of_alphas()){
    print_error_message("Not a good value");
    return;
  }

  A.set_alpha(alpha==0?(std::max)(std::numeric_limits<double>::epsilon(),A.get_nth_alpha(0)/2.):
              (std::size_t) alpha==A.number_of_alphas()?A.get_nth_alpha(alpha-1)+1:A.get_nth_alpha(alpha-1)/2.+A.get_nth_alpha(alpha)/2.);
  for ( Alpha_shape_2::Alpha_shape_edges_iterator it=A.alpha_shape_edges_begin();it!=A.alpha_shape_edges_end();++it)
    draw_in_ipe(A.segment(*it));

  for (Alpha_shape_2::Finite_faces_iterator it=A.finite_faces_begin();it!=A.finite_faces_end();++it){
    if (A.classify(it)==Alpha_shape_2::INTERIOR){
      std::list<Point_2> LP;
      LP.push_back(Point_2(it->vertex(0)->point()));
      LP.push_back(Point_2(it->vertex(1)->point()));
      LP.push_back(Point_2(it->vertex(2)->point()));
      draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,true);
    }
  }
  group_selected_objects_();
  return;
}

}

CGAL_IPELET(CGAL_alpha_shapes::ASphapeIpelet)


#ifndef LIBRE_BOOK_MATRIX
#define LIBRE_BOOK_MATRIX

#include <iostream>
#include <vector>
#include <map>
#include <string>
#include <fstream>
#include <boost/multiprecision/cpp_int.hpp>

#include "primes/primes.hpp"

using namespace boost::multiprecision;
typedef number<cpp_int_backend<2048, 2048, unsigned_magnitude, unchecked, void>> uint2048_t;

namespace Libre {
	class BookMatrix {
		private:
			std::map<std::string, uint2048_t> words;
			std::vector<uint2048_t> matrix;
			Libre::Primes primes;
		public:
			BookMatrix(const std::string &);
			BookMatrix() = default;
			~BookMatrix() = default;
			void load_file(const std::string &);
			const std::map<std::string, uint2048_t> & get_words();
			bool verse_has_word(const std::string &, const size_t &);
			bool verse_has_mod_index(const uint2048_t &, const size_t &);
	};
}

#endif


#ifndef PYTHONIC_INCLUDE_NUMPY_SQRT_HPP
#define PYTHONIC_INCLUDE_NUMPY_SQRT_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/sqrt.hpp>
#include <cmath>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> sqrt(std::complex<T> const &val)
    {
      return std::sqrt(val);
    }
    template <class T>
    auto sqrt(T const &val) -> decltype(boost::simd::sqrt(val))
    {
      return boost::simd::sqrt(val);
    }
  }

#define NUMPY_NARY_FUNC_NAME sqrt
#define NUMPY_NARY_FUNC_SYM wrapper::sqrt
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <cmath>
#include "../include/array.h"
#include <iostream>
#include "GPU_Advection.h"
#include "BimocqSolver.h"
#include <boost/filesystem.hpp>

int main(int argc, char** argv) {
    uint ni;
    uint nj;
    uint nk;
    uint total_frame;
    float L;
    float h;
    float dt;
    float mapping_blend_coeff;
    float viscosity;
    float half_width;
    float smoke_rise;
    float smoke_drop;
    Scheme sim_scheme;
    string filepath = "../Out";
    boost::filesystem::create_directories(filepath);

    std::vector<Emitter> emitter_list;
    std::vector<Boundary> boundary_list;

    // 3D vortex collision example setup
    if (1)
    {
        // simulation resolution
	    ni = 100;
	    nj = 200;
	    nk = 200;
	    total_frame = 300;
	    // length in x direction
	    L = 0.2f;
	    // grid size for simulation
	    h = L / ni;
	    // time step
	    dt = 0.08f;
	    // smoke properties
	    smoke_rise = 0.f;
	    smoke_drop = 0.f;
	    viscosity = 1.0*1e-6;
	    // blend coefficient that will blend 1-level mapping result with 2-level mapping result
        // phi_t = blend_coeff * phi_curr + (1 - blend_coeff) * phi_prev
        mapping_blend_coeff = 1.f;
        // levelset half width, used when blending semi-lagrangian result near the boundary
        half_width = 3.f;
        // simulation scheme, semi-lagrangian, MacCormack, Reflection and BIMOCQ are implemented
        sim_scheme = BIMOCQ;
        auto vel_func_a = [](Vec3f pos)
        {
            Vec3f center(0.04f, 0.2f, 0.2f);
            Vec2f dir = Vec2f(pos[1] - center[1], pos[2] - center[2]);
            dir = normalized(dir);
            float theta = acos(dot(dir, Vec2f(1.f, 0.f)));
            float vel_x = 0.06f*(1.0f + 0.01f*cos(8.f*theta));
            float vel_y = 0.f;
            float vel_z = 0.f;
            return Vec3f(vel_x, vel_y, vel_z);
        };
        auto vel_func_b = [](Vec3f pos)
        {
            Vec3f center(0.16f, 0.201f, 0.2f);
            Vec2f dir = Vec2f(pos[1] - center[1], pos[2] - center[2]);
            dir = normalized(dir);
            float theta = acos(dot(dir, Vec2f(1.f, 0.f)));
            float vel_x = -0.06f*(1.0f + 0.01f*cos(8.f*theta));
            float vel_y = 0.f;
            float vel_z = 0.f;
            return Vec3f(vel_x, vel_y, vel_z);
        };
        openvdb::FloatGrid::Ptr sphere_sdf_a = openvdb::tools::createLevelSetSphere<openvdb::FloatGrid>(0.015f, openvdb::Vec3f(0.f,0.f,0.f), h, half_width);
        openvdb::FloatGrid::Ptr sphere_sdf_b = openvdb::tools::createLevelSetSphere<openvdb::FloatGrid>(0.015f, openvdb::Vec3f(0.f,0.f,0.f), h, half_width);
        Emitter e_sphere_a(10, 1.f, 50.f, Vec3f(0.04f, 0.2f, 0.2f), sphere_sdf_a, [](float frame)->Vec3f{return Vec3f(0.f, 0.f, 0.f);}, vel_func_a);
        Emitter e_sphere_b(10, 1.f, 50.f, Vec3f(0.16f, 0.201f, 0.2f), sphere_sdf_b, [](float frame)->Vec3f{return Vec3f(0.f, 0.f, 0.f);}, vel_func_b);
        emitter_list.push_back(e_sphere_a);
        emitter_list.push_back(e_sphere_b);
    }

	auto *myGPUmapper = new gpuMapper(ni, nj, nk, h);
	BimocqSolver mysolver(ni, nj, nk, L, viscosity, mapping_blend_coeff, sim_scheme, myGPUmapper);
	mysolver.setSmoke(smoke_rise, smoke_drop, emitter_list);
    mysolver.setBoundary(boundary_list);
	for (uint i = 0; i < total_frame; i++)
	{
        cout << "Frame " << i << " Starts !!!" << std::endl;
	    mysolver.updateBoundary(i, dt);
		mysolver.advance(i, dt);
        mysolver.outputResult(i, filepath);
    }
	return 0;
}


#pragma once

#include "deom.hpp"
#include "algebra.hpp"
#include <Eigen/Eigenvalues>
#include <experimental/random>

// syl： solve syl equation AX-XB=C
// note here A and B are both hermmitian, so it canbe deocomposite to URU^T VSV^T where R and S are diag matrices, the equation then canbe convert to URU^TX-XVSV^T=C multi U^T on left and V on right so we can get RU^TXV-U^TXVS=U^TCV, denote U^TXV as Y and U^TCV as D,so \sum_j (R_ij Y_jk - Y_ij S_jk) = D_ik and note that R and S are diag matrices so R_ii Y_ik - Y_ik S_kk = D_ik.
// SelfAdjointEigenSolver(A,B) -> R S U V
// Y_ik = (U^TCV)_ik / (R_ii - S_kk)
// X = UYV^T
// check AX-XB=C
// note that syl_complex will deal with no SelfAdjoint situation, but we will no use that in normal scenario.
// pref
// syl :1Gflops 10000000 at 3.6s
// syl_debug :1Gflops 10000000 at 3.85s
// syl_complex :2Gflops 10000000 at 5.4s

void syl(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  SelfAdjointEigenSolver<MatrixNcd> eigensolverA(A);
  SelfAdjointEigenSolver<MatrixNcd> eigensolverB(B);
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.transpose() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.transpose();
}

void syl_debug(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  SelfAdjointEigenSolver<MatrixNcd> eigensolverA(A);
  SelfAdjointEigenSolver<MatrixNcd> eigensolverB(B);
  if (eigensolverA.info() != Success || eigensolverB.info() != Success)
    abort();
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.transpose() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.transpose();
  if (is_valid(A * X - X * B - C)) {
    exit(1);
  }
}

void syl_complex(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  ComplexEigenSolver<MatrixNcd> eigensolverA(A);
  ComplexEigenSolver<MatrixNcd> eigensolverB(B);
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.inverse() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.inverse();
}

void syl_complex_debug(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  ComplexEigenSolver<MatrixNcd> eigensolverA(A);
  ComplexEigenSolver<MatrixNcd> eigensolverB(B);
  if (eigensolverA.info() != Success || eigensolverB.info() != Success)
    abort();
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.inverse() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.inverse();
  if (is_valid(A * X - X * B - C)) {
    exit(1);
  }
}

// int main(int argc, const char **argv) {
//   MatrixNcd A;
//   MatrixNcd B;
//   MatrixNcd C;
//   MatrixNcd X;
//   std::srand(std::time(0));
//   A(0, 0) = std::experimental::randint(-999, 999);
//   A(1, 1) = std::experimental::randint(-999, 999);
//   A(0, 1) = std::experimental::randint(-999, 999);
//   A(1, 0) = A(0, 1);

//   B(0, 0) = std::experimental::randint(-999, 999);
//   B(1, 1) = std::experimental::randint(-999, 999);
//   B(0, 1) = std::experimental::randint(-999, 999);
//   B(1, 0) = B(0, 1);

//   C(0, 0) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(1, 1) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(0, 1) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(1, 0) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   for (size_t i = 0; i < 10000000; i++) {
//     syl_complex(X, A, B, C);
//   }
//   return 0;
// }


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include "recti/recti.hpp"
// #include <boost/multiprecision/cpp_int.hpp>
#include <doctest/doctest.h>
#include <iostream>

using namespace recti;

TEST_CASE("vector2")
{
    // using boost::multiprecision::cpp_int;
    // static_assert(Integral<cpp_int>);
    const auto a = 3;
    const auto b = 4;
    const auto c = 5;
    const auto d = 6;
    // const auto f = -30;
    // const auto g = 4;
    // const auto z = 0;
    // const auto h = -g;

    const auto p = vector2 {a, b};
    const auto q = vector2 {c, d};

    CHECK(vector2 {8, 10} == (p + q));
    CHECK(vector2 {-2, -2} == (p - q));
    CHECK(vector2 {6, 8} == (p * 2));
    CHECK(vector2 {4, 5} == (p + q) / 2);
    CHECK(p != q);
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/generic/erf_kernel.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/zero.hpp>

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( erfc_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
    A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      A0 x =  bs::abs(a0);
      A0 xx =  bs::sqr(x);
      A0 lim1 = A0(0.65);
      A0 lim2 = A0(2.2);
      auto test0 = bs::is_ltz(a0);
      auto test1 = bs::is_less(x, lim1);
      A0 r1 = bs::Zero<A0>();
      std::size_t nb = bs::nbtrue(test1);
      if(nb > 0)
      {
        r1 = bs::oneminus(x*detail::erf_kernel<A0>::erf1(xx));
        if (nb >= A0::static_size)
          return bs::if_else(test0, bs::Two<A0>()-r1, r1);
      }
      auto test2 = bs::is_less(x, lim2);
      auto test3 = bs::logical_andnot(test2, test1);
      A0 ex = bs::exp(-xx);

      std::size_t nb1 = bs::nbtrue(test3);
      if(nb1 > 0)
      {
        A0 z = ex*detail::erf_kernel<A0>::erfc2(x);
        r1 = bs::if_else(test1, r1, z);
        nb += nb1;
        if (nb >= A0::static_size)
          return bs::if_else(test0, Two<A0>()-r1, r1);
      }
      A0 z =  ex*detail::erf_kernel<A0>::erfc3(x);
      r1 = bs::if_else(test2, r1, z);
#ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( is_equal(x, Inf<A0>()), r1);
#endif
      return  bs::if_else(test0, bs::Two<A0>()-r1, r1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( erfc_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      A0 x =  bs::abs(a0);
      auto test0 = bs::is_ltz(a0);
      A0 r1 = bs::Zero<A0>();
      auto test1 = bs::is_less(x, bs::Ratio<A0, 2, 3>());
      A0 z = x/inc(x);

      std::size_t nb = bs::nbtrue(test1);
      if(nb > 0)
      {
        r1 = detail::erf_kernel<A0>::erfc3(z);
        if (nb >= A0::static_size)
          return bs::if_else(test0, bs::Two<A0>()-r1, r1);
      }
      z -= A0(0.4);
      A0 r2 = exp(-sqr(x))*detail::erf_kernel<A0>::erfc2(z);
      r1 = if_else(test1, r1, r2);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( is_equal(x, Inf<A0>()), r1);
      #endif
      return bs::if_else(test0, bs::Two<A0>()-r1, r1);
    }
  };

} } }

#endif


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/LU>

// N.B. this would equally work with Eigen-types that are not predefined. For example replacing
// all occurrences of "Eigen::MatrixXd" with "MatD", with the following definition:
//
//  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatD;

// ----------------
// regular C++ code
// ----------------

Eigen::MatrixXd inv(const Eigen::MatrixXd &xs)
{
  return xs.inverse();
}

double det(const Eigen::MatrixXd &xs)
{
  return xs.determinant();
}

// ----------------
// Python interface
// ----------------

namespace py = pybind11;

PYBIND11_MODULE(example,m)
{
  m.doc() = "pybind11 example plugin";

  m.def("inv", &inv);

  m.def("det", &det);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/fma.hpp>
#include <boost/simd/include/functions/sqr.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/constants/real_splat.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

typedef float T;
typedef boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION> vT;
typedef vT::native_type nT;
#define C0 0x3c19c53b
#define C1 0x3b4c779c
#define C2 0x3cc821b5
#define C3 0x3d5ac5c9
#define C4 0x3e0896dd

extern "C"
{

/* horner0_case:
 *   vmulps  .LC0(%rip), %ymm0, %ymm1
 *   vaddps  .LC1(%rip), %ymm1, %ymm1
 *   vmulps  %ymm1, %ymm0, %ymm1
 *   vaddps  .LC2(%rip), %ymm1, %ymm1
 *   vmulps  %ymm1, %ymm0, %ymm1
 *   vaddps  .LC3(%rip), %ymm1, %ymm1
 *   vmulps  %ymm1, %ymm0, %ymm0
 *   vaddps  .LC4(%rip), %ymm0, %ymm0
 *   ret
 */
BOOST_FORCEINLINE vT horner0_case_impl(vT const& x)
{
  return nt2::horner< NT2_HORNER_COEFF(T, 5, (C0, C1, C2, C3, C4)) >(x);
}
BOOST_NOINLINE nT horner0_case(nT x_)
{
  vT x = x_;
  return horner0_case_impl(x);
}

BOOST_FORCEINLINE vT horner1_case_impl(vT const& x)
{
  using boost::simd::fma;
  using boost::simd::single_constant;
  return fma(x, fma(x, fma(x, fma(x, single_constant<vT, C0>(), single_constant<vT, C1>()), single_constant<vT, C2>()), single_constant<vT, C3>()), single_constant<vT, C4>());
}
BOOST_NOINLINE nT horner1_case(nT x_)
{
  vT x = x_;
  return horner1_case_impl(x);
}

BOOST_FORCEINLINE vT horner2_case_impl(vT const& x)
{
  using boost::simd::fma;
  using boost::simd::single_constant;

  vT y = single_constant<vT, C0>();
  vT c = single_constant<vT, C1>();

  y = fma(x, y, c);
  c = single_constant<vT, C2>();
  y = fma(x, y, c);
  c = single_constant<vT, C3>();
  y = fma(x, y, c);
  c = single_constant<vT, C4>();
  y = fma(x, y, c);

  return y;
}
BOOST_NOINLINE nT horner2_case(nT x_)
{
  vT x = x_;
  return horner2_case_impl(x);
}

/* estrin_case:
 *   vmulps  .LC1(%rip), %ymm0, %ymm1
 *   vmulps  .LC3(%rip), %ymm0, %ymm2
 *   vmulps  %ymm0, %ymm0, %ymm0
 *   vaddps  .LC2(%rip), %ymm1, %ymm1
 *   vaddps  .LC4(%rip), %ymm2, %ymm2
 *   vmulps  %ymm1, %ymm0, %ymm1
 *   vmulps  %ymm0, %ymm0, %ymm0
 *   vaddps  %ymm2, %ymm1, %ymm1
 *   vmulps  .LC0(%rip), %ymm0, %ymm0
 *   vaddps  %ymm1, %ymm0, %ymm0
 *   ret
 */
BOOST_FORCEINLINE vT estrin_case_impl(vT const& x)
{
  using boost::simd::fma;
  using boost::simd::sqr;
  using boost::simd::single_constant;

  return fma(sqr(x)*sqr(x), single_constant<vT, C0>(), fma(sqr(x), fma(x, single_constant<vT, C1>(), single_constant<vT, C2>()), fma(x, single_constant<vT, C3>(), single_constant<vT, C4>())));
}
BOOST_NOINLINE nT estrin_case(nT x_)
{
  vT x = x_;
  return estrin_case_impl(x);
}

}

NT2_TEST_CASE(consistency_test)
{
  vT x = boost::simd::splat<vT>(10.2);

  vT y1 = horner0_case(x);
  vT y2 = horner1_case(x);
  vT y3 = horner2_case(x);
  vT y4 = estrin_case(x);

  NT2_TEST_ULP_EQUAL(y1, y2, 0.5);
  NT2_TEST_ULP_EQUAL(y1, y3, 0.5);
  NT2_TEST_ULP_EQUAL(y1, y4, 0.5);
}


//
// Created by James Noeckel on 12/2/19.
//

#include <Eigen/Dense>
#include <geometry/primitives3/compute_basis.h>
#include "BoundedPlane.h"
#include "intersect_planes.h"
#include "geometry/csg1d.h"

using namespace Eigen;

BoundedPlane::BoundedPlane(std::shared_ptr<Primitive> shape, const Ref<const Matrix<double, 3, 3>> &basis, double offset) : shapes_(1), basis_(basis), offset_(offset) {
    shapes_[0] = (std::move(shape));
    setCurrentShape(0);
}

BoundedPlane::BoundedPlane(const Ref< const Eigen::Matrix<double, 3, 1>> &normal, double offset) : offset_(offset) {
    basis_.row(2) = normal.transpose();
    basis_.block<2, 3>(0, 0) = compute_basis(normal);
}

void BoundedPlane::changeBasis(const Ref< const Eigen::Matrix2d> &newbasis) {
    basis_.block<2, 3>(0, 0) = newbasis * basis_.block<2, 3>(0, 0);
}

void BoundedPlane::flip() {
    basis_.row(2) = -basis_.row(2);
    basis_.row(0) = -basis_.row(0);
    offset_ = -offset_;
}

double BoundedPlane::offset() const {
    return offset_;
}

Vector3d BoundedPlane::normal() const {
    return basis_.row(2).transpose();
}

MatrixX3d BoundedPlane::points3D() const {
    MatrixX2d pts = curr_shape_ptr_->points();
    return points3D(pts);
}

MatrixX3d BoundedPlane::points3D(const Ref<const MatrixX2d> &points) const {
    return (points * basis_.block<2, 3>(0, 0)).rowwise() - offset_ * basis_.row(2);
}

MatrixX2d BoundedPlane::project(const Ref<const MatrixX3d> &points) const {
    return points * basis_.block<2, 3>(0, 0).transpose();
}

VectorXd BoundedPlane::normalDistance(const Eigen::Ref<const Eigen::MatrixX3d> &points) const {
    return (points * basis_.row(2).transpose()).array() + offset_;
}

const Matrix<double, 3, 3> &BoundedPlane::basis() const {
    return basis_;
}

bool BoundedPlane::contains(const Ref<const Vector2d> &point, double margin) const {
    return curr_shape_ptr_->contains(point, margin);
}

bool BoundedPlane::contains3D(const Ref<const Vector3d> &point, double threshold, double margin, double offset) const {
    if (threshold < 0 || std::abs(basis_.row(2) * point + offset_ - offset) <= threshold) {
        RowVector2d p2d = project(point.transpose());
        return contains(p2d.transpose(), margin);
    }
    return false;
}

bool BoundedPlane::intersectRay(const Ref<const Vector3d> &ray_origin, const Ref<const Vector3d> &ray_direction, double &t, bool ignore_shape, double margin) const {
    Vector3d plane_center = - offset_ * basis_.row(2).transpose();
    Vector3d offset = plane_center - ray_origin;
    t = (-offset_- basis_.row(2) * ray_origin)/(basis_.row(2) * ray_direction);
    if (t > 0) {
        if (!ignore_shape && hasShape()) {
            Vector3d projected_pt = ray_origin + t * ray_direction;
            return contains(project(projected_pt.transpose()).transpose());
        } else {
            return true;
        }
    }
    return false;
}

void BoundedPlane::intersectHelper(const Eigen::Vector3d &p, const Eigen::Vector3d &d, MultiRay3d &outRay, double margin) const {
    outRay.o = p;
    outRay.d = d;
    if (hasShape()) {
        Matrix<double, 2, 3> pd;
        pd << p.transpose(), d.transpose();
        Matrix<double, 2, 2> pd2d = project(pd);
        Ray2d ray2d(pd2d.row(0).transpose(), pd2d.row(1).transpose());
        auto intersections = curr_shape_ptr_->intersect(ray2d);
        double intersectionLength = 0;
        if (!intersections.empty() && intersections.size() % 2 == 0) {
            for (size_t j = 0; j < intersections.size(); j += 2) {
                intersectionLength += intersections[j + 1].t - intersections[j].t;
            }
        }
        if (margin > 0) {
            Eigen::Vector2d n(pd2d.row(1).y(), -pd2d.row(1).x());
            for (int i = -1; i <= 1; i += 2) {
                auto offsetIntersections = curr_shape_ptr_->intersect(
                        Ray2d(pd2d.row(0).transpose() + (i * margin) * n, pd2d.row(1).transpose()));
                if (!offsetIntersections.empty() && offsetIntersections.size() % 2 == 0) {
                    double offsetLength = 0;
                    for (int j = 0; j < offsetIntersections.size(); j += 2) {
                        offsetLength += offsetIntersections[j + 1].t - offsetIntersections[j].t;
                    }
                    if (offsetLength > intersectionLength) {
                        intersectionLength = offsetLength;
                        intersections = std::move(offsetIntersections);
                    }
                }
            }
        }
        if (!intersections.empty() && intersections.size() % 2 == 0) {
            for (size_t i = 0; i < intersections.size(); i += 2) {
                outRay.ranges.emplace_back(intersections[i].t, intersections[i + 1].t);
            }
        }
    } else {
        outRay.ranges.emplace_back(std::numeric_limits<double>::lowest(), std::numeric_limits<double>::max());
    }
}

bool BoundedPlane::intersect(const BoundedPlane &other, MultiRay3d &outRay, double margin) const {
    // get point-vector line intersection of planes
    Vector3d p;
    Vector3d d;
    intersect_planes(basis_.row(2), other.basis_.row(2), offset_, other.offset_, p, d);
    MultiRay3d rayThis;
    intersectHelper(p, d, rayThis, margin);
    MultiRay3d rayOther;
    other.intersectHelper(p, d, rayOther, margin);
    outRay = rayThis;
    outRay.ranges = csg1d(rayThis.ranges, rayOther.ranges);
    if (margin > 0) {
        //stitch together ranges closer than margin, and remove ranges smaller than margin
        std::vector<std::pair<double, double>> newRanges;
        newRanges.reserve(outRay.size());
        for (size_t i = 0; i < outRay.size(); ++i) {
            if (i < outRay.size() - 1 && outRay.ranges[i + 1].first - outRay.ranges[i].second < margin) {
                newRanges.emplace_back(outRay.ranges[i].first, outRay.ranges[i + 1].second);
                ++i;
                continue;
            } else {
                newRanges.push_back(outRay.ranges[i]);
            }
        }
        newRanges.erase(std::remove_if(newRanges.begin(), newRanges.end(),
                                       [=](const auto &range) { return range.second - range.first < margin; }),
                        newRanges.end());
        outRay.ranges = std::move(newRanges);
    }
    return !outRay.ranges.empty();

}

void BoundedPlane::addShape(int idx, std::shared_ptr<Primitive> shape) {
    shapes_[idx] = std::move(shape);
}

void BoundedPlane::setCurrentShape(int idx) {
    if (shapes_.find(idx) != shapes_.end()) {
        curr_shape_ptr_ = shapes_[idx].get();
    } else {
        curr_shape_ptr_ = nullptr;
    }
    curr_shape_id_ = idx;
}

void BoundedPlane::clearCurrentShape() {
    curr_shape_ptr_ = nullptr;
}

void BoundedPlane::clearShapes() {

}

int BoundedPlane::getCurrentShape() const {
    return curr_shape_id_;
}

int BoundedPlane::getNumShapes() const {
    return shapes_.size();
}

void BoundedPlane::serialize(std::ostream &o) const {
    Eigen::MatrixXd basisd = basis();
    basisd.resize(1, 9);
    o << "<plane basis=\"" << basisd << "\" offset=\"" << offset() << "\">" << std::endl;
//    if (hasShape())
//        o << *curr_shape_ptr_ << std::endl;
    o << "</plane>";
}

bool BoundedPlane::overlap(const BoundedPlane &other, double threshold, double margin, double offset) const {
    //Eigen::MatrixX2d projected = project(other.points3D());
    Eigen::MatrixX3d points = other.points3D();
    for (int i=0; i < points.rows(); i++) {
        if (contains3D(points.row(i).transpose(), threshold, margin, offset)) {
            return true;
        }
    }
    points = points3D();
    for (int i=0; i < points.rows(); i++) {
        if (other.contains3D((points.row(i) + basis_.row(2) * offset).transpose(), threshold, margin)) {
            return true;
        }
    }
    return false;
}

const Primitive &BoundedPlane::getShape(int idx) const {
    return *(shapes_.find(idx)->second);
}

Primitive &BoundedPlane::getShape(int idx) {
    return *(shapes_.find(idx)->second);
}

bool BoundedPlane::hasShape(int idx) const {
    return shapes_.find(idx) != shapes_.end();
}

bool BoundedPlane::hasShape() const {
    return curr_shape_ptr_ != nullptr;
}

std::ostream &operator<<(std::ostream &o, const BoundedPlane &plane) {
    plane.serialize(o);
    return o;
}


#ifndef RANDOM_HPP_INCLUDED
#define RANDOM_HPP_INCLUDED

#include "e8core/plugin/plugin.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <stdint.h>
#include <time.h>

/**E8.Include random.hpp
 */

const uint32_t RANDOM_MAX = 4294967295ul; /*!< 2^32 - 1*/

/**E8.Class RandomNumberGenerator RandomNumberGenerator|ГенераторСлучайныхЧисел
 */
class RandomNumberGenerator {

    boost::random::mt19937 gen;

public:
    RandomNumberGenerator() : gen(time(0)) {}
    RandomNumberGenerator(long seed) : gen(seed) {}

    uint32_t
    next(uint32_t lo = 0, uint32_t hi = RANDOM_MAX)
    {
        boost::random::uniform_int_distribution<> dist(lo, hi);
        return dist(gen);
    }

    /**E8.Constructor [IN:Зерно]
     */
    static RandomNumberGenerator* Constructor(E8_IN Seed);

    /**E8.Method RandomNumber|СлучайноеЧисло int [IN:LO] [IN:HI]
     */
    long RandomNumber(E8_IN LO, E8_IN HI);

};
/*E8.EndClass*/

#endif // RANDOM_HPP_INCLUDED