/* $Id: step-37.cc 28005 2013-01-10 16:31:06Z kronbichler $ */
/* Author: Katharina Kormann, Martin Kronbichler, Uppsala University, 2009-2012 */

/*    $Id: step-37.cc 28005 2013-01-10 16:31:06Z kronbichler $    */
/*                                                                */
/*    Copyright (C) 2009-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.                        */

// First include the necessary files from the deal.II library.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/timer.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_dof_accessor.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

// This includes the data structures for the efficient implementation of
// matrix-free methods or more generic finite element operators with the class
// MatrixFree.
#include <deal.II/matrix_free/matrix_free.h>
#include <deal.II/matrix_free/fe_evaluation.h>

#include <fstream>
#include <sstream>

namespace Step37
{
  using namespace dealii;

  // To be efficient, the operations performed in the matrix-free
  // implementation require knowledge of loop lengths at compile time, which
  // are given by the degree of the finite element. Hence, we collect the
  // values of the two template parameters that can be changed at one place in
  // the code. Of course, one could make the degree of the finite element a
  // run-time parameter by compiling the computational kernels for all degrees
  // that are likely (say, between 1 and 6) and selecting the appropriate
  // kernel at run time. Here, we simply choose second order $Q_2$ elements
  // and choose dimension 3 as standard.
  const unsigned int degree_finite_element = 2;
  const unsigned int dimension = 3;

  // @sect3{Equation data}

  // We define a variable coefficient function for the Poisson problem. It is
  // similar to the function in step-5 but we use the form $a(\mathbf
  // x)=\frac{1}{0.05 + 2\|\bf x\|^2}$ instead of a discontinuous one. It is
  // merely to demonstrate the possibilities of this implementation, rather
  // than making much sense physically. We define the coefficient in the same
  // way as functions in earlier tutorial programs. There is one new function,
  // namely a @p value method with template argument @p number.
  template <int dim>
  class Coefficient : public Function<dim>
  {
  public:
    Coefficient ()  : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;

    template <typename number>
    number value (const Point<dim,number> &p,
                  const unsigned int component = 0) const;

    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<double>            &values,
                             const unsigned int              component = 0) const;
  };

  // This is the new function mentioned above: Evaluate the coefficient for
  // abstract type @p number: It might be just a usual double, but it can also
  // be a somewhat more complicated type that we call VectorizedArray. This
  // data type is essentially a short array of doubles whose length depends on
  // the particular computer system in use. For example, systems based on
  // x86-64 support the streaming SIMD extensions (SSE), where the processor's
  // vector units can process two doubles (or four single-precision floats) by
  // one CPU instruction. Newer processors with support for the so-called
  // advanced vector extensions (AVX) with 256 bit operands can use four
  // doubles and eight floats, respectively. Vectorization is a
  // single-instruction/multiple-data (SIMD) concept, that is, one CPU
  // instruction is used to process multiple data values at once. Often,
  // finite element programs do not use vectorization explicitly as the
  // benefits of this concept are only in arithmetic intensive operations. The
  // bulk of typical finite element workloads are memory bandwidth limited
  // (operations on sparse matrices and vectors) where the additional
  // computational power is useless.
  //
  // Behind the scenes, optimized BLAS packages might heavily rely on
  // vectorization, though. Also, optimizing compilers might automatically
  // transform loops involving standard code into more efficient vectorized
  // form. However, the data flow must be very regular in order for compilers
  // to produce efficient code. For example, already the automatic
  // vectorization of the prototype operation that benefits from
  // vectorization, matrix-matrix products, fails on most compilers (as of
  // writing this tutorial in early 2012, neither gcc-4.6 nor the Intel
  // compiler v. 12 manage to produce useful vectorized code for the
  // FullMatrix::mmult function, and not even on the simpler case where
  // the matrix bounds are compile-time constants instead of run-time
  // constants as in FullMatrix::mmult). The main reason for this is that the
  // information to be processed at the innermost loop (that is where
  // vectorization is applied) is not necessarily a multiple of the vector
  // length, leaving parts of the resources unused. Moreover, the data that
  // can potentially be processed together might not be laid out in a
  // contiguous way in memory or not with the necessary alignment to address
  // boundaries that are needed by the processor. Or the compiler might not be
  // able to prove that.
  //
  // In the matrix-free implementation in deal.II, we have therefore chosen to
  // apply vectorization at the level which is most appropriate for finite
  // element computations: The cell-wise computations are typically exactly
  // the same for all cells (except for reading from and writing to vectors),
  // and hence SIMD can be used to process several cells at once. In all what
  // follows, you can think of a VectorizedArray to hold data from several
  // cells. For example, we evaluate the coefficient shown here not on a
  // simple point as usually done, but we hand it a
  // Point<dim,VectorizedArray<double> > point, which is actually a collection
  // of two points in the case of SSE2. Do not confuse the entries in
  // VectorizedArray<double> with the different coordinates of the
  // point. Indeed, the data is laid out such that p[0] returns a
  // VectorizedArray<double>, which in turn contains the x-coordinate for the
  // first point and the second point. You may access the coordinates
  // individually using e.g. p[0][j], j=0,1, but it is
  // recommended to define operations on a VectorizedArray as much as possible
  // in order to make use of vectorized operations.
  //
  // In the function implementation, we assume that the number type overloads
  // basic arithmetic operations, so we just write the code as usual. The
  // standard functions @p value and value_list that are virtual functions
  // contained in the base class are then computed from the templated function
  // with double type, in order to avoid duplicating code.
  template <int dim>
  template <typename number>
  number Coefficient<dim>::value (const Point<dim,number> &p,
                                  const unsigned int /*component*/) const
  {
    return 1. / (0.05 + 2.*p.square());
  }

  template <int dim>
  double Coefficient<dim>::value (const Point<dim> &p,
                                  const unsigned int component) const
  {
    return value<double>(p,component);
  }

  template <int dim>
  void Coefficient<dim>::value_list (const std::vector<Point<dim> > &points,
                                     std::vector<double>            &values,
                                     const unsigned int              component) const
  {
    Assert (values.size() == points.size(),
            ExcDimensionMismatch (values.size(), points.size()));
    Assert (component == 0,
            ExcIndexRange (component, 0, 1));

    const unsigned int n_points = points.size();
    for (unsigned int i=0; i<n_points; ++i)
      values[i] = value<double>(points[i],component);
  }

  // @sect3{Matrix-free implementation}

  // The following class, called LaplaceOperator, implements the
  // differential operator. For all practical purposes, it is a matrix, i.e.,
  // you can ask it for its size (member functions m(), n()) and
  // you can apply it to a vector (the various variants of the
  // vmult() function). The difference to a real matrix of course
  // lies in the fact that this class doesn't actually store the
  // elements of the matrix, but only knows how to compute the action
  // of the operator when applied to a vector.

  // In this program, we want to make use of the data cache for finite element
  // operator application that is integrated in deal.II. The main class that
  // collects all data is called MatrixFree. It contains mapping information
  // (Jacobians) and index relations between local and global degrees of
  // freedom. It also contains constraints like the ones from Dirichlet
  // boundary conditions (or hanging nodes, if we had any). Moreover, it can
  // issue a loop over all cells in %parallel, where it makes sure that only
  // cells are worked on that do not share any degree of freedom (this makes
  // the loop thread-safe when writing into destination vectors). This is a
  // more advanced strategy compared to the WorkStream class described in the
  // @ref threads module that serializes operations that might not be
  // thread-safe. Of course, to not destroy thread-safety, we have to be
  // careful when writing into class-global structures.
  //
  // First comes the implementation of the matrix-free class. It provides some
  // standard information we expect for matrices (like returning the
  // dimensions of the matrix), it implements matrix-vector multiplications in
  // several forms (transposed and untransposed), and it provides functions
  // for initializing the structure with data. The class has three template
  // arguments, one for the dimension (as many deal.II classes carry), one for
  // the degree of the finite element (which we need to enable efficient
  // computations through the FEEvaluation class), and one for the underlying
  // scalar type. We want to use double numbers (i.e., double
  // precision, 64-bit floating point) for the final matrix, but floats
  // (single precision, 32-bit floating point numbers) for the multigrid level
  // matrices (as that is only a preconditioner, and floats can be worked with
  // twice as fast).
  //
  // In this class, we store the actual MatrixFree object, the variable
  // coefficient that is evaluated at all quadrature points (so that we don't
  // have to recompute it during matrix-vector products), and a vector that
  // contains the diagonal of the matrix that we need for the multigrid
  // smoother. We choose to let the user provide the diagonal in this program,
  // but we could also integrate a function in this class to evaluate the
  // diagonal. Unfortunately, this forces us to define matrix entries at two
  // places, once when we evaluate the product and once for the diagonal, but
  // the work is still much less than when we compute sparse matrices.
  //
  // As a sidenote, if we implemented several different operations on the same
  // grid and degrees of freedom (like a mass matrix and a Laplace matrix), we
  // would have to have two classes like the current one for each of the
  // operators (maybe with a common base class). However, in that case, we
  // would not store a MatrixFree object in this class to avoid doing the
  // expensive work of pre-computing everything MatrixFree stores
  // twice. Rather, we would keep this object in the main class and simply
  // store a reference.
  //
  // @note Observe how we store the values for the coefficient: We use a
  // vector type AlignedVector<VectorizedArray<number> >
  // structure. One would think that one can use
  // std::vector<VectorizedArray<number> > as well, but there are
  // some technicalities with vectorization: A certain alignment of the data
  // with the memory address boundaries is required (essentially, a
  // VectorizedArray of 16 bytes length as in SSE needs to start at a memory
  // address that is divisible by 16). The chosen class makes sure that this
  // alignment is respected, whereas std::vector can in general not, which may
  // lead to segmentation faults at strange places for some systems or
  // suboptimal performance for other systems.
  template <int dim, int fe_degree, typename number>
  class LaplaceOperator : public Subscriptor
  {
  public:
    LaplaceOperator ();

    void clear();

    void reinit (const DoFHandler<dim>  &dof_handler,
                 const ConstraintMatrix &constraints,
                 const unsigned int      level = numbers::invalid_unsigned_int);

    unsigned int m () const;
    unsigned int n () const;

    void vmult (Vector<double> &dst,
                const Vector<double> &src) const;
    void Tvmult (Vector<double> &dst,
                 const Vector<double> &src) const;
    void vmult_add (Vector<double> &dst,
                    const Vector<double> &src) const;
    void Tvmult_add (Vector<double> &dst,
                     const Vector<double> &src) const;

    number el (const unsigned int row,
               const unsigned int col) const;
    void set_diagonal (const Vector<number> &diagonal);

    std::size_t memory_consumption () const;

  private:
    void local_apply (const MatrixFree<dim,number>    &data,
                      Vector<double>                      &dst,
                      const Vector<double>                &src,
                      const std::pair<unsigned int,unsigned int> &cell_range) const;

    void evaluate_coefficient(const Coefficient<dim> &function);

    MatrixFree<dim,number>      data;
    AlignedVector<VectorizedArray<number> > coefficient;

    Vector<number>  diagonal_values;
    bool            diagonal_is_available;
  };

  // This is the constructor of the @p LaplaceOperator class. All it does is
  // to subscribe to the general deal.II @p Subscriptor scheme that makes sure
  // that we do not delete an object of this class as long as it used
  // somewhere else, e.g. in a preconditioner.
  template <int dim, int fe_degree, typename number>
  LaplaceOperator<dim,fe_degree,number>::LaplaceOperator ()
    :
    Subscriptor()
  {}

  // The next functions return the number of rows and columns of the global
  // matrix (i.e. the dimensions of the operator this class represents, the
  // point of this tutorial program was, after all, that we don't actually
  // store the elements of the rows and columns of this operator). Since the
  // matrix is square, the returned numbers are the same. We get the number
  // from the vector partitioner stored in the data field (a partitioner
  // distributes elements of a vector onto a number of different machines if
  // programs are run in %parallel; since this program is written to run on
  // only a single machine, the partitioner will simply say that all elements
  // of the vector -- or, in the current case, all rows and columns of a
  // matrix -- are stored on the current machine).
  template <int dim, int fe_degree, typename number>
  unsigned int
  LaplaceOperator<dim,fe_degree,number>::m () const
  {
    return data.get_vector_partitioner()->size();
  }

  template <int dim, int fe_degree, typename number>
  unsigned int
  LaplaceOperator<dim,fe_degree,number>::n () const
  {
    return data.get_vector_partitioner()->size();
  }

  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::clear ()
  {
    data.clear();
    diagonal_is_available = false;
    diagonal_values.reinit(0);
  }

  // @sect4{Initialization}

  // Once we have created the multi-grid dof_handler and the constraints, we
  // can call the reinit function for each level of the multi-grid routine
  // (and the active cells). The main purpose of the reinit function is to
  // setup the  MatrixFree  instance for the problem. Also, the
  // coefficient is evaluated. For this, we need to activate the update flag
  // in the AdditionalData field of MatrixFree that enables the storage of
  // quadrature point coordinates in real space (by default, it only caches
  // data for gradients (inverse transposed Jacobians) and JxW values). Note
  // that if we call the reinit function without specifying the level (i.e.,
  // giving level = numbers::invalid_unsigned_int), we have told
  // the class to loop over the active cells.
  //
  // We also set one option regarding task parallelism. We choose to use the
  // @p partition_color strategy, which is based on subdivision of cells into
  // partitions where cells in partition $k$ (or, more precisely, the degrees
  // of freedom on these cells) only interact with cells in partitions $k-1$,
  // $k$, and $k+1$. Within each partition, cells are colored in such a way
  // that cells with the same color do not share degrees of freedom and can,
  // therefore, be worked on at the same time without interference. This
  // determines a task dependency graph that is scheduled by the Intel
  // Threading Building Blocks library. Another option would be the strategy
  // @p partition_partition, which performs better when the grid is more
  // unstructured. We could also manually set the size of chunks that form one
  // task in the scheduling process by setting @p tasks_block_size, but the
  // default strategy to let the function decide works well already.
  //
  // To initialize the coefficient, we directly give it the Coefficient class
  // defined above and then select the method
  // coefficient_function.value with vectorized number (which the
  // compiler can deduce from the point data type). The use of the
  // FEEvaluation class (and its template arguments) will be explained below.
  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::reinit (const DoFHandler<dim>  &dof_handler,
                                                 const ConstraintMatrix &constraints,
                                                 const unsigned int      level)
  {
    typename MatrixFree<dim,number>::AdditionalData additional_data;
    additional_data.tasks_parallel_scheme =
      MatrixFree<dim,number>::AdditionalData::partition_color;
    additional_data.level_mg_handler = level;
    additional_data.mapping_update_flags = (update_gradients | update_JxW_values |
                                            update_quadrature_points);
    data.reinit (dof_handler, constraints, QGauss<1>(fe_degree+1),
                 additional_data);
    evaluate_coefficient(Coefficient<dim>());
  }

  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::
  evaluate_coefficient (const Coefficient<dim> &coefficient_function)
  {
    const unsigned int n_cells = data.n_macro_cells();
    FEEvaluation<dim,fe_degree,fe_degree+1,1,number> phi (data);
    coefficient.resize (n_cells * phi.n_q_points);
    for (unsigned int cell=0; cell<n_cells; ++cell)
      {
        phi.reinit (cell);
        for (unsigned int q=0; q<phi.n_q_points; ++q)
          coefficient[cell*phi.n_q_points+q] =
            coefficient_function.value(phi.quadrature_point(q));
      }
  }

  // @sect4{Local evaluation of Laplace operator}

  // Here comes the main function of this class, the evaluation of the
  // matrix-vector product (or, in general, a finite element operator
  // evaluation). This is done in a function that takes exactly four
  // arguments, the MatrixFree object, the destination and source vectors, and
  // a range of cells that are to be worked on. The method
  // cell_loop in the MatrixFree class will internally call this
  // function with some range of cells that is obtained by checking which
  // cells are possible to work on simultaneously so that write operations do
  // not cause any race condition. Note that the total range of cells as
  // visible in this class is usually not equal to the number of (active)
  // cells in the triangulation.  In fact, "cell" may be the wrong term to
  // begin with, since it is rather a collection of quadrature points from
  // several cells, and the MatrixFree class groups the quadrature points of
  // several cells into one block to enable a higher degree of vectorization.
  // The number of such "cells" is stored in MatrixFree and can be queried
  // through MatrixFree::n_macro_cells(). Compared to the
  // deal.II cell iterators, in this class all cells are laid out in a plain
  // array with no direct knowledge of level or neighborship relations, which
  // makes it possible to index the cells by unsigned integers.
  //
  // The implementation of the Laplace operator is quite simple: First, we
  // need to create an object FEEvaluation that contains the computational
  // kernels and has data fields to store temporary results (e.g. gradients
  // evaluated on all quadrature points on a collection of a few cells). Note
  // that temporary results do not use a lot of memory, and since we specify
  // template arguments with the element order, the data is stored on the
  // stack (without expensive memory allocation). Usually, one only needs to
  // set two template arguments, the dimension as first argument and the
  // degree of the finite element as the second argument (this is equal to the
  // number of degrees of freedom per dimension minus one for FE_Q
  // elements). However, here we also want to be able to use float numbers for
  // the multigrid preconditioner, which is the last (fifth) template
  // argument. Therefore, we cannot rely on the default template arguments and
  // must also fill the third and fourth field, consequently. The third
  // argument specifies the number of quadrature points per direction and has
  // a default value equal to the degree of the element plus one. The fourth
  // argument sets the number of components (one can also evaluate
  // vector-valued functions in systems of PDEs, but the default is a scalar
  // element), and finally the last argument sets the number type.
  //
  // Next, we loop over the given cell range and then we continue with the
  // actual implementation:

Tell the FEEvaluation object the (macro)
  // cell we want to work on.
Read in the values of the source vectors
  // (@p read_dof_values), including the resolution of constraints. This
  // stores $u_\mathrm{cell}$ as described in the introduction.
Compute
  // the unit-cell gradient (the evaluation of finite element
  // functions). Since FEEvaluation can combine value computations with
  // gradient computations, it uses a unified interface to all kinds of
  // derivatives of order between zero and two. We only want gradients, no
  // values and no second derivatives, so we set the function arguments to
  // true in the gradient slot (second slot), and to false in the values slot
  // (first slot) and Hessian slot (third slot). Note that the FEEvaluation
  // class internally evaluates shape functions in an efficient way where one
  // dimension is worked on at a time (using the tensor product form of shape
  // functions and quadrature points as mentioned in the introduction). This
  // gives complexity equal to $\mathcal O(d^2 (p+1)^{d+1})$ for polynomial
  // degree $p$ in $d$ dimensions, compared to the naive approach with loops
  // over all local degrees of freedom and quadrature points that is used in
  // FEValues and costs $\mathcal O(d (p+1)^{2d})$.
Next comes the
  // application of the Jacobian transformation, the multiplication by the
  // variable coefficient and the quadrature weight. FEEvaluation has an
  // access function @p get_gradient that applies the Jacobian and returns the
  // gradient in real space. Then, we just need to multiply by the (scalar)
  // coefficient, and let the function @p submit_gradient apply the second
  // Jacobian (for the test function) and the quadrature weight and Jacobian
  // determinant (JxW). Note that the submitted gradient is stored in the same
  // data field as where it is read from in @p get_gradient. Therefore, you
  // need to make sure to not read from the same quadrature point again after
  // having called @p submit_gradient on that particular quadrature point. In
  // general, it is a good idea to copy the result of @p get_gradient when it
  // is used more often than once.
Next follows the summation over
  // quadrature points for all test functions that corresponds to the actual
  // integration step. For the Laplace operator, we just multiply by the
  // gradient, so we call the integrate function with the respective argument
  // set. If you have an equation where you test by both the values of the
  // test functions and the gradients, both template arguments need to be set
  // to true. Calling first the integrate function for values and then
  // gradients in a separate call leads to wrong results, since the second
  // call will internally overwrite the results from the first call. Note that
  // there is no function argument for the second derivative for integrate
  // step.
Eventually, the local contributions in the vector
  // $v_\mathrm{cell}$ as mentioned in the introduction need to be added into
  // the result vector (and constraints are applied). This is done with a call
  // to @p distribute_local_to_global, the same name as the corresponding
  // function in the ConstraintMatrix (only that we now store the local vector
  // in the FEEvaluation object, as are the indices between local and global
  // degrees of freedom).

  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::
  local_apply (const MatrixFree<dim,number>         &data,
               Vector<double>                       &dst,
               const Vector<double>                 &src,
               const std::pair<unsigned int,unsigned int> &cell_range) const
  {
    FEEvaluation<dim,fe_degree,fe_degree+1,1,number> phi (data);
    AssertDimension (coefficient.size(), data.n_macro_cells() * phi.n_q_points);

    for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
      {
        phi.reinit (cell);
        phi.read_dof_values(src);
        phi.evaluate (false,true,false);
        for (unsigned int q=0; q<phi.n_q_points; ++q)
          phi.submit_gradient (coefficient[cell*phi.n_q_points+q] *
                               phi.get_gradient(q), q);
        phi.integrate (false,true);
        phi.distribute_local_to_global (dst);
      }
  }

  // @sect4{vmult functions}

  // Now to the @p vmult function that is called externally: In addition to
  // what we do in a @p vmult_add function further down, we set the
  // destination to zero first. The transposed matrix-vector is needed for
  // well-defined multigrid preconditioner operations. Since we solve a
  // Laplace problem, this is the same operation, and we just refer to the
  // vmult operation.
  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::vmult (Vector<double>       &dst,
                                                const Vector<double> &src) const
  {
    dst = 0;
    vmult_add (dst, src);
  }

  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::Tvmult (Vector<double>       &dst,
                                                 const Vector<double> &src) const
  {
    dst = 0;
    vmult_add (dst,src);
  }

  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::Tvmult_add (Vector<double>       &dst,
                                                     const Vector<double> &src) const
  {
    vmult_add (dst,src);
  }

  // This function implements the loop over all cells. This is done with the
  // @p cell_loop of the MatrixFree class, which takes the operator() of this
  // class with arguments MatrixFree, OutVector, InVector, cell_range. Note
  // that we could also use a simple function as local operation in case we
  // had constant coefficients (all we need then is the MatrixFree, the
  // vectors and the cell range), but since the coefficient is stored in a
  // variable of this class, we cannot use that variant here. The cell loop is
  // automatically performed on several threads if multithreading is enabled
  // (this class uses a quite elaborate algorithm to work on cells that do not
  // share any degrees of freedom that could possibly give rise to race
  // conditions, using the dynamic task scheduler of the Intel Threading
  // Building Blocks).
  //
  // After the cell loop, we need to touch the constrained degrees of freedom:
  // Since the assembly loop automatically resolves constraints (just as the
  // ConstraintMatrix::distribute_local_to_global call does), it does not
  // compute any contribution for constrained degrees of freedom. In other
  // words, the entries for constrained DoFs remain zero after the first part
  // of this function, as if the matrix had empty rows and columns for
  // constrained degrees of freedom. On the other hand, iterative solvers like
  // CG only work for non-singular matrices, so we have to modify the
  // operation on constrained DoFs. The easiest way to do that is to pretend
  // that the sub-block of the matrix that corresponds to constrained DoFs is
  // the identity matrix, in which case application of the matrix would simply
  // copy the elements of the right hand side vector into the left hand
  // side. In general, however, one needs to make sure that the diagonal
  // entries of this sub-block are of the same order of magnitude as the
  // diagonal elements of the rest of the matrix.  Here, the domain extent is
  // of unit size, so we can simply choose unit size. If we had domains that
  // are far away from unit size, we would need to choose a number that is
  // close to the size of other diagonal matrix entries, so that these
  // artificial eigenvalues do not change the eigenvalue spectrum (and make
  // convergence with CG more difficult).
  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::vmult_add (Vector<double>       &dst,
                                                    const Vector<double> &src) const
  {
    data.cell_loop (&LaplaceOperator::local_apply, this, dst, src);

    const std::vector<unsigned int> &
    constrained_dofs = data.get_constrained_dofs();
    for (unsigned int i=0; i<constrained_dofs.size(); ++i)
      dst(constrained_dofs[i]) += src(constrained_dofs[i]);
  }

  // The next function is used to return entries of the matrix. Since this
  // class is intended not to store the matrix entries, it would make no sense
  // to provide access to all those elements. However, diagonal entries are
  // explicitly needed for the implementation of the Chebyshev smoother that
  // we intend to use in the multigrid preconditioner. This matrix is equipped
  // with a vector that stores the diagonal.
  template <int dim, int fe_degree, typename number>
  number
  LaplaceOperator<dim,fe_degree,number>::el (const unsigned int row,
                                             const unsigned int col) const
  {
    Assert (row == col, ExcNotImplemented());
    Assert (diagonal_is_available == true, ExcNotInitialized());
    return diagonal_values(row);
  }

  // Regarding the calculation of the diagonal, we expect the user to provide
  // a vector with the diagonal entries (and we will compute them in the code
  // below). We only need it for the level matrices of multigrid, not the
  // system matrix (since we only need these diagonals for the multigrid
  // smoother). Since we fill only elements into unconstrained entries, we
  // have to set constrained entries to one in order to avoid the same
  // problems as discussed above.
  template <int dim, int fe_degree, typename number>
  void
  LaplaceOperator<dim,fe_degree,number>::set_diagonal(const Vector<number> &diagonal)
  {
    AssertDimension (m(), diagonal.size());

    diagonal_values = diagonal;

    const std::vector<unsigned int> &
    constrained_dofs = data.get_constrained_dofs();
    for (unsigned int i=0; i<constrained_dofs.size(); ++i)
      diagonal_values(constrained_dofs[i]) = 1.0;

    diagonal_is_available = true;
  }

  // Eventually, we provide a function that calculates how much memory this
  // class uses. We just need to sum up the memory consumption in the
  // MatrixFree object and the memory for storing the other member
  // variables. As a remark: In 3D and for Cartesian meshes, most memory is
  // consumed for storing the vector indices on the local cells (corresponding
  // to local_dof_indices). For general (non-Cartesian) meshes, the cached
  // Jacobian transformation consumes most memory.
  template <int dim, int fe_degree, typename number>
  std::size_t
  LaplaceOperator<dim,fe_degree,number>::memory_consumption () const
  {
    return (data.memory_consumption () +
            coefficient.memory_consumption() +
            diagonal_values.memory_consumption() +
            MemoryConsumption::memory_consumption(diagonal_is_available));
  }

  // @sect3{LaplaceProblem class}

  // This class is based on the one in step-16. However, we replaced the
  // SparseMatrix<double> class by our matrix-free implementation, which means
  // that we can also skip the sparsity patterns. Notice that we define the
  // LaplaceOperator class with the degree of finite element as template
  // argument (the value is defined at the top of the file), and that we use
  // float numbers for the multigrid level matrices.
  //
  // The class also has a member variable to keep track of all the time we
  // spend on setting up the entire chain of data before we actually go about
  // solving the problem. In addition, there is an output stream (that is
  // disabled by default) that can be used to output details for the
  // individual setup operations instead of the summary only that is printed
  // out by default.
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem ();
    void run ();

  private:
    void setup_system ();
    void assemble_system ();
    void assemble_multigrid ();
    void solve ();
    void output_results (const unsigned int cycle) const;

    typedef LaplaceOperator<dim,degree_finite_element,double> SystemMatrixType;
    typedef LaplaceOperator<dim,degree_finite_element,float>  LevelMatrixType;

    Triangulation<dim>               triangulation;
    FE_Q<dim>                        fe;
    DoFHandler<dim>                  dof_handler;
    ConstraintMatrix                 constraints;

    SystemMatrixType                 system_matrix;
    MGLevelObject<LevelMatrixType>   mg_matrices;
    FullMatrix<float>                coarse_matrix;
    MGLevelObject<ConstraintMatrix>  mg_constraints;

    Vector<double>                   solution;
    Vector<double>                   system_rhs;

    double                           setup_time;
    ConditionalOStream               time_details;
  };

  // When we initialize the finite element, we of course have to use the
  // degree specified at the top of the file as well (otherwise, an exception
  // will be thrown at some point, since the computational kernel defined in
  // the templated LaplaceOperator class and the information from the finite
  // element read out by MatrixFree will not match).
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem ()
    :
    fe (degree_finite_element),
    dof_handler (triangulation),
    time_details (std::cout, false)
  {}

  // @sect4{LaplaceProblem::setup_system}

  // This is the function of step-16 with relevant changes due to the
  // LaplaceOperator class. We do not use adaptive grids, so we do not have to
  // compute edge matrices. Thus, all we do is to implement Dirichlet boundary
  // conditions through the ConstraintMatrix, set up the (one-dimensional)
  // quadrature that should be used by the matrix-free class, and call the
  // initialization functions.
  //
  // In the process, we output data on both the run time of the program as
  // well as on memory consumption, where we output memory data in megabytes
  // (1 million bytes).
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    Timer time;
    time.start ();
    setup_time = 0;

    system_matrix.clear();
    mg_matrices.clear();
    mg_constraints.clear();

    dof_handler.distribute_dofs (fe);
    dof_handler.distribute_mg_dofs (fe);

    std::cout << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << std::endl;

    constraints.clear();
    VectorTools::interpolate_boundary_values (dof_handler,
                                              0,
                                              ZeroFunction<dim>(),
                                              constraints);
    constraints.close();
    setup_time += time.wall_time();
    time_details << "Distribute DoFs & B.C.     (CPU/wall) "
                 << time() << "s/" << time.wall_time() << "s" << std::endl;
    time.restart();

    system_matrix.reinit (dof_handler, constraints);
    std::cout.precision(4);
    std::cout << "System matrix memory consumption:     "
              << system_matrix.memory_consumption()*1e-6
              << " MB."
              << std::endl;

    solution.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());

    setup_time += time.wall_time();
    time_details << "Setup matrix-free system   (CPU/wall) "
                 << time() << "s/" << time.wall_time() << "s" << std::endl;
    time.restart();

    // Next, initialize the matrices for the multigrid method on all the
    // levels. The function MGTools::make_boundary_list returns for each
    // multigrid level which degrees of freedom are located on a Dirichlet
    // boundary; we force these DoFs to have value zero by adding to the
    // ConstraintMatrix object a zero condition by using the command
    // ConstraintMatrix::add_line. Once this is done, we close the
    // ConstraintMatrix on each level so it can be used to read out indices
    // internally in the MatrixFree.
    const unsigned int nlevels = triangulation.n_levels();
    mg_matrices.resize(0, nlevels-1);
    mg_constraints.resize (0, nlevels-1);

    typename FunctionMap<dim>::type dirichlet_boundary;
    ZeroFunction<dim>               homogeneous_dirichlet_bc (1);
    dirichlet_boundary[0] = &homogeneous_dirichlet_bc;
    std::vector<std::set<unsigned int> > boundary_indices(triangulation.n_levels());
    MGTools::make_boundary_list (dof_handler,
                                 dirichlet_boundary,
                                 boundary_indices);
    for (unsigned int level=0; level<nlevels; ++level)
      {
        std::set<unsigned int>::iterator bc_it = boundary_indices[level].begin();
        for ( ; bc_it != boundary_indices[level].end(); ++bc_it)
          mg_constraints[level].add_line(*bc_it);

        mg_constraints[level].close();
        mg_matrices[level].reinit(dof_handler,
                                  mg_constraints[level],
                                  level);
      }
    coarse_matrix.reinit (dof_handler.n_dofs(0),
                          dof_handler.n_dofs(0));
    setup_time += time.wall_time();
    time_details << "Setup matrix-free levels   (CPU/wall) "
                 << time() << "s/" << time.wall_time() << "s" << std::endl;
  }

  // @sect4{LaplaceProblem::assemble_system}

  // The assemble function is significantly reduced compared to step-16. All
  // we need to do is to assemble the right hand side. That is the same as in
  // many other tutorial programs. In the end, we condense the constraints
  // from Dirichlet boundary conditions away from the right hand side.
  template <int dim>
  void LaplaceProblem<dim>::assemble_system ()
  {
    Timer time;
    QGauss<dim>  quadrature_formula(fe.degree+1);
    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values   | update_JxW_values);

    const unsigned int   dofs_per_cell = fe.dofs_per_cell;
    const unsigned int   n_q_points    = quadrature_formula.size();

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    const Coefficient<dim> coefficient;
    std::vector<double>    coefficient_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator cell = dof_handler.begin_active(),
                                                   endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        cell->get_dof_indices (local_dof_indices);
        fe_values.reinit (cell);
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          {
            double rhs_val = 0;
            for (unsigned int q=0; q<n_q_points; ++q)
              rhs_val += (fe_values.shape_value(i,q) * 1.0 *
                          fe_values.JxW(q));
            system_rhs(local_dof_indices[i]) += rhs_val;
          }
      }
    constraints.condense(system_rhs);
    setup_time += time.wall_time();
    time_details << "Assemble right hand side   (CPU/wall) "
                 << time() << "s/" << time.wall_time() << "s" << std::endl;
  }

  // @sect4{LaplaceProblem::assemble_multigrid}

  // Here is another assemble function. Again, it is simpler than assembling
  // matrices. We need to compute the diagonal of the Laplace matrices on the
  // individual levels, send the final matrices to the LaplaceOperator class,
  // and we need to compute the full matrix on the coarsest level (since that
  // is inverted exactly in the deal.II multigrid implementation).
  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid ()
  {
    Timer time;
    coarse_matrix = 0;
    QGauss<dim>  quadrature_formula(fe.degree+1);
    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_gradients  | update_inverse_jacobians |
                             update_quadrature_points | update_JxW_values);

    const unsigned int   dofs_per_cell = fe.dofs_per_cell;
    const unsigned int   n_q_points    = quadrature_formula.size();

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    const Coefficient<dim>    coefficient;
    std::vector<double>       coefficient_values (n_q_points);
    FullMatrix<double>        local_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>            local_diagonal (dofs_per_cell);

    const unsigned int n_levels = triangulation.n_levels();
    std::vector<Vector<float> > diagonals (n_levels);
    for (unsigned int level=0; level<n_levels; ++level)
      diagonals[level].reinit (dof_handler.n_dofs(level));

    std::vector<unsigned int> cell_no(triangulation.n_levels());
    typename DoFHandler<dim>::cell_iterator cell = dof_handler.begin(),
                                            endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        const unsigned int level = cell->level();
        cell->get_mg_dof_indices (local_dof_indices);
        fe_values.reinit (cell);
        coefficient.value_list (fe_values.get_quadrature_points(),
                                coefficient_values);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          {
            double local_diag = 0;
            for (unsigned int q=0; q<n_q_points; ++q)
              local_diag += ((fe_values.shape_grad(i,q) *
                              fe_values.shape_grad(i,q)) *
                             coefficient_values[q] * fe_values.JxW(q));
            local_diagonal(i) = local_diag;
          }
        mg_constraints[level].distribute_local_to_global(local_diagonal,
                                                         local_dof_indices,
                                                         diagonals[level]);

        if (level == 0)
          {
            local_matrix = 0;

            for (unsigned int i=0; i<dofs_per_cell; ++i)
              for (unsigned int j=0; j<dofs_per_cell; ++j)
                {
                  double add_value = 0;
                  for (unsigned int q=0; q<n_q_points; ++q)
                    add_value += (fe_values.shape_grad(i,q) *
                                  fe_values.shape_grad(j,q) *
                                  coefficient_values[q] *
                                  fe_values.JxW(q));
                  local_matrix(i,j) = add_value;
                }
            mg_constraints[0].distribute_local_to_global (local_matrix,
                                                          local_dof_indices,
                                                          coarse_matrix);
          }
      }

    for (unsigned int level=0; level<n_levels; ++level)
      mg_matrices[level].set_diagonal (diagonals[level]);

    setup_time += time.wall_time();
    time_details << "Assemble MG diagonal       (CPU/wall) "
                 << time() << "s/" << time.wall_time() << "s" << std::endl;
  }

  // @sect4{LaplaceProblem::solve}

  // The solution process again looks like step-16. We now use a Chebyshev
  // smoother instead of SOR (SOR would be very difficult to implement because
  // we do not have the matrix elements available explicitly, and it is
  // difficult to make it work efficiently in %parallel). The multigrid
  // classes provide a simple interface for using the Chebyshev smoother which
  // is defined in a preconditioner class: MGSmootherPrecondition.
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {
    Timer time;
    GrowingVectorMemory<>   vector_memory;

    MGTransferPrebuilt<Vector<double> > mg_transfer;
    mg_transfer.build_matrices(dof_handler);
    setup_time += time.wall_time();
    time_details << "MG build transfer time     (CPU/wall) " << time()
                 << "s/" << time.wall_time() << "s\n";
    time.restart();

    MGCoarseGridHouseholder<float, Vector<double> > mg_coarse;
    mg_coarse.initialize(coarse_matrix);
    setup_time += time.wall_time();
    time_details << "MG coarse time             (CPU/wall) " << time()
                 << "s/" << time.wall_time() << "s\n";
    time.restart();

    typedef PreconditionChebyshev<LevelMatrixType,Vector<double> > SMOOTHER;
    MGSmootherPrecondition<LevelMatrixType, SMOOTHER, Vector<double> >
    mg_smoother(vector_memory);

    // Then, we initialize the smoother with our level matrices and the
    // mandatory additional data for the Chebyshev smoother. We use quite a
    // high degree here (6), since matrix-vector products are comparably cheap
    // and more parallel than the level-transfer operations. We choose to
    // smooth out a range of $[1.2 \hat{\lambda}_{\max}/10,1.2
    // \hat{\lambda}_{\max}]$ in the smoother where $\hat{\lambda}_{\max}$ is
    // an estimate of the largest eigenvalue. In order to compute that
    // eigenvalue, the Chebyshev initializations performs a few steps of a CG
    // algorithm without preconditioner. Since the highest eigenvalue is
    // usually the easiest one to find and a rough estimate is enough, we
    // choose 10 iterations.
    typename SMOOTHER::AdditionalData smoother_data;
    smoother_data.smoothing_range = 10.;
    smoother_data.degree = 6;
    smoother_data.eig_cg_n_iterations = 10;
    mg_smoother.initialize(mg_matrices, smoother_data);

    MGMatrix<LevelMatrixType, Vector<double> >
    mg_matrix(&mg_matrices);

    Multigrid<Vector<double> > mg(dof_handler,
                                  mg_matrix,
                                  mg_coarse,
                                  mg_transfer,
                                  mg_smoother,
                                  mg_smoother);
    PreconditionMG<dim, Vector<double>,
                   MGTransferPrebuilt<Vector<double> > >
                   preconditioner(dof_handler, mg, mg_transfer);

    // Finally, write out the memory consumption of the Multigrid object (or
    // rather, of its most significant components, since there is no built-in
    // function for the total multigrid object), then create the solver object
    // and solve the system. This is very easy, and we didn't even see any
    // difference in the solve process compared to step-16. The magic is all
    // hidden behind the implementation of the LaplaceOperator::vmult
    // operation. Note that we print out the solve time and the accumulated
    // setup time through standard out, i.e., in any case, whereas detailed
    // times for the setup operations are only printed in case the flag for
    // detail_times in the constructor is changed.
    const std::size_t multigrid_memory
      = (mg_matrices.memory_consumption() +
         mg_transfer.memory_consumption() +
         coarse_matrix.memory_consumption());
    std::cout << "Multigrid objects memory consumption: "
              << multigrid_memory * 1e-6
              << " MB."
              << std::endl;

    SolverControl           solver_control (1000, 1e-12*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);
    setup_time += time.wall_time();
    time_details << "MG build smoother time     (CPU/wall) " << time()
                 << "s/" << time.wall_time() << "s\n";
    std::cout << "Total setup time               (wall) " << setup_time
              << "s\n";

    time.reset();
    time.start();
    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);

    std::cout << "Time solve ("
              << solver_control.last_step()
              << " iterations)  (CPU/wall) " << time() << "s/"
              << time.wall_time() << "s\n";
  }

  // @sect4{LaplaceProblem::output_results}

  // Here is the data output, which is a simplified version of step-5. We use
  // the standard VTU (= compressed VTK) output for each grid produced in the
  // refinement process.
  template <int dim>
  void LaplaceProblem<dim>::output_results (const unsigned int cycle) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, "solution");
    data_out.build_patches ();

    std::ostringstream filename;
    filename << "solution-"
             << cycle
             << ".vtu";

    std::ofstream output (filename.str().c_str());
    data_out.write_vtu (output);
  }

  // @sect4{LaplaceProblem::run}

  // The function that runs the program is very similar to the one in
  // step-16. We make less refinement steps in 3D compared to 2D, but that's
  // it.
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    for (unsigned int cycle=0; cycle<9-dim; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube (triangulation, 0., 1.);
            triangulation.refine_global (3-dim);
          }
        triangulation.refine_global (1);
        setup_system ();
        assemble_system ();
        assemble_multigrid ();
        solve ();
        output_results (cycle);
        std::cout << std::endl;
      };
  }
}

// @sect3{The main function}

// This is as in most other programs.
int main ()
{
  try
    {
      using namespace Step37;

      deallog.depth_console(0);
      LaplaceProblem<dimension> laplace_problem;
      laplace_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


//  (C) Copyright Eric Niebler 2005.
//  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)

#define BOOST_TEST_MODULE accumulators
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::min> > acc;

    acc(1);
    BOOST_CHECK_EQUAL(1, (min)(acc));

    acc(0);
    BOOST_CHECK_EQUAL(0, (min)(acc));

    acc(2);
    BOOST_CHECK_EQUAL(0, (min)(acc));
}

BOOST_AUTO_TEST_CASE( min_accumulator )
{
  test_stat();
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 FCPPT_MPL_PARTIAL_SUMS_HPP_INCLUDED
#define FCPPT_MPL_PARTIAL_SUMS_HPP_INCLUDED

#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/vector_c.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief Calculates a new sequence that consists of the sums of the old sequence

\ingroup fcpptmpl

For the integral constantsn (c_1, ..., c_n) in \a Sequence, the resulting
sequence will consist of the values (0, c_1, c_1 + c_2, ..., c_1 + ... c_n)

\snippet mpl/various.cpp mpl_partial_sums

\tparam Sequence An MPL sequence of integral constant types
*/
template<
	typename Sequence
>
struct partial_sums
:
boost::mpl::fold<
	Sequence,
	// mpl::plus<> converts its operands, so int is most conservative
	boost::mpl::vector_c<
		int,
		0
	>,
	boost::mpl::push_back<
		boost::mpl::_1,
		boost::mpl::plus<
			boost::mpl::back<
				boost::mpl::_1
			>,
			boost::mpl::_2
		>
	>
>
{
};

FCPPT_PP_POP_WARNING

}
}

#endif


#include <iostream>
#include <mutex>
#include <boost/thread/thread.hpp>
#include <boost/thread/tss.hpp>
#include <chrono>
#include <atomic>
#include <iomanip>

#define LOOP 200000000
#define TARGET 50000000
#define BENCH_RUNS 5

using namespace std;

template <typename V=int> class counter {
private:
  atomic<V> gcount;
  mutex m;
  boost::thread_specific_ptr<V> lcount;
  boost::thread_specific_ptr<V> lgcount;

public:

 void init(){ //instatiate thread local object parts
    lcount.reset(new V(0));
    lgcount.reset(new V(0));
  }

  void weakInc(V tosum={1}){ //weakUpdate
    *lcount += tosum;
  }

  void strongInc(V tosum={1}){ //strongUpdate
   gcount.fetch_add(tosum);
  }

  V weakValue(){ //weakRead
    V readed = *lgcount + *lcount;
    return readed;
  }

  V strongValue(){ //strongRead
    V readed = gcount + *lcount;
    return readed;
  }

  void merge(){ //mergeToGlobal
    gcount.fetch_add(*lcount); //merging thread local state with global state
    *lgcount = gcount;  // updating the snapshot
    *lcount = 0; //resetting thread local state
  }

  V atomicInc(){
    V oldValue = gcount.load();
    V newValue;

    do{
        if (oldValue < TARGET) newValue = oldValue + 1;
        else break;
    }
    while (!gcount.compare_exchange_weak(oldValue, newValue));

    return oldValue;
  }

  ////////////////ONLY FOR DEBUGGING/BENCHMARKING PURPOSES//////
  V getLgcount(){
    V i = *lgcount;
    return i;
  }

  V getLcount(){
    V i = *lcount;
    return i;
  }

  V readP(){ //To read the global state on main
      V ret;
      ret=gcount;
    return ret;
  }

  void reset(){
    gcount = 0;
  }

};

counter<int> mdt;
vector<int> NTHREADS;
int SYNCFREQ [6] = {1,8,64,512,4096,32768};
boost::condition_variable cond;
boost::mutex mut;
int thresholdThreadNumber = 0;

void work(int syncFreqIndex, int nThreadsIndex){
  static mutex m;
  bool switchToStrong = false;

  mdt.init();
  mdt.merge();

  int threshold = TARGET - ((NTHREADS[nThreadsIndex]) * SYNCFREQ[syncFreqIndex]);

  for (int i=0; i < LOOP; i++){
    if(!switchToStrong){
      if(i%SYNCFREQ[syncFreqIndex] == 1){
        mdt.merge();
      }

      if(mdt.weakValue() >= threshold) {
        mdt.merge();

        mut.lock();
        thresholdThreadNumber++;
        if(thresholdThreadNumber==NTHREADS[nThreadsIndex]) cond.notify_all();
        mut.unlock();

        boost::unique_lock<boost::mutex> lock(mut);
        while(thresholdThreadNumber < NTHREADS[nThreadsIndex]){
            cond.wait(lock);
        }

        switchToStrong = true;
        continue;
      }

      mdt.weakInc();

      if(i%SYNCFREQ[syncFreqIndex] == 0){
        mdt.merge();
      }

    }
    else{
      if (mdt.atomicInc() >= TARGET){
        break;
      }
    }
  }
}

void benchmarkPerFreq(int syncFreqIndex){
    using namespace std::chrono;
    for(int k = 0; k < NTHREADS.size(); k++){
      std::list<double> times;
      std::list<double> counters;
      std::list<double> throughs;

      int threshold = TARGET - ((NTHREADS[k]) * SYNCFREQ[syncFreqIndex]);

      for(int i= 0; i< BENCH_RUNS; i++){
        steady_clock::time_point t1 = steady_clock::now();

        thresholdThreadNumber = 0;
        boost::thread_group threads;
        for (int a=0; a < NTHREADS[k]; a++){
          threads.create_thread(boost::bind(work, boost::cref(syncFreqIndex),boost::cref(k)));
        }

        threads.join_all();

        steady_clock::time_point t2 = steady_clock::now();

        duration<double> ti = duration_cast<duration<double>>(t2 - t1);

        times.push_back(ti.count());
        counters.push_back(mdt.readP());
        throughs.push_back(mdt.readP()/ti.count());

        mdt.reset();
      }

      double sumTimes = 0;
      for(double t: times){
        sumTimes += t;
      }

      double finalTime = sumTimes/times.size();
      cout << fixed;

      double sumCounters = 0;
      for(double c: counters){
        sumCounters += c;
      }
      double finalCounter = (int)(sumCounters/counters.size());

      double sumThroughs = 0;
      for(double th: throughs){
        sumThroughs += th;
      }
      double finalThroughs = sumThroughs/throughs.size();

      //double overshoot = ((finalCounter-TARGET)*100)/TARGET;

      cout << (int)finalThroughs << "," << NTHREADS[k] << endl;
    }
}

int main(int argc, char** argv){
    for(int i=1; i<argc; i++){
      NTHREADS.push_back(atoi(argv[i]));
    }

    for(int i=0; i<6;i++){
        cout << "***************SYNCFREQ: " << SYNCFREQ[i] << " ***************"<< endl;
        benchmarkPerFreq(i);
        cout << endl;
    }

    return 0;
}


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

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "map_request.pb.h"

#include "yaml-cpp/yaml.h"
#include <ros/ros.h>
#include <fstream>
#include <boost/filesystem.hpp>

namespace gazebo
{
typedef const boost::shared_ptr<const dronedoc::msgs::MapRequest> MapRequestPtr;

class MapCreator : public WorldPlugin
{
  transport::NodePtr node;
  transport::PublisherPtr imagePub;
  transport::SubscriberPtr commandSubscriber;
  physics::WorldPtr world;

  public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    node = transport::NodePtr(new transport::Node());
    world = _parent;
    // Initialize the node with the world name
    node->Init(world->GetName());
    ROS_INFO("Subscribing to: ~/collision_map/command");
    commandSubscriber = node->Subscribe("~/collision_map/command",
      &MapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
  }

  public: void create(MapRequestPtr &msg)
  {
    ROS_INFO("Received message");

    ROS_INFO("Creating collision map with origin at (%f, %f), width %f and height %f "
              "with collision projected from z = %f\n"
              "Resolution = %f m\n"
              "Occupied spaces will be filled with: %d",
             msg->origin().x(),
             msg->origin().y(),
             msg->map_width(),
             msg->map_height(),
             msg->height(),
             msg->resolution(),
             msg->threshold());

    int count_vertical = msg->map_height() / msg->resolution();
    int count_horizontal = msg->map_width() / msg->resolution();

    if (count_vertical == 0 || count_horizontal == 0)
    {
      ROS_ERROR("Image has a zero dimensions, check coordinates");
      return;
    }
    double x,y;

    boost::gil::gray8_pixel_t fill(255-msg->threshold());
    boost::gil::gray8_pixel_t blank(255);
    boost::gil::gray8_image_t image(count_horizontal, count_vertical);

    double dist;
    std::string entityName;
    math::Vector3 start, end;
    start.z = msg->height();
    end.z = 0.001;

    gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
    engine->InitForThread();
    gazebo::physics::RayShapePtr ray =
      boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
        engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    ROS_INFO("Rasterizing model and checking collisions");
    boost::gil::fill_pixels(image._view, blank);

    int progress = 0;
    for (int i = 0; i < count_vertical; ++i)
    {
      if(progress != std::round(i * 100.0 / count_vertical)){
        ROS_INFO("%d%% complete", progress);
      }
      progress = std::round(i * 100.0 / count_vertical);
      x = msg->origin().x();
      y = i * msg->resolution() + msg->origin().y();
      for (int j = 0; j < count_horizontal; ++j)
      {
        x += msg->resolution();

        start.x = end.x = x;
        start.y = end.y = y;
        ray->SetPoints(start, end);
        ray->GetIntersection(dist, entityName);
        if (!entityName.empty())
        {
          image._view(j,(count_vertical-1)-i) = fill;
        }
      }
    }

    boost::filesystem::path fname(msg->filename());
    fname = boost::filesystem::absolute(fname);

    ROS_INFO("Completed calculations, writing to image");
    if (!msg->filename().empty())
    {
      boost::gil::gray8_view_t view = image._view;
      boost::gil::png_write_view(fname.string(), view);
    }

    ROS_INFO("Writing map info");

    YAML::Emitter out;
    out << YAML::BeginMap;
    out << YAML::Key << "image";
    out << YAML::Value << fname.filename().string();
    out << YAML::Key << "resolution";
    out << YAML::Value << msg->resolution();
    out << YAML::Key << "negate";
    out << YAML::Value << 0;
    out << YAML::Key << "free_thresh";
    out << YAML::Value << 0.196;
    out << YAML::Key << "occupied_thresh";
    out << YAML::Value << 0.65;
    out << YAML::Key << "origin";
    out << YAML::Value;
    out << YAML::Flow;
    out << YAML::BeginSeq << msg->origin().x() << msg->origin().y() << 0.00 << YAML::EndSeq;

    std::ofstream yaml_out(fname.replace_extension(".yaml").string());
    yaml_out << out.c_str();
    yaml_out.close();

    ROS_INFO("Map built successfully");
  }
};

// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(MapCreator)
}


#include "catch.hpp"

#include "libirc/molecule.h"

#include "libirc/periodic_table.h"

#include <cassert>
#include <iostream>

#ifdef HAVE_ARMA
#include <armadillo>
using arma::vec3;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
#else
#error
#endif

using namespace irc;

TEST_CASE("Molecule") {

  using namespace std;
  using namespace molecule;
  using namespace periodic_table;

  const auto molecule = Molecule<vec3>{
      {{1, {0.0, 1.1, 2.2}}, {2, {0.0, 1.1, 2.2}}, {3, {0.0, 1.1, 2.2}}}};

  CHECK(mass(molecule) == Approx(masses[1] + masses[2] + masses[3]));

  SECTION("Position multiplier") {
    auto multiplier = 2.;

    auto scaled_molecule = molecule;
    multiply_positions(scaled_molecule, multiplier);

    for (std::size_t i{0}; i < scaled_molecule.size(); i++) {
      for (std::size_t j{0}; j < 3; j++) {
        CHECK(scaled_molecule[i].position(j) ==
              Approx(multiplier * molecule[i].position(j)));
      }
    }

    scaled_molecule = molecule * multiplier;
    for (std::size_t i{0}; i < scaled_molecule.size(); i++) {
      for (std::size_t j{0}; j < 3; j++) {
        CHECK(scaled_molecule[i].position(j) ==
              Approx(multiplier * molecule[i].position(j)));
      }
    }

    scaled_molecule = multiplier * molecule;
    for (std::size_t i{0}; i < scaled_molecule.size(); i++) {
      for (std::size_t j{0}; j < 3; j++) {
        CHECK(scaled_molecule[i].position(j) ==
              Approx(multiplier * molecule[i].position(j)));
      }
    }
  }
}


#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <math.h>
#include <chrono>
#include <exception>
#include <stdexcept>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include "ptss_dse.hpp"
#include "ptss_config.hpp"
#include "ptss_pkmin.hpp"
#include "ptss_dse.hpp"

using namespace std;

/* Evaluate the relative difference of two objective functions */
double eval_rel(const alloc2_t &pt1, const alloc2_t &pt2) {
    double f1 = compute_pkpower(pt1);
    double f2 = compute_pkpower(pt2);

    if (f1 == 0 && f2 == 0)
        return 0.0;
    else if (f1 != 0) {
        double rel = (abs(f1-f2))/abs(f1);
        return rel;
    }
    else {
        double rel = (abs(f1-f2))/abs(f2);
        return rel;
    }
    return 0.0;
}

/* Evaluate the relative difference of two objective functions */
double eval_rel2(const alloc2_t &pt1, const alloc2_t &pt2) {
    double f1 = compute_execution_time(pt1);
    double f2 = compute_execution_time(pt2);

    if (f1 == 0 && f2 == 0)
        return 0.0;
    else if (f1 != 0) {
        double rel = (abs(f1-f2))/abs(f1);
        return rel;
    }
    else {
        double rel = (abs(f1-f2))/abs(f2);
        return rel;
    }
    return 0.0;
}

/* Nowhere else to go (lowest allocation possible) */
bool hit_the_wall(const alloc2_t &pt1) {
    for (unsigned int i = 0; i < pt1.size(); i++) {
        if (LLIM[pt1[i].bench_id] < pt1[i].alloc)
            return false;
    }
    return true;
}

double ptss_DSE_hrt::ptss_pkmin() {
    struct timeval t1, t2;
    gettimeofday(&t1,NULL);
    alloc2_t tmp = this->cvx_point;
    alloc2_t opt_dggd;
    if (tmp.size() < NPH) {
        throw invalid_argument( "Invalid Initial Point for DGGD\n" );
    }
    set<unsigned int> phs;
    unsigned int ph;
    bool stop = false; /* Stopping criteria */
    unsigned int iter = 0;

    /* Test for negative execution time */
    // if(compute_execution_time(tmp) < 0) {
    //     cout <<"\n\n\n"<< "DGGD Init Value Illegal" <<tmp<<"et" << compute_execution_time(tmp)<<"\n\n\n";
    //     for (auto it = phs.begin(); it != phs.end(); it++) {
    //         cout << *it << endl;
    //     }
    //     throw invalid_argument("--------------------Illegal Value of tmp*****");
    // }

    //  cout << "iter-" << iter << ",point:" << tmp << ",power:" << compute_pkpower(tmp) << endl;
     do { /* Check after balancing */
        // cout << "iter-" << iter << ",point:" << tmp << ",power:" << compute_pkpower(tmp) << endl;
        iter++;
        opt_dggd = tmp;
        phs = compute_bottleneck(tmp);

         /* Test for negative execution time */
        // cout << "DGGD-Pre@"<<iter<<":"<<tmp<<",et:" << compute_execution_time(tmp)<<"\n";

        // cout << "DGGD:PHS SIZE : " <<phs.size()<<endl;
        for (auto it = phs.begin(); it != phs.end(); it++) {
            ph = *it;
            // cout << "DGGD"<<iter<<" : Peak Phase:"<<ph<<",power:"<<compute_pkpower(tmp)<<endl;
            /* Decrement the number of cores (if possible) (Guaranteed to reduce power if successful) */
            if (tmp[ph].alloc > LLIM[this->bench[ph]]) {
                // cout << "DGGD@"<<iter<<",ph:"<<ph<<",allocated:"<<tmp[ph].alloc<<",LLIM:"<<LLIM[this->bench[ph]]<< endl;
                tmp[ph].alloc--;
            } else if (tmp[ph].alloc < LLIM[this->bench[ph]]) {
                cout << "\n\n\nph:"<<ph<<" is already below the lower limit\n\n\n"<<endl;
                throw invalid_argument("already below the lower limit");
            }
            // cout << "DGGD"<<iter<<" : (New) Peak Phase:"<<ph<<",power:"<<compute_pkpower(tmp)<<endl;
        }

        /* Test for negative execution time */
        if(compute_execution_time(tmp) < 0) {
            cout << "DGGD@"<<iter<<":"<<tmp<<",et:" << compute_execution_time(tmp)<<"\n";
            throw invalid_argument("Negative Value of ET not allowed");
        }

        // cout << "iter-" << iter << ",point:" << tmp << ",power:" << compute_pkpower(tmp) << "\n\n";
        stop = (compute_execution_time(tmp) > this->deadline) || (eval_rel(tmp,opt_dggd) < 1e-3);
        if (!stop) continue;

        /* Balance the allocation (Will not change the power consumption) */
        balance_out(tmp);
        if(compute_execution_time(tmp) < 0) {
            cout << "DGGD-Balancing-Out@"<<iter<<":"<<tmp<<",et:" << compute_execution_time(tmp)<<"\n";
            throw invalid_argument("Negative Value of ET not allowed");
        }
        stop = (compute_execution_time(tmp) > this->deadline) || (eval_rel(tmp,opt_dggd) < 1e-3);
    } while (!stop);

    gettimeofday(&t2,NULL);
    long long diff = (t2.tv_sec-t1.tv_sec)*1000000 + (t2.tv_usec-t1.tv_usec);
    // cout << "dbcks{iter|elapsed time},"<<iter<<","<<diff<<endl;
    // cout << "DGGD Point : "<<opt_dggd<<"\n\n";
    this->dggd_point = opt_dggd;
    return compute_pkpower(opt_dggd);
}

double ptss_DSE_hrt::ptss_etmin() {
    alloc2_t tmp = this->ext_point2;
    alloc2_t opt_dggd;
    set<unsigned int> phs;
    unsigned int ph;
    bool stop = false; /* Stopping criteria */
    unsigned int iter = 0;

    // cout << "iter-" << iter << ",point:" << tmp \
    //      << ",power:" << compute_pkpower(tmp) \
    //      << ",et:"<<compute_execution_time(tmp)<<endl;

    do { /* Check after balancing */
        iter++;
        opt_dggd = tmp;

        /* Increment the phases that results in maximum increase in et */
        phs = compute_maxgrad(tmp);
        for (auto it = phs.begin(); it != phs.end(); it++) {
            ph = *it;
            // cout << "DGGD"<<iter<<" : Peak Phase:"<<ph<<",power:"<<compute_pkpower(tmp)<<endl;
            /* Increment the number of cores (if possible) (Guaranteed to reduce power if successful) */
            if (tmp[ph].alloc < ULIM) {
                // cout << "DGGD@"<<iter<<",ph:"<<ph<<",allocated:"<<tmp[ph].alloc<<",LLIM:"<<LLIM[this->bench[ph]]<< endl;
                tmp[ph].alloc++;
            } else if (tmp[ph].alloc > ULIM) {
                cout << "\n\n\nph:"<<ph<<" is already below the lower limit\n\n\n"<<endl;
                throw invalid_argument("already below the lower limit");
            }
            // cout << "DGGD"<<iter<<" : (New) Peak Phase:"<<ph<<",power:"<<compute_pkpower(tmp)<<endl;
        }

        /* Test for negative execution time */
        if(compute_pkpower(tmp) < 0) {
            cout << "DGGD2@"<<iter<<":"<<tmp<<",pkp:" << compute_execution_time(tmp)<<"\n";
            throw invalid_argument("Negative Value of pkp not allowed");
        }
        // cout << "iter-" << iter << ",point:" << tmp \
        //  << ",power:" << compute_pkpower(tmp) \
        //  << ",et:"<<compute_execution_time(tmp)<<endl;

        stop = (compute_pkpower(tmp) > this->pkp_cap) || (eval_rel2(tmp,opt_dggd) < 1e-3);
        if (!stop) continue;

        // /* Balance the allocation (Will not change the power consumption) */
        // balance_out(tmp);

        // if(compute_pkpower(tmp) < 0) {
        //     cout << "DGGD2-Balancing-Out@"<<iter<<":"<<tmp<<",pkp:" << compute_execution_time(tmp)<<"\n";
        //     throw invalid_argument("Negative Value of pkp not allowed");
        // }
        // stop = (compute_pkpower(tmp) > this->pkp_cap) || (eval_rel2(tmp,opt_dggd) < 1e-3);

        // cout << "(post balancing) iter-" << iter << ",point:" << tmp \
        //  << ",power:" << compute_pkpower(tmp) \
        //  << ",et:"<<compute_execution_time(tmp)<<endl;

    } while (!stop);
    /* Balance Out Optimal Allocation */
    balance_out(opt_dggd);
    // cout << "(2nd post balancing) iter-" << iter << ",point:" << opt_dggd \
    //      << ",power:" << compute_pkpower(opt_dggd) \
    //      << ",et:"<<compute_execution_time(opt_dggd)<<endl;
    this->dggd_point2 = opt_dggd;
    return compute_execution_time(opt_dggd);
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::distributions::poisson::include.hpp 	//
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_POISSON_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_POISSON_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/poisson.hpp>

#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/description.hpp>
//#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/derivative_log_unnormalized_pdf.hpp>
//#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/is_math_distribution.hpp>
//#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/poisson/os.hpp>

#endif