/* Author: Wolfgang Bangerth, 1999, Guido Kanschat, 2011 */

/*    $Id: step-3.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 1999-2004, 2006-2008, 2010-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Many new include files}

// These include files are already known to you. They declare the classes
// which handle triangulations and enumeration of degrees of freedom:
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
// And this is the file in which the functions are declared that create grids:
#include <deal.II/grid/grid_generator.h>

// The next three files contain classes which are needed for loops over all
// cells and to get the information from the cell objects. The first two have
// been used before to get geometric information from cells; the last one is
// new and provides information about the degrees of freedom local to a cell:
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>

// In this file contains the description of the Lagrange interpolation finite
// element:
#include <deal.II/fe/fe_q.h>

// And this file is needed for the creation of sparsity patterns of sparse
// matrices, as shown in previous examples:
#include <deal.II/dofs/dof_tools.h>

// The next two file are needed for assembling the matrix using quadrature on
// each cell. The classes declared in them will be explained below:
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>

// The following three include files we need for the treatment of boundary
// values:
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

// We're now almost to the end. The second to last group of include files is
// for the linear algebra which we employ to solve the system of equations
// arising from the finite element discretization of the Laplace equation. We
// will use vectors and full matrices for assembling the system of equations
// locally on each cell, and transfer the results into a sparse matrix. We
// will then use a Conjugate Gradient solver to solve the problem, for which
// we need a preconditioner (in this program, we use the identity
// preconditioner which does nothing, but we need to include the file anyway):
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

// Finally, this is for output to a file and to the console:
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>

// ...and this is to import the deal.II namespace into the global scope:
using namespace dealii;

// @sect3{The Step3 class}

// Instead of the procedural programming of previous examples, we encapsulate
// everything into a class for this program. The class consists of functions
// which each perform certain aspects of a finite element program, a `main'
// function which controls what is done first and what is done next, and a
// list of member variables.

// The public part of the class is rather short: it has a constructor and a
// function `run' that is called from the outside and acts as something like
// the `main' function: it coordinates which operations of this class shall be
// run in which order. Everything else in the class, i.e. all the functions
// that actually do anything, are in the private section of the class:
class Step3
{
public:
  Step3 ();

  void run ();

  // Then there are the member functions that mostly do what their names
  // suggest and whose have been discussed in the introduction already. Since
  // they do not need to be called from outside, they are made private to this
  // class.

private:
  void make_grid ();
  void setup_system ();
  void assemble_system ();
  void solve ();
  void output_results () const;

  // And finally we have some member variables. There are variables describing
  // the triangulation and the global numbering of the degrees of freedom (we
  // will specify the exact polynomial degree of the finite element in the
  // constructor of this class)...
  Triangulation<2>     triangulation;
  FE_Q<2>              fe;
  DoFHandler<2>        dof_handler;

  // ...variables for the sparsity pattern and values of the system matrix
  // resulting from the discretization of the Laplace equation...
  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  // ...and variables which will hold the right hand side and solution
  // vectors.
  Vector<double>       solution;
  Vector<double>       system_rhs;
};

// @sect4{Step3::Step3}

// Here comes the constructor. It does not much more than first to specify
// that we want bi-linear elements (denoted by the parameter to the finite
// element object, which indicates the polynomial degree), and to associate
// the dof_handler variable to the triangulation we use. (Note that the
// triangulation isn't set up with a mesh at all at the present time, but the
// DoFHandler doesn't care: it only wants to know which triangulation it will
// be associated with, and it only starts to care about an actual mesh once
// you try to distribute degree of freedom on the mesh using the
// distribute_dofs() function.) All the other member variables of the Step3
// class have a default constructor which does all we want.
Step3::Step3 ()
  :
  fe (1),
  dof_handler (triangulation)
{}

// @sect4{Step3::make_grid}

// Now, the first thing we've got to do is to generate the triangulation on
// which we would like to do our computation and number each vertex with a
// degree of freedom. We have seen this in the previous examples before.
void Step3::make_grid ()
{
  // First create the grid and refine all cells five times. Since the initial
  // grid (which is the square [-1,1]x[-1,1]) consists of only one cell, the
  // final grid has 32 times 32 cells, for a total of 1024.
  GridGenerator::hyper_cube (triangulation, -1, 1);
  triangulation.refine_global (5);
  // Unsure that 1024 is the correct number?  Let's see: n_active_cells
  // returns the number of active cells:
  std::cout << "Number of active cells: "
            << triangulation.n_active_cells()
            << std::endl;
  // Here, by active we mean the cells that aren't refined any further.  We
  // stress the adjective `active', since there are more cells, namely the
  // parent cells of the finest cells, their parents, etc, up to the one cell
  // which made up the initial grid. Of course, on the next coarser level, the
  // number of cells is one quarter that of the cells on the finest level,
  // i.e. 256, then 64, 16, 4, and 1. We can get the total number of cells
  // like this:
  std::cout << "Total number of cells: "
            << triangulation.n_cells()
            << std::endl;
  // Note the distinction between n_active_cells() and n_cells().
}

// @sect4{Step3::setup_system}

// Next we enumerate all the degrees of freedom and set up matrix and vector
// objects to hold the system data. Enumerating is done by using
// DoFHandler::distribute_dofs(), as we have seen in the step-2 example. Since
// we use the FE_Q class and have set the polynomial degree to 1 in the
// constructor, i.e. bilinear elements, this associates one degree of freedom
// with each vertex. While we're at generating output, let us also take a look
// at how many degrees of freedom are generated:
void Step3::setup_system ()
{
  dof_handler.distribute_dofs (fe);
  std::cout << "Number of degrees of freedom: "
            << dof_handler.n_dofs()
            << std::endl;
  // There should be one DoF for each vertex. Since we have a 32 times 32
  // grid, the number of DoFs should be 33 times 33, or 1089.

  // As we have seen in the previous example, we set up a sparsity pattern by
  // first creating a temporary structure, tagging those entries that might be
  // nonzero, and then copying the data over to the SparsityPattern object
  // that can then be used by the system matrix.
  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, c_sparsity);
  sparsity_pattern.copy_from(c_sparsity);

  // Note that the SparsityPattern object does not hold the values of the
  // matrix, it only stores the places where entries are. The entries
  // themselves are stored in objects of type SparseMatrix, of which our
  // variable system_matrix is one.
  //
  // The distinction between sparsity pattern and matrix was made to allow
  // several matrices to use the same sparsity pattern. This may not seem
  // relevant here, but when you consider the size which matrices can have,
  // and that it may take some time to build the sparsity pattern, this
  // becomes important in large-scale problems if you have to store several
  // matrices in your program.
  system_matrix.reinit (sparsity_pattern);

  // The last thing to do in this function is to set the sizes of the right
  // hand side vector and the solution vector to the right values:
  solution.reinit (dof_handler.n_dofs());
  system_rhs.reinit (dof_handler.n_dofs());
}

// @sect4{Step3::assemble_system}

// The next step is to compute the entries of the matrix and right hand side
// that form the linear system from which we compute the solution. This is the
// central function of each finite element program and we have discussed the
// primary steps in the introduction already.
//
// The general approach to assemble matrices and vectors is to loop over all
// cells, and on each cell compute the contribution of that cell to the global
// matrix and right hand side by quadrature. The point to realize now is that
// we need the values of the shape functions at the locations of quadrature
// points on the real cell. However, both the finite element shape functions
// as well as the quadrature points are only defined on the reference
// cell. They are therefore of little help to us, and we will in fact hardly
// ever query information about finite element shape functions or quadrature
// points from these objects directly.
//
// Rather, what is required is a way to map this data from the reference cell
// to the real cell. Classes that can do that are derived from the Mapping
// class, though one again often does not have to deal with them directly:
// many functions in the library can take a mapping object as argument, but
// when it is omitted they simply resort to the standard bilinear Q1
// mapping. We will go this route, and not bother with it for the moment (we
// come back to this in step-10, step-11, and step-12).
//
// So what we now have is a collection of three classes to deal with: finite
// element, quadrature, and mapping objects. That's too much, so there is one
// type of class that orchestrates information exchange between these three:
// the FEValues class. If given one instance of each three of these objects
// (or two, and an implicit linear mapping), it will be able to provide you
// with information about values and gradients of shape functions at
// quadrature points on a real cell.
//
// Using all this, we will assemble the linear system for this problem in the
// following function:
void Step3::assemble_system ()
{
  // Ok, let's start: we need a quadrature formula for the evaluation of the
  // integrals on each cell. Let's take a Gauss formula with two quadrature
  // points in each direction, i.e. a total of four points since we are in
  // 2D. This quadrature formula integrates polynomials of degrees up to three
  // exactly (in 1D). It is easy to check that this is sufficient for the
  // present problem:
  QGauss<2>  quadrature_formula(2);
  // And we initialize the object which we have briefly talked about above. It
  // needs to be told which finite element we want to use, and the quadrature
  // points and their weights (jointly described by a Quadrature object). As
  // mentioned, we use the implied Q1 mapping, rather than specifying one
  // ourselves explicitly. Finally, we have to tell it what we want it to
  // compute on each cell: we need the values of the shape functions at the
  // quadrature points (for the right hand side $(\varphi,f)$), their
  // gradients (for the matrix entries $(\nabla \varphi_i, \nabla
  // \varphi_j)$), and also the weights of the quadrature points and the
  // determinants of the Jacobian transformations from the reference cell to
  // the real cells.
  //
  // This list of what kind of information we actually need is given as a
  // collection of flags as the third argument to the constructor of
  // FEValues. Since these values have to be recomputed, or updated, every
  // time we go to a new cell, all of these flags start with the prefix
  // update_ and then indicate what it actually is that we want
  // updated. The flag to give if we want the values of the shape functions
  // computed is #update_values; for the gradients it is
  // #update_gradients. The determinants of the Jacobians and the quadrature
  // weights are always used together, so only the products (Jacobians times
  // weights, or short JxW) are computed; since we need them, we
  // have to list #update_JxW_values as well:
  FEValues<2> fe_values (fe, quadrature_formula,
                         update_values | update_gradients | update_JxW_values);
  // The advantage of this approach is that we can specify what kind of
  // information we actually need on each cell. It is easily understandable
  // that this approach can significant speed up finite element computations,
  // compared to approaches where everything, including second derivatives,
  // normal vectors to cells, etc are computed on each cell, regardless
  // whether they are needed or not.

  // For use further down below, we define two short cuts for values that will
  // be used very frequently. First, an abbreviation for the number of degrees
  // of freedom on each cell (since we are in 2D and degrees of freedom are
  // associated with vertices only, this number is four, but we rather want to
  // write the definition of this variable in a way that does not preclude us
  // from later choosing a different finite element that has a different
  // number of degrees of freedom per cell, or work in a different space
  // dimension).
  //
  // Secondly, we also define an abbreviation for the number of quadrature
  // points (here that should be four). In general, it is a good idea to use
  // their symbolic names instead of hard-coding these number even if you know
  // them, since you may want to change the quadrature formula and/or finite
  // element at some time; the program will just work with these changes,
  // without the need to change anything in this function.
  //
  // The shortcuts, finally, are only defined to make the following loops a
  // bit more readable. You will see them in many places in larger programs,
  // and `dofs_per_cell' and `n_q_points' are more or less by convention the
  // standard names for these purposes:
  const unsigned int   dofs_per_cell = fe.dofs_per_cell;
  const unsigned int   n_q_points    = quadrature_formula.size();

  // Now, we said that we wanted to assemble the global matrix and vector
  // cell-by-cell. We could write the results directly into the global matrix,
  // but this is not very efficient since access to the elements of a sparse
  // matrix is slow. Rather, we first compute the contribution of each cell in
  // a small matrix with the degrees of freedom on the present cell, and only
  // transfer them to the global matrix when the computations are finished for
  // this cell. We do the same for the right hand side vector. So let's first
  // allocate these objects (these being local objects, all degrees of freedom
  // are coupling with all others, and we should use a full matrix object
  // rather than a sparse one for the local operations; everything will be
  // transferred to a global sparse matrix later on):
  FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
  Vector<double>       cell_rhs (dofs_per_cell);

  // When assembling the contributions of each cell, we do this with the local
  // numbering of the degrees of freedom (i.e. the number running from zero
  // through dofs_per_cell-1). However, when we transfer the result into the
  // global matrix, we have to know the global numbers of the degrees of
  // freedom. When we query them, we need a scratch (temporary) array for
  // these numbers:
  std::vector<unsigned int> local_dof_indices (dofs_per_cell);

  // Now for the loop over all cells. We have seen before how this works, so
  // this should be familiar including the conventional names for these
  // variables:
  DoFHandler<2>::active_cell_iterator
  cell = dof_handler.begin_active(),
  endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      // We are now sitting on one cell, and we would like the values and
      // gradients of the shape functions be computed, as well as the
      // determinants of the Jacobian matrices of the mapping between
      // reference cell and true cell, at the quadrature points. Since all
      // these values depend on the geometry of the cell, we have to have the
      // FEValues object re-compute them on each cell:
      fe_values.reinit (cell);

      // Next, reset the local cell's contributions to global matrix and
      // global right hand side to zero, before we fill them:
      cell_matrix = 0;
      cell_rhs = 0;

      // Then finally assemble the matrix: For the Laplace problem, the matrix
      // on each cell is the integral over the gradients of shape function i
      // and j. Since we do not integrate, but rather use quadrature, this is
      // the sum over all quadrature points of the integrands times the
      // determinant of the Jacobian matrix at the quadrature point times the
      // weight of this quadrature point. You can get the gradient of shape
      // function $i$ at quadrature point q_point by using
      // fe_values.shape_grad(i,q_point); this gradient is a
      // 2-dimensional vector (in fact it is of type Tensor@<1,dim@>, with
      // here dim=2) and the product of two such vectors is the scalar
      // product, i.e. the product of the two shape_grad function calls is the
      // dot product. This is in turn multiplied by the Jacobian determinant
      // and the quadrature point weight (that one gets together by the call
      // to FEValues::JxW() ). Finally, this is repeated for all shape
      // functions $i$ and $j$:
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
            cell_matrix(i,j) += (fe_values.shape_grad (i, q_point) *
                                 fe_values.shape_grad (j, q_point) *
                                 fe_values.JxW (q_point));

      // We then do the same thing for the right hand side. Here, the integral
      // is over the shape function i times the right hand side function,
      // which we choose to be the function with constant value one (more
      // interesting examples will be considered in the following
      // programs). Again, we compute the integral by quadrature, which
      // transforms the integral to a sum over all quadrature points of the
      // value of the shape function at that point times the right hand side
      // function, here the constant function equal to one, times the Jacobian
      // determinant times the weight of that quadrature point:
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          cell_rhs(i) += (fe_values.shape_value (i, q_point) *
                          1 *
                          fe_values.JxW (q_point));

      // Now that we have the contribution of this cell, we have to transfer
      // it to the global matrix and right hand side. To this end, we first
      // have to find out which global numbers the degrees of freedom on this
      // cell have. Let's simply ask the cell for that information:
      cell->get_dof_indices (local_dof_indices);

      // Then again loop over all shape functions i and j and transfer the
      // local elements to the global matrix. The global numbers can be
      // obtained using local_dof_indices[i]:
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          system_matrix.add (local_dof_indices[i],
                             local_dof_indices[j],
                             cell_matrix(i,j));

      // And again, we do the same thing for the right hand side vector.
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  // Now almost everything is set up for the solution of the discrete
  // system. However, we have not yet taken care of boundary values (in fact,
  // Laplace's equation without Dirichlet boundary values is not even uniquely
  // solvable, since you can add an arbitrary constant to the discrete
  // solution). We therefore have to do something about the situation.
  //
  // For this, we first obtain a list of the degrees of freedom on the
  // boundary and the value the shape function shall have there. For
  // simplicity, we only interpolate the boundary value function, rather than
  // projecting it onto the boundary. There is a function in the library which
  // does exactly this: VectorTools::interpolate_boundary_values(). Its
  // parameters are (omitting parameters for which default values exist and
  // that we don't care about): the DoFHandler object to get the global
  // numbers of the degrees of freedom on the boundary; the component of the
  // boundary where the boundary values shall be interpolated; the boundary
  // value function itself; and the output object.
  //
  // The component of the boundary is meant as follows: in many cases, you may
  // want to impose certain boundary values only on parts of the boundary. For
  // example, you may have inflow and outflow boundaries in fluid dynamics, or
  // clamped and free parts of bodies in deformation computations of
  // bodies. Then you will want to denote these different parts of the
  // boundary by different numbers and tell the interpolate_boundary_values
  // function to only compute the boundary values on a certain part of the
  // boundary (e.g. the clamped part, or the inflow boundary). By default, all
  // boundaries have the number `0', and since we have not changed that, this
  // is still so; therefore, if we give `0' as the desired portion of the
  // boundary, this means we get the whole boundary. If you have boundaries
  // with kinds of boundaries, you have to number them differently. The
  // function call below will then only determine boundary values for parts of
  // the boundary.
  //
  // The function describing the boundary values is an object of type Function
  // or of a derived class. One of the derived classes is ZeroFunction, which
  // describes (not unexpectedly) a function which is zero everywhere. We
  // create such an object in-place and pass it to the
  // VectorTools::interpolate_boundary_values() function.
  //
  // Finally, the output object is a list of pairs of global degree of freedom
  // numbers (i.e. the number of the degrees of freedom on the boundary) and
  // their boundary values (which are zero here for all entries). This mapping
  // of DoF numbers to boundary values is done by the std::map
  // class.
  std::map<unsigned int,double> boundary_values;
  VectorTools::interpolate_boundary_values (dof_handler,
                                            0,
                                            ZeroFunction<2>(),
                                            boundary_values);
  // Now that we got the list of boundary DoFs and their respective boundary
  // values, let's use them to modify the system of equations
  // accordingly. This is done by the following function call:
  MatrixTools::apply_boundary_values (boundary_values,
                                      system_matrix,
                                      solution,
                                      system_rhs);
}

// @sect4{Step3::solve}

// The following function simply solves the discretized equation. As the
// system is quite a large one for direct solvers such as Gauss elimination or
// LU decomposition, we use a Conjugate Gradient algorithm. You should
// remember that the number of variables here (only 1089) is a very small
// number for finite element computations, where 100.000 is a more usual
// number.  For this number of variables, direct methods are no longer usable
// and you are forced to use methods like CG.
void Step3::solve ()
{
  // First, we need to have an object that knows how to tell the CG algorithm
  // when to stop. This is done by using a SolverControl object, and as
  // stopping criterion we say: stop after a maximum of 1000 iterations (which
  // is far more than is needed for 1089 variables; see the results section to
  // find out how many were really used), and stop if the norm of the residual
  // is below $10^{-12}$. In practice, the latter criterion will be the one
  // which stops the iteration:
  SolverControl           solver_control (1000, 1e-12);
  // Then we need the solver itself. The template parameters to the SolverCG
  // class are the matrix type and the type of the vectors, but the empty
  // angle brackets indicate that we simply take the default arguments (which
  // are SparseMatrix@<double@> and
  // Vector@<double@>):
  SolverCG<>              solver (solver_control);

  // Now solve the system of equations. The CG solver takes a preconditioner
  // as its fourth argument. We don't feel ready to delve into this yet, so we
  // tell it to use the identity operation as preconditioner:
  solver.solve (system_matrix, solution, system_rhs,
                PreconditionIdentity());
  // Now that the solver has done its job, the solution variable contains the
  // nodal values of the solution function.
}

// @sect4{Step3::output_results}

// The last part of a typical finite element program is to output the results
// and maybe do some postprocessing (for example compute the maximal stress
// values at the boundary, or the average flux across the outflow, etc). We
// have no such postprocessing here, but we would like to write the solution
// to a file.
void Step3::output_results () const
{
  // To write the output to a file, we need an object which knows about output
  // formats and the like. This is the DataOut class, and we need an object of
  // that type:
  DataOut<2> data_out;
  // Now we have to tell it where to take the values from which it shall
  // write. We tell it which DoFHandler object to use, and the solution vector
  // (and the name by which the solution variable shall appear in the output
  // file). If we had more than one vector which we would like to look at in
  // the output (for example right hand sides, errors per cell, etc) we would
  // add them as well:
  data_out.attach_dof_handler (dof_handler);
  data_out.add_data_vector (solution, "solution");
  // After the DataOut object knows which data it is to work on, we have to
  // tell it to process them into something the back ends can handle. The
  // reason is that we have separated the frontend (which knows about how to
  // treat DoFHandler objects and data vectors) from the back end (which knows
  // many different output formats) and use an intermediate data format to
  // transfer data from the front- to the backend. The data is transformed
  // into this intermediate format by the following function:
  data_out.build_patches ();

  // Now we have everything in place for the actual output. Just open a file
  // and write the data into it, using GNUPLOT format (there are other
  // functions which write their data in postscript, AVS, GMV, or some other
  // format):
  std::ofstream output ("solution.gpl");
  data_out.write_gnuplot (output);
}

// @sect4{Step3::run}

// Finally, the last function of this class is the main function which calls
// all the other functions of the Step3 class. The order in which
// this is done resembles the order in which most finite element programs
// work. Since the names are mostly self-explanatory, there is not much to
// comment about:
void Step3::run ()
{
  make_grid ();
  setup_system();
  assemble_system ();
  solve ();
  output_results ();
}

// @sect3{The main function}

// This is the main function of the program. Since the concept of a main
// function is mostly a remnant from the pre-object era in C/C++ programming,
// it often does not much more than creating an object of the top-level class
// and calling its principle function. This is what is done here as well:
int main ()
{
  Step3 laplace_problem;
  laplace_problem.run ();

  return 0;
}


#ifndef ELASTY_CONSTRAINT_HPP
#define ELASTY_CONSTRAINT_HPP

#include <Eigen/Core>
#include <elasty/algorithm-type.hpp>
#include <elasty/particle.hpp>
#include <memory>
#include <vector>

namespace elasty
{
    enum class ConstraintType
    {
        Bilateral,
        Unilateral
    };

    class AbstractConstraint
    {
    public:
        AbstractConstraint(const std::vector<std::shared_ptr<Particle>>& particles,
                           const double                                  stiffness,  ///< for PBD
                           const double                                  compliance, ///< for XPBD
                           const double                                  delta_time  ///< for XPBD
                           )
            : m_stiffness(stiffness),
              m_lagrange_multiplier(0.0),
              m_compliance(compliance),
              m_delta_time(delta_time),
              m_particles(particles)
        {
        }

        /// \brief Calculates the constraint function value C(x).
        virtual double calculateValue() = 0;

        /// \brief Calculate the derivative of the constraint function grad C(x).
        ///
        /// \details As constraints can have different vector sizes, it will store the result to the passed raw buffer
        /// that should be allocated in the caller, rather than returning a dynamically allocated variable-length
        /// vector. This method does not check whether the buffer is adequately allocated, or not.
        virtual void calculateGrad(double* grad_C) = 0;

        /// \brief Manipulate the associated particles by projecting them to the constraint manifold.
        ///
        /// \details This method should be called by the core engine. As this method directly updates the predicted
        /// positions of the associated particles, it is intended to be used in a Gauss-Seidel-style solver.
        virtual void projectParticles(const AlgorithmType type) = 0;

        /// \brief Return the constraint type (i.e., either unilateral or bilateral).
        virtual ConstraintType getType() = 0;

        /// \brief Stiffness of this constraint, which should be in [0, 1].
        ///
        /// \details This value will not be used in XPBD; instead, the compliance value will be used.
        double m_stiffness;

        /// \brief Lagrange multipler for this constraint, used in XPBD only.
        double m_lagrange_multiplier;

        /// \brief Compliance (inverse stiffness), used in XPBD only.
        ///
        /// \details Should be set to zero if the constraint is hard (e.g., collision constraints).
        double m_compliance;

        /// \brief Time step, used in XPBD only.
        double m_delta_time;

    protected:
        /// \brief Associated particles.
        ///
        /// \details The number of particles is (in most cases) solely determined in each constraint. For example, a
        /// distance constraint need to have exactly two particles. Some special constraints (e.g., shape-matching
        /// constraint) could have a variable number of particles.
        std::vector<std::shared_ptr<Particle>> m_particles;
    };

    template <int Num> class FixedNumAbstractConstraint : public AbstractConstraint
    {
    public:
        FixedNumAbstractConstraint(const std::vector<std::shared_ptr<Particle>>& particles,
                                   const double                                  stiffness,  ///< for PBD
                                   const double                                  compliance, ///< for XPBD
                                   const double                                  delta_time  ///< for XPBD
                                   )
            : AbstractConstraint(particles, stiffness, compliance, delta_time),
              m_inv_M(constructInverseMassMatrix(m_particles))
        {
        }

        virtual double calculateValue()              = 0;
        virtual void   calculateGrad(double* grad_C) = 0;

        void projectParticles(const AlgorithmType type) final
        {
            // Calculate the constraint function value
            const double C = calculateValue();

            // Skip if it is a unilateral constraint and is satisfied
            if (getType() == ConstraintType::Unilateral && C >= 0.0)
            {
                return;
            }

            // Calculate the derivative of the constraint function
            const Eigen::Matrix<double, Num * 3, 1> grad_C = calculateGrad();

            // Skip if the gradient is sufficiently small
            constexpr double very_small_value = 1e-12;
            if (grad_C.norm() < very_small_value)
            {
                return;
            }

            switch (type)
            {
                case AlgorithmType::Pbd:
                {
                    // Calculate s
                    const double s = -C / (grad_C.transpose() * m_inv_M.asDiagonal() * grad_C);

                    // Calculate \Delta x
                    const Eigen::Matrix<double, Num * 3, 1> delta_x = m_stiffness * s * m_inv_M.asDiagonal() * grad_C;
                    assert(!delta_x.hasNaN());

                    // Update predicted positions
                    for (unsigned int j = 0; j < Num; ++j)
                    {
                        m_particles[j]->p += delta_x.segment(3 * j, 3);
                    }

                    break;
                }
                case AlgorithmType::Xpbd:
                {
                    // Calculate time-scaled compliance
                    const double alpha_tilde = m_compliance / (m_delta_time * m_delta_time);

                    // Calculate \Delta lagrange multiplier
                    const double delta_lagrange_multiplier =
                        (-C - alpha_tilde * m_lagrange_multiplier) /
                        (grad_C.transpose() * m_inv_M.asDiagonal() * grad_C + alpha_tilde);

                    // Calculate \Delta x
                    const Eigen::Matrix<double, Num * 3, 1> delta_x =
                        delta_lagrange_multiplier * m_inv_M.asDiagonal() * grad_C;
                    assert(!delta_x.hasNaN());

                    // Update predicted positions
                    for (unsigned int j = 0; j < Num; ++j)
                    {
                        m_particles[j]->p += delta_x.segment(3 * j, 3);
                    }

                    // Update the lagrange multiplier
                    m_lagrange_multiplier += delta_lagrange_multiplier;

                    break;
                }
            }
        }

    private:
        const Eigen::Matrix<double, Num * 3, 1> m_inv_M;

        Eigen::Matrix<double, Num * 3, 1> calculateGrad()
        {
            Eigen::Matrix<double, Num * 3, 1> grad_C;
            calculateGrad(grad_C.data());
            return grad_C;
        }

        static Eigen::Matrix<double, Num * 3, 1>
        constructInverseMassMatrix(const std::vector<std::shared_ptr<elasty::Particle>>& particles)
        {
            Eigen::Matrix<double, Num * 3, 1> inv_M;
            for (unsigned int j = 0; j < Num; ++j)
            {
                inv_M(j * 3 + 0) = particles[j]->w;
                inv_M(j * 3 + 1) = particles[j]->w;
                inv_M(j * 3 + 2) = particles[j]->w;
            }
            return inv_M;
        }
    };

    class VariableNumConstraint : public AbstractConstraint
    {
    public:
        VariableNumConstraint(const std::vector<std::shared_ptr<Particle>>& particles,
                              const double                                  stiffness,  ///< for PBD
                              const double                                  compliance, ///< for XPBD
                              const double                                  delta_time  ///< for XPBD
                              )
            : AbstractConstraint(particles, stiffness, compliance, delta_time),
              m_inv_M(constructInverseMassMatrix(m_particles))
        {
        }

        virtual double calculateValue()              = 0;
        virtual void   calculateGrad(double* grad_C) = 0;

        virtual void projectParticles(const AlgorithmType type)
        {
            // Calculate the constraint function value
            const double C = calculateValue();

            // Skip if it is a unilateral constraint and is satisfied
            if (getType() == ConstraintType::Unilateral && C >= 0.0)
            {
                return;
            }

            // Calculate the derivative of the constraint function
            const Eigen::VectorXd grad_C = calculateGrad();

            // Skip if the gradient is sufficiently small
            constexpr double very_small_value = 1e-12;
            if (grad_C.norm() < very_small_value)
            {
                return;
            }

            switch (type)
            {
                case AlgorithmType::Pbd:
                {
                    // Calculate s
                    const double s = -C / (grad_C.transpose() * m_inv_M.asDiagonal() * grad_C);

                    // Calculate \Delta x
                    const Eigen::VectorXd delta_x = m_stiffness * s * m_inv_M.asDiagonal() * grad_C;
                    assert(!delta_x.hasNaN());

                    // Update predicted positions
                    for (unsigned int j = 0; j < m_particles.size(); ++j)
                    {
                        m_particles[j]->p += delta_x.segment(3 * j, 3);
                    }

                    break;
                }
                case AlgorithmType::Xpbd:
                {
                    // Calculate time-scaled compliance
                    const double alpha_tilde = m_compliance / (m_delta_time * m_delta_time);

                    // Calculate \Delta lagrange multiplier
                    const double delta_lagrange_multiplier =
                        (-C - alpha_tilde * m_lagrange_multiplier) /
                        (grad_C.transpose() * m_inv_M.asDiagonal() * grad_C + alpha_tilde);

                    // Calculate \Delta x
                    const Eigen::VectorXd delta_x = delta_lagrange_multiplier * m_inv_M.asDiagonal() * grad_C;
                    assert(!delta_x.hasNaN());

                    // Update predicted positions
                    for (unsigned int j = 0; j < m_particles.size(); ++j)
                    {
                        m_particles[j]->p += delta_x.segment(3 * j, 3);
                    }

                    // Update the lagrange multiplier
                    m_lagrange_multiplier += delta_lagrange_multiplier;

                    break;
                }
            }
        }

    private:
        const Eigen::VectorXd m_inv_M;

        Eigen::VectorXd calculateGrad()
        {
            Eigen::VectorXd grad_C(m_particles.size() * 3);
            calculateGrad(grad_C.data());
            return grad_C;
        }

        static Eigen::VectorXd
        constructInverseMassMatrix(const std::vector<std::shared_ptr<elasty::Particle>>& particles)
        {
            Eigen::VectorXd inv_M(particles.size() * 3);
            for (unsigned int j = 0; j < particles.size(); ++j)
            {
                inv_M(j * 3 + 0) = particles[j]->w;
                inv_M(j * 3 + 1) = particles[j]->w;
                inv_M(j * 3 + 2) = particles[j]->w;
            }
            return inv_M;
        }
    };

    class BendingConstraint final : public FixedNumAbstractConstraint<4>
    {
    public:
        BendingConstraint(const std::shared_ptr<Particle> p_0,
                          const std::shared_ptr<Particle> p_1,
                          const std::shared_ptr<Particle> p_2,
                          const std::shared_ptr<Particle> p_3,
                          const double                    stiffness,  ///< for PBD
                          const double                    compliance, ///< for XPBD
                          const double                    delta_time, ///< for XPBD
                          const double                    dihedral_angle);

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:
        const double m_dihedral_angle;
    };

    class ContinuumTriangleConstraint final : public FixedNumAbstractConstraint<3>
    {
    public:
        ContinuumTriangleConstraint(const std::shared_ptr<Particle> p_0,
                                    const std::shared_ptr<Particle> p_1,
                                    const std::shared_ptr<Particle> p_2,
                                    const double                    stiffness,  ///< for PBD
                                    const double                    compliance, ///< for XPBD
                                    const double                    delta_time, ///< for XPBD
                                    const double                    youngs_modulus,
                                    const double                    poisson_ratio);

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:
        const double m_first_lame;
        const double m_second_lame;

        double          m_rest_area;
        Eigen::Matrix2d m_rest_D_inv;
    };

    /// \details When the two particles are degenerated (i.e., at an exactly same posinion), the gradient will be set as
    /// a random direction to recover from the degenerated state.
    class DistanceConstraint final : public FixedNumAbstractConstraint<2>
    {
    public:
        DistanceConstraint(const std::shared_ptr<Particle> p_0,
                           const std::shared_ptr<Particle> p_1,
                           const double                    stiffness,  ///< for PBD
                           const double                    compliance, ///< for XPBD
                           const double                    delta_time, ///< for XPBD
                           const double                    d);

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:
        const double m_d;
    };

    class EnvironmentalCollisionConstraint final : public FixedNumAbstractConstraint<1>
    {
    public:
        EnvironmentalCollisionConstraint(const std::shared_ptr<Particle> p_0,
                                         const double                    stiffness,  ///< for PBD
                                         const double                    compliance, ///< for XPBD
                                         const double                    delta_time, ///< for XPBD
                                         const Eigen::Vector3d&          n,
                                         const double                    d);

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Unilateral; }

    private:
        const Eigen::Vector3d m_n;
        const double          m_d;
    };

    class FixedPointConstraint final : public FixedNumAbstractConstraint<1>
    {
    public:
        FixedPointConstraint(const std::shared_ptr<Particle> p_0,
                             const double                    stiffness,  ///< for PBD
                             const double                    compliance, ///< for XPBD
                             const double                    delta_time, ///< for XPBD
                             const Eigen::Vector3d&          point);

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:
        const Eigen::Vector3d m_point;
    };

    class IsometricBendingConstraint final : public FixedNumAbstractConstraint<4>
    {
    public:
        IsometricBendingConstraint(const std::shared_ptr<Particle> p_0,
                                   const std::shared_ptr<Particle> p_1,
                                   const std::shared_ptr<Particle> p_2,
                                   const std::shared_ptr<Particle> p_3,
                                   const double                    stiffness,  ///< for PBD
                                   const double                    compliance, ///< for XPBD
                                   const double                    delta_time  ///< for XPBD
        );

        double         calculateValue() override;
        void           calculateGrad(double* grad_C) override;
        ConstraintType getType() override { return ConstraintType::Bilateral; }

    private:
        Eigen::Matrix4d m_Q;
    };

    class ShapeMatchingConstraint final : public VariableNumConstraint
    {
    public:
        ShapeMatchingConstraint(const std::vector<std::shared_ptr<Particle>>& particles,
                                const double                                  stiffness,  ///< for PBD
                                const double                                  compliance, ///< for XPBD
                                const double                                  delta_time  ///< for XPBD
        );

        /// \details This method should never be called.
        double calculateValue() override;

        /// \details This method should never be called.
        void calculateGrad(double* grad_C) override;

        ConstraintType getType() override { return ConstraintType::Bilateral; }

        /// \details Unlike other constraints, ShapeMatchingConstraint does not offer a closed-form constraint function
        /// (i.e., it cannot be represented as a cost function), but does offer a particle projection procedure
        /// directly. This means that does not support XPBD.
        ///
        /// Thus, even when the "type" is specified as XPBD, it uses the PBD update scheme and the "stiffness" value to
        /// adjust the constraint stiffness. Thus, it is not generally recommended to use this constraint with XPBD
        /// scenes for consistency.
        ///
        /// This is why this class needs to override the particle projection procedure.
        void projectParticles(const AlgorithmType type) override;

    private:
        double                       m_total_mass;
        std::vector<Eigen::Vector3d> m_q;
    };
} // namespace elasty

#endif // ELASTY_CONSTRAINT_HPP


#pragma once

#include <wmtk/ConcurrentTetMesh.h>
#include <wmtk/utils/PartitionMesh.h>
#include <wmtk/AttributeCollection.hpp>

// clang-format off
#include <wmtk/utils/DisableWarnings.hpp>
#include <tbb/concurrent_queue.h>
#include <tbb/concurrent_vector.h>
#include <tbb/enumerable_thread_specific.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_sort.h>
#include <tbb/task_arena.h>
#include <wmtk/utils/EnableWarnings.hpp>
// clang-format on

#include <wmtk/utils/Morton.h>
#include "wmtk/utils/Logger.hpp"

#include <Eigen/Core>
#include <atomic>
#include <memory>

namespace harmonic_tet {

class HarmonicTet : public wmtk::ConcurrentTetMesh
{
public:
    struct VertexAttributes
    {
        Eigen::Vector3d pos;
        size_t partition_id = 0;
    };
    using VertAttCol = wmtk::AttributeCollection<VertexAttributes>;
    VertAttCol vertex_attrs;

    HarmonicTet(
        const std::vector<Eigen::Vector3d>& _vertex_attribute,
        const std::vector<std::array<size_t, 4>>& tets,
        int num_threads = 1)
    {
        p_vertex_attrs = &vertex_attrs;

        vertex_attrs.resize(_vertex_attribute.size());

        for (auto i = 0; i < _vertex_attribute.size(); i++)
            vertex_attrs[i].pos = _vertex_attribute[i];

        NUM_THREADS = num_threads;
        init(_vertex_attribute.size(), tets);

        // compute_vertex_partition();
        compute_vertex_partition_morton();
    }
    HarmonicTet(){};
    ~HarmonicTet(){};

    ////// Attributes related

    void compute_vertex_partition_morton()
    {
        if (NUM_THREADS==0) return;

        wmtk::logger().info("Number of parts: {} by morton", NUM_THREADS);

        tbb::task_arena arena(NUM_THREADS);

        arena.execute([&] {

            std::vector<Eigen::Vector3d> V_v(vert_capacity());

            tbb::parallel_for(
                tbb::blocked_range<int>(0, V_v.size()),
                [&](tbb::blocked_range<int> r) {
                    for (int i = r.begin(); i < r.end(); i++) {
                        V_v[i] = vertex_attrs[i].pos;
                    }
                });

            struct sortstruct
            {
                int order;
                Resorting::MortonCode64 morton;
            };

            std::vector<sortstruct> list_v;
            list_v.resize(V_v.size());
            // std::vector<sortstruct> list;
            const int multi = 1000;
            // since the morton code requires a correct scale of input vertices,
            //  we need to scale the vertices if their coordinates are out of range
            std::vector<Eigen::Vector3d> V = V_v; // this is for rescaling vertices
            Eigen::Vector3d vmin, vmax;
            vmin = V.front();
            vmax = V.front();

            for (size_t j = 0; j < V.size(); j++) {
                for (int i = 0; i < 3; i++) {
                    vmin(i) = std::min(vmin(i), V[j](i));
                    vmax(i) = std::max(vmax(i), V[j](i));
                }
            }

            // get_bb_corners(V, vmin, vmax);
            Eigen::Vector3d center = (vmin + vmax) / 2;

            tbb::parallel_for(tbb::blocked_range<int>(0, V.size()), [&](tbb::blocked_range<int> r) {
                for (int i = r.begin(); i < r.end(); i++) {
                    V[i] = V[i] - center;
                }
            });

            Eigen::Vector3d scale_point =
                vmax - center; // after placing box at origin, vmax and vmin are symetric.

            double xscale, yscale, zscale;
            xscale = fabs(scale_point[0]);
            yscale = fabs(scale_point[1]);
            zscale = fabs(scale_point[2]);
            double scale = std::max(std::max(xscale, yscale), zscale);
            if (scale > 300) {
                tbb::parallel_for(
                    tbb::blocked_range<int>(0, V.size()),
                    [&](tbb::blocked_range<int> r) {
                        for (int i = r.begin(); i < r.end(); i++) {
                            V[i] = V[i] / scale;
                        }
                    });
            }

            tbb::parallel_for(tbb::blocked_range<int>(0, V.size()), [&](tbb::blocked_range<int> r) {
                for (int i = r.begin(); i < r.end(); i++) {
                    list_v[i].morton = Resorting::MortonCode64(
                        int(V[i][0] * multi),
                        int(V[i][1] * multi),
                        int(V[i][2] * multi));
                    list_v[i].order = i;
                }
            });

            const auto morton_compare = [](const sortstruct& a, const sortstruct& b) {
                return (a.morton < b.morton);
            };

            tbb::parallel_sort(list_v.begin(), list_v.end(), morton_compare);

            int interval = list_v.size() / NUM_THREADS + 1;

            tbb::parallel_for(
                tbb::blocked_range<int>(0, list_v.size()),
                [&](tbb::blocked_range<int> r) {
                    for (int i = r.begin(); i < r.end(); i++) {
                        vertex_attrs[list_v[i].order].partition_id = i / interval;
                    }
                });

        });
    }

    void output_mesh(std::string file) const;
    void compute_vertex_partition()
    {
        auto partition_id = partition_TetMesh(*this, NUM_THREADS);
        for (auto i = 0; i < vert_capacity(); i++)
            vertex_attrs[i].partition_id = partition_id[i];
    }
    size_t get_partition_id(const Tuple& loc) const
    {
        return vertex_attrs[loc.vid(*this)].partition_id;
    }

    // parallel containers
    ////// Operations

    struct SwapInfoCache
    {
        double total_energy = 0.;
    };
    tbb::enumerable_thread_specific<SwapInfoCache> edgeswap_cache, faceswap_cache;

    void smooth_all_vertices(bool interior_only = false);
    bool smooth_after(const Tuple& t) override;

    void swap_all_edges(bool parallel = false);
    bool swap_edge_before(const Tuple& t) override;
    bool swap_edge_after(const Tuple& t) override;

    int swap_all();
    void swap_all_faces(bool parallel = false);
    bool swap_face_before(const Tuple& t) override;
    bool swap_face_after(const Tuple& t) override;

    bool is_inverted(const Tuple& loc);
    double get_quality(const Tuple& loc) const;
    double get_quality(const std::array<size_t, 4>& vids) const;

    bool invariants(const std::vector<Tuple>&) override;
};

} // namespace harmonic_tet


/**
 * @file budgeted_maximum_coverage_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-17
 */

#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/budgeted_maximum_coverage_check.hpp"

#include "paal/greedy/set_cover/budgeted_maximum_coverage.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/irange.hpp>

#include <vector>
#include <iterator>
#include <cmath>

using paal::utils::make_array_to_functor;
using paal::utils::identity_functor;
using paal::utils::return_one_functor;

BOOST_AUTO_TEST_CASE(budgeted_maximum_coverage_3_set_optimal) {
    LOGLN("budgeted_maximum_coverage_3_set_optimal");
    const int OPTIMAL = 111;
    const int BUDGET = 111;
    const int ELEMENTS = 3;
    const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    std::vector<std::vector<int>> sets_element = { { 0 }, { 1 }, { 2 } };
    std::vector<int> weight = { 100, 10, 1 }, cost_of_set = { 100, 10, 1 };
    auto sets = paal::irange(3);

    std::vector<int> result;
    auto element_index = identity_functor{};
    auto set_to_cost = make_array_to_functor(cost_of_set);
    auto element_to_weight = make_array_to_functor(weight);
    auto covered_weight = paal::greedy::budgeted_maximum_coverage(
        sets, set_to_cost, make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, BUDGET, element_to_weight,
        3);
    budgeted_maximum_coverage_result_check(
        result, sets_element, element_to_weight, set_to_cost, ELEMENTS, OPTIMAL,
        covered_weight, APPROXIMATION_RATIO, BUDGET);
    BOOST_CHECK_EQUAL(covered_weight, OPTIMAL);
}

BOOST_AUTO_TEST_CASE(budgeted_maximum_coverage_empty_set) {
    LOGLN("budgeted_maximum_coverage_empty_set");
    const int OPTIMAL = 0;
    const int BUDGET = 1000;
    const int ELEMENTS = 0;
    const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    std::vector<std::vector<int>> sets_element(1);
    std::vector<int> weight, cost_of_set{1000};
    auto sets = paal::irange(1);

    std::vector<int> result;
    auto element_index = identity_functor{};
    auto set_to_cost = make_array_to_functor(cost_of_set);
    auto element_to_weight = make_array_to_functor(weight);
    auto covered_weight = paal::greedy::budgeted_maximum_coverage(
        sets, set_to_cost,
        make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, BUDGET,
        element_to_weight);
    budgeted_maximum_coverage_result_check(
        result, sets_element, element_to_weight, set_to_cost, ELEMENTS, OPTIMAL,
        covered_weight, APPROXIMATION_RATIO, BUDGET);
}

BOOST_AUTO_TEST_CASE(budgeted_maximum_coverage) {
    LOGLN("budgeted_maximum_coverage");
    const int OPTIMAL = 200;
    const int BUDGET = 1000;
    const int ELEMENTS = 21;
    const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    std::vector<std::vector<int>> sets_element(21);
    std::vector<int> weight, cost_of_set;
    auto sets = paal::irange(ELEMENTS);
    for (auto i : sets) {
        sets_element[i].push_back(i);
        if (i < 10) {
            weight.push_back(10);
            cost_of_set.push_back(1);
        } else if (i != 20) {
            weight.push_back(1);
            cost_of_set.push_back(1);
        } else {
            weight.push_back(100);
            cost_of_set.push_back(990);
        }
    }
    std::vector<int> result;
    auto element_index = identity_functor{};
    auto set_to_cost = make_array_to_functor(cost_of_set);
    auto element_to_weight = make_array_to_functor(weight);
    auto covered_weight = paal::greedy::budgeted_maximum_coverage(
        sets, set_to_cost,
        make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, BUDGET,
        element_to_weight);
    budgeted_maximum_coverage_result_check(
        result, sets_element, element_to_weight, set_to_cost, ELEMENTS, OPTIMAL,
        covered_weight, APPROXIMATION_RATIO, BUDGET);
}

BOOST_AUTO_TEST_CASE(BudgetMaximumCoverage2) {
    LOGLN("BudgetMaximumCoverage2");
    static const int OPTIMAL = 14;
    static const int ELEMENTS = 14;
    static const int BUDGET = 2;
    static const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    std::vector<std::vector<int>> sets_element = { { 1, 2 },
                                                   { 3, 4, 5, 6 },
                                                   { 7, 8, 9, 10, 11, 12, 13,
                                                     0 },
                                                   { 1, 3, 5, 7, 9, 11, 13 },
                                                   { 2, 4, 6, 8, 10, 12, 0 } };
    auto sets = paal::irange(5);
    std::vector<int> result;
    auto element_index = identity_functor{};
    auto element_to_weight = return_one_functor{};
    auto cost_of_set = return_one_functor{};
    auto covered_weight = paal::greedy::budgeted_maximum_coverage(
        sets, cost_of_set, make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, BUDGET, element_to_weight);
    budgeted_maximum_coverage_result_check(
        result, sets_element, element_to_weight, cost_of_set, ELEMENTS, OPTIMAL,
        covered_weight, APPROXIMATION_RATIO, BUDGET);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/functional.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/type_list.hpp>

#include <type_traits>
using namespace boost::hana;

//! [main]
template <typename ...T>
using largest = decltype(maximum_by(less ^on^ sizeof_, type_list<T...>));

template <int size>
struct storage { char s[size]; };

static_assert(std::is_same<
    largest<storage<1>, storage<2>, storage<3>>::type,
    storage<3>
>{}, "");
//! [main]

int main() { }


//
// Original authors: Andrew Dowsey <andrew.dowsey@manchester.ac.uk>
//
// Copyright 2014 CADET Bioinformatics Laboratory
//                University of Manchester, UK
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "aux_Eigen.hpp"

#include <iostream>
#include <sstream>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <math.h>

extern ClrMap COLOR_TABLE;

// http://stackoverflow.com/questions/7706339/grayscale-to-red-green-blue-matlab-jet-color-scale

COLOUR GetColour(fp v,fp vmin,fp vmax)
{
   COLOUR c = {1.0f,1.0f,1.0f}; // white
   fp dv;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;

   dv = vmax - vmin;

   if (COLOR_TABLE == simpleJet)
   {
	   //dv = vmax - vmin;

	   if (v < (vmin + 0.25f * dv)) {
		  c.r = 0;
		  c.g = 4 * (v - vmin) / dv;
	   } else if (v < (vmin + 0.5f * dv)) {
		  c.r = 0;
		  c.b = 1 + 4 * (vmin + 0.25f * dv - v) / dv;
	   } else if (v < (vmin + 0.75f * dv)) {
		  c.r = 4 * (v - vmin - 0.5f * dv) / dv;
		  c.b = 0;
	   } else {
		  c.g = 1 + 4 * (vmin + 0.75f * dv - v) / dv;
		  c.b = 0;
	   }

// end http://stackoverflow.com/questions/7706339/grayscale-to-red-green-blue-matlab-jet-color-scale

   }
   else if (COLOR_TABLE == matlabJet)
   {
	   //dv = vmax - vmin;

	   if (v < (vmin + 0.125f * dv)) {
		   c.b = 0.5f + 4*(v -vmin)/dv ;
		   c.g = 0;
		   c.r = 0;
	   }
	   else if (v < (vmin + 0.375f * dv) ) {
		  c.r = 0;
		  c.g = 4 * (v - vmin - 0.125f * dv) / dv;
	   }
	   else if (v < (vmin + 0.625f * dv)) {
		  c.r = 4 * (v - vmin - 0.375f * dv) / dv;
		  c.b = 1 + 4 * (vmin + 0.375f * dv - v) / dv;
	   }
	   else if (v < (vmin + 0.875f * dv)) {
		  c.g = 1 + 4 * (vmin + 0.625f * dv - v) / dv;
		  c.b = 0;
	   }
	   else {
		  c.r = 1 + 4 * (vmin + 0.875f * dv - v) / dv;
		  c.g = 0;
		  c.b = 0;
	   }
   }
   else if (COLOR_TABLE == cubeHelix)
   {
		fp START, ROTS, HUE, GAMMA;
		START = 0.5f;
		ROTS = -1.5f;
		HUE = 1.0f;
		GAMMA = 1.0f;

		int NLEV = 256;

		fp fract, angle, amp;
		fp PI = 4.0f*atan(1.0f);
		fract = (v - vmin)/dv;
		angle = 2 * PI * (START/3.0f + 1.0f + ROTS*fract);
		fract = fract * GAMMA;
		amp = HUE*fract*(1-fract)/2.0;

		c.r = 1.0 - fract+amp*(-0.14861*cos(angle)+1.78277*sin(angle));
        c.g = 1.0 - fract+amp*(-0.29227*cos(angle)-0.90649*sin(angle));
        c.b = 1.0 - fract+amp*(+1.97294*cos(angle));

		if (c.r < 0.0f) c.r = 0.0;
		if (c.g < 0.0f) c.g = 0.0;
		if (c.b < 0.0f) c.b = 0.0;
		if (c.r > 1.0f) c.r = 1.0f;
		if (c.g > 1.0f) c.g = 1.0f;
		if (c.b > 1.0f) c.b = 1.0f;
   }

   return(c);
}


#ifndef PYTHONIC_INCLUDE_NUMPY_LOG2_HPP
#define PYTHONIC_INCLUDE_NUMPY_LOG2_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/log2.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME log2
#define NUMPY_NARY_FUNC_SYM boost::simd::log2
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


//  Copyright (c) 2013 Anton Bikineev
//  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_BESSEL_DERIVATIVES_HPP
#define BOOST_MATH_BESSEL_DERIVATIVES_HPP

#ifdef _MSC_VER
#  pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/detail/bessel_jy_derivatives_asym.hpp>
#include <boost/math/special_functions/detail/bessel_jy_derivatives_series.hpp>
#include <boost/math/special_functions/detail/bessel_derivatives_linear.hpp>

namespace boost{ namespace math{

namespace detail{

template <class Tag, class T, class Policy>
inline T cyl_bessel_j_prime_imp(T v, T x, const Policy& pol)
{
   static const char* const function = "boost::math::cyl_bessel_j_prime<%1%>(%1%,%1%)";
   BOOST_MATH_STD_USING
   //
   // Prevent complex result:
   //
   if (x < 0 && floor(v) != v)
      return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x >= 0", x, pol);
   //
   // Special cases for x == 0:
   //
   if (x == 0)
   {
      if (v == 1)
         return 0.5;
      else if (v == -1)
         return -0.5;
      else if (floor(v) == v || v > 1)
         return 0;
      else return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function is indeterminate for this order", x, pol);
   }
   //
   // Special case for large x: use asymptotic expansion:
   //
   if (boost::math::detail::asymptotic_bessel_derivative_large_x_limit(v, x))
      return boost::math::detail::asymptotic_bessel_j_derivative_large_x_2(v, x, pol);
   //
   // Special case for small x: use Taylor series:
   //
   if ((abs(x) < 5) || (abs(v) > x * x / 4))
   {
      bool inversed = false;
      if (floor(v) == v && v < 0)
      {
         v = -v;
         if (itrunc(v, pol) & 1)
            inversed = true;
      }
      T r = boost::math::detail::bessel_j_derivative_small_z_series(v, x, pol);
      return inversed ? T(-r) : r;
   }
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return -boost::math::detail::cyl_bessel_j_imp<T>(1, x, Tag(), pol);
   //
   // Default case:
   //
   return boost::math::detail::bessel_j_derivative_linear(v, x, Tag(), pol);
}

template <class T, class Policy>
inline T sph_bessel_j_prime_imp(unsigned v, T x, const Policy& pol)
{
   static const char* const function = "boost::math::sph_bessel_prime<%1%>(%1%,%1%)";
   //
   // Prevent complex result:
   //
   if (x < 0)
      return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x >= 0.", x, pol);
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return (x == 0) ? boost::math::policies::raise_overflow_error<T>(function, 0, pol)
         : static_cast<T>(-boost::math::detail::sph_bessel_j_imp<T>(1, x, pol));
   //
   // Special case for x == 0 and v > 0:
   //
   if (x == 0)
      return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function is indeterminate for this order", x, pol);
   //
   // Default case:
   //
   return boost::math::detail::sph_bessel_j_derivative_linear(v, x, pol);
}

template <class T, class Policy>
inline T cyl_bessel_i_prime_imp(T v, T x, const Policy& pol)
{
   static const char* const function = "boost::math::cyl_bessel_i_prime<%1%>(%1%,%1%)";
   BOOST_MATH_STD_USING
   //
   // Prevent complex result:
   //
   if (x < 0 && floor(v) != v)
      return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x >= 0", x, pol);
   //
   // Special cases for x == 0:
   //
   if (x == 0)
   {
      if (v == 1 || v == -1)
         return 0.5;
      else if (floor(v) == v || v > 1)
         return 0;
      else return boost::math::policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function is indeterminate for this order", x, pol);
   }
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return boost::math::detail::cyl_bessel_i_imp<T>(1, x, pol);
   //
   // Default case:
   //
   return boost::math::detail::bessel_i_derivative_linear(v, x, pol);
}

template <class Tag, class T, class Policy>
inline T cyl_bessel_k_prime_imp(T v, T x, const Policy& pol)
{
   //
   // Prevent complex and indeterminate results:
   //
   if (x <= 0)
      return boost::math::policies::raise_domain_error<T>(
         "boost::math::cyl_bessel_k_prime<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0", x, pol);
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return -boost::math::detail::cyl_bessel_k_imp<T>(1, x, Tag(), pol);
   //
   // Default case:
   //
   return boost::math::detail::bessel_k_derivative_linear(v, x, Tag(), pol);
}

template <class Tag, class T, class Policy>
inline T cyl_neumann_prime_imp(T v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   //
   // Prevent complex and indeterminate results:
   //
   if (x <= 0)
      return boost::math::policies::raise_domain_error<T>(
         "boost::math::cyl_neumann_prime<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0", x, pol);
   //
   // Special case for large x: use asymptotic expansion:
   //
   if (boost::math::detail::asymptotic_bessel_derivative_large_x_limit(v, x))
      return boost::math::detail::asymptotic_bessel_y_derivative_large_x_2(v, x, pol);
   //
   // Special case for small x: use Taylor series:
   //
   if (v > 0 && floor(v) != v)
   {
      const T eps = boost::math::policies::get_epsilon<T, Policy>();
      if (log(eps / 2) > v * log((x * x) / (v * 4)))
         return boost::math::detail::bessel_y_derivative_small_z_series(v, x, pol);
   }
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return -boost::math::detail::cyl_neumann_imp<T>(1, x, Tag(), pol);
   //
   // Default case:
   //
   return boost::math::detail::bessel_y_derivative_linear(v, x, Tag(), pol);
}

template <class T, class Policy>
inline T sph_neumann_prime_imp(unsigned v, T x, const Policy& pol)
{
   //
   // Prevent complex and indeterminate result:
   //
   if (x <= 0)
      return boost::math::policies::raise_domain_error<T>(
         "boost::math::sph_neumann_prime<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0.", x, pol);
   //
   // Special case for v == 0:
   //
   if (v == 0)
      return -boost::math::detail::sph_neumann_imp<T>(1, x, pol);
   //
   // Default case:
   //
   return boost::math::detail::sph_neumann_derivative_linear(v, x, pol);
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_j_prime(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_prime_imp<tag_type, value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_bessel_j_prime<%1%,%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_j_prime(T1 v, T2 x)
{
   return cyl_bessel_j_prime(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_bessel_prime(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_bessel_j_prime_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_bessel_j_prime<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_bessel_prime(unsigned v, T x)
{
   return sph_bessel_prime(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_i_prime(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_i_prime_imp<value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_bessel_i_prime<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_i_prime(T1 v, T2 x)
{
   return cyl_bessel_i_prime(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_k_prime(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_k_prime_imp<tag_type, value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_bessel_k_prime<%1%,%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_k_prime(T1 v, T2 x)
{
   return cyl_bessel_k_prime(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_neumann_prime(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_prime_imp<tag_type, value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_neumann_prime<%1%,%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_neumann_prime(T1 v, T2 x)
{
   return cyl_neumann_prime(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_neumann_prime(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_neumann_prime_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_neumann_prime<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_neumann_prime(unsigned v, T x)
{
   return sph_neumann_prime(v, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_BESSEL_DERIVATIVES_HPP


/**
 *  testGrid.cpp
 *
 *  Boost test of grid class.
 *
 *  Created by Yinan Li on Jan. 21, 2017.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Grid

#include <boost/test/unit_test.hpp>
#include "src/grid.h"
#include "src/matlabio.h"

/*
 * test case 1: initialization of grid
 */
BOOST_AUTO_TEST_CASE(test_init_grid)
{
    const int XD = 3;

    /* define the state and input space by upper/lower bounds */
    double xlb[XD] = {0, 0, -M_PI};
    double xub[XD] = {10, 10, M_PI};
    double eta[XD] = {0.2, 0.2, 2*M_PI/35.0};

    rocs::grid xg(XD, eta, xlb, xub);
    xg.gridding();
    xg.print_info();

    rocs::matWriter wtr("grids_unicycle.mat");
    wtr.open();
    wtr.write_uniform_grids(xg, "xrv");
    wtr.close();

    // grid xg1;
    // xg1.gridding(XD, eta, xlb, xub);
    // xg1.print_data();

    std::vector<double> rp(XD, 0.2);  // rate of eta
    std::vector< std::vector<double> > sub = xg.subgridding(xg._data[0], rp);
    for (size_t row = 0; row < sub.size(); ++row) {

	std::cout << row << " ";

	for (int col = 0; col < XD; ++col) {

	    std::cout << sub[row][col] << ' ';
	}

	std::cout << '\n';
    }
}

/*
 * test case 2: multidimensional grid test
 */
BOOST_AUTO_TEST_CASE(test_grid_subset_nd)
{
    const int XD = 3;
    double xlb[XD] = {1, -0.4, -5};
    double xub[XD] = {6, 1.2, -3};

    /* gridding 1 */
    double eta[XD] = {1, 0.1, 0.5};
    rocs::grid xg(XD, eta, xlb, xub);
    // xg.gridding();
    // xg.print_info();
    // xg.print_data();

    /* area 1: partially outside */
    rocs::ivec box(XD);
    box[0] = rocs::interval(2.6, 3.1);
    box[1] = rocs::interval(1.1, 1.23);
    box[2] = rocs::interval(-4, -3.9);

    std::vector<size_t> act1 = xg.subset(box, false, false);
    std::vector<size_t> ref(2);
    ref[0] = 296;
    ref[1] = 302;
    // ref[0] = 236;
    // ref[1] = 237;

    BOOST_CHECK(act1 == ref);
    BOOST_CHECK(xg.subset(box, false, true).empty());
    BOOST_CHECK(xg.subset(box, true, false).empty());

    /* gridding 2  */
    double eta2[XD] = {1.3, 0.14, 0.4};
    rocs::grid xg2(XD, eta2, xlb, xub);
    xg2.gridding();
    // xg2.print_info();
    // xg2.print_data();

    /* area 2: fully inside */
    rocs::ivec box2(XD);
    box2[0] = rocs::interval(2.6, 3.1);
    box2[1] = rocs::interval(1.1, 1.2);
    box2[2] = rocs::interval(-4, -3.9);
    std::vector<size_t> act2 = xg2.subset(box2, true, false);
    std::vector<size_t> ref2(1);
    ref2[0] = 189;
    BOOST_CHECK(act2 == ref2);
    BOOST_CHECK(xg2.subset(box2, true, true).empty());

    /* area 3: fully inside, no empty */
    rocs::ivec box3(XD);
    box3[0] = rocs::interval(2.9, 5.4);
    box3[1] = rocs::interval(-0.08, 0.42);
    box3[2] = rocs::interval(-3.6, -3.2);
    std::vector<size_t> act31 = xg2.subset(box3, true, false);
    std::vector<size_t> act32 = xg2.subset(box3, true, true);
    size_t d1[] = {201, 202, 203, 205, 206, 207, 209, 210, 211, 213, 214, 215, 217, 218, 219};
    size_t d2[] = {206, 210, 214};
    std::vector<size_t> ref31(d1, d1+15);
    std::vector<size_t> ref32(d2, d2+3);
    BOOST_CHECK(act31 == ref31);
    BOOST_CHECK(act32 == ref32);
}

/*
 * test case 3: one dimensional grid subset
 */
BOOST_AUTO_TEST_CASE(test_grid_subset_1d)
{
    /* define the state and input space by upper/lower bounds */
    double xlb[1] = {-1.4};
    double xub[1] = {-1.15};
    double eta[1] = {0.03};

    rocs::grid xg(1, eta, xlb, xub);
    xg.gridding();
    // xg.print_info();
    // xg.print_data();

    /* area 1: partially out of range */
    rocs::ivec box(1);
    box[0] = rocs::interval(-1.279, 0);

    std::vector<size_t> act1 = xg.subset(box, false, false);
    size_t d[] = {4, 5, 6, 7 ,8};
    std::vector<size_t> ref(d, d+5);
    BOOST_CHECK(act1 == ref);

    /* area 2: completely out of range */
    rocs::ivec box2(1);
    box2[0] = rocs::interval(-3, -2.7);
    BOOST_CHECK(xg.subset(box2, false, false).empty());

    /* area 3: on the grid boundary */
    rocs::ivec box3(1);
    box3[0] = rocs::interval(-1.32, -1.25);

    std::vector<size_t> ref31(2);
    ref31[0] = 3;
    ref31[1] = 4;
    std::vector<size_t> act31 = xg.subset(box3, true, true);
    BOOST_CHECK(act31 == ref31);

    std::vector<size_t> ref32(3);
    ref32[0] = 3;
    ref32[1] = 4;
    ref32[2] = 5;
    std::vector<size_t> act32 = xg.subset(box3, true, false);
    BOOST_CHECK(act32 == ref32);
}

/*
 * test case 4: test ipdl grids
 */
BOOST_AUTO_TEST_CASE(test_grid_ipdl)
{
    const int XD = 2;
    double xlb[XD] = {-0.1, -0.05};
    double xub[XD] = {0.1, 0.05};
    double eta[XD] = {0.001, 0.001};

    rocs::grid xg(XD, eta, xlb, xub);
    xg.gridding();
    xg.print_info();
}


// Copyright (c) 2012-2013 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "mruset.h"

#include "random.h"
#include "util.h"
#include "test/test_allforonebusiness.h"

#include <set>

#include <boost/test/unit_test.hpp>

#define NUM_TESTS 16
#define MAX_SIZE 100

using namespace std;

class mrutester
{
private:
    mruset<int> mru;
    std::set<int> set;

public:
    mrutester() { mru.max_size(MAX_SIZE); }
    int size() const { return set.size(); }

    void insert(int n)
    {
        mru.insert(n);
        set.insert(n);
        BOOST_CHECK(mru == set);
    }
};

BOOST_FIXTURE_TEST_SUITE(mruset_tests, BasicTestingSetup)

// Test that an mruset behaves like a set, as long as no more than MAX_SIZE elements are in it
BOOST_AUTO_TEST_CASE(mruset_like_set)
{

    for (int nTest=0; nTest<NUM_TESTS; nTest++)
    {
        mrutester tester;
        while (tester.size() < MAX_SIZE)
            tester.insert(GetRandInt(2 * MAX_SIZE));
    }

}

// Test that an mruset's size never exceeds its max_size
BOOST_AUTO_TEST_CASE(mruset_limited_size)
{
    for (int nTest=0; nTest<NUM_TESTS; nTest++)
    {
        mruset<int> mru(MAX_SIZE);
        for (int nAction=0; nAction<3*MAX_SIZE; nAction++)
        {
            int n = GetRandInt(2 * MAX_SIZE);
            mru.insert(n);
            BOOST_CHECK(mru.size() <= MAX_SIZE);
        }
    }
}

// 16-bit permutation function
int static permute(int n)
{
    // hexadecimals of pi; verified to be linearly independent
    static const int table[16] = {0x243F, 0x6A88, 0x85A3, 0x08D3, 0x1319, 0x8A2E, 0x0370, 0x7344,
                                  0xA409, 0x3822, 0x299F, 0x31D0, 0x082E, 0xFA98, 0xEC4E, 0x6C89};

    int ret = 0;
    for (int bit=0; bit<16; bit++)
         if (n & (1<<bit))
             ret ^= table[bit];

    return ret;
}

// Test that an mruset acts like a moving window, if no duplicate elements are added
BOOST_AUTO_TEST_CASE(mruset_window)
{
    mruset<int> mru(MAX_SIZE);
    for (int n=0; n<10*MAX_SIZE; n++)
    {
        mru.insert(permute(n));

        set<int> tester;
        for (int m=max(0,n-MAX_SIZE+1); m<=n; m++)
            tester.insert(permute(m));

        BOOST_CHECK(mru == tester);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/test/unit_test.hpp>
#include <shz/spatial/kd_tree.hpp>
#include <shz/math/bbox.hpp>
#include <shz/math/ray.hpp>
#include <shz/math/intersection.hpp>

using namespace shz::math;
using namespace shz::spatial;

std::ostream& operator<<(std::ostream& os, const kd_tree<bbox<f32, 3>, std::string>::pair_type & p)
{
    os << "[ first:" << p.first << " second:" << p.second << " ]";
    return os;
}

std::ostream& operator<<(std::ostream& os, const std::vector<kd_tree<bbox<f32, 3>, std::string>::pair_type>& p)
{
    os << "[ " << std::endl;

    for(auto element : p){
        os << element << "," << std::endl;
    }

    os << " ]" << std::endl;

    return os;
}

typedef kd_tree<bbox<f32, 3>, std::string>::pair_type pair_type;

std::vector<pair_type> create_evenly_distributed_set(const size_t dimension1, const size_t dimension2, const size_t dimension3, const float size, const float separation){
	std::vector<pair_type> vec;

	// Create a set of bounding boxes to be partitioned by the kd-tree
    for (size_t i=0; i < dimension1; ++i) {
        for (size_t j=0; j < dimension2; ++j) {
            for (size_t k=0; k< dimension3; ++k) {

                std::stringstream id;
                id << i << "_" << j << "_" << k;

                bbox<f32, 3> bounding_box;
                bounding_box.lower_bound[0] = i*size + separation*i;
                bounding_box.lower_bound[1] = j*size + separation*j;
                bounding_box.lower_bound[2] = j*size + separation*j;
                bounding_box.upper_bound[0] = i*size + size + separation*i;
                bounding_box.upper_bound[1] = j*size + size + separation*j;
                bounding_box.upper_bound[2] = j*size + size + separation*j;

                vec.push_back(pair_type(bounding_box, id.str()));
            }
        }
    }

	return vec;
}

BOOST_AUTO_TEST_CASE(kdtreeConstructors)
{

    const f32 separation = 1.f;
    const f32 size = 1.f;
	const size_t test_bboxes_dimension1 = 2, test_bboxes_dimension2 = 2, test_bboxes_dimension3 = 2;

	std::vector<pair_type> vec = create_evenly_distributed_set(test_bboxes_dimension1, test_bboxes_dimension2, test_bboxes_dimension3, size, separation);

    std::cout << vec;

	kd_tree<bbox<f32, 3>, std::string> tree;
	BOOST_CHECK(tree.empty());
	BOOST_CHECK(tree.size() == 0);
	tree.build_from_set(vec.begin(), vec.end());
	BOOST_CHECK(!tree.empty());
	BOOST_CHECK(tree.size() == test_bboxes_dimension2*test_bboxes_dimension1*test_bboxes_dimension3);

}

BOOST_AUTO_TEST_CASE(kdtreeTraverse)
{
    const f32 separation = 1.f;
    const f32 size = 1.f;
	const size_t test_bboxes_dimension1 = 2, test_bboxes_dimension2 = 2, test_bboxes_dimension3 = 2;

	std::vector<pair_type> vec = create_evenly_distributed_set(test_bboxes_dimension1, test_bboxes_dimension2, test_bboxes_dimension3, size, separation);

    kd_tree<bbox<f32, 3>, std::string> tree;
    tree.build_from_set(vec.begin(), vec.end());
	BOOST_CHECK(!tree.empty());
	BOOST_CHECK(tree.size() == test_bboxes_dimension2*test_bboxes_dimension1*test_bboxes_dimension3);

    vector<shz::math::f32, 3> o;
	o.data[0] = 0;
	o.data[1] = 0;
    o.data[2] = 0;
	vector<shz::math::f32, 3> d;
	d.data[0] = 0;
	d.data[1] = 1;
    d.data[3] = 0;
    shz::math::ray<f32> r(o, d, 20.f, 0.f);
    BOOST_CHECK(tree.traverse(r, [](shz::math::ray<f32> input, bbox<f32, 3> key, shz::math::intersector<ray<f32>, bbox<f32, 3> > inter) -> bool {
        return false;
    }));
}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_SUM_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_SUM_HPP

#include <boost/histogram/fwd.hpp>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/// Holds sum of weights and its variance estimate
template <typename RealType>
class weighted_sum {
public:
  weighted_sum() = default;
  explicit weighted_sum(const RealType& value) noexcept
      : sum_of_weights_(value), sum_of_weights_squared_(value) {}
  weighted_sum(const RealType& value, const RealType& variance) noexcept
      : sum_of_weights_(value), sum_of_weights_squared_(variance) {}

  /// Increment by one.
  weighted_sum& operator++() { return operator+=(1); }

  /// Increment by value.
  template <typename T>
  weighted_sum& operator+=(const T& value) {
    sum_of_weights_ += value;
    sum_of_weights_squared_ += value * value;
    return *this;
  }

  /// Added another weighted sum.
  template <typename T>
  weighted_sum& operator+=(const weighted_sum<T>& rhs) {
    sum_of_weights_ += static_cast<RealType>(rhs.sum_of_weights_);
    sum_of_weights_squared_ += static_cast<RealType>(rhs.sum_of_weights_squared_);
    return *this;
  }

  /// Scale by value.
  weighted_sum& operator*=(const RealType& x) {
    sum_of_weights_ *= x;
    sum_of_weights_squared_ *= x * x;
    return *this;
  }

  bool operator==(const RealType& rhs) const noexcept {
    return sum_of_weights_ == rhs && sum_of_weights_squared_ == rhs;
  }

  template <typename T>
  bool operator==(const weighted_sum<T>& rhs) const noexcept {
    return sum_of_weights_ == rhs.sum_of_weights_ &&
           sum_of_weights_squared_ == rhs.sum_of_weights_squared_;
  }

  template <typename T>
  bool operator!=(const T& rhs) const noexcept {
    return !operator==(rhs);
  }

  /// Return value of the sum.
  const RealType& value() const noexcept { return sum_of_weights_; }

  /// Return estimated variance of the sum.
  const RealType& variance() const noexcept { return sum_of_weights_squared_; }

  // lossy conversion must be explicit
  template <class T>
  explicit operator T() const {
    return static_cast<T>(sum_of_weights_);
  }

  template <class Archive>
  void serialize(Archive&, unsigned /* version */);

private:
  RealType sum_of_weights_ = RealType();
  RealType sum_of_weights_squared_ = RealType();
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
struct common_type<boost::histogram::accumulators::weighted_sum<T>,
                   boost::histogram::accumulators::weighted_sum<U>> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};

template <class T, class U>
struct common_type<boost::histogram::accumulators::weighted_sum<T>, U> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};

template <class T, class U>
struct common_type<T, boost::histogram::accumulators::weighted_sum<U>> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/abs.hpp
 *
 * \brief Apply the \c std::abs function to a vector or matrix expression.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_ABS_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_ABS_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_abs_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef typename type_traits<signature_argument_type>::real_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_abs_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef typename type_traits<signature_argument_type>::real_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

} // Namespace detail

/**
 * \brief Applies the \c std::abs function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::abs to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_abs_functor_traits<VectorExprT>::result_type abs(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_abs_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_abs_functor_traits<VectorExprT>::signature_result_type signature_result_type;

    return expression_type(ve(), ::std::abs<signature_result_type>);
}

/**
 * \brief Applies the \c std::abs function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::abs to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_abs_functor_traits<MatrixExprT>::result_type abs(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_abs_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_abs_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

    return expression_type(me(), ::std::abs<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_ABS_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief composite_output.cpp

\details An example of textual representations of units.

Output:
@verbatim

//[conversion_output_output
2 dyn
2 dyn
2 dyne
cm g s^-1
centimeter gram second^-1
dyn
dyne
n
nano
n
nano
F
farad
1 F
1 farad
nF
nanofarad
1 nF
1 nanofarad
n(cm g s^-1)
nano(centimeter gram second^-1)
//]

@endverbatim
**/
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/io.hpp>
#include <boost/units/scale.hpp>

#include <boost/units/detail/utility.hpp>

#include <boost/units/systems/si/capacitance.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/prefixes.hpp>

#include <iostream>
#include <sstream>

namespace boost {

namespace units {

//[composite_output_snippet_1

std::string name_string(const cgs::force&)
{
    return "dyne";
}

std::string symbol_string(const cgs::force&)
{
    return "dyn";
}

//]

}

}

int main()
{
    using namespace boost::units;
    using boost::units::cgs::centimeter;
    using boost::units::cgs::gram;
    using boost::units::cgs::second;
    using boost::units::cgs::dyne;

    //[composite_output_snippet_2]
    std::cout << 2.0 * dyne << std::endl
              << symbol_format << 2.0 * dyne << std::endl
              << name_format << 2.0 * dyne << std::endl
              << symbol_format << gram*centimeter/second << std::endl
              << name_format << gram*centimeter/second << std::endl
              << symbol_format << gram*centimeter/(second*second) << std::endl
              << name_format << gram*centimeter/(second*second) << std::endl
              << symbol_string(scale<10,static_rational<-9> >()) << std::endl
              << name_string(scale<10,static_rational<-9> >()) << std::endl
              << symbol_format << si::nano << std::endl
              << name_format << si::nano << std::endl
              << symbol_format << si::farad << std::endl
              << name_format << si::farad << std::endl
              << symbol_format << 1.0*si::farad << std::endl
              << name_format << 1.0*si::farad << std::endl
              << symbol_format << si::farad*si::nano << std::endl
              << name_format << si::farad*si::nano << std::endl
              << symbol_format << 1.0*si::farad*si::nano << std::endl
              << name_format << 1.0*si::farad*si::nano << std::endl
              << symbol_format << si::nano*gram*centimeter/second << std::endl
              << name_format << si::nano*gram*centimeter/second << std::endl;
    //]

    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 5, 3> test_double_5_3(const Eigen::Matrix<double, 5, 3> & M)
{
	return M;
}
void export_double_5_3()
{
	boost::python::def("test_double_5_3",test_double_5_3);
}