// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm statistic count
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/data_customization_point/vector.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_array.h"
#include "fern/feature/core/data_customization_point/masked_scalar.h"
#include "fern/algorithm/statistic/count.h"

namespace fa = fern::algorithm;

template<
    class A,
    class R>
void verify_value(
    A const& array,
    A const& value,
    R const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    R result_we_get;
    fa::statistic::count(sequential, array, value, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(d0_array)
{
    verify_value<int8_t, uint64_t>(-5, 6, 0u);
    verify_value<int8_t, uint64_t>(-5, -5, 1u);
    verify_value<double, uint64_t>(-5.5, -5.5, 1u);
    verify_value<double, uint64_t>(-5.5, -5.4, 0u);
}

BOOST_AUTO_TEST_CASE(masked_d0_array)
{
    fa::SequentialExecutionPolicy sequential;

    fern::MaskedScalar<int32_t> constant;
    fern::MaskedScalar<uint64_t> result_we_get;

    // MaskedScalar with non-masking count. ----------------------------------
    // Constant is not masked.
    constant.mask() = false;
    constant.value() = 5;
    result_we_get.value() = 9;
    result_we_get.mask() = false;
    BOOST_CHECK(!constant.mask());
    fa::statistic::count(sequential, constant, 5, result_we_get);
    BOOST_CHECK(!result_we_get.mask());
    BOOST_CHECK_EQUAL(result_we_get.value(), 1u);

    // Constant is masked.
    constant.mask() = true;
    constant.value() = 5;
    result_we_get.value() = 9;
    result_we_get.mask() = false;
    BOOST_CHECK(constant.mask());
    fa::statistic::count(sequential, constant, 5, result_we_get);
    BOOST_CHECK(!result_we_get.mask());
    BOOST_CHECK_EQUAL(result_we_get.value(), 1u);

    // MaskedScalar with masking count. --------------------------------------
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<bool>, fa::SkipNoData>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

    // Constant is not masked.
    constant.value() = 5;
    constant.mask() = false;
    result_we_get.value() = 9;
    result_we_get.mask() = false;
    BOOST_CHECK(!constant.mask());
    BOOST_CHECK(!result_we_get.mask());
    OutputNoDataPolicy output_no_data_policy(result_we_get.mask(), true);
    fa::statistic::count(
        InputNoDataPolicy{{constant.mask(), true}, {}},
        output_no_data_policy,
        sequential,
        constant, 5, result_we_get);
    BOOST_CHECK(!result_we_get.mask());
    BOOST_CHECK_EQUAL(result_we_get.value(), 1u);

    // Constant is masked.
    constant.value() = 5;
    constant.mask() = true;
    result_we_get.value() = 9;
    result_we_get.mask() = false;
    BOOST_CHECK(constant.mask());
    BOOST_CHECK(!result_we_get.mask());
    fa::statistic::count(
        InputNoDataPolicy{{constant.mask(), true}, {}},
        output_no_data_policy,
        sequential,
        constant, 5, result_we_get);
    BOOST_CHECK(result_we_get.mask());
    BOOST_CHECK_EQUAL(result_we_get.value(), 9u);
}

BOOST_AUTO_TEST_CASE(d1_array)
{
    fa::SequentialExecutionPolicy sequential;

    uint64_t result;

    // vector
    {
        std::vector<int32_t> array{ 1, 2, 3, 5 };
        fa::statistic::count(sequential, array, 2, result);
        BOOST_CHECK_EQUAL(result, 1u);
    }

    // 1d array
    {
        fern::Array<int32_t, 1> array{ 1, 2, 2, 5 };
        fa::statistic::count(sequential, array, 2, result);
        BOOST_CHECK_EQUAL(result, 2u);
    }

    // empty
    {
        // The result value is not touched.
        std::vector<int32_t> array;
        result = 5;
        fa::statistic::count(sequential, array, 2, result);
        BOOST_CHECK_EQUAL(result, 5u);
    }
}

BOOST_AUTO_TEST_CASE(masked_d1_array)
{
    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<1>>, fa::SkipNoData>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

    fa::SequentialExecutionPolicy sequential;

    fern::MaskedArray<int32_t, 1> array{ 1, 2, 3, 5 };
    fern::MaskedScalar<size_t> result;

    // 1d masked array with non-masking count
    {
        result.value() = 9;
        result.mask() = false;
        fa::statistic::count(sequential, array, 2, result);
        BOOST_CHECK_EQUAL(result.value(), 1u);
    }

    // 1d masked array with masking count
    {
        array.mask()[2] = true;
        result.value() = 9;
        result.mask() = false;
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);
        fa::statistic::count(
            InputNoDataPolicy{{array.mask(), true}, {}},
            output_no_data_policy,
            sequential, array, 2, result);
        BOOST_CHECK(!result.mask());
        BOOST_CHECK_EQUAL(result.value(), 1u);

        // Mask the whole input. Result must be masked too.
        array.mask_all();
        fa::statistic::count(
            InputNoDataPolicy{{array.mask(), true}, {}},
            output_no_data_policy,
            sequential, array, 2, result);
        BOOST_CHECK(result.mask());
    }

    // empty
    {
        fern::MaskedArray<int32_t, 1> empty_array;
        result.value() = 9;
        result.mask() = false;
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);
        fa::statistic::count(
            InputNoDataPolicy{{empty_array.mask(), true}, {}},
            output_no_data_policy,
            sequential, empty_array, 2, result);
        BOOST_CHECK(result.mask());
        BOOST_CHECK_EQUAL(result.value(), 9u);
    }
}

BOOST_AUTO_TEST_CASE(d2_array)
{
    fa::SequentialExecutionPolicy sequential;

    // 2d array
    {
        fern::Array<int8_t, 2> array{
            { -2, -1 },
            {  0,  9 },
            {  1,  2 }
        };
        uint64_t result;
        fa::statistic::count(sequential, array, 1, result);
        BOOST_CHECK_EQUAL(result, 1u);
    }
}

BOOST_AUTO_TEST_CASE(masked_d2_array)
{
    fa::SequentialExecutionPolicy sequential;

    fern::MaskedArray<int8_t, 2> array{
        { -2, -1 },
        {  5,  9 },
        {  1,  2 }
    };

    // 2d masked array with non-masking count
    {
        fern::MaskedScalar<uint64_t> result;
        fa::statistic::count(sequential, array, -2, result);
        BOOST_CHECK(!result.mask());
        BOOST_CHECK_EQUAL(result.value(), 1u);
    }

    // 2d masked array with masking count
    {
        using InputNoDataPolicy = fa::InputNoDataPolicies<
            fa::DetectNoDataByValue<fern::Mask<2>>, fa::SkipNoData>;
        using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;

        // Mask the 9.
        array.mask()[1][1] = true;

        // Count 2's.
        fern::MaskedScalar<uint64_t> result;
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);
        fa::statistic::count(
            InputNoDataPolicy{{array.mask(), true}, {}}, output_no_data_policy,
            sequential, array, 2, result);
        BOOST_CHECK(!result.mask());
        BOOST_CHECK_EQUAL(result.value(), 1u);

        // Count 9's. The one present is not visible, so the result is 0.
        result.value() = 999;
        fa::statistic::count(
            InputNoDataPolicy{{array.mask(), true}, {}}, output_no_data_policy,
            sequential, array, 9, result);
        BOOST_CHECK(!result.mask());
        BOOST_CHECK_EQUAL(result.value(), 0u);

        // Mask the whole input. Result must be masked too.
        array.mask_all();
        result.mask() = false;
        result.value() = 999;
        fa::statistic::count(
            InputNoDataPolicy{{array.mask(), true}, {}}, output_no_data_policy,
            sequential, array, 9, result);
        BOOST_CHECK(result.mask());
    }
}

BOOST_AUTO_TEST_CASE(concurrent)
{
    fa::ParallelExecutionPolicy parallel;
    fa::SequentialExecutionPolicy sequential;

    // Create a somewhat larger array.
    size_t const nr_rows = 600;
    size_t const nr_cols = 400;
    auto const extents = fern::extents[nr_rows][nr_cols];
    fern::Array<int32_t, 2> argument(extents);
    uint64_t result_we_got;
    uint64_t result_we_want;

    std::iota(argument.data(), argument.data() + argument.num_elements(), 0);
    result_we_want = 1;

    // Serial.
    {
        result_we_got = 9;
        fa::statistic::count(sequential, argument, 5, result_we_got);
        BOOST_CHECK_EQUAL(result_we_got, result_we_want);
    }

    // Concurrent.
    {
        result_we_got = 9;
        fa::statistic::count(parallel, argument, 5, result_we_got);
        BOOST_CHECK_EQUAL(result_we_got, result_we_want);
    }

    {
        using InputNoDataPolicy = fa::InputNoDataPolicies<fa::SkipNoData,
              fa::SkipNoData>;
        using OutputNoDataPolicy = fa::MarkNoDataByValue<bool>;
        fern::MaskedScalar<size_t> result_we_got;
        OutputNoDataPolicy output_no_data_policy(result_we_got.mask(), true);

        // Verify executor can handle masked result.
        fa::statistic::count(
            InputNoDataPolicy{{}, {}}, output_no_data_policy,
            parallel, argument, 5, result_we_got);
    }
}


#include <string>
#include <boost/random/additive_combine.hpp>
#include <stan/io/dump.hpp>
#include <test/unit/mcmc/hmc/mock_hmc.hpp>
#include <stan/mcmc/hmc/hamiltonians/diag_e_metric.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <test/test-models/good/mcmc/hmc/hamiltonians/funnel.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>

typedef boost::ecuyer1988 rng_t;

TEST(McmcDiagEMetric, sample_p) {
  rng_t base_rng(0);

  Eigen::VectorXd q(2);
  q(0) = 5;
  q(1) = 1;

  stan::mcmc::mock_model model(q.size());
  stan::mcmc::diag_e_metric<stan::mcmc::mock_model, rng_t> metric(model);
  stan::mcmc::diag_e_point z(q.size());

  int n_samples = 1000;
  double m = 0;
  double m2 = 0;

  for (int i = 0; i < n_samples; ++i) {
    metric.sample_p(z, base_rng);
    double tau = metric.tau(z);

    double delta = tau - m;
    m += delta / static_cast<double>(i + 1);
    m2 += delta * (tau - m);
  }

  double var = m2 / (n_samples + 1.0);

  // Mean within 5sigma of expected value (d / 2)
  EXPECT_TRUE(std::fabs(m - 0.5 * q.size()) < 5.0 * sqrt(var));

  // Variance within 10% of expected value (d / 2)
  EXPECT_TRUE(std::fabs(var - 0.5 * q.size()) < 0.1 * q.size());
}

TEST(McmcDiagEMetric, gradients) {
  rng_t base_rng(0);

  Eigen::VectorXd q = Eigen::VectorXd::Ones(11);

  stan::mcmc::diag_e_point z(q.size());
  z.q = q;
  z.p.setOnes();

  std::fstream data_stream(std::string("").c_str(), std::fstream::in);
  stan::io::dump data_var_context(data_stream);
  data_stream.close();

  std::stringstream model_output;
  funnel_model_namespace::funnel_model model(data_var_context, &model_output);

  std::stringstream debug, info, warn, error, fatal;
  stan::callbacks::stream_logger logger(debug, info, warn, error, fatal);

  stan::mcmc::diag_e_metric<funnel_model_namespace::funnel_model, rng_t> metric(
      model);

  double epsilon = 1e-6;

  metric.init(z, logger);
  Eigen::VectorXd g1 = metric.dtau_dq(z, logger);

  for (int i = 0; i < z.q.size(); ++i) {
    double delta = 0;

    z.q(i) += epsilon;
    metric.update_potential(z, logger);
    delta += metric.tau(z);

    z.q(i) -= 2 * epsilon;
    metric.update_potential(z, logger);
    delta -= metric.tau(z);

    z.q(i) += epsilon;
    metric.update_potential(z, logger);

    delta /= 2 * epsilon;

    EXPECT_NEAR(delta, g1(i), epsilon);
  }

  Eigen::VectorXd g2 = metric.dtau_dp(z);

  for (int i = 0; i < z.q.size(); ++i) {
    double delta = 0;

    z.p(i) += epsilon;
    delta += metric.tau(z);

    z.p(i) -= 2 * epsilon;
    delta -= metric.tau(z);

    z.p(i) += epsilon;

    delta /= 2 * epsilon;

    EXPECT_NEAR(delta, g2(i), epsilon);
  }

  Eigen::VectorXd g3 = metric.dphi_dq(z, logger);

  for (int i = 0; i < z.q.size(); ++i) {
    double delta = 0;

    z.q(i) += epsilon;
    metric.update_potential(z, logger);
    delta += metric.phi(z);

    z.q(i) -= 2 * epsilon;
    metric.update_potential(z, logger);
    delta -= metric.phi(z);

    z.q(i) += epsilon;
    metric.update_potential(z, logger);

    delta /= 2 * epsilon;

    EXPECT_NEAR(delta, g3(i), epsilon);
  }
  EXPECT_EQ("", model_output.str());
  EXPECT_EQ("", debug.str());
  EXPECT_EQ("", info.str());
  EXPECT_EQ("", warn.str());
  EXPECT_EQ("", error.str());
  EXPECT_EQ("", fatal.str());
}

TEST(McmcDiagEMetric, streams) {
  stan::test::capture_std_streams();

  rng_t base_rng(0);

  Eigen::VectorXd q(2);
  q(0) = 5;
  q(1) = 1;

  stan::mcmc::mock_model model(q.size());

  // typedef to use within Google Test macros
  typedef stan::mcmc::diag_e_metric<stan::mcmc::mock_model, rng_t> diag_e;

  EXPECT_NO_THROW(diag_e metric(model));

  stan::test::reset_std_streams();
  EXPECT_EQ("", stan::test::cout_ss.str());
  EXPECT_EQ("", stan::test::cerr_ss.str());
}


/* Author: Wolfgang Bangerth, Texas A&M University, 2011 */

/*    $Id: step-46.cc 27737 2012-12-03 19:44:54Z heister $       */
/*                                                                */
/*    Copyright (C) 2011-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{Include files}

// The include files for this program are the same as for many others
// before. The only new one is the one that declares FE_Nothing as discussed
// in the introduction. The ones in the hp directory have already been
// discussed in step-27.

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/constraint_matrix.h>

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

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_accessor.h>

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

#include <deal.II/hp/dof_handler.h>
#include <deal.II/hp/fe_collection.h>
#include <deal.II/hp/fe_values.h>

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

#include <fstream>
#include <sstream>

namespace Step46
{
  using namespace dealii;

  // @sect3{The FluidStructureProblem class template}

  // This is the main class. It is, if you want, a combination of step-8 and
  // step-22 in that it has member variables that either address the global
  // problem (the Triangulation and hp::DoFHandler objects, as well as the
  // hp::FECollection and various linear algebra objects) or that pertain to
  // either the elasticity or Stokes sub-problems. The general structure of
  // the class, however, is like that of most of the other programs
  // implementing stationary problems.
  //
  // There are a few helper functions (cell_is_in_fluid_domain,
  // cell_is_in_solid_domain) of self-explanatory nature (operating on
  // the symbolic names for the two subdomains that will be used as
  // material_ids for cells belonging to the subdomains, as explained in the
  // introduction) and a few functions (make_grid,
  // set_active_fe_indices, assemble_interface_terms) that have been
  // broken out of other functions that can be found in many of the other
  // tutorial programs and that will be discussed as we get to their
  // implementation.
  //
  // The final set of variables (viscosity, lambda, eta)
  // describes the material properties used for the two physics models.
  template <int dim>
  class FluidStructureProblem
  {
  public:
    FluidStructureProblem (const unsigned int stokes_degree,
                           const unsigned int elasticity_degree);
    void run ();

  private:
    enum
    {
      fluid_domain_id,
      solid_domain_id
    };

    static bool
    cell_is_in_fluid_domain (const typename hp::DoFHandler<dim>::cell_iterator &cell);

    static bool
    cell_is_in_solid_domain (const typename hp::DoFHandler<dim>::cell_iterator &cell);

    void make_grid ();
    void set_active_fe_indices ();
    void setup_dofs ();
    void assemble_system ();
    void assemble_interface_term (const FEFaceValuesBase<dim>          &elasticity_fe_face_values,
                                  const FEFaceValuesBase<dim>          &stokes_fe_face_values,
                                  std::vector<Tensor<1,dim> >          &elasticity_phi,
                                  std::vector<SymmetricTensor<2,dim> > &stokes_symgrad_phi_u,
                                  std::vector<double>                  &stokes_phi_p,
                                  FullMatrix<double>                   &local_interface_matrix) const;
    void solve ();
    void output_results (const unsigned int refinement_cycle) const;
    void refine_mesh ();

    const unsigned int    stokes_degree;
    const unsigned int    elasticity_degree;

    Triangulation<dim>    triangulation;
    FESystem<dim>         stokes_fe;
    FESystem<dim>         elasticity_fe;
    hp::FECollection<dim> fe_collection;
    hp::DoFHandler<dim>   dof_handler;

    ConstraintMatrix      constraints;

    SparsityPattern       sparsity_pattern;
    SparseMatrix<double>  system_matrix;

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

    const double          viscosity;
    const double          lambda;
    const double          mu;
  };

  // @sect3{Boundary values and right hand side}

  // The following classes do as their names suggest. The boundary values for
  // the velocity are $\mathbf u=(0, \sin(\pi x))^T$ in 2d and $\mathbf u=(0,
  // 0, \sin(\pi x)\sin(\pi y))^T$ in 3d, respectively. The remaining boundary
  // conditions for this problem are all homogenous and have been discussed in
  // the introduction. The right hand side forcing term is zero for both the
  // fluid and the solid.
  template <int dim>
  class StokesBoundaryValues : public Function<dim>
  {
  public:
    StokesBoundaryValues () : Function<dim>(dim+1+dim) {}

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

    virtual void vector_value (const Point<dim> &p,
                               Vector<double>   &value) const;
  };

  template <int dim>
  double
  StokesBoundaryValues<dim>::value (const Point<dim> &p,
                                    const unsigned int component) const
  {
    Assert (component < this->n_components,
            ExcIndexRange (component, 0, this->n_components));

    if (component == dim-1)
      switch (dim)
        {
        case 2:
          return std::sin(numbers::PI*p[0]);
        case 3:
          return std::sin(numbers::PI*p[0]) * std::sin(numbers::PI*p[1]);
        default:
          Assert (false, ExcNotImplemented());
        }

    return 0;
  }

  template <int dim>
  void
  StokesBoundaryValues<dim>::vector_value (const Point<dim> &p,
                                           Vector<double>   &values) const
  {
    for (unsigned int c=0; c<this->n_components; ++c)
      values(c) = StokesBoundaryValues<dim>::value (p, c);
  }

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>(dim+1) {}

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

    virtual void vector_value (const Point<dim> &p,
                               Vector<double>   &value) const;

  };

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

  template <int dim>
  void
  RightHandSide<dim>::vector_value (const Point<dim> &p,
                                    Vector<double>   &values) const
  {
    for (unsigned int c=0; c<this->n_components; ++c)
      values(c) = RightHandSide<dim>::value (p, c);
  }

  // @sect3{The FluidStructureProblem implementation}

  // @sect4{Constructors and helper functions}

  // Let's now get to the implementation of the primary class of this
  // program. The first few functions are the constructor and the helper
  // functions that can be used to determine which part of the domain a cell
  // is in. Given the discussion of these topics in the introduction, their
  // implementation is rather obvious. In the constructor, note that we have
  // to construct the hp::FECollection object from the base elements for
  // Stokes and elasticity; using the hp::FECollection::push_back function
  // assigns them spots zero and one in this collection, an order that we have
  // to remember and use consistently in the rest of the program.
  template <int dim>
  FluidStructureProblem<dim>::
  FluidStructureProblem (const unsigned int stokes_degree,
                         const unsigned int elasticity_degree)
    :
    stokes_degree (stokes_degree),
    elasticity_degree (elasticity_degree),
    triangulation (Triangulation<dim>::maximum_smoothing),
    stokes_fe (FE_Q<dim>(stokes_degree+1), dim,
               FE_Q<dim>(stokes_degree), 1,
               FE_Nothing<dim>(), dim),
    elasticity_fe (FE_Nothing<dim>(), dim,
                   FE_Nothing<dim>(), 1,
                   FE_Q<dim>(elasticity_degree), dim),
    dof_handler (triangulation),
    viscosity (2),
    lambda (1),
    mu (1)
  {
    fe_collection.push_back (stokes_fe);
    fe_collection.push_back (elasticity_fe);
  }

  template <int dim>
  bool
  FluidStructureProblem<dim>::
  cell_is_in_fluid_domain (const typename hp::DoFHandler<dim>::cell_iterator &cell)
  {
    return (cell->material_id() == fluid_domain_id);
  }

  template <int dim>
  bool
  FluidStructureProblem<dim>::
  cell_is_in_solid_domain (const typename hp::DoFHandler<dim>::cell_iterator &cell)
  {
    return (cell->material_id() == solid_domain_id);
  }

  // @sect4{Meshes and assigning subdomains}

  // The next pair of functions deals with generating a mesh and making sure
  // all flags that denote subdomains are correct. make_grid, as
  // discussed in the introduction, generates an $8\times 8$ mesh (or an
  // $8\times 8\times 8$ mesh in 3d) to make sure that each coarse mesh cell
  // is completely within one of the subdomains. After generating this mesh,
  // we loop over its boundary and set the boundary indicator to one at the
  // top boundary, the only place where we set nonzero Dirichlet boundary
  // conditions. After this, we loop again over all cells to set the material
  // indicator — used to denote which part of the domain we are in, to
  // either the fluid or solid indicator.
  template <int dim>
  void
  FluidStructureProblem<dim>::make_grid ()
  {
    GridGenerator::subdivided_hyper_cube (triangulation, 8, -1, 1);

    for (typename Triangulation<dim>::active_cell_iterator
         cell = triangulation.begin_active();
         cell != triangulation.end(); ++cell)
      for (unsigned int f=0; f<GeometryInfo<dim>::faces_per_cell; ++f)
        if (cell->face(f)->at_boundary()
            &&
            (cell->face(f)->center()[dim-1] == 1))
          cell->face(f)->set_all_boundary_indicators(1);

    for (typename Triangulation<dim>::active_cell_iterator
         cell = dof_handler.begin_active();
         cell != dof_handler.end(); ++cell)
      if (((std::fabs(cell->center()[0]) < 0.25)
           &&
           (cell->center()[dim-1] > 0.5))
          ||
          ((std::fabs(cell->center()[0]) >= 0.25)
           &&
           (cell->center()[dim-1] > -0.5)))
        cell->set_material_id (fluid_domain_id);
      else
        cell->set_material_id (solid_domain_id);
  }

  // The second part of this pair of functions determines which finite element
  // to use on each cell. Above we have set the material indicator for each
  // coarse mesh cell, and as mentioned in the introduction, this information
  // is inherited from mother to child cell upon mesh refinement.
  //
  // In other words, whenever we have refined (or created) the mesh, we can
  // rely on the material indicators to be a correct description of which part
  // of the domain a cell is in. We then use this to set the active FE index
  // of the cell to the corresponding element of the hp::FECollection member
  // variable of this class: zero for fluid cells, one for solid cells.
  template <int dim>
  void
  FluidStructureProblem<dim>::set_active_fe_indices ()
  {
    for (typename hp::DoFHandler<dim>::active_cell_iterator
         cell = dof_handler.begin_active();
         cell != dof_handler.end(); ++cell)
      {
        if (cell_is_in_fluid_domain(cell))
          cell->set_active_fe_index (0);
        else if (cell_is_in_solid_domain(cell))
          cell->set_active_fe_index (1);
        else
          Assert (false, ExcNotImplemented());
      }
  }

  // @sect4{FluidStructureProblem::setup_dofs}

  // The next step is to setup the data structures for the linear system. To
  // this end, we first have to set the active FE indices with the function
  // immediately above, then distribute degrees of freedom, and then determine
  // constraints on the linear system. The latter includes hanging node
  // constraints as usual, but also the inhomogenous boundary values at the
  // top fluid boundary, and zero boundary values along the perimeter of the
  // solid subdomain.
  template <int dim>
  void
  FluidStructureProblem<dim>::setup_dofs ()
  {
    set_active_fe_indices ();
    dof_handler.distribute_dofs (fe_collection);

    {
      constraints.clear ();
      DoFTools::make_hanging_node_constraints (dof_handler,
                                               constraints);

      const FEValuesExtractors::Vector velocities(0);
      VectorTools::interpolate_boundary_values (dof_handler,
                                                1,
                                                StokesBoundaryValues<dim>(),
                                                constraints,
                                                fe_collection.component_mask(velocities));

      const FEValuesExtractors::Vector displacements(dim+1);
      VectorTools::interpolate_boundary_values (dof_handler,
                                                0,
                                                ZeroFunction<dim>(dim+1+dim),
                                                constraints,
                                                fe_collection.component_mask(displacements));
    }

    // There are more constraints we have to handle, though: we have to make
    // sure that the velocity is zero at the interface between fluid and
    // solid. The following piece of code was already presented in the
    // introduction:
    {
      std::vector<unsigned int> local_face_dof_indices (stokes_fe.dofs_per_face);
      for (typename hp::DoFHandler<dim>::active_cell_iterator
           cell = dof_handler.begin_active();
           cell != dof_handler.end(); ++cell)
        if (cell_is_in_fluid_domain (cell))
          for (unsigned int f=0; f<GeometryInfo<dim>::faces_per_cell; ++f)
            if (!cell->at_boundary(f))
              {
                bool face_is_on_interface = false;

                if ((cell->neighbor(f)->has_children() == false)
                    &&
                    (cell_is_in_solid_domain (cell->neighbor(f))))
                  face_is_on_interface = true;
                else if (cell->neighbor(f)->has_children() == true)
                  {
                    for (unsigned int sf=0; sf<cell->face(f)->n_children(); ++sf)
                      if (cell_is_in_solid_domain (cell->neighbor_child_on_subface
                                                   (f, sf)))
                        {
                          face_is_on_interface = true;
                          break;
                        }
                  }

                if (face_is_on_interface)
                  {
                    cell->face(f)->get_dof_indices (local_face_dof_indices, 0);
                    for (unsigned int i=0; i<local_face_dof_indices.size(); ++i)
                      if (stokes_fe.face_system_to_component_index(i).first < dim)
                        constraints.add_line (local_face_dof_indices[i]);
                  }
              }
    }

    // At the end of all this, we can declare to the constraints object that
    // we now have all constraints ready to go and that the object can rebuild
    // its internal data structures for better efficiency:
    constraints.close ();

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

    // In the rest of this function we create a sparsity pattern as discussed
    // extensively in the introduction, and use it to initialize the matrix;
    // then also set vectors to their correct sizes:
    {
      CompressedSimpleSparsityPattern csp (dof_handler.n_dofs(),
                                           dof_handler.n_dofs());

      Table<2,DoFTools::Coupling> cell_coupling (fe_collection.n_components(),
                                                 fe_collection.n_components());
      Table<2,DoFTools::Coupling> face_coupling (fe_collection.n_components(),
                                                 fe_collection.n_components());

      for (unsigned int c=0; c<fe_collection.n_components(); ++c)
        for (unsigned int d=0; d<fe_collection.n_components(); ++d)
          {
            if (((c<dim+1) && (d<dim+1)
                 && !((c==dim) && (d==dim)))
                ||
                ((c>=dim+1) && (d>=dim+1)))
              cell_coupling[c][d] = DoFTools::always;

            if ((c>=dim+1) && (d<dim+1))
              face_coupling[c][d] = DoFTools::always;
          }

      DoFTools::make_flux_sparsity_pattern (dof_handler, csp,
                                            cell_coupling, face_coupling);
      constraints.condense (csp);
      sparsity_pattern.copy_from (csp);
    }

    system_matrix.reinit (sparsity_pattern);

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

  // @sect4{FluidStructureProblem::assemble_system}

  // Following is the central function of this program: the one that assembles
  // the linear system. It has a long section of setting up auxiliary
  // functions at the beginning: from creating the quadrature formulas and
  // setting up the FEValues, FEFaceValues and FESubfaceValues objects
  // necessary to integrate the cell terms as well as the interface terms for
  // the case where cells along the interface come together at same size or
  // with differing levels of refinement...
  template <int dim>
  void FluidStructureProblem<dim>::assemble_system ()
  {
    system_matrix=0;
    system_rhs=0;

    const QGauss<dim> stokes_quadrature(stokes_degree+2);
    const QGauss<dim> elasticity_quadrature(elasticity_degree+2);

    hp::QCollection<dim>  q_collection;
    q_collection.push_back (stokes_quadrature);
    q_collection.push_back (elasticity_quadrature);

    hp::FEValues<dim> hp_fe_values (fe_collection, q_collection,
                                    update_values    |
                                    update_quadrature_points  |
                                    update_JxW_values |
                                    update_gradients);

    const QGauss<dim-1> common_face_quadrature(std::max (stokes_degree+2,
                                                         elasticity_degree+2));

    FEFaceValues<dim>    stokes_fe_face_values (stokes_fe,
                                                common_face_quadrature,
                                                update_JxW_values |
                                                update_normal_vectors |
                                                update_gradients);
    FEFaceValues<dim>    elasticity_fe_face_values (elasticity_fe,
                                                    common_face_quadrature,
                                                    update_values);
    FESubfaceValues<dim> stokes_fe_subface_values (stokes_fe,
                                                   common_face_quadrature,
                                                   update_JxW_values |
                                                   update_normal_vectors |
                                                   update_gradients);
    FESubfaceValues<dim> elasticity_fe_subface_values (elasticity_fe,
                                                       common_face_quadrature,
                                                       update_values);

    // ...to objects that are needed to describe the local contributions to
    // the global linear system...
    const unsigned int        stokes_dofs_per_cell     = stokes_fe.dofs_per_cell;
    const unsigned int        elasticity_dofs_per_cell = elasticity_fe.dofs_per_cell;

    FullMatrix<double>        local_matrix;
    FullMatrix<double>        local_interface_matrix (elasticity_dofs_per_cell,
                                                      stokes_dofs_per_cell);
    Vector<double>            local_rhs;

    std::vector<unsigned int> local_dof_indices;
    std::vector<unsigned int> neighbor_dof_indices (stokes_dofs_per_cell);

    const RightHandSide<dim>  right_hand_side;

    // ...to variables that allow us to extract certain components of the
    // shape functions and cache their values rather than having to recompute
    // them at every quadrature point:
    const FEValuesExtractors::Vector     velocities (0);
    const FEValuesExtractors::Scalar     pressure (dim);
    const FEValuesExtractors::Vector     displacements (dim+1);

    std::vector<SymmetricTensor<2,dim> > stokes_symgrad_phi_u (stokes_dofs_per_cell);
    std::vector<double>                  stokes_div_phi_u     (stokes_dofs_per_cell);
    std::vector<double>                  stokes_phi_p         (stokes_dofs_per_cell);

    std::vector<Tensor<2,dim> >          elasticity_grad_phi (elasticity_dofs_per_cell);
    std::vector<double>                  elasticity_div_phi  (elasticity_dofs_per_cell);
    std::vector<Tensor<1,dim> >          elasticity_phi      (elasticity_dofs_per_cell);

    // Then comes the main loop over all cells and, as in step-27, the
    // initialization of the hp::FEValues object for the current cell and the
    // extraction of a FEValues object that is appropriate for the current
    // cell:
    typename hp::DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        hp_fe_values.reinit (cell);

        const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();

        local_matrix.reinit (cell->get_fe().dofs_per_cell,
                             cell->get_fe().dofs_per_cell);
        local_rhs.reinit (cell->get_fe().dofs_per_cell);

        // With all of this done, we continue to assemble the cell terms for
        // cells that are part of the Stokes and elastic regions. While we
        // could in principle do this in one formula, in effect implementing
        // the one bilinear form stated in the introduction, we realize that
        // our finite element spaces are chosen in such a way that on each
        // cell, one set of variables (either velocities and pressure, or
        // displacements) are always zero, and consequently a more efficient
        // way of computing local integrals is to do only what's necessary
        // based on an if clause that tests which part of the
        // domain we are in.
        //
        // The actual computation of the local matrix is the same as in
        // step-22 as well as that given in the @ref vector_valued
        // documentation module for the elasticity equations:
        if (cell_is_in_fluid_domain (cell))
          {
            const unsigned int dofs_per_cell = cell->get_fe().dofs_per_cell;
            Assert (dofs_per_cell == stokes_dofs_per_cell,
                    ExcInternalError());

            for (unsigned int q=0; q<fe_values.n_quadrature_points; ++q)
              {
                for (unsigned int k=0; k<dofs_per_cell; ++k)
                  {
                    stokes_symgrad_phi_u[k] = fe_values[velocities].symmetric_gradient (k, q);
                    stokes_div_phi_u[k]     = fe_values[velocities].divergence (k, q);
                    stokes_phi_p[k]         = fe_values[pressure].value (k, q);
                  }

                for (unsigned int i=0; i<dofs_per_cell; ++i)
                  for (unsigned int j=0; j<dofs_per_cell; ++j)
                    local_matrix(i,j) += (2 * viscosity * stokes_symgrad_phi_u[i] * stokes_symgrad_phi_u[j]
                                          - stokes_div_phi_u[i] * stokes_phi_p[j]
                                          - stokes_phi_p[i] * stokes_div_phi_u[j])
                                         * fe_values.JxW(q);
              }
          }
        else
          {
            const unsigned int dofs_per_cell = cell->get_fe().dofs_per_cell;
            Assert (dofs_per_cell == elasticity_dofs_per_cell,
                    ExcInternalError());

            for (unsigned int q=0; q<fe_values.n_quadrature_points; ++q)
              {
                for (unsigned int k=0; k<dofs_per_cell; ++k)
                  {
                    elasticity_grad_phi[k] = fe_values[displacements].gradient (k, q);
                    elasticity_div_phi[k]  = fe_values[displacements].divergence (k, q);
                  }

                for (unsigned int i=0; i<dofs_per_cell; ++i)
                  for (unsigned int j=0; j<dofs_per_cell; ++j)
                    {
                      local_matrix(i,j)
                      +=  (lambda *
                           elasticity_div_phi[i] * elasticity_div_phi[j]
                           +
                           mu *
                           scalar_product(elasticity_grad_phi[i], elasticity_grad_phi[j])
                           +
                           mu *
                           scalar_product(elasticity_grad_phi[i], transpose(elasticity_grad_phi[j]))
                          )
                          *
                          fe_values.JxW(q);
                    }
              }
          }

        // Once we have the contributions from cell integrals, we copy them
        // into the global matrix (taking care of constraints right away,
        // through the ConstraintMatrix::distribute_local_to_global
        // function). Note that we have not written anything into the
        // local_rhs variable, though we still need to pass it
        // along since the elimination of nonzero boundary values requires the
        // modification of local and consequently also global right hand side
        // values:
        local_dof_indices.resize (cell->get_fe().dofs_per_cell);
        cell->get_dof_indices (local_dof_indices);
        constraints.distribute_local_to_global (local_matrix, local_rhs,
                                                local_dof_indices,
                                                system_matrix, system_rhs);

        // The more interesting part of this function is where we see about
        // face terms along the interface between the two subdomains. To this
        // end, we first have to make sure that we only assemble them once
        // even though a loop over all faces of all cells would encounter each
        // part of the interface twice. We arbitrarily make the decision that
        // we will only evaluate interface terms if the current cell is part
        // of the solid subdomain and if, consequently, a face is not at the
        // boundary and the potential neighbor behind it is part of the fluid
        // domain. Let's start with these conditions:
        if (cell_is_in_solid_domain (cell))
          for (unsigned int f=0; f<GeometryInfo<dim>::faces_per_cell; ++f)
            if (cell->at_boundary(f) == false)
              {
                // At this point we know that the current cell is a candidate
                // for integration and that a neighbor behind face
                // f exists. There are now three possibilities:
                //
                // - The neighbor is at the same refinement level and has no
                //   children.
                // - The neighbor has children.
                // - The neighbor is coarser.
                //
                // In all three cases, we are only interested in it if it is
                // part of the fluid subdomain. So let us start with the first
                // and simplest case: if the neighbor is at the same level,
                // has no children, and is a fluid cell, then the two cells
                // share a boundary that is part of the interface along which
                // we want to integrate interface terms. All we have to do is
                // initialize two FEFaceValues object with the current face
                // and the face of the neighboring cell (note how we find out
                // which face of the neighboring cell borders on the current
                // cell) and pass things off to the function that evaluates
                // the interface terms (the third through fifth arguments to
                // this function provide it with scratch arrays). The result
                // is then again copied into the global matrix, using a
                // function that knows that the DoF indices of rows and
                // columns of the local matrix result from different cells:
                if ((cell->neighbor(f)->level() == cell->level())
                    &&
                    (cell->neighbor(f)->has_children() == false)
                    &&
                    cell_is_in_fluid_domain (cell->neighbor(f)))
                  {
                    elasticity_fe_face_values.reinit (cell, f);
                    stokes_fe_face_values.reinit (cell->neighbor(f),
                                                  cell->neighbor_of_neighbor(f));

                    assemble_interface_term (elasticity_fe_face_values, stokes_fe_face_values,
                                             elasticity_phi, stokes_symgrad_phi_u, stokes_phi_p,
                                             local_interface_matrix);

                    cell->neighbor(f)->get_dof_indices (neighbor_dof_indices);
                    constraints.distribute_local_to_global(local_interface_matrix,
                                                           local_dof_indices,
                                                           neighbor_dof_indices,
                                                           system_matrix);
                  }

                // The second case is if the neighbor has further children. In
                // that case, we have to loop over all the children of the
                // neighbor to see if they are part of the fluid subdomain. If
                // they are, then we integrate over the common interface,
                // which is a face for the neighbor and a subface of the
                // current cell, requiring us to use an FEFaceValues for the
                // neighbor and an FESubfaceValues for the current cell:
                else if ((cell->neighbor(f)->level() == cell->level())
                         &&
                         (cell->neighbor(f)->has_children() == true))
                  {
                    for (unsigned int subface=0;
                         subface<cell->face(f)->n_children();
                         ++subface)
                      if (cell_is_in_fluid_domain (cell->neighbor_child_on_subface
                                                   (f, subface)))
                        {
                          elasticity_fe_subface_values.reinit (cell,
                                                               f,
                                                               subface);
                          stokes_fe_face_values.reinit (cell->neighbor_child_on_subface (f, subface),
                                                        cell->neighbor_of_neighbor(f));

                          assemble_interface_term (elasticity_fe_subface_values,
                                                   stokes_fe_face_values,
                                                   elasticity_phi,
                                                   stokes_symgrad_phi_u, stokes_phi_p,
                                                   local_interface_matrix);

                          cell->neighbor_child_on_subface (f, subface)
                          ->get_dof_indices (neighbor_dof_indices);
                          constraints.distribute_local_to_global(local_interface_matrix,
                                                                 local_dof_indices,
                                                                 neighbor_dof_indices,
                                                                 system_matrix);
                        }
                  }

                // The last option is that the neighbor is coarser. In that
                // case we have to use an FESubfaceValues object for the
                // neighbor and a FEFaceValues for the current cell; the rest
                // is the same as before:
                else if (cell->neighbor_is_coarser(f)
                         &&
                         cell_is_in_fluid_domain(cell->neighbor(f)))
                  {
                    elasticity_fe_face_values.reinit (cell, f);
                    stokes_fe_subface_values.reinit (cell->neighbor(f),
                                                     cell->neighbor_of_coarser_neighbor(f).first,
                                                     cell->neighbor_of_coarser_neighbor(f).second);

                    assemble_interface_term (elasticity_fe_face_values,
                                             stokes_fe_subface_values,
                                             elasticity_phi,
                                             stokes_symgrad_phi_u, stokes_phi_p,
                                             local_interface_matrix);

                    cell->neighbor(f)->get_dof_indices (neighbor_dof_indices);
                    constraints.distribute_local_to_global(local_interface_matrix,
                                                           local_dof_indices,
                                                           neighbor_dof_indices,
                                                           system_matrix);

                  }
              }
      }
  }

  // In the function that assembles the global system, we passed computing
  // interface terms to a separate function we discuss here. The key is that
  // even though we can't predict the combination of FEFaceValues and
  // FESubfaceValues objects, they are both derived from the FEFaceValuesBase
  // class and consequently we don't have to care: the function is simply
  // called with two such objects denoting the values of the shape functions
  // on the quadrature points of the two sides of the face. We then do what we
  // always do: we fill the scratch arrays with the values of shape functions
  // and their derivatives, and then loop over all entries of the matrix to
  // compute the local integrals. The details of the bilinear form we evaluate
  // here are given in the introduction.
  template <int dim>
  void
  FluidStructureProblem<dim>::
  assemble_interface_term (const FEFaceValuesBase<dim>          &elasticity_fe_face_values,
                           const FEFaceValuesBase<dim>          &stokes_fe_face_values,
                           std::vector<Tensor<1,dim> >          &elasticity_phi,
                           std::vector<SymmetricTensor<2,dim> > &stokes_symgrad_phi_u,
                           std::vector<double>                  &stokes_phi_p,
                           FullMatrix<double>                   &local_interface_matrix) const
  {
    Assert (stokes_fe_face_values.n_quadrature_points ==
            elasticity_fe_face_values.n_quadrature_points,
            ExcInternalError());
    const unsigned int n_face_quadrature_points
      = elasticity_fe_face_values.n_quadrature_points;

    const FEValuesExtractors::Vector velocities (0);
    const FEValuesExtractors::Scalar pressure (dim);
    const FEValuesExtractors::Vector displacements (dim+1);

    local_interface_matrix = 0;
    for (unsigned int q=0; q<n_face_quadrature_points; ++q)
      {
        const Tensor<1,dim> normal_vector = stokes_fe_face_values.normal_vector(q);

        for (unsigned int k=0; k<stokes_fe_face_values.dofs_per_cell; ++k)
          stokes_symgrad_phi_u[k] = stokes_fe_face_values[velocities].symmetric_gradient (k, q);
        for (unsigned int k=0; k<elasticity_fe_face_values.dofs_per_cell; ++k)
          elasticity_phi[k] = elasticity_fe_face_values[displacements].value (k,q);

        for (unsigned int i=0; i<elasticity_fe_face_values.dofs_per_cell; ++i)
          for (unsigned int j=0; j<stokes_fe_face_values.dofs_per_cell; ++j)
            local_interface_matrix(i,j) += -((2 * viscosity *
                                              (stokes_symgrad_phi_u[j] *
                                               normal_vector)
                                              +
                                              stokes_phi_p[j] *
                                              normal_vector) *
                                             elasticity_phi[i] *
                                             stokes_fe_face_values.JxW(q));
      }
  }

  // @sect4{FluidStructureProblem::solve}

  // As discussed in the introduction, we use a rather trivial solver here: we
  // just pass the linear system off to the SparseDirectUMFPACK direct solver
  // (see, for example, step-29). The only thing we have to do after solving
  // is ensure that hanging node and boundary value constraints are correct.
  template <int dim>
  void
  FluidStructureProblem<dim>::solve ()
  {
    SparseDirectUMFPACK direct_solver;
    direct_solver.initialize (system_matrix);
    direct_solver.vmult (solution, system_rhs);

    constraints.distribute (solution);
  }

  // @sect4{FluidStructureProblem::output_results}

  // Generating graphical output is rather trivial here: all we have to do is
  // identify which components of the solution vector belong to scalars and/or
  // vectors (see, for example, step-22 for a previous example), and then pass
  // it all on to the DataOut class (with the second template argument equal
  // to hp::DoFHandler instead of the usual default DoFHandler):
  template <int dim>
  void
  FluidStructureProblem<dim>::
  output_results (const unsigned int refinement_cycle)  const
  {
    std::vector<std::string> solution_names (dim, "velocity");
    solution_names.push_back ("pressure");
    for (unsigned int d=0; d<dim; ++d)
      solution_names.push_back ("displacement");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
    data_component_interpretation
    (dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation
    .push_back (DataComponentInterpretation::component_is_scalar);
    for (unsigned int d=0; d<dim; ++d)
      data_component_interpretation
      .push_back (DataComponentInterpretation::component_is_part_of_vector);

    DataOut<dim,hp::DoFHandler<dim> > data_out;
    data_out.attach_dof_handler (dof_handler);

    data_out.add_data_vector (solution, solution_names,
                              DataOut<dim,hp::DoFHandler<dim> >::type_dof_data,
                              data_component_interpretation);
    data_out.build_patches ();

    std::ostringstream filename;
    filename << "solution-"
             << Utilities::int_to_string (refinement_cycle, 2)
             << ".vtk";

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

  // @sect4{FluidStructureProblem::refine_mesh}

  // The next step is to refine the mesh. As was discussed in the
  // introduction, this is a bit tricky primarily because the fluid and the
  // solid subdomains use variables that have different physical dimensions
  // and for which the absolute magnitude of error estimates is consequently
  // not directly comparable. We will therefore have to scale them. At the top
  // of the function, we therefore first compute error estimates for the
  // different variables separately (using the velocities but not the pressure
  // for the fluid domain, and the displacements in the solid domain):
  template <int dim>
  void
  FluidStructureProblem<dim>::refine_mesh ()
  {
    Vector<float>
    stokes_estimated_error_per_cell (triangulation.n_active_cells());
    Vector<float>
    elasticity_estimated_error_per_cell (triangulation.n_active_cells());

    const QGauss<dim-1> stokes_face_quadrature(stokes_degree+2);
    const QGauss<dim-1> elasticity_face_quadrature(elasticity_degree+2);

    hp::QCollection<dim-1> face_q_collection;
    face_q_collection.push_back (stokes_face_quadrature);
    face_q_collection.push_back (elasticity_face_quadrature);

    const FEValuesExtractors::Vector velocities(0);
    KellyErrorEstimator<dim>::estimate (dof_handler,
                                        face_q_collection,
                                        typename FunctionMap<dim>::type(),
                                        solution,
                                        stokes_estimated_error_per_cell,
                                        fe_collection.component_mask(velocities));

    const FEValuesExtractors::Vector displacements(dim+1);
    KellyErrorEstimator<dim>::estimate (dof_handler,
                                        face_q_collection,
                                        typename FunctionMap<dim>::type(),
                                        solution,
                                        elasticity_estimated_error_per_cell,
                                        fe_collection.component_mask(displacements));

    // We then normalize error estimates by dividing by their norm and scale
    // the fluid error indicators by a factor of 4 as discussed in the
    // introduction. The results are then added together into a vector that
    // contains error indicators for all cells:
    stokes_estimated_error_per_cell
    *= 4. / stokes_estimated_error_per_cell.l2_norm();
    elasticity_estimated_error_per_cell
    *= 1. / elasticity_estimated_error_per_cell.l2_norm();

    Vector<float>
    estimated_error_per_cell (triangulation.n_active_cells());

    estimated_error_per_cell += stokes_estimated_error_per_cell;
    estimated_error_per_cell += elasticity_estimated_error_per_cell;

    // The second to last part of the function, before actually refining the
    // mesh, involves a heuristic that we have already mentioned in the
    // introduction: because the solution is discontinuous, the
    // KellyErrorEstimator class gets all confused about cells that sit at the
    // boundary between subdomains: it believes that the error is large there
    // because the jump in the gradient is large, even though this is entirely
    // expected and a feature that is in fact present in the exact solution as
    // well and therefore not indicative of any numerical error.
    //
    // Consequently, we set the error indicators to zero for all cells at the
    // interface; the conditions determining which cells this affects are
    // slightly awkward because we have to account for the possibility of
    // adaptively refined meshes, meaning that the neighboring cell can be
    // coarser than the current one, or could in fact be refined some
    // more. The structure of these nested conditions is much the same as we
    // encountered when assembling interface terms in
    // assemble_system.
    {
      unsigned int cell_index = 0;
      for (typename hp::DoFHandler<dim>::active_cell_iterator
           cell = dof_handler.begin_active();
           cell != dof_handler.end(); ++cell, ++cell_index)
        for (unsigned int f=0; f<GeometryInfo<dim>::faces_per_cell; ++f)
          if (cell_is_in_solid_domain (cell))
            {
              if ((cell->at_boundary(f) == false)
                  &&
                  (((cell->neighbor(f)->level() == cell->level())
                    &&
                    (cell->neighbor(f)->has_children() == false)
                    &&
                    cell_is_in_fluid_domain (cell->neighbor(f)))
                   ||
                   ((cell->neighbor(f)->level() == cell->level())
                    &&
                    (cell->neighbor(f)->has_children() == true)
                    &&
                    (cell_is_in_fluid_domain (cell->neighbor_child_on_subface
                                              (f, 0))))
                   ||
                   (cell->neighbor_is_coarser(f)
                    &&
                    cell_is_in_fluid_domain(cell->neighbor(f)))
                  ))
                estimated_error_per_cell(cell_index) = 0;
            }
          else
            {
              if ((cell->at_boundary(f) == false)
                  &&
                  (((cell->neighbor(f)->level() == cell->level())
                    &&
                    (cell->neighbor(f)->has_children() == false)
                    &&
                    cell_is_in_solid_domain (cell->neighbor(f)))
                   ||
                   ((cell->neighbor(f)->level() == cell->level())
                    &&
                    (cell->neighbor(f)->has_children() == true)
                    &&
                    (cell_is_in_solid_domain (cell->neighbor_child_on_subface
                                              (f, 0))))
                   ||
                   (cell->neighbor_is_coarser(f)
                    &&
                    cell_is_in_solid_domain(cell->neighbor(f)))
                  ))
                estimated_error_per_cell(cell_index) = 0;
            }
    }

    GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                     estimated_error_per_cell,
                                                     0.3, 0.0);
    triangulation.execute_coarsening_and_refinement ();
  }

  // @sect4{FluidStructureProblem::run}

  // This is, as usual, the function that controls the overall flow of
  // operation. If you've read through tutorial programs step-1 through
  // step-6, for example, then you are already quite familiar with the
  // following structure:
  template <int dim>
  void FluidStructureProblem<dim>::run ()
  {
    make_grid ();

    for (unsigned int refinement_cycle = 0; refinement_cycle<10-2*dim;
         ++refinement_cycle)
      {
        std::cout << "Refinement cycle " << refinement_cycle << std::endl;

        if (refinement_cycle > 0)
          refine_mesh ();

        setup_dofs ();

        std::cout << "   Assembling..." << std::endl;
        assemble_system ();

        std::cout << "   Solving..." << std::endl;
        solve ();

        std::cout << "   Writing output..." << std::endl;
        output_results (refinement_cycle);

        std::cout << std::endl;
      }
  }
}

// @sect4{The main() function}

// This, final, function contains pretty much exactly what most of the other
// tutorial programs have:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step46;

      deallog.depth_console (0);

      FluidStructureProblem<2> flow_problem(1, 1);
      flow_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;
}


#ifndef MULTIPRECISION_ENCMATRIX_HPP
#define MULTIPRECISION_ENCMATRIX_HPP
#include "MPEncVector.hpp"
#include "algebra/EncMatrix.hpp"
#include "algebra/Matrix.hpp"
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>

class MPPubKey;
class MPEncArray;
class MPSecKey;

class MPEncMatrix {
public:
    MPEncMatrix() {}

    MPEncMatrix(const std::vector<MPEncVector> &copy);

    size_t rowsNum() const { return ctxts.size(); }

    void pack(const MDL::Matrix<long> &mat,
              const MPPubKey &pk,
              const MPEncArray &ea);

    void unpack(MDL::Matrix<NTL::ZZ> &result,
                const MPSecKey &sk,
                const MPEncArray &ea,
                bool negate = true);
    /// assume that the matrix is symmetric
    MPEncVector sDot(const MPEncVector &oth,
                     const MPPubKey &pk,
                     const MPEncArray &ea) const;

    MPEncMatrix& dot(const MPEncMatrix &oth,
                     const MPEncArray &ea,
                     const MPPubKey &pk,
                     long columnToProces = 0);

    MPEncMatrix& dot2(const MPEncMatrix &oth,
                      const MPEncArray &ea,
                      const MPPubKey &pk,
                      long columnToProces = 0);

    MPEncMatrix& addConstant(const MDL::Matrix<long> &con,
                             const MPEncArray &ea);

    MPEncMatrix& mulConstant(const MDL::Matrix<long> &con,
                             const MPEncArray &ea);

    MPEncMatrix& mulConstant(const NTL::ZZX &con);

    MPEncMatrix& negate();

    MPEncMatrix& operator+=(const MPEncMatrix &oth);

    MPEncMatrix& operator-=(const MPEncMatrix &oth);

    const MPEncVector& get(int index) const { return ctxts[index]; }

    long getColumns() const { return columns; }

    void setColumns(long c) { columns = c; }
private:
    long columns = -1;
    // MPPubKey &_pk;
    std::vector<MPEncVector> ctxts;
};

MPEncMatrix mulMatrix(const MPEncVector &vec,
                      const MDL::Matrix<long> &mat,
                      const MPEncArray &ea);
#endif // multiprecision/EncMatrix.hpp


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/multiprecision/eigen.hpp>
#include <iostream>
#include <Eigen/Dense>
#include "test.hpp"

using namespace Eigen;

template <class T>
struct related_number
{
   typedef T type;
};

template <class Num>
void example1()
{
   // expected results first:
   Matrix<Num, 2, 2> r1, r2;
   r1 << 3, 5, 4, 8;
   r2 << -1, -1, 2, 0;
   Matrix<Num, 3, 1> r3;
   r3 << -1, -4, -6;

   Matrix<Num, 2, 2> a;
   a << 1, 2, 3, 4;
   Matrix<Num, Dynamic, Dynamic> b(2, 2);
   b << 2, 3, 1, 4;
   std::cout << "a + b =\n" << a + b << std::endl;
   BOOST_CHECK_EQUAL(a + b, r1);
   std::cout << "a - b =\n" << a - b << std::endl;
   BOOST_CHECK_EQUAL(a - b, r2);
   std::cout << "Doing a += b;" << std::endl;
   a += b;
   std::cout << "Now a =\n" << a << std::endl;
   Matrix<Num, 3, 1> v(1, 2, 3);
   Matrix<Num, 3, 1> w(1, 0, 0);
   std::cout << "-v + w - v =\n" << -v + w - v << std::endl;
   BOOST_CHECK_EQUAL(-v + w - v, r3);
}

template <class Num>
void example2()
{
   Matrix<Num, 2, 2> a;
   a << 1, 2, 3, 4;
   Matrix<Num, 3, 1> v(1, 2, 3);
   std::cout << "a * 2.5 =\n" << a * 2.5 << std::endl;
   std::cout << "0.1 * v =\n" << 0.1 * v << std::endl;
   std::cout << "Doing v *= 2;" << std::endl;
   v *= 2;
   std::cout << "Now v =\n" << v << std::endl;
   Num n(4);
   std::cout << "Doing v *= Num;" << std::endl;
   v *= n;
   std::cout << "Now v =\n" << v << std::endl;
   typedef typename related_number<Num>::type related_type;
   related_type r(6);
   std::cout << "Doing v *= RelatedType;" << std::endl;
   v *= r;
   std::cout << "Now v =\n" << v << std::endl;
   std::cout << "RelatedType * v =\n" << r * v << std::endl;
   std::cout << "Doing v *= RelatedType^2;" << std::endl;
   v *= r * r;
   std::cout << "Now v =\n" << v << std::endl;
   std::cout << "RelatedType^2 * v =\n" << r * r * v << std::endl;
}

template <class Num>
void example3()
{
   using namespace std;
   Matrix<Num, Dynamic, Dynamic> a = Matrix<Num, Dynamic, Dynamic>::Random(2, 2);
   cout << "Here is the matrix a\n" << a << endl;
   cout << "Here is the matrix a^T\n" << a.transpose() << endl;
   cout << "Here is the conjugate of a\n" << a.conjugate() << endl;
   cout << "Here is the matrix a^*\n" << a.adjoint() << endl;
}

template <class Num>
void example4()
{
   Matrix<Num, 2, 2> mat;
   mat << 1, 2,
      3, 4;
   Matrix<Num, 2, 1> u(-1, 1), v(2, 0);
   std::cout << "Here is mat*mat:\n" << mat * mat << std::endl;
   std::cout << "Here is mat*u:\n" << mat * u << std::endl;
   std::cout << "Here is u^T*mat:\n" << u.transpose()*mat << std::endl;
   std::cout << "Here is u^T*v:\n" << u.transpose()*v << std::endl;
   std::cout << "Here is u*v^T:\n" << u * v.transpose() << std::endl;
   std::cout << "Let's multiply mat by itself" << std::endl;
   mat = mat * mat;
   std::cout << "Now mat is mat:\n" << mat << std::endl;
}

template <class Num>
void example5()
{
   using namespace std;
   Matrix<Num, 3, 1> v(1, 2, 3);
   Matrix<Num, 3, 1> w(0, 1, 2);
   cout << "Dot product: " << v.dot(w) << endl;
   Num dp = v.adjoint()*w; // automatic conversion of the inner product to a scalar
   cout << "Dot product via a matrix product: " << dp << endl;
   cout << "Cross product:\n" << v.cross(w) << endl;
}

template <class Num>
void example6()
{
   using namespace std;
   Matrix<Num, 2, 2> mat;
   mat << 1, 2,
      3, 4;
   cout << "Here is mat.sum():       " << mat.sum() << endl;
   cout << "Here is mat.prod():      " << mat.prod() << endl;
   cout << "Here is mat.mean():      " << mat.mean() << endl;
   cout << "Here is mat.minCoeff():  " << mat.minCoeff() << endl;
   cout << "Here is mat.maxCoeff():  " << mat.maxCoeff() << endl;
   cout << "Here is mat.trace():     " << mat.trace() << endl;
}

template <class Num>
void example7()
{
   using namespace std;

   Array<Num, Dynamic, Dynamic>  m(2, 2);

   // assign some values coefficient by coefficient
   m(0, 0) = 1.0; m(0, 1) = 2.0;
   m(1, 0) = 3.0; m(1, 1) = m(0, 1) + m(1, 0);

   // print values to standard output
   cout << m << endl << endl;

   // using the comma-initializer is also allowed
   m << 1.0, 2.0,
      3.0, 4.0;

   // print values to standard output
   cout << m << endl;
}

template <class Num>
void example8()
{
   using namespace std;
   Array<Num, Dynamic, Dynamic> a(3, 3);
   Array<Num, Dynamic, Dynamic> b(3, 3);
   a << 1, 2, 3,
      4, 5, 6,
      7, 8, 9;
   b << 1, 2, 3,
      1, 2, 3,
      1, 2, 3;

   // Adding two arrays
   cout << "a + b = " << endl << a + b << endl << endl;
   // Subtracting a scalar from an array
   cout << "a - 2 = " << endl << a - 2 << endl;
}

template <class Num>
void example9()
{
   using namespace std;
   Array<Num, Dynamic, Dynamic> a(2, 2);
   Array<Num, Dynamic, Dynamic> b(2, 2);
   a << 1, 2,
      3, 4;
   b << 5, 6,
      7, 8;
   cout << "a * b = " << endl << a * b << endl;
}

template <class Num>
void example10()
{
   using namespace std;
   Array<Num, Dynamic, 1> a = Array<Num, Dynamic, 1>::Random(5);
   a *= 2;
   cout << "a =" << endl
      << a << endl;
   cout << "a.abs() =" << endl
      << a.abs() << endl;
   cout << "a.abs().sqrt() =" << endl
      << a.abs().sqrt() << endl;
   cout << "a.min(a.abs().sqrt()) =" << endl
      << a.min(a.abs().sqrt()) << endl;
}

template <class Num>
void example11()
{
   using namespace std;
   Matrix<Num, Dynamic, Dynamic> m(2, 2);
   Matrix<Num, Dynamic, Dynamic> n(2, 2);
   Matrix<Num, Dynamic, Dynamic> result(2, 2);
   m << 1, 2,
      3, 4;
   n << 5, 6,
      7, 8;
   result = m * n;
   cout << "-- Matrix m*n: --" << endl << result << endl << endl;
   result = m.array() * n.array();
   cout << "-- Array m*n: --" << endl << result << endl << endl;
   result = m.cwiseProduct(n);
   cout << "-- With cwiseProduct: --" << endl << result << endl << endl;
   result = m.array() + 4;
   cout << "-- Array m + 4: --" << endl << result << endl << endl;
}

template <class Num>
void example12()
{
   using namespace std;
   Matrix<Num, Dynamic, Dynamic> m(2, 2);
   Matrix<Num, Dynamic, Dynamic> n(2, 2);
   Matrix<Num, Dynamic, Dynamic> result(2, 2);
   m << 1, 2,
      3, 4;
   n << 5, 6,
      7, 8;

   result = (m.array() + 4).matrix() * m;
   cout << "-- Combination 1: --" << endl << result << endl << endl;
   result = (m.array() * n.array()).matrix() * m;
   cout << "-- Combination 2: --" << endl << result << endl << endl;
}

template <class Num>
void example13()
{
   using namespace std;
   Matrix<Num, Dynamic, Dynamic> m(4, 4);
   m << 1, 2, 3, 4,
      5, 6, 7, 8,
      9, 10, 11, 12,
      13, 14, 15, 16;
   cout << "Block in the middle" << endl;
   cout << m.template block<2, 2>(1, 1) << endl << endl;
   for (int i = 1; i <= 3; ++i)
   {
      cout << "Block of size " << i << "x" << i << endl;
      cout << m.block(0, 0, i, i) << endl << endl;
   }
}

template <class Num>
void example14()
{
   using namespace std;
   Array<Num, 2, 2> m;
   m << 1, 2,
      3, 4;
   Array<Num, 4, 4> a = Array<Num, 4, 4>::Constant(0.6);
   cout << "Here is the array a:" << endl << a << endl << endl;
   a.template block<2, 2>(1, 1) = m;
   cout << "Here is now a with m copied into its central 2x2 block:" << endl << a << endl << endl;
   a.block(0, 0, 2, 3) = a.block(2, 1, 2, 3);
   cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:" << endl << a << endl << endl;
}

template <class Num>
void example15()
{
   using namespace std;
   Eigen::Matrix<Num, Dynamic, Dynamic> m(3, 3);
   m << 1, 2, 3,
      4, 5, 6,
      7, 8, 9;
   cout << "Here is the matrix m:" << endl << m << endl;
   cout << "2nd Row: " << m.row(1) << endl;
   m.col(2) += 3 * m.col(0);
   cout << "After adding 3 times the first column into the third column, the matrix m is:\n";
   cout << m << endl;
}

template <class Num>
void example16()
{
   using namespace std;
   Matrix<Num, 4, 4> m;
   m << 1, 2, 3, 4,
      5, 6, 7, 8,
      9, 10, 11, 12,
      13, 14, 15, 16;
   cout << "m.leftCols(2) =" << endl << m.leftCols(2) << endl << endl;
   cout << "m.bottomRows<2>() =" << endl << m.template bottomRows<2>() << endl << endl;
   m.topLeftCorner(1, 3) = m.bottomRightCorner(3, 1).transpose();
   cout << "After assignment, m = " << endl << m << endl;
}

template <class Num>
void example17()
{
   using namespace std;
   Array<Num, Dynamic, 1> v(6);
   v << 1, 2, 3, 4, 5, 6;
   cout << "v.head(3) =" << endl << v.head(3) << endl << endl;
   cout << "v.tail<3>() = " << endl << v.template tail<3>() << endl << endl;
   v.segment(1, 4) *= 2;
   cout << "after 'v.segment(1,4) *= 2', v =" << endl << v << endl;
}

template <class Num>
void example18()
{
   using namespace std;
   Matrix<Num, 2, 2> mat;
   mat << 1, 2,
      3, 4;
   cout << "Here is mat.sum():       " << mat.sum() << endl;
   cout << "Here is mat.prod():      " << mat.prod() << endl;
   cout << "Here is mat.mean():      " << mat.mean() << endl;
   cout << "Here is mat.minCoeff():  " << mat.minCoeff() << endl;
   cout << "Here is mat.maxCoeff():  " << mat.maxCoeff() << endl;
   cout << "Here is mat.trace():     " << mat.trace() << endl;

   BOOST_CHECK_EQUAL(mat.sum(), 10);
   BOOST_CHECK_EQUAL(mat.prod(), 24);
   BOOST_CHECK_EQUAL(mat.mean(), Num(5) / 2);
   BOOST_CHECK_EQUAL(mat.minCoeff(), 1);
   BOOST_CHECK_EQUAL(mat.maxCoeff(), 4);
   BOOST_CHECK_EQUAL(mat.trace(), 5);
}

template <class Num>
void example18a()
{
   using namespace std;
   Matrix<Num, 2, 2> mat;
   mat << 1, 2,
      3, 4;
   cout << "Here is mat.sum():       " << mat.sum() << endl;
   cout << "Here is mat.prod():      " << mat.prod() << endl;
   cout << "Here is mat.mean():      " << mat.mean() << endl;
   //cout << "Here is mat.minCoeff():  " << mat.minCoeff() << endl;
   //cout << "Here is mat.maxCoeff():  " << mat.maxCoeff() << endl;
   cout << "Here is mat.trace():     " << mat.trace() << endl;
}

template <class Num>
void example19()
{
   using namespace std;
   Matrix<Num, Dynamic, 1> v(2);
   Matrix<Num, Dynamic, Dynamic> m(2, 2), n(2, 2);

   v << -1,
      2;

   m << 1, -2,
      -3, 4;
   cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
   cout << "v.norm() = " << v.norm() << endl;
   cout << "v.lpNorm<1>() = " << v.template lpNorm<1>() << endl;
   cout << "v.lpNorm<Infinity>() = " << v.template lpNorm<Infinity>() << endl;
   cout << endl;
   cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
   cout << "m.norm() = " << m.norm() << endl;
   cout << "m.lpNorm<1>() = " << m.template lpNorm<1>() << endl;
   cout << "m.lpNorm<Infinity>() = " << m.template lpNorm<Infinity>() << endl;
}

template <class Num>
void example20()
{
   using namespace std;
   Matrix<Num, 3, 3> A;
   Matrix<Num, 3, 1> b;
   A << 1, 2, 3, 4, 5, 6, 7, 8, 10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Matrix<Num, 3, 1> x = A.colPivHouseholderQr().solve(b);
   cout << "The solution is:\n" << x << endl;
}

template <class Num>
void example21()
{
   using namespace std;
   Matrix<Num, 2, 2> A, b;
   A << 2, -1, -1, 3;
   b << 1, 2, 3, 1;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the right hand side b:\n" << b << endl;
   Matrix<Num, 2, 2> x = A.ldlt().solve(b);
   cout << "The solution is:\n" << x << endl;
}

template <class Num>
void example22()
{
   using namespace std;
   Matrix<Num, Dynamic, Dynamic> A = Matrix<Num, Dynamic, Dynamic>::Random(100, 100);
   Matrix<Num, Dynamic, Dynamic> b = Matrix<Num, Dynamic, Dynamic>::Random(100, 50);
   Matrix<Num, Dynamic, Dynamic> x = A.fullPivLu().solve(b);
   Matrix<Num, Dynamic, Dynamic> axmb = A * x - b;
   double relative_error = static_cast<double>(abs(axmb.norm() / b.norm())); // norm() is L2 norm
   cout << "norm1 = " << axmb.norm() << endl;
   cout << "norm2 = " << b.norm() << endl;
   cout << "The relative error is:\n" << relative_error << endl;
}

template <class Num>
void example23()
{
   using namespace std;
   Matrix<Num, 2, 2> A;
   A << 1, 2, 2, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   SelfAdjointEigenSolver<Matrix<Num, 2, 2> > eigensolver(A);
   if (eigensolver.info() != Success)
   {
      std::cout << "Eigenvalue solver failed!" << endl;
   }
   else
   {
      cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
      cout << "Here's a matrix whose columns are eigenvectors of A \n"
         << "corresponding to these eigenvalues:\n"
         << eigensolver.eigenvectors() << endl;
   }
}

template <class Num>
void example24()
{
   using namespace std;
   Matrix<Num, 3, 3> A;
   A << 1, 2, 1,
      2, 1, 0,
      -1, 1, 2;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "The determinant of A is " << A.determinant() << endl;
   cout << "The inverse of A is:\n" << A.inverse() << endl;
}

template <class Num>
void test_integer_type()
{
   example1<Num>();
   //example2<Num>();
   example18<Num>();
}

template <class Num>
void test_float_type()
{
   std::cout << "Epsilon    = " << Eigen::NumTraits<Num>::epsilon() << std::endl;
   std::cout << "Dummy Prec = " << Eigen::NumTraits<Num>::dummy_precision() << std::endl;
   std::cout << "Highest    = " << Eigen::NumTraits<Num>::highest() << std::endl;
   std::cout << "Lowest     = " << Eigen::NumTraits<Num>::lowest() << std::endl;
   std::cout << "Digits10   = " << Eigen::NumTraits<Num>::digits10() << std::endl;

   example1<Num>();
   example2<Num>();
   example4<Num>();
   example5<Num>();
   example6<Num>();
   example7<Num>();
   example8<Num>();
   example9<Num>();
   example10<Num>();
   example11<Num>();
   example12<Num>();
   example13<Num>();
   example14<Num>();
   example15<Num>();
   example16<Num>();
   example17<Num>();
   /*
   example18<Num>();
   example19<Num>();
   example20<Num>();
   example21<Num>();
   example22<Num>();
   example23<Num>();
   example24<Num>();
   */
}

template <class Num>
void test_float_type_2()
{
   std::cout << "Epsilon    = " << Eigen::NumTraits<Num>::epsilon() << std::endl;
   std::cout << "Dummy Prec = " << Eigen::NumTraits<Num>::dummy_precision() << std::endl;
   std::cout << "Highest    = " << Eigen::NumTraits<Num>::highest() << std::endl;
   std::cout << "Lowest     = " << Eigen::NumTraits<Num>::lowest() << std::endl;
   std::cout << "Digits10   = " << Eigen::NumTraits<Num>::digits10() << std::endl;

   example18<Num>();
   example19<Num>();
   example20<Num>();
   example21<Num>();

   //example22<Num>();
   //example23<Num>();
   //example24<Num>();
}

template <class Num>
void test_float_type_3()
{
   std::cout << "Epsilon    = " << Eigen::NumTraits<Num>::epsilon() << std::endl;
   std::cout << "Dummy Prec = " << Eigen::NumTraits<Num>::dummy_precision() << std::endl;
   std::cout << "Highest    = " << Eigen::NumTraits<Num>::highest() << std::endl;
   std::cout << "Lowest     = " << Eigen::NumTraits<Num>::lowest() << std::endl;
   std::cout << "Digits10   = " << Eigen::NumTraits<Num>::digits10() << std::endl;

   example22<Num>();
   example23<Num>();
   example24<Num>();
}

template <class Num>
void test_complex_type()
{
   std::cout << "Epsilon    = " << Eigen::NumTraits<Num>::epsilon() << std::endl;
   std::cout << "Dummy Prec = " << Eigen::NumTraits<Num>::dummy_precision() << std::endl;
   std::cout << "Highest    = " << Eigen::NumTraits<Num>::highest() << std::endl;
   std::cout << "Lowest     = " << Eigen::NumTraits<Num>::lowest() << std::endl;
   std::cout << "Digits10   = " << Eigen::NumTraits<Num>::digits10() << std::endl;

   example1<Num>();
   example2<Num>();
   example3<Num>();
   example4<Num>();
   example5<Num>();
   example7<Num>();
   example8<Num>();
   example9<Num>();
   example11<Num>();
   example12<Num>();
   example13<Num>();
   example14<Num>();
   example15<Num>();
   example16<Num>();
   example17<Num>();
   example18a<Num>();
   example19<Num>();
   example20<Num>();
   example21<Num>();
   example22<Num>();
   // example23<Num>();  //requires comparisons.
   example24<Num>();
}


/*************************************************************************
	> File Name: src/pcd2octomap.cpp
	> Author: Gao Xiang
	> Mail: gaoxiang12@mails.tsinghua.edu.cn
	> Created Time: 2015年12月12日 星期六 15时51分45秒
 ************************************************************************/

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// octomap
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h>

// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry>

// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

// boost.format 字符串处理
#include <boost/format.hpp>

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

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: fues_rs_img <input_file.png> <rslidar.pcd>"<<endl;
        return -1;
    }

    string img_file = argv[1], pcd_file = argv[2];
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZI> ( pcd_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    cv::Mat img = cv::imread(img_file);

    float  cx     = 651.5;
    float  cy     = 370.5;
    float  fx     = 534.0;
    float  fy     = 533.0;

    Eigen::Quaterniond q(1,0,0,0);
    Eigen::Isometry3d T(q);
    T.pretranslate(Vector3d(-0.02  , -0.17,  0.1)); //-0.02   -0.17  0.1

    cout<<"the T  mat is: "<<T.matrix()<<endl;

        // 将cloud旋转之后插入全局地图
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcd_trans( new pcl::PointCloud<pcl::PointXYZI>() );
    pcl::transformPointCloud( cloud, *pcd_trans, T.matrix() );

    cv::Mat new_img = img.clone();

    for (auto p:pcd_trans->points){
        Vector2d pixel = Vector2d(p.y/p.x*fx+cx,p.z/p.x*fy+cy);
        cout<<"the pixel pos is :"<<pixel[0]<<" "<<pixel[1]<<endl;

        if (int(p.y/p.x*fx+cx)>0 &&  int(p.y/p.x*fx+cx)<1280  && int(p.z/p.x*fy+cy)>0 && int(p.z/p.x*fy+cy)<720 )
       // img.at<Vec3b>(int(p.y/p.x*fx+cx),int(p.z/p.x*fy+cy)) = 250,0,0;//        img.at<Vec3b>(int(p.z/p.x*fy+cy),int(p.y/p.x*fx+cx))

        new_img.at<Vec3b>(int(p.z/p.x*fy+cy),int(p.y/p.x*fx+cx)) = int(p.intensity),0,0;

    }
    cv::imshow("",new_img);
    cv::waitKey();
    cv::imwrite("/home/jinjing/octomap/octomap_tutor_gx/NEW_DATA/fuse.jpeg",new_img);

    octomap::ColorOcTree tree( 0.05 );

    for (auto p:pcd_trans->points)
    {
        // 将点云里的点插入到octomap中

        Vector2d pixel = Vector2d(p.y/p.x*fx+cx,p.z/p.x*fy+cy);
        Vec3b color;
        cout<<"the pixel pos is :"<<pixel[0]<<" "<<pixel[1]<<endl;

        if (int(p.y/p.x*fx+cx)>0 &&  int(p.y/p.x*fx+cx)<1280  && int(p.z/p.x*fy+cy)>0 && int(p.z/p.x*fy+cy)<720 ){

            tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
            color = img.at<Vec3b>(int(p.z/p.x*fy+cy),int(p.y/p.x*fx+cx)); // bgr
            cout<<"color: "<<color<<endl;
            tree.integrateNodeColor( p.x, p.y, p.z, color[2],color[1],color[0]); //rgb

        }
    }

    tree.updateInnerOccupancy();
    // 存储octomap, 注意要存成.ot文件而非.bt文件
    tree.write( "hhh.ot" );
    cout<<"done."<<endl;

    return 0;
}


#include <iostream>
#include <boost/format.hpp>

using namespace std;

int main(){
    cout << boost::format("%s:%d+%d=%d") %"sum" % 1 % 2 % (1+2) << endl;
    boost::format fmt("(%1% + %2%) * %2% = %3%\n");
    fmt % 2 % 5 ;
    fmt % ((2+5)*5);
    cout << fmt.str() ;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/containers/t4mesh/t4mesh.h>
#include <OpenTissue/core/containers/t4mesh/io/t4mesh_xml_write.h>
#include <OpenTissue/core/containers/t4mesh/io/t4mesh_xml_read.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_t4mesh_t4mesh);

BOOST_AUTO_TEST_CASE(xml_testing)
{
  typedef OpenTissue::math::BasicMathTypes<int, size_t>    math_types;
  typedef OpenTissue::t4mesh::T4Mesh< math_types >         mesh_type;
  typedef math_types::vector3_type                         vector3_type;

  std::vector<vector3_type> c;
  c.resize( 6 );
  c[0] = vector3_type(0,0,0);
  c[1] = vector3_type(0,0,1);
  c[2] = vector3_type(0,1,0);
  c[3] = vector3_type(0,1,1);
  c[4] = vector3_type(1,0,0);
  c[5] = vector3_type(1,0,1);

  mesh_type M;
  M.insert( c[0] );
  M.insert( c[1] );
  M.insert( c[2] );
  M.insert( c[3] );
  M.insert( c[4] );
  M.insert( c[5] );
  M.insert(0,1,2,3);
  M.insert(0,1,2,4);

  {
    mesh_type W;
    bool success = true;
    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_read("do_not_exist.xml",W) );
    BOOST_CHECK(!success);
  }
  {
    mesh_type W;
    std::vector<vector3_type> d;
    bool success = true;
    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_read("do_not_exist.xml",W,d) );
    BOOST_CHECK(!success);
  }
  {
    mesh_type W;

    bool success = false;
    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_write("t4mesh_test1.xml",M) );
    BOOST_CHECK(success);

    bool has_tags = true;
    BOOST_CHECK_NO_THROW( has_tags = OpenTissue::t4mesh::xml_has_tags("t4mesh_test1.xml") );
    BOOST_CHECK(!has_tags);

    BOOST_CHECK( W.size_nodes() == 0 );
    BOOST_CHECK( W.size_tetrahedra() == 0 );

    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_read("t4mesh_test1.xml",W) );
    BOOST_CHECK(success);

    BOOST_CHECK( M.size_nodes() == W.size_nodes() );
    BOOST_CHECK( M.size_tetrahedra() == W.size_tetrahedra() );
    for(size_t n = 0; n < M.size_nodes(); ++n)
    {
      BOOST_CHECK( M.node(n)->idx() == W.node(n)->idx());
      BOOST_CHECK( M.node(n)->m_coord == W.node(n)->m_coord );
    }
    for(size_t t = 0; t < M.size_tetrahedra(); ++t)
    {
      BOOST_CHECK( M.tetrahedron(t)->idx() == W.tetrahedron(t)->idx());
      BOOST_CHECK( M.tetrahedron(t)->i()->idx() == W.tetrahedron(t)->i()->idx());
      BOOST_CHECK( M.tetrahedron(t)->j()->idx() == W.tetrahedron(t)->j()->idx());
      BOOST_CHECK( M.tetrahedron(t)->k()->idx() == W.tetrahedron(t)->k()->idx());
      BOOST_CHECK( M.tetrahedron(t)->m()->idx() == W.tetrahedron(t)->m()->idx());

    }
  }
  {
    mesh_type W;
    std::vector<vector3_type> d;

    bool success = false;
    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_write("t4mesh_test2.xml",M,c) );
    BOOST_CHECK(success);

    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_read("t4mesh_test2.xml",W,d) );
    BOOST_CHECK(success);

    BOOST_CHECK( M.size_nodes() == W.size_nodes() );
    BOOST_CHECK( M.size_tetrahedra() == W.size_tetrahedra() );
    for(size_t n = 0; n < M.size_nodes(); ++n)
    {
      BOOST_CHECK( M.node(n)->idx() == W.node(n)->idx());
      BOOST_CHECK( d[n] == c[n] );
    }
    for(size_t t = 0; t < M.size_tetrahedra(); ++t)
    {
      BOOST_CHECK( M.tetrahedron(t)->idx() == W.tetrahedron(t)->idx());
      BOOST_CHECK( M.tetrahedron(t)->i()->idx() == W.tetrahedron(t)->i()->idx());
      BOOST_CHECK( M.tetrahedron(t)->j()->idx() == W.tetrahedron(t)->j()->idx());
      BOOST_CHECK( M.tetrahedron(t)->k()->idx() == W.tetrahedron(t)->k()->idx());
      BOOST_CHECK( M.tetrahedron(t)->m()->idx() == W.tetrahedron(t)->m()->idx());

    }
  }
}

    template<typename math_types>
  class NodeTraits
    : public OpenTissue::t4mesh::DefaultNodeTraits<math_types>
    , public OpenTissue::utility::default_tag_supported_type
  {
  };

//    template<typename math_types>
  class TetrahedronTraits
    : public OpenTissue::t4mesh::DefaultTetrahedronTraits
    , public OpenTissue::utility::default_tag_supported_type
  {
  };

BOOST_AUTO_TEST_CASE(xml_testing_with_tags)
{

  typedef OpenTissue::math::BasicMathTypes<int, size_t>                            math_types;
  typedef OpenTissue::t4mesh::T4Mesh< math_types, NodeTraits<math_types>, TetrahedronTraits >  mesh_type;
  typedef math_types::vector3_type                                                 vector3_type;

  std::vector<vector3_type> c;
  c.resize( 6 );
  c[0] = vector3_type(0,0,0);
  c[1] = vector3_type(0,0,1);
  c[2] = vector3_type(0,1,0);
  c[3] = vector3_type(0,1,1);
  c[4] = vector3_type(1,0,0);
  c[5] = vector3_type(1,0,1);

  mesh_type M;
  M.insert( c[0] );
  M.insert( c[1] );
  M.insert( c[2] );
  M.insert( c[3] );
  M.insert( c[4] );
  M.insert( c[5] );
  M.insert(0,1,2,3);
  M.insert(0,1,2,4);

  OpenTissue::utility::set_tag( *(M.node(0)) , 0 );
  OpenTissue::utility::set_tag( *(M.node(1)) , 1 );
  OpenTissue::utility::set_tag( *(M.node(2)) , 2 );
  OpenTissue::utility::set_tag( *(M.node(3)) , 3 );
  OpenTissue::utility::set_tag( *(M.node(4)) , 4 );
  OpenTissue::utility::set_tag( *(M.node(5)) , 5 );
  OpenTissue::utility::set_tag( *(M.tetrahedron(0)) , 0 );
  OpenTissue::utility::set_tag( *(M.tetrahedron(1)) , 1 );

  {
    mesh_type W;

    bool success = false;
    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_write("t4mesh_test3.xml",M) );
    BOOST_CHECK(success);

    bool has_tags = true;
    BOOST_CHECK_NO_THROW( has_tags = OpenTissue::t4mesh::xml_has_tags("t4mesh_test3.xml") );
    BOOST_CHECK(has_tags);

    BOOST_CHECK( W.size_nodes() == 0 );
    BOOST_CHECK( W.size_tetrahedra() == 0 );

    BOOST_CHECK_NO_THROW( success = OpenTissue::t4mesh::xml_read("t4mesh_test3.xml",W) );
    BOOST_CHECK(success);

    BOOST_CHECK( M.size_nodes() == W.size_nodes() );
    BOOST_CHECK( M.size_tetrahedra() == W.size_tetrahedra() );
    for(size_t n = 0; n < M.size_nodes(); ++n)
    {
      BOOST_CHECK( M.node(n)->idx() == W.node(n)->idx());
      BOOST_CHECK( M.node(n)->m_coord == W.node(n)->m_coord );
      BOOST_CHECK( tag_value( *( M.node(n))) ==  OpenTissue::utility::tag_value( *(W.node(n)))  );
    }
    for(size_t t = 0; t < M.size_tetrahedra(); ++t)
    {
      BOOST_CHECK( M.tetrahedron(t)->idx() == W.tetrahedron(t)->idx());
      BOOST_CHECK( M.tetrahedron(t)->i()->idx() == W.tetrahedron(t)->i()->idx());
      BOOST_CHECK( M.tetrahedron(t)->j()->idx() == W.tetrahedron(t)->j()->idx());
      BOOST_CHECK( M.tetrahedron(t)->k()->idx() == W.tetrahedron(t)->k()->idx());
      BOOST_CHECK( M.tetrahedron(t)->m()->idx() == W.tetrahedron(t)->m()->idx());
      BOOST_CHECK( OpenTissue::utility::tag_value( *(M.tetrahedron(t))) ==  OpenTissue::utility::tag_value( *(W.tetrahedron(t)) ) );
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();