/**
 * \file boost/numeric/ublasx/operation/find.hpp
 *
 * \brief Find the elments of a given container which satisfy a given unary
 *  predicate.
 *
 * \todo Implementation for matrix expressions.
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompfinding file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_FIND_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_FIND_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <functional>

namespace boost { namespace numeric { namespace ublasx {

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

//@{ Declarations

/**
 * \brief Find the elments of the given vector expression which satisfy the
 *  given unary predicate.
 * \tparam VectorExprT The type of the vector expression.
 * \tparam UnaryPredicateT The type of the unary predicate functor.
 * \param ve The vector expression over which check for the predicate.
 * \param p The unary predicate functor: must accept one argument and return
 * a boolean value.
 * \return A vector of the elements of the given vector expression which satisfy
 *  the given predicate; if no element satisfies the given predicate.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename VectorExprT, typename UnaryPredicateT>
vector<typename vector_traits<VectorExprT>::value_type> find(vector_expression<VectorExprT> const& ve, UnaryPredicateT p);

/**
 * \brief Find the non-zero elments of the given vector expression.
 * \tparam VectorExprT The type of the vector expression.
 * \param ve The vector expression over which check for existence of non-zero
 *  elements.
 * \return \c true if the given vector expression contains at least one non-zero
 *  element; \c false otherwise.
 *
 * \note The test for zero equality is done in the strong sense, that is by not
 *  using find tolerance.
 *  For checking for "weak" zero equality between a given tolerance use the
 *  two-argument version of this function with an appropriate predicate.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename VectorExprT>
vector<typename vector_traits<VectorExprT>::value_type> find(vector_expression<VectorExprT> const& ve);

//@} Declarations

//@{ Definitions

template <typename VectorExprT, typename UnaryPredicateT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> find(vector_expression<VectorExprT> const& ve, UnaryPredicateT p)
{
	typedef typename vector_traits<VectorExprT>::size_type size_type;
	typedef typename vector_traits<VectorExprT>::value_type value_type;

	vector<value_type> res;
	size_type n = size(ve);
	size_type j = 0;
	for (size_type i = 0; i < n; ++i)
	{
		if (p(ve()(i)))
		{
			res.resize(res.size()+1);
			res(j++) = ve()(i);
		}
	}

	return res;
}

template <typename VectorExprT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> find(vector_expression<VectorExprT> const& ve)
{
	typedef typename vector_traits<VectorExprT>::value_type value_type;

    return find(ve, ::std::bind2nd(::std::not_equal_to<value_type>(), 0));
}

//@} Definitions

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_FIND_HPP


// Program that estimates the visual utility timing for one estimator
// and spits out a file where each line is of the form:
//
// <nWindows>,<seconds>
//
// Usage: EstimateVUTiming [options] <outputFile> <sampleImage0> <sampleImage1> ... <sampleImageN>

#include "visual_utility/TimingEstimator.h"
#include "visual_utility/VisualUtilityEstimator.h"
#include "visual_utility/VisualUtilityROSParams.h"
#include <ros/ros.h>
#include <fstream>
#include <boost/scoped_ptr.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>
#include <vector>
#include <math.h>

using namespace std;
using namespace visual_utility;
using namespace boost;
using namespace cv;

int main(int argc, char **argv) {
  ros::init(argc, argv, "EstimateVUTiming",
            ros::init_options::AnonymousName);

  string outputFile(argv[1]);

  ros::NodeHandle handle("~");

  // Set the stride to enable caching in the HOG detector
  double winStride = 8;
  handle.setParam("win_stride", (int)winStride);
  bool useCache;
  handle.param<bool>("use_cache", useCache, true);
  handle.setParam("hog_do_cache", useCache);

  // Create the visual utility estimator we want to use
  scoped_ptr<TransformEstimator> transformEstimator(
    CreateTransformEstimator(handle));
  scoped_ptr<VisualUtilityEstimator> vuEstimator(
    CreateVisualUtilityEstimator(handle, *transformEstimator));

  // Grab the list of images
  vector<string> imageFiles;
  for (int i = 2; i < argc; ++i) {
    imageFiles.push_back(string(argv[i]));
  }

  // Create a list of rectangles to search for
  Mat image = imread(imageFiles[0]);
  // TODO(mdesnoyer): Make these parameters so that they aren't hard
  // coded for people detection.
  double curHeight = 128;
  double curWidth = 64;
  double scaleStride = 1.10;
  vector<Rect> rois;
  while (curWidth < image.rows && curHeight < image.cols) {
    for (double x = 0; x < image.cols - curWidth; x += winStride) {
      for (double y = 0; y < image.rows - curHeight; y += winStride) {
        rois.push_back(Rect(round(x),
                            round(y),
                            round(curWidth),
                            round(curHeight)));
      }
    }
    curWidth *= scaleStride;
    curHeight *= scaleStride;
    winStride *= scaleStride;
  }

  // Now build the timing estimator and figure out the timing
  int nSamples;
  handle.param<int>("samples", nSamples, 100);
  TimingEstimator timingEstimator(nSamples);
  ROS_INFO_STREAM("Starting to time the visual utility estimator");
  timingEstimator.LearnVUEstimatorTiming(vuEstimator.get(),
                                         imageFiles,
                                         rois);

  // Print to the output file
  ROS_INFO_STREAM("Outputing results to " << outputFile);
  fstream outStream(outputFile.c_str(),
                    ios_base::out | ios_base::trunc);
  timingEstimator.OutputToStream(outStream);
  outStream.close();

  return 0;
}


#include "BayesFilters/SIS.h"

#include <fstream>
#include <iostream>
#include <utility>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

SIS::SIS() noexcept { }

SIS::~SIS() noexcept { }

SIS::SIS(SIS&& sir_pf) noexcept :
    ParticleFilter(std::move(sir_pf)) { }

SIS& SIS::operator=(SIS&& sir_pf) noexcept
{
    ParticleFilter::operator=(std::move(sir_pf));

    return *this;
}

void SIS::initialization()
{
    /* INITIALIZATION */
    simulation_time_ = 100;
    num_particle_    = 900;
    surv_x_          = 1000;
    surv_y_          = 1000;

    /* GENERATE MEASUREMENTS */
    measurement_.resize(2, simulation_time_);
    object_.resize(4, simulation_time_);

    object_.col(0) << 0, 10, 0, 10;
    correction_->getObservationModel().measure(object_.col(0), measurement_.col(0));
    for (int k = 1; k < simulation_time_; ++k)
    {
        prediction_->getStateModel().motion(object_.col(k-1), object_.col(k));
        correction_->getObservationModel().measure(object_.col(k), measurement_.col(k));
    }

    /* INITIALIZE FILTER */
    pred_particle_.resize(4, num_particle_);
    pred_weight_.resize(num_particle_, 1);

    cor_particle_.resize(4, num_particle_);
    cor_weight_.resize(num_particle_, 1);

    pred_weight_.setConstant(1.0/num_particle_);

    int particle_spread = std::sqrt(num_particle_);
    for (int i = 0; i < particle_spread; ++i)
        for (int j = 0; j < particle_spread; ++j)
            pred_particle_.col(i*particle_spread + j) << (surv_x_ / particle_spread) * i, 0, (surv_y_ / particle_spread) * j, 0;

    result_pred_particle_.resize(simulation_time_);
    result_pred_weight_.resize(simulation_time_);

    result_cor_particle_.resize(simulation_time_);
    result_cor_weight_.resize(simulation_time_);
}

void SIS::filteringStep()
{
    unsigned int k = getFilteringStep();

    if (k != 0)
        prediction_->predict(cor_particle_, cor_weight_,
                             pred_particle_, pred_weight_);

    correction_->correct(pred_particle_, pred_weight_, measurement_.col(k),
                         cor_particle_, cor_weight_);

    cor_weight_ /= cor_weight_.sum();

    /* Here results should be save. */
    /* Proper stragy is WIP. */
    result_pred_particle_[k] = pred_particle_;
    result_pred_weight_  [k] = pred_weight_;

    result_cor_particle_[k]  = cor_particle_;
    result_cor_weight_  [k]  = cor_weight_;

    if (resampling_->neff(cor_weight_) < static_cast<float>(num_particle_)/3.0)
    {
        MatrixXf res_particle(4, num_particle_);
        VectorXf res_weight(num_particle_, 1);
        VectorXf res_parent(num_particle_, 1);

        resampling_->resample(cor_particle_, cor_weight_,
                              res_particle, res_weight, res_parent);

        cor_particle_ = res_particle;
        cor_weight_   = res_weight;
    }
}

void SIS::getResult()
{
    std::ofstream result_file_object;
    std::ofstream result_file_measurement;
    std::ofstream result_file_pred_particle;
    std::ofstream result_file_pred_weight;
    std::ofstream result_file_cor_particle;
    std::ofstream result_file_cor_weight;

    result_file_object.open       ("./result_object.txt");
    result_file_measurement.open  ("./result_measurement.txt");
    result_file_pred_particle.open("./result_pred_particle.txt");
    result_file_pred_weight.open  ("./result_pred_weight.txt");
    result_file_cor_particle.open ("./result_cor_particle.txt");
    result_file_cor_weight.open   ("./result_cor_weight.txt");

    result_file_object       << object_;
    result_file_measurement  << measurement_;
    for (unsigned int k = 0; k < getFilteringStep(); ++k)
    {
        result_file_pred_particle << result_pred_particle_[k] << std::endl << std::endl;
        result_file_pred_weight   << result_pred_weight_[k]   << std::endl << std::endl;

        result_file_cor_particle  << result_cor_particle_[k]  << std::endl << std::endl;
        result_file_cor_weight    << result_cor_weight_[k]    << std::endl << std::endl;
    }

    result_file_object.close();
    result_file_measurement.close();
    result_file_pred_particle.close();
    result_file_pred_weight.close();
    result_file_cor_particle.close();
    result_file_cor_weight.close();
}


#include <catch.hpp>
#include <Spirit/State.h>
#include <Spirit/System.h>
#include <Spirit/Simulation.h>
#include <Spirit/Configurations.h>
#include <Spirit/Hamiltonian.h>
#include <Spirit/Constants.h>
#include <Spirit/Parameters.h>
#include <data/State.hpp>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <iostream>
#include <iomanip>
#include <sstream>

TEST_CASE( "Larmor Precession","[physics]" )
{
    // input file
    auto inputfile = "core/test/input/physics_larmor.cfg";

    // create State
    auto state = std::shared_ptr<State>( State_Setup( inputfile ), State_Delete );

    // choose method
    auto method = "LLG";

    // Solvers to be tested
    std::vector<const char *>  solvers{ "Depondt", "Heun", "SIB" };

    // set up one the initial direction of the spin
    float init_direction[3] = { 1., 0., 0. };                // vec parallel to x-axis
    Configuration_Domain( state.get(), init_direction );     // set spin parallel to x-axis

    // assure that the initial direction is set
    auto direction = System_Get_Spin_Directions( state.get() );
    REQUIRE( direction[0] == 1 );
    REQUIRE( direction[1] == 0 );
    REQUIRE( direction[2] == 0 );

    // make sure that mu_s is the same as the one define in input file
    float mu_s;
    Hamiltonian_Get_mu_s( state.get(), &mu_s );
    REQUIRE( mu_s == 2 );

    // get the magnitude of the magnetic field ( it has only z-axis component )
    float B_mag;
    float normal[3];
    Hamiltonian_Get_Field( state.get(), &B_mag, normal );

    // get time step of method
    float tstep = Parameters_Get_LLG_Time_Step( state.get() );

    // define damping values
    scalar no_damping = 0.0;        // for measuring Larmor precession
    scalar damping    = 0.3;        // for measuring precession projection to z-axis

    // testing values
    float angle, projection;

    for( auto opt : solvers )
    {
        // Test Larmor frequency

        Configuration_Domain( state.get(), init_direction );     // set spin parallel to x-axis
        Parameters_Set_LLG_Damping( state.get(), no_damping );

        for( int i=0; i<5; i++ )
        {
            INFO( std::string( opt ) << " failed Larmor frequency test at " << i << " iteration." );

            Simulation_SingleShot( state.get(), method, opt );

            direction = System_Get_Spin_Directions( state.get() );
            angle = std::atan2( direction[1] , direction[0] );

            REQUIRE( direction[2] == 0 );     // spin should always be to the xy plane

            // gamma must be scalled by mu_s
            REQUIRE( Approx(angle)  == ( (i+1) * tstep * mu_s * Constants_gamma() * B_mag ) );
        }

        // Test precession orbit projection on z-axis

        Configuration_Domain( state.get(), init_direction );     // set spin parallel to x-axis
        Parameters_Set_LLG_Damping( state.get(), damping );

        REQUIRE( direction[2] == 0 );     // initial spin position must have z=0

        for( int i=0; i<5; i++ )
        {
            INFO( std::string( opt ) << " failed orbit projection to z-axis test at " <<
                  i << " iteration." );

            Simulation_SingleShot( state.get(), method, opt );

            // analytical calculation of the projection in the z-axis
            projection = std::tanh( damping * (i+1) * tstep * mu_s * B_mag / (2*Constants_Pi()) );

            direction = System_Get_Spin_Directions( state.get() );

            REQUIRE( Approx( projection ) == direction[2] );
        }
    }
}

TEST_CASE( "Finite Differences", "[physics]" )
{
    // Hamiltonians to be tested
    std::vector<const char *>  hamiltonians{ "core/test/input/fd_pairs.cfg" };
                                             //"core/test/input/fd_neighbours",
                                             //"core/test/input/fd_gaussian.cfg"};
    for( auto ham: hamiltonians )
    {
        INFO( " Testing " << ham );

        // create state
        auto state = std::shared_ptr<State>( State_Setup( ham ), State_Delete );

        Configuration_Random( state.get() );

        auto& vf = *state->active_image->spins;
        auto grad = vectorfield( state->nos );
        auto grad_fd = vectorfield( state->nos );

        state->active_image->hamiltonian->Gradient_FD( vf, grad_fd );
        state->active_image->hamiltonian->Gradient( vf, grad );

        for( int i=0; i<state->nos; i++)
            REQUIRE( grad_fd[i].isApprox( grad[i] ) );

        auto hessian = MatrixX( 3*state->nos, 3*state->nos );
        auto hessian_fd = MatrixX( 3*state->nos, 3*state->nos );

        state->active_image->hamiltonian->Hessian_FD( vf, hessian_fd );
        state->active_image->hamiltonian->Hessian( vf, hessian );

        REQUIRE( hessian_fd.isApprox( hessian ) );
    }
}


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

/*    $Id: step-27.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2006-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 first few files have already been covered in previous examples and will
// thus not be further commented on.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.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/solver_cg.h>
#include <deal.II/lac/precondition.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_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// These are the new files we need. The first one provides an alternative to
// the usual SparsityPattern class and the CompressedSparsityPattern class
// already discussed in step-11 and step-18. The last two provide hp
// versions of the DoFHandler and FEValues classes as described in the
// introduction of this program.
#include <deal.II/lac/compressed_set_sparsity_pattern.h>
#include <deal.II/hp/dof_handler.h>
#include <deal.II/hp/fe_values.h>

// The last set of include files are standard C++ headers. We need support for
// complex numbers when we compute the Fourier transform.
#include <fstream>
#include <iostream>
#include <complex>

// Finally, this is as in previous programs:
namespace Step27
{
  using namespace dealii;

  // @sect3{The main class}

  // The main class of this program looks very much like the one already used
  // in the first few tutorial programs, for example the one in step-6. The
  // main difference is that we have merged the refine_grid and output_results
  // functions into one since we will also want to output some of the
  // quantities used in deciding how to refine the mesh (in particular the
  // estimated smoothness of the solution). There is also a function that
  // computes this estimated smoothness, as discussed in the introduction.
  //
  // As far as member variables are concerned, we use the same structure as
  // already used in step-6, but instead of a regular DoFHandler we use an
  // object of type hp::DoFHandler, and we need collections instead of
  // individual finite element, quadrature, and face quadrature objects. We
  // will fill these collections in the constructor of the class. The last
  // variable, max_degree, indicates the maximal polynomial
  // degree of shape functions used.
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem ();
    ~LaplaceProblem ();

    void run ();

  private:
    void setup_system ();
    void assemble_system ();
    void solve ();
    void create_coarse_grid ();
    void estimate_smoothness (Vector<float> &smoothness_indicators) const;
    void postprocess (const unsigned int cycle);

    Triangulation<dim>   triangulation;

    hp::DoFHandler<dim>      dof_handler;
    hp::FECollection<dim>    fe_collection;
    hp::QCollection<dim>     quadrature_collection;
    hp::QCollection<dim-1>   face_quadrature_collection;

    ConstraintMatrix     constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

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

    const unsigned int max_degree;
  };

  // @sect3{Equation data}
  //
  // Next, let us define the right hand side function for this problem. It is
  // $x+1$ in 1d, $(x+1)(y+1)$ in 2d, and so on.
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim> () {}

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

  template <int dim>
  double
  RightHandSide<dim>::value (const Point<dim>   &p,
                             const unsigned int  /*component*/) const
  {
    double product = 1;
    for (unsigned int d=0; d<dim; ++d)
      product *= (p[d]+1);
    return product;
  }

  // @sect3{Implementation of the main class}

  // @sect4{LaplaceProblem::LaplaceProblem}

  // The constructor of this class is fairly straightforward. It associates
  // the hp::DoFHandler object with the triangulation, and then sets the
  // maximal polynomial degree to 7 (in 1d and 2d) or 5 (in 3d and higher). We
  // do so because using higher order polynomial degrees becomes prohibitively
  // expensive, especially in higher space dimensions.
  //
  // Following this, we fill the collections of finite element, and cell and
  // face quadrature objects. We start with quadratic elements, and each
  // quadrature formula is chosen so that it is appropriate for the matching
  // finite element in the hp::FECollection object.
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem ()
    :
    dof_handler (triangulation),
    max_degree (dim <= 2 ? 7 : 5)
  {
    for (unsigned int degree=2; degree<=max_degree; ++degree)
      {
        fe_collection.push_back (FE_Q<dim>(degree));
        quadrature_collection.push_back (QGauss<dim>(degree+1));
        face_quadrature_collection.push_back (QGauss<dim-1>(degree+1));
      }
  }

  // @sect4{LaplaceProblem::~LaplaceProblem}

  // The destructor is unchanged from what we already did in step-6:
  template <int dim>
  LaplaceProblem<dim>::~LaplaceProblem ()
  {
    dof_handler.clear ();
  }

  // @sect4{LaplaceProblem::setup_system}
  //
  // This function is again an almost verbatim copy of what we already did in
  // step-6. The first change is that we append the Dirichlet boundary
  // conditions to the ConstraintMatrix object, which we consequently call
  // just constraints instead of
  // hanging_node_constraints. The second difference is that we
  // don't directly build the sparsity pattern, but first create an
  // intermediate object that we later copy into the usual SparsityPattern
  // data structure, since this is more efficient for the problem with many
  // entries per row (and different number of entries in different rows). In
  // another slight deviation, we do not first build the sparsity pattern and
  // then condense away constrained degrees of freedom, but pass the
  // constraint matrix object directly to the function that builds the
  // sparsity pattern. We disable the insertion of constrained entries with
  // <tt>false</tt> as fourth argument in the DoFTools::make_sparsity_pattern
  // function. All of these changes are explained in the introduction of this
  // program.
  //
  // The last change, maybe hidden in plain sight, is that the dof_handler
  // variable here is an hp object -- nevertheless all the function calls we
  // had before still work in exactly the same way as they always did.
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    dof_handler.distribute_dofs (fe_collection);

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

    constraints.clear ();
    DoFTools::make_hanging_node_constraints (dof_handler,
                                             constraints);
    VectorTools::interpolate_boundary_values (dof_handler,
                                              0,
                                              ZeroFunction<dim>(),
                                              constraints);
    constraints.close ();

    CompressedSetSparsityPattern csp (dof_handler.n_dofs(),
                                      dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, csp, constraints, false);
    sparsity_pattern.copy_from (csp);

    system_matrix.reinit (sparsity_pattern);
  }

  // @sect4{LaplaceProblem::assemble_system}

  // This is the function that assembles the global matrix and right hand side
  // vector from the local contributions of each cell. Its main working is as
  // has been described in many of the tutorial programs before. The
  // significant deviations are the ones necessary for hp finite
  // element methods. In particular, that we need to use a collection of
  // FEValues object (implemented through the hp::FEValues class), and that we
  // have to eliminate constrained degrees of freedom already when copying
  // local contributions into global objects. Both of these are explained in
  // detail in the introduction of this program.
  //
  // One other slight complication is the fact that because we use different
  // polynomial degrees on different cells, the matrices and vectors holding
  // local contributions do not have the same size on all cells. At the
  // beginning of the loop over all cells, we therefore each time have to
  // resize them to the correct size (given by
  // dofs_per_cell). Because these classes are implement in such
  // a way that reducing the size of a matrix or vector does not release the
  // currently allocated memory (unless the new size is zero), the process of
  // resizing at the beginning of the loop will only require re-allocation of
  // memory during the first few iterations. Once we have found in a cell with
  // the maximal finite element degree, no more re-allocations will happen
  // because all subsequent reinit calls will only set the size
  // to something that fits the currently allocated memory. This is important
  // since allocating memory is expensive, and doing so every time we visit a
  // new cell would take significant compute time.
  template <int dim>
  void LaplaceProblem<dim>::assemble_system ()
  {
    hp::FEValues<dim> hp_fe_values (fe_collection,
                                    quadrature_collection,
                                    update_values    |  update_gradients |
                                    update_quadrature_points  |  update_JxW_values);

    const RightHandSide<dim> rhs_function;

    FullMatrix<double>   cell_matrix;
    Vector<double>       cell_rhs;

    std::vector<unsigned int> local_dof_indices;

    typename hp::DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        const unsigned int   dofs_per_cell = cell->get_fe().dofs_per_cell;

        cell_matrix.reinit (dofs_per_cell, dofs_per_cell);
        cell_matrix = 0;

        cell_rhs.reinit (dofs_per_cell);
        cell_rhs = 0;

        hp_fe_values.reinit (cell);

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

        std::vector<double>  rhs_values (fe_values.n_quadrature_points);
        rhs_function.value_list (fe_values.get_quadrature_points(),
                                 rhs_values);

        for (unsigned int q_point=0;
             q_point<fe_values.n_quadrature_points;
             ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            {
              for (unsigned int j=0; j<dofs_per_cell; ++j)
                cell_matrix(i,j) += (fe_values.shape_grad(i,q_point) *
                                     fe_values.shape_grad(j,q_point) *
                                     fe_values.JxW(q_point));

              cell_rhs(i) += (fe_values.shape_value(i,q_point) *
                              rhs_values[q_point] *
                              fe_values.JxW(q_point));
            }

        local_dof_indices.resize (dofs_per_cell);
        cell->get_dof_indices (local_dof_indices);

        constraints.distribute_local_to_global (cell_matrix, cell_rhs,
                                                local_dof_indices,
                                                system_matrix, system_rhs);
      }

    // Now with the loop over all cells finished, we are done for this
    // function. The steps we still had to do at this point in earlier
    // tutorial programs, namely condensing hanging node constraints and
    // applying Dirichlet boundary conditions, have been taken care of by the
    // ConstraintMatrix object constraints on the fly.
  }

  // @sect4{LaplaceProblem::solve}

  // The function solving the linear system is entirely unchanged from
  // previous examples. We simply try to reduce the initial residual (which
  // equals the $l_2$ norm of the right hand side) by a certain factor:
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {
    SolverControl           solver_control (system_rhs.size(),
                                            1e-8*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);

    constraints.distribute (solution);
  }

  // @sect4{LaplaceProblem::postprocess}

  // After solving the linear system, we will want to postprocess the
  // solution. Here, all we do is to estimate the error, estimate the local
  // smoothness of the solution as described in the introduction, then write
  // graphical output, and finally refine the mesh in both $h$ and $p$
  // according to the indicators computed before. We do all this in the same
  // function because we want the estimated error and smoothness indicators
  // not only for refinement, but also include them in the graphical output.
  template <int dim>
  void LaplaceProblem<dim>::postprocess (const unsigned int cycle)
  {
    // Let us start with computing estimated error and smoothness indicators,
    // which each are one number for each active cell of our
    // triangulation. For the error indicator, we use the KellyErrorEstimator
    // class as always. Estimating the smoothness is done in the respective
    // function of this class; that function is discussed further down below:
    Vector<float> estimated_error_per_cell (triangulation.n_active_cells());
    KellyErrorEstimator<dim>::estimate (dof_handler,
                                        face_quadrature_collection,
                                        typename FunctionMap<dim>::type(),
                                        solution,
                                        estimated_error_per_cell);

    Vector<float> smoothness_indicators (triangulation.n_active_cells());
    estimate_smoothness (smoothness_indicators);

    // Next we want to generate graphical output. In addition to the two
    // estimated quantities derived above, we would also like to output the
    // polynomial degree of the finite elements used on each of the elements
    // on the mesh.
    //
    // The way to do that requires that we loop over all cells and poll the
    // active finite element index of them using
    // cell-@>active_fe_index(). We then use the result of this
    // operation and query the finite element collection for the finite
    // element with that index, and finally determine the polynomial degree of
    // that element. The result we put into a vector with one element per
    // cell. The DataOut class requires this to be a vector of
    // float or double, even though our values are
    // all integers, so that it what we use:
    {
      Vector<float> fe_degrees (triangulation.n_active_cells());
      {
        typename hp::DoFHandler<dim>::active_cell_iterator
        cell = dof_handler.begin_active(),
        endc = dof_handler.end();
        for (unsigned int index=0; cell!=endc; ++cell, ++index)
          fe_degrees(index)
            = fe_collection[cell->active_fe_index()].degree;
      }

      // With now all data vectors available -- solution, estimated errors and
      // smoothness indicators, and finite element degrees --, we create a
      // DataOut object for graphical output and attach all data. Note that
      // the DataOut class has a second template argument (which defaults to
      // DoFHandler@<dim@>, which is why we have never seen it in previous
      // tutorial programs) that indicates the type of DoF handler to be
      // used. Here, we have to use the hp::DoFHandler class:
      DataOut<dim,hp::DoFHandler<dim> > data_out;

      data_out.attach_dof_handler (dof_handler);
      data_out.add_data_vector (solution, "solution");
      data_out.add_data_vector (estimated_error_per_cell, "error");
      data_out.add_data_vector (smoothness_indicators, "smoothness");
      data_out.add_data_vector (fe_degrees, "fe_degree");
      data_out.build_patches ();

      // The final step in generating output is to determine a file name, open
      // the file, and write the data into it (here, we use VTK format):
      const std::string filename = "solution-" +
                                   Utilities::int_to_string (cycle, 2) +
                                   ".vtk";
      std::ofstream output (filename.c_str());
      data_out.write_vtk (output);
    }

    // After this, we would like to actually refine the mesh, in both $h$ and
    // $p$. The way we are going to do this is as follows: first, we use the
    // estimated error to flag those cells for refinement that have the
    // largest error. This is what we have always done:
    {
      GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                       estimated_error_per_cell,
                                                       0.3, 0.03);

      // Next we would like to figure out which of the cells that have been
      // flagged for refinement should actually have $p$ increased instead of
      // $h$ decreased. The strategy we choose here is that we look at the
      // smoothness indicators of those cells that are flagged for refinement,
      // and increase $p$ for those with a smoothness larger than a certain
      // threshold. For this, we first have to determine the maximal and
      // minimal values of the smoothness indicators of all flagged cells,
      // which we do using a loop over all cells and comparing current minimal
      // and maximal values. (We start with the minimal and maximal values of
      // all cells, a range within which the minimal and maximal values
      // on cells flagged for refinement must surely lie.) Absent any better
      // strategies, we will then set the threshold above which will increase
      // $p$ instead of reducing $h$ as the mean value between minimal and
      // maximal smoothness indicators on cells flagged for refinement:
      float max_smoothness = *std::min_element (smoothness_indicators.begin(),
                                                smoothness_indicators.end()),
                             min_smoothness = *std::max_element (smoothness_indicators.begin(),
                                                                 smoothness_indicators.end());
      {
        typename hp::DoFHandler<dim>::active_cell_iterator
        cell = dof_handler.begin_active(),
        endc = dof_handler.end();
        for (unsigned int index=0; cell!=endc; ++cell, ++index)
          if (cell->refine_flag_set())
            {
              max_smoothness = std::max (max_smoothness,
                                         smoothness_indicators(index));
              min_smoothness = std::min (min_smoothness,
                                         smoothness_indicators(index));
            }
      }
      const float threshold_smoothness = (max_smoothness + min_smoothness) / 2;

      // With this, we can go back, loop over all cells again, and for those
      // cells for which (i) the refinement flag is set, (ii) the smoothness
      // indicator is larger than the threshold, and (iii) we still have a
      // finite element with a polynomial degree higher than the current one
      // in the finite element collection, we then increase the polynomial
      // degree and in return remove the flag indicating that the cell should
      // undergo bisection. For all other cells, the refinement flags remain
      // untouched:
      {
        typename hp::DoFHandler<dim>::active_cell_iterator
        cell = dof_handler.begin_active(),
        endc = dof_handler.end();
        for (unsigned int index=0; cell!=endc; ++cell, ++index)
          if (cell->refine_flag_set()
              &&
              (smoothness_indicators(index) > threshold_smoothness)
              &&
              (cell->active_fe_index()+1 < fe_collection.size()))
            {
              cell->clear_refine_flag();
              cell->set_active_fe_index (cell->active_fe_index() + 1);
            }
      }

      // At the end of this procedure, we then refine the mesh. During this
      // process, children of cells undergoing bisection inherit their mother
      // cell's finite element index:
      triangulation.execute_coarsening_and_refinement ();
    }
  }

  // @sect4{LaplaceProblem::create_coarse_grid}

  // The following function is used when creating the initial grid. It is a
  // specialization for the 2d case, i.e. a corresponding function needs to be
  // implemented if the program is run in anything other then 2d. The function
  // is actually stolen from step-14 and generates the same mesh used already
  // there, i.e. the square domain with the square hole in the middle. The
  // meaning of the different parts of this function are explained in the
  // documentation of step-14:
  template <>
  void LaplaceProblem<2>::create_coarse_grid ()
  {
    const unsigned int dim = 2;

    static const Point<2> vertices_1[]
      = {  Point<2> (-1.,   -1.),
           Point<2> (-1./2, -1.),
           Point<2> (0.,    -1.),
           Point<2> (+1./2, -1.),
           Point<2> (+1,    -1.),

           Point<2> (-1.,   -1./2.),
           Point<2> (-1./2, -1./2.),
           Point<2> (0.,    -1./2.),
           Point<2> (+1./2, -1./2.),
           Point<2> (+1,    -1./2.),

           Point<2> (-1.,   0.),
           Point<2> (-1./2, 0.),
           Point<2> (+1./2, 0.),
           Point<2> (+1,    0.),

           Point<2> (-1.,   1./2.),
           Point<2> (-1./2, 1./2.),
           Point<2> (0.,    1./2.),
           Point<2> (+1./2, 1./2.),
           Point<2> (+1,    1./2.),

           Point<2> (-1.,   1.),
           Point<2> (-1./2, 1.),
           Point<2> (0.,    1.),
           Point<2> (+1./2, 1.),
           Point<2> (+1,    1.)
        };
    const unsigned int
    n_vertices = sizeof(vertices_1) / sizeof(vertices_1[0]);
    const std::vector<Point<dim> > vertices (&vertices_1[0],
                                             &vertices_1[n_vertices]);
    static const int cell_vertices[][GeometryInfo<dim>::vertices_per_cell]
    = {{0, 1, 5, 6},
      {1, 2, 6, 7},
      {2, 3, 7, 8},
      {3, 4, 8, 9},
      {5, 6, 10, 11},
      {8, 9, 12, 13},
      {10, 11, 14, 15},
      {12, 13, 17, 18},
      {14, 15, 19, 20},
      {15, 16, 20, 21},
      {16, 17, 21, 22},
      {17, 18, 22, 23}
    };
    const unsigned int
    n_cells = sizeof(cell_vertices) / sizeof(cell_vertices[0]);

    std::vector<CellData<dim> > cells (n_cells, CellData<dim>());
    for (unsigned int i=0; i<n_cells; ++i)
      {
        for (unsigned int j=0;
             j<GeometryInfo<dim>::vertices_per_cell;
             ++j)
          cells[i].vertices[j] = cell_vertices[i][j];
        cells[i].material_id = 0;
      }

    triangulation.create_triangulation (vertices,
                                        cells,
                                        SubCellData());
    triangulation.refine_global (3);
  }

  // @sect4{LaplaceProblem::run}

  // This function implements the logic of the program, as did the respective
  // function in most of the previous programs already, see for example
  // step-6.
  //
  // Basically, it contains the adaptive loop: in the first iteration create a
  // coarse grid, and then set up the linear system, assemble it, solve, and
  // postprocess the solution including mesh refinement. Then start over
  // again. In the meantime, also output some information for those staring at
  // the screen trying to figure out what the program does:
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    for (unsigned int cycle=0; cycle<6; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          create_coarse_grid ();

        setup_system ();

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

        assemble_system ();
        solve ();
        postprocess (cycle);
      }
  }

  // @sect4{LaplaceProblem::estimate_smoothness}

  // This last function of significance implements the algorithm to estimate
  // the smoothness exponent using the algorithms explained in detail in the
  // introduction. We will therefore only comment on those points that are of
  // implementational importance.
  template <int dim>
  void
  LaplaceProblem<dim>::
  estimate_smoothness (Vector<float> &smoothness_indicators) const
  {
    // The first thing we need to do is to define the Fourier vectors ${\bf
    // k}$ for which we want to compute Fourier coefficients of the solution
    // on each cell. In 2d, we pick those vectors ${\bf k}=(\pi i, \pi j)^T$
    // for which $\sqrt{i^2+j^2}\le N$, with $i,j$ integers and $N$ being the
    // maximal polynomial degree we use for the finite elements in this
    // program. The 3d case is handled analogously. 1d and dimensions higher
    // than 3 are not implemented, and we guard our implementation by making
    // sure that we receive an exception in case someone tries to compile the
    // program for any of these dimensions.
    //
    // We exclude ${\bf k}=0$ to avoid problems computing $|{\bf k}|^{-mu}$
    // and $\ln |{\bf k}|$. The other vectors are stored in the field
    // k_vectors. In addition, we store the square of the
    // magnitude of each of these vectors (up to a factor $\pi^2$) in the
    // k_vectors_magnitude array -- we will need that when we
    // attempt to find out which of those Fourier coefficients corresponding
    // to Fourier vectors of the same magnitude is the largest:
    const unsigned int N = max_degree;

    std::vector<Tensor<1,dim> > k_vectors;
    std::vector<unsigned int>   k_vectors_magnitude;
    switch (dim)
      {
      case 2:
      {
        for (unsigned int i=0; i<N; ++i)
          for (unsigned int j=0; j<N; ++j)
            if (!((i==0) && (j==0))
                &&
                (i*i + j*j < N*N))
              {
                k_vectors.push_back (Point<dim>(numbers::PI * i,
                                                numbers::PI * j));
                k_vectors_magnitude.push_back (i*i+j*j);
              }

        break;
      }

      case 3:
      {
        for (unsigned int i=0; i<N; ++i)
          for (unsigned int j=0; j<N; ++j)
            for (unsigned int k=0; k<N; ++k)
              if (!((i==0) && (j==0) && (k==0))
                  &&
                  (i*i + j*j + k*k < N*N))
                {
                  k_vectors.push_back (Point<dim>(numbers::PI * i,
                                                  numbers::PI * j,
                                                  numbers::PI * k));
                  k_vectors_magnitude.push_back (i*i+j*j+k*k);
                }

        break;
      }

      default:
        Assert (false, ExcNotImplemented());
      }

    // After we have set up the Fourier vectors, we also store their total
    // number for simplicity, and compute the logarithm of the magnitude of
    // each of these vectors since we will need it many times over further
    // down below:
    const unsigned n_fourier_modes = k_vectors.size();
    std::vector<double> ln_k (n_fourier_modes);
    for (unsigned int i=0; i<n_fourier_modes; ++i)
      ln_k[i] = std::log (k_vectors[i].norm());

    // Next, we need to assemble the matrices that do the Fourier transforms
    // for each of the finite elements we deal with, i.e. the matrices ${\cal
    // F}_{{\bf k},j}$ defined in the introduction. We have to do that for
    // each of the finite elements in use. Note that these matrices are
    // complex-valued, so we can't use the FullMatrix class. Instead, we use
    // the Table class template.
    std::vector<Table<2,std::complex<double> > >
    fourier_transform_matrices (fe_collection.size());

    // In order to compute them, we of course can't perform the Fourier
    // transform analytically, but have to approximate it using quadrature. To
    // this end, we use a quadrature formula that is obtained by iterating a
    // 2-point Gauss formula as many times as the maximal exponent we use for
    // the term $e^{i{\bf k}\cdot{\bf x}}$:
    QGauss<1>      base_quadrature (2);
    QIterated<dim> quadrature (base_quadrature, N);

    // With this, we then loop over all finite elements in use, reinitialize
    // the respective matrix ${\cal F}$ to the right size, and integrate each
    // entry of the matrix numerically as ${\cal F}_{{\bf k},j}=\sum_q
    // e^{i{\bf k}\cdot {\bf x}}\varphi_j({\bf x}_q) w_q$, where $x_q$ are the
    // quadrature points and $w_q$ are the quadrature weights. Note that the
    // imaginary unit $i=\sqrt{-1}$ is obtained from the standard C++ classes
    // using std::complex@<double@>(0,1).

    // Because we work on the unit cell, we can do all this work without a
    // mapping from reference to real cell and consequently do not need the
    // FEValues class.
    for (unsigned int fe=0; fe<fe_collection.size(); ++fe)
      {
        fourier_transform_matrices[fe].reinit (n_fourier_modes,
                                               fe_collection[fe].dofs_per_cell);

        for (unsigned int k=0; k<n_fourier_modes; ++k)
          for (unsigned int j=0; j<fe_collection[fe].dofs_per_cell; ++j)
            {
              std::complex<double> sum = 0;
              for (unsigned int q=0; q<quadrature.size(); ++q)
                {
                  const Point<dim> x_q = quadrature.point(q);
                  sum += std::exp(std::complex<double>(0,1) *
                                  (k_vectors[k] * x_q)) *
                         fe_collection[fe].shape_value(j,x_q) *
                         quadrature.weight(q);
                }
              fourier_transform_matrices[fe](k,j)
                = sum / std::pow(2*numbers::PI, 1.*dim/2);
            }
      }

    // The next thing is to loop over all cells and do our work there, i.e. to
    // locally do the Fourier transform and estimate the decay coefficient. We
    // will use the following two arrays as scratch arrays in the loop and
    // allocate them here to avoid repeated memory allocations:
    std::vector<std::complex<double> > fourier_coefficients (n_fourier_modes);
    Vector<double>                     local_dof_values;

    // Then here is the loop:
    typename hp::DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (unsigned int index=0; cell!=endc; ++cell, ++index)
      {
        // Inside the loop, we first need to get the values of the local
        // degrees of freedom (which we put into the
        // local_dof_values array after setting it to the right
        // size) and then need to compute the Fourier transform by multiplying
        // this vector with the matrix ${\cal F}$ corresponding to this finite
        // element. We need to write out the multiplication by hand because
        // the objects holding the data do not have vmult-like
        // functions declared:
        local_dof_values.reinit (cell->get_fe().dofs_per_cell);
        cell->get_dof_values (solution, local_dof_values);

        for (unsigned int f=0; f<n_fourier_modes; ++f)
          {
            fourier_coefficients[f] = 0;

            for (unsigned int i=0; i<cell->get_fe().dofs_per_cell; ++i)
              fourier_coefficients[f] +=
                fourier_transform_matrices[cell->active_fe_index()](f,i)
                *
                local_dof_values(i);
          }

        // The next thing, as explained in the introduction, is that we wanted
        // to only fit our exponential decay of Fourier coefficients to the
        // largest coefficients for each possible value of $|{\bf k}|$. To
        // this end, we create a map that for each magnitude $|{\bf k}|$
        // stores the largest $|\hat U_{{\bf k}}|$ found so far, i.e. we
        // overwrite the existing value (or add it to the map) if no value for
        // the current $|{\bf k}|$ exists yet, or if the current value is
        // larger than the previously stored one:
        std::map<unsigned int, double> k_to_max_U_map;
        for (unsigned int f=0; f<n_fourier_modes; ++f)
          if ((k_to_max_U_map.find (k_vectors_magnitude[f]) ==
               k_to_max_U_map.end())
              ||
              (k_to_max_U_map[k_vectors_magnitude[f]] <
               std::abs (fourier_coefficients[f])))
            k_to_max_U_map[k_vectors_magnitude[f]]
              = std::abs (fourier_coefficients[f]);
        // Note that it comes in handy here that we have stored the magnitudes
        // of vectors as integers, since this way we do not have to deal with
        // round-off-sized differences between different values of $|{\bf
        // k}|$.

        // As the final task, we have to calculate the various contributions
        // to the formula for $\mu$. We'll only take those Fourier
        // coefficients with the largest magnitude for a given value of $|{\bf
        // k}|$ as explained above:
        double  sum_1           = 0,
                sum_ln_k        = 0,
                sum_ln_k_square = 0,
                sum_ln_U        = 0,
                sum_ln_U_ln_k   = 0;
        for (unsigned int f=0; f<n_fourier_modes; ++f)
          if (k_to_max_U_map[k_vectors_magnitude[f]] ==
              std::abs (fourier_coefficients[f]))
            {
              sum_1 += 1;
              sum_ln_k += ln_k[f];
              sum_ln_k_square += ln_k[f]*ln_k[f];
              sum_ln_U += std::log (std::abs (fourier_coefficients[f]));
              sum_ln_U_ln_k += std::log (std::abs (fourier_coefficients[f])) *
                               ln_k[f];
            }

        // With these so-computed sums, we can now evaluate the formula for
        // $\mu$ derived in the introduction:
        const double mu
          = (1./(sum_1*sum_ln_k_square - sum_ln_k*sum_ln_k)
             *
             (sum_ln_k*sum_ln_U - sum_1*sum_ln_U_ln_k));

        // The final step is to compute the Sobolev index $s=\mu-\frac d2$ and
        // store it in the vector of estimated values for each cell:
        smoothness_indicators(index) = mu - 1.*dim/2;
      }
  }
}

// @sect3{The main function}

// The main function is again verbatim what we had before: wrap creating and
// running an object of the main class into a try block and catch
// whatever exceptions are thrown, thereby producing meaningful output if
// anything should go wrong:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step27;

      deallog.depth_console (0);

      LaplaceProblem<2> 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;
}


// This is an open source non-commercial project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++ and C#: http://www.viva64.com

#include <numeric>
#include <boost/algorithm/string.hpp>
#include <memory>
#include <algorithm>
#include "../../configurator/job_config.h"

using map_reduce::KeyValueType;
using map_reduce::IntKeyValueType;
using map_reduce::StringKeyValueType;
using map_reduce::IntKeyValueTypeFactory;
using map_reduce::StringKeyValueTypeFactory;

#include "boost/locale/boundary.hpp"
#include "boost/locale.hpp"

namespace ba=boost::locale::boundary;

std::vector<std::string> split_to_words(std::string &data) {
    boost::locale::generator gen;
    std::locale::global(gen("en_US.UTF-8"));

    std::vector<std::string> words_vec;
    ba::ssegment_index words(ba::word, data.begin(), data.end());
    for (auto &&x: words) {
        std::string word = boost::locale::fold_case(boost::locale::normalize(x.str()));
        if (word.length() >= 1 && std::any_of(word.begin(), word.end(), [](const auto &c) { return isalpha(c); }))
            words_vec.push_back(move(word));
    }
    return words_vec;
}

class word_count_map : public map_reduce::map_base {
public:
    void map(const std::unique_ptr<KeyValueType> &key, const std::unique_ptr<KeyValueType> &value) override {
        auto key_in = dynamic_cast<IntKeyValueType &>(*key).value;
        auto value_in = dynamic_cast<StringKeyValueType &>(*value).value;

        std::vector<std::string> words = split_to_words(value_in);
        for (auto &word: words) {
            if (word.empty())
                continue;
            emit(std::make_unique<StringKeyValueType>(word),
                 std::make_unique<IntKeyValueType>(1));
        }
    };

    std::unique_ptr<map_reduce::map_base> clone() override {
        return std::make_unique<word_count_map>();
    }
};

class sum_reduce : public map_reduce::reduce_base {
public:
    std::pair<std::unique_ptr<KeyValueType>, std::unique_ptr<KeyValueType>>
    reduce(const std::unique_ptr<KeyValueType> &key,
           const std::vector<std::unique_ptr<KeyValueType>> &outputs) override {

        const auto key_out = dynamic_cast<StringKeyValueType &>(*key).value;
        const auto value_res = std::accumulate(outputs.begin(), outputs.end(), 0, [](int lhs, const auto &rhs) {
            const auto value_out = dynamic_cast<IntKeyValueType &>(*rhs).value;
            return lhs + value_out;
        });

        return {std::make_unique<StringKeyValueType>(key_out),
                std::make_unique<IntKeyValueType>(value_res)};
    }
};

std::shared_ptr<job_config> get_config() {
    return std::make_shared<job_config>("Count words",
                                        std::move(std::make_unique<word_count_map>()),
                                        std::move(std::make_unique<sum_reduce>()),
                                        std::move(std::make_unique<IntKeyValueTypeFactory>()),
                                        std::move(std::make_unique<StringKeyValueTypeFactory>()),
                                        std::move(std::make_unique<StringKeyValueTypeFactory>()),
                                        std::move(std::make_unique<IntKeyValueTypeFactory>()),
                                        std::move(std::make_unique<IntKeyValueTypeFactory>())
    );
}


#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <iostream>
#include <queue>
#include <stdio.h>

#include <boost/python.hpp>
#include "conversion.h"

namespace py = boost::python;

// constants.h
// Debugging
const bool kPlotVectorField = false;
// Size constants
const int kEyePercentTop = 25;
const int kEyePercentSide = 13;
const int kEyePercentHeight = 30;
const int kEyePercentWidth = 35;
// Preprocessing
const bool kSmoothFaceImage = false;
const float kSmoothFaceFactor = 0.005;
// Algorithm Parameters
const int kFastEyeWidth = 50;
const int kWeightBlurSize = 5;
const bool kEnableWeight = true;
const float kWeightDivisor = 1.0;
const double kGradientThreshold = 50.0;
// Postprocessing
const bool kEnablePostProcess = true;
const float kPostProcessThreshold = 0.97;
// Eye Corner
const bool kEnableEyeCorner = false;

// helpers.h
bool rectInImage(cv::Rect rect, cv::Mat image);
bool inMat(cv::Point p,int rows,int cols);
cv::Mat matrixMagnitude(const cv::Mat &matX, const cv::Mat &matY);
double computeDynamicThreshold(const cv::Mat &mat, double stdDevFactor);

// helpers.cpp
bool rectInImage(cv::Rect rect, cv::Mat image) {
  return rect.x > 0 && rect.y > 0 && rect.x+rect.width < image.cols &&
  rect.y+rect.height < image.rows;
}

bool inMat(cv::Point p,int rows,int cols) {
  return p.x >= 0 && p.x < cols && p.y >= 0 && p.y < rows;
}

cv::Mat matrixMagnitude(const cv::Mat &matX, const cv::Mat &matY) {
  cv::Mat mags(matX.rows,matX.cols,CV_64F);
  for (int y = 0; y < matX.rows; ++y) {
    const double *Xr = matX.ptr<double>(y), *Yr = matY.ptr<double>(y);
    double *Mr = mags.ptr<double>(y);
    for (int x = 0; x < matX.cols; ++x) {
      double gX = Xr[x], gY = Yr[x];
      double magnitude = sqrt((gX * gX) + (gY * gY));
      Mr[x] = magnitude;
    }
  }
  return mags;
}

double computeDynamicThreshold(const cv::Mat &mat, double stdDevFactor) {
  cv::Scalar stdMagnGrad, meanMagnGrad;
  cv::meanStdDev(mat, meanMagnGrad, stdMagnGrad);
  double stdDev = stdMagnGrad[0] / sqrt(mat.rows*mat.cols);
  return stdDevFactor * stdDev + meanMagnGrad[0];
}

// findEyeCenter.h
cv::Mat findEyeCenter_aux(cv::Mat face, cv::Mat eye_);

// findEyeCenter.cpp
// Pre-declarations
cv::Mat floodKillEdges(cv::Mat &mat);

cv::Point unscalePoint(cv::Point p, cv::Rect origSize) {
  float ratio = (((float)kFastEyeWidth)/origSize.width);
  int x = round(p.x / ratio);
  int y = round(p.y / ratio);
  return cv::Point(x,y);
}

void scaleToFastSize(const cv::Mat &src,cv::Mat &dst) {
  cv::resize(src, dst, cv::Size(kFastEyeWidth,(((float)kFastEyeWidth)/src.cols) * src.rows));
}

cv::Mat computeMatXGradient(const cv::Mat &mat) {
  cv::Mat out(mat.rows,mat.cols,CV_64F);

  for (int y = 0; y < mat.rows; ++y) {
    const uchar *Mr = mat.ptr<uchar>(y);
    double *Or = out.ptr<double>(y);

    Or[0] = Mr[1] - Mr[0];
    for (int x = 1; x < mat.cols - 1; ++x) {
      Or[x] = (Mr[x+1] - Mr[x-1])/2.0;
    }
    Or[mat.cols-1] = Mr[mat.cols-1] - Mr[mat.cols-2];
  }

  return out;
}

void testPossibleCentersFormula(int x, int y, const cv::Mat &weight,double gx, double gy, cv::Mat &out) {
  // for all possible centers
  for (int cy = 0; cy < out.rows; ++cy) {
    double *Or = out.ptr<double>(cy);
    const unsigned char *Wr = weight.ptr<unsigned char>(cy);
    for (int cx = 0; cx < out.cols; ++cx) {
      if (x == cx && y == cy) {
        continue;
      }
      // create a vector from the possible center to the gradient origin
      double dx = x - cx;
      double dy = y - cy;
      // normalize d
      double magnitude = sqrt((dx * dx) + (dy * dy));
      dx = dx / magnitude;
      dy = dy / magnitude;
      double dotProduct = dx*gx + dy*gy;
      dotProduct = std::max(0.0,dotProduct);
      // square and multiply by the weight
      if (kEnableWeight) {
        Or[cx] += dotProduct * dotProduct * (Wr[cx]/kWeightDivisor);
      } else {
        Or[cx] += dotProduct * dotProduct;
      }
    }
  }
}

// r.a.d. Altered this function. No longer takes debugWindow as parameter.
// eye was changed to eye_ and type changed from cv::Rect to cv::Mat
// return type was changed from cv::Point to cv::Mat
cv::Mat findEyeCenter_aux(cv::Mat face, cv::Mat eye_) {
  //std::cout << "findEyeCenter_aux";
  cv::Rect eye = cv::Rect(eye_.at<double>(0,0), eye_.at<double>(1,0), eye_.at<double>(2,0), eye_.at<double>(3,0));
  cv::Mat eyeROIUnscaled = face(eye);
  //std::cout << eyeROIUnscaled.cols << " " << eyeROIUnscaled.rows << "\n";
  cv::Mat eyeROI;
  scaleToFastSize(eyeROIUnscaled, eyeROI);
  // draw eye region
  rectangle(face,eye,1234);
  //-- Find the gradient
  cv::Mat gradientX = computeMatXGradient(eyeROI);
  cv::Mat gradientY = computeMatXGradient(eyeROI.t()).t();
  //-- Normalize and threshold the gradient
  // compute all the magnitudes
  cv::Mat mags = matrixMagnitude(gradientX, gradientY);
  //compute the threshold
  double gradientThresh = computeDynamicThreshold(mags, kGradientThreshold);
  //double gradientThresh = kGradientThreshold;
  //double gradientThresh = 0;
  //normalize
  for (int y = 0; y < eyeROI.rows; ++y) {
    double *Xr = gradientX.ptr<double>(y), *Yr = gradientY.ptr<double>(y);
    const double *Mr = mags.ptr<double>(y);
    for (int x = 0; x < eyeROI.cols; ++x) {
      double gX = Xr[x], gY = Yr[x];
      double magnitude = Mr[x];
      if (magnitude > gradientThresh) {
        Xr[x] = gX/magnitude;
        Yr[x] = gY/magnitude;
      } else {
        Xr[x] = 0.0;
        Yr[x] = 0.0;
      }
    }
  }
  //-- Create a blurred and inverted image for weighting
  cv::Mat weight;
  GaussianBlur( eyeROI, weight, cv::Size( kWeightBlurSize, kWeightBlurSize ), 0, 0 );
  for (int y = 0; y < weight.rows; ++y) {
    unsigned char *row = weight.ptr<unsigned char>(y);
    for (int x = 0; x < weight.cols; ++x) {
      row[x] = (255 - row[x]);
    }
  }
  //-- Run the algorithm!
  cv::Mat outSum = cv::Mat::zeros(eyeROI.rows,eyeROI.cols,CV_64F);
  // for each possible gradient location
  // Note: these loops are reversed from the way the paper does them
  // it evaluates every possible center for each gradient location instead of
  // every possible gradient location for every center.
  //printf("Eye Size: %ix%i\n",outSum.cols,outSum.rows);
  for (int y = 0; y < weight.rows; ++y) {
    const double *Xr = gradientX.ptr<double>(y), *Yr = gradientY.ptr<double>(y);
    for (int x = 0; x < weight.cols; ++x) {
      double gX = Xr[x], gY = Yr[x];
      if (gX == 0.0 && gY == 0.0) {
        continue;
      }
      testPossibleCentersFormula(x, y, weight, gX, gY, outSum);
    }
  }
  // scale all the values down, basically averaging them
  double numGradients = (weight.rows*weight.cols);
  cv::Mat out;
  outSum.convertTo(out, CV_32F,1.0/numGradients);
  //-- Find the maximum point
  cv::Point maxP;
  double maxVal;
  cv::minMaxLoc(out, NULL,&maxVal,NULL,&maxP);
  //-- Flood fill the edges
  if(kEnablePostProcess) {
    cv::Mat floodClone;
    //double floodThresh = computeDynamicThreshold(out, 1.5);
    double floodThresh = maxVal * kPostProcessThreshold;
    cv::threshold(out, floodClone, floodThresh, 0.0f, cv::THRESH_TOZERO);
    if(kPlotVectorField) {
      //plotVecField(gradientX, gradientY, floodClone);
      imwrite("eyeFrame.png",eyeROIUnscaled);
    }
    cv::Mat mask = floodKillEdges(floodClone);
    // redo max
    cv::minMaxLoc(out, NULL,&maxVal,NULL,&maxP,mask);
  }
  cv::Point ret_p = unscalePoint(maxP, eye);
  cv::Mat ret = cv::Mat(2, 1, CV_64F); // rows, cols, type
  ret.at<double>(0,0) = (double)ret_p.x;
  ret.at<double>(1,0) = (double)ret_p.y;
  return ret;
}

bool floodShouldPushPoint(const cv::Point &np, const cv::Mat &mat) {
  return inMat(np, mat.rows, mat.cols);
}

// returns a mask
cv::Mat floodKillEdges(cv::Mat &mat) {
  rectangle(mat,cv::Rect(0,0,mat.cols,mat.rows),255);

  cv::Mat mask(mat.rows, mat.cols, CV_8U, 255);
  std::queue<cv::Point> toDo;
  toDo.push(cv::Point(0,0));
  while (!toDo.empty()) {
    cv::Point p = toDo.front();
    toDo.pop();
    if (mat.at<float>(p) == 0.0f) {
      continue;
    }
    // add in every direction
    cv::Point np(p.x + 1, p.y); // right
    if (floodShouldPushPoint(np, mat)) toDo.push(np);
    np.x = p.x - 1; np.y = p.y; // left
    if (floodShouldPushPoint(np, mat)) toDo.push(np);
    np.x = p.x; np.y = p.y + 1; // down
    if (floodShouldPushPoint(np, mat)) toDo.push(np);
    np.x = p.x; np.y = p.y - 1; // up
    if (floodShouldPushPoint(np, mat)) toDo.push(np);
    // kill it
    mat.at<float>(p) = 0.0f;
    mask.at<uchar>(p) = 0;
  }
  return mask;
}

PyObject* findEyeCenter(PyObject *face, PyObject *eye)
{
    NDArrayConverter cvt;
    cv::Mat eyeMat, faceMat;
    faceMat = cvt.toMat(face);
    eyeMat = cvt.toMat(eye);

    cv::Mat result = findEyeCenter_aux(faceMat, eyeMat);

    PyObject* ret = cvt.toNDArray(result);

    return ret;
}

static void init()
{
    Py_Initialize();
    import_array();
}

BOOST_PYTHON_MODULE(eyeLike)
{
    init();
    py::def("findEyeCenter", findEyeCenter);
}


#include <UnitTest++/UnitTest++.h>
#include "../pixel_array_routines.h"
#include "../image_utils.h"

#include <iostream>
#include <sstream>
#include <boost/filesystem.hpp>
#include <cmath>
using namespace coela;
using namespace std;

SUITE(image_utils)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";
    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(bin_image) {
        CcdImage<double> input_image;
        input_image.pix = PixelArray2d<double>(42,42, 0.0);

        input_image.pix(1,1)=5;
        input_image.pix(1,2)=5;
        input_image.pix(2,1)=5;
        input_image.pix(2,2)=5;

        input_image.pix(10,10)=20;
        input_image.pix(20,20)=55;

        input_image.initialize_CCD_grid_for_raw_data();

        CcdImage<double> bin_1x = image_utils::bin_image(input_image, 1);
        CcdImage<double> bin_2x = image_utils::bin_image(input_image, 2);
        CcdImage<double> bin_3x = image_utils::bin_image(input_image, 3);
        CcdImage<double> bin_42x = image_utils::bin_image(input_image, 42);

        CHECK(bin_1x.pix==input_image.pix);
//        CHECK(bin_1x==input_image);

        CHECK_EQUAL(input_image.CCD_grid.image_outline_, bin_1x.CCD_grid.image_outline_);
        CHECK_EQUAL(input_image.CCD_grid.image_outline_, bin_2x.CCD_grid.image_outline_);
        CHECK_EQUAL(input_image.CCD_grid.image_outline_, bin_3x.CCD_grid.image_outline_);

        CHECK_EQUAL(input_image.pix.sum(), bin_2x.pix.sum());
        CHECK_EQUAL(input_image.pix.sum(), bin_3x.pix.sum());

        CHECK_EQUAL(20.0, bin_2x.pix(1,1));

//        input_image.write_to_file("rebin_input.fits");
//        bin_2x.write_to_file("rebin_2x.fits");
//        bin_3x.write_to_file("rebin_3x.fits");
//        bin_42x.write_to_file("rebin_42x.fits");
    }

    TEST(resample_image) {
        CcdImage<double> input_image;
        input_image.pix = PixelArray2d<double>(40,40, 0.0);
        input_image.initialize_CCD_grid_for_raw_data();

        input_image.pix(20,20)=5;
        input_image.pix(20,21)=5;
        input_image.pix(21,20)=5;
        input_image.pix(21,21)=5;

        CcdImage<double> downsample4x = image_utils::bin_image(input_image, 4);
        CcdImage<double> downsample2x = image_utils::bin_image(input_image, 2);

        CcdImage<double> interp4x = image_utils::bicubic_resample_4x(downsample4x);
        CcdImage<double> interp2x = image_utils::bicubic_resample_2x(downsample2x);

        CHECK_EQUAL(input_image.pix.sum(), interp2x.pix.sum());
        CHECK_EQUAL(input_image.pix.sum(), interp4x.pix.sum());

        CcdPosition orig_centroid = input_image.CCD_grid.corresponding_grid_Position(
                                         pixel_array_routines::centroid(
                                             input_image.pix, input_image.pix.range())
                                     );

        CcdPosition centroid_4x = interp4x.CCD_grid.corresponding_grid_Position(
                                       pixel_array_routines::centroid(interp4x.pix, interp4x.pix.range())
                                   );

        CHECK_CLOSE(orig_centroid.x, centroid_4x.x, 1e-6);
        CHECK_CLOSE(orig_centroid.y, centroid_4x.y, 1e-6);

//        input_image.write_to_file("resample_input.fits");
//        interp4x.write_to_file("resample_4x.fits");
//        interp2x.write_to_file("resample_2x.fits");
    }

}


#include <boost/test/unit_test.hpp>
#include <sudoku/dimensions.h>
#include <sudoku/groups.h>
#include <iostream>
#include "util.h"

BOOST_AUTO_TEST_CASE(Dimensions_4x4)
{
    auto rowGroups = sudoku::computeRowGroups(4, 4);        // Groups 0-3
    auto columnGroups = sudoku::computeColumnGroups(4, 4);  // Groups 4-7
    auto squareGroups = sudoku::computeGroupsFromMap(       // Groups 8-11
        " 0 0 1 1 "
        " 0 0 1 1 "
        " 2 2 3 3 "
        " 2 2 3 3 "
    );
    auto allGroups = sudoku::joinGroups({rowGroups, columnGroups, squareGroups});
    auto dims = sudoku::Dimensions(16, 4, allGroups);

    // Verify sudoku limits
    BOOST_REQUIRE_EQUAL(dims.getCellCount(), 16);
    BOOST_REQUIRE_EQUAL(dims.getMaxCellValue(), 4);

    // Groups should be the same as what was passed to constructor
    for (sudoku::GroupCount groupNum = 0; groupNum < allGroups.size(); ++groupNum) {
        BOOST_REQUIRE_EQUAL_VECTORS(allGroups[groupNum], dims.getCellsInGroup(groupNum));
    }

    // Should have assigned group numbers to each cell
    std::vector<std::vector<sudoku::CellCount>> expectedGroupsForEachCell {
        {0, 4, 8},  {0, 5, 8},  {0, 6, 9},  {0, 7, 9},
        {1, 4, 8},  {1, 5, 8},  {1, 6, 9},  {1, 7, 9},
        {2, 4, 10}, {2, 5, 10}, {2, 6, 11}, {2, 7, 11},
        {3, 4, 10}, {3, 5, 10}, {3, 6, 11}, {3, 7, 11}
    };
    for (sudoku::CellCount cellPos = 0; cellPos < expectedGroupsForEachCell.size(); ++cellPos) {
        BOOST_REQUIRE_EQUAL_VECTORS(dims.getGroupsForCell(cellPos), expectedGroupsForEachCell[cellPos]);
    }
}

BOOST_AUTO_TEST_CASE(Dimensions_constructor_NoGroupsSuccessful)
{
    sudoku::Dimensions grouplessDims(81, 9, {});  // Does not throw.
}

BOOST_AUTO_TEST_CASE(Dimensions_constructor_GroupTooLarge)
{
    // If group sizes are less than max cell value, then dimensions
    // are valid and should not throw an exception.
    sudoku::Dimensions a(16, 4, { {0}, {1} });
    sudoku::Dimensions b(16, 4, { {0, 1}, {2, 3} });
    sudoku::Dimensions c(16, 4, { {0, 1, 2}, {3, 4, 5} });
    sudoku::Dimensions d(16, 4, { {0, 1, 2, 3}, {4, 5, 6, 7} });

    // These dimensions have groups with size 5. We cannot possibly
    // assign 4 unique values across a group with 5 positions.
    // This should throw an exception.
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(16, 4, { {0, 1, 2, 3, 4}, {5, 6, 7, 8, 9} }),
        sudoku::DimensionsException
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_constructor_CellPositionOutOfRange)
{
    // The 2nd group contains position 81 which is too large an index
    // for a size-81 array (max is 80).
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(81, 9, { {0, 1, 2}, {79, 80, 81} }),
        sudoku::DimensionsException
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_constructor_CellCountIsZero)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(0, 9, {}),
        sudoku::DimensionsException
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_constructor_MaxCellValueIsZero)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(81, 0, {}),
        sudoku::DimensionsException
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_validateCellValues_notEnoughValues)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(4, 4, {}).validateCellValues({1, 2, 3}),
        std::exception
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_validateCellValues_tooManyValues)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(4, 4, {}).validateCellValues({0, 0, 0, 0, 0}),
        std::exception
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_validateCellValues_valueTooLarge)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(4, 4, {}).validateCellValues({2, 3, 4, 5}),
        std::exception
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_validateCellValues_repeatedGroupValue)
{
    BOOST_REQUIRE_THROW(
        sudoku::Dimensions(4, 4, { {0, 1, 2, 3} }).validateCellValues({1, 0, 0, 1}),
        std::exception
    );
}

BOOST_AUTO_TEST_CASE(Dimensions_validateCellValues_valid)
{
    sudoku::Dimensions dims(4, 4, { {0, 1, 2, 3} });
    dims.validateCellValues({1, 2, 3, 4});
}


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */

#pragma once

#include <boost/mpl/fold.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector_c.hpp>

#include "../meta/utility.hpp"
#include "generic_metafunctions/replace.hpp"
#include "generic_metafunctions/sequence_unpacker.hpp"
#include "layout_map.hpp"
#include "selector.hpp"

namespace gridtools {
    /** \ingroup common
        @{
        \ingroup layout
        @{
    */

    /** \brief Compute the reverse of a layout. For instance the reverse of
        `layout_map<1,0,-1,2>` is `layout<1,2,-1,0>`

        \tparam LayoutMap The layout map to process
    */
    template <typename LayoutMap>
    struct reverse_map;

    /// \private
    template <int_t... Is>
    struct reverse_map<layout_map<Is...>> {
        static constexpr int max = layout_map<Is...>::max();
        using type = layout_map<(Is < 0 ? Is : max - Is)...>;
    };

    template <typename DATALO, typename PROCLO>
    struct layout_transform;

    template <int_t I1, int_t I2, int_t P1, int_t P2>
    struct layout_transform<layout_map<I1, I2>, layout_map<P1, P2>> {
        typedef layout_map<I1, I2> L1;
        typedef layout_map<P1, P2> L2;

        static constexpr int_t N1 = L1::template at<P1>();
        static constexpr int_t N2 = L1::template at<P2>();

        typedef layout_map<N1, N2> type;
    };

    template <int_t I1, int_t I2, int_t I3, int_t P1, int_t P2, int_t P3>
    struct layout_transform<layout_map<I1, I2, I3>, layout_map<P1, P2, P3>> {
        typedef layout_map<I1, I2, I3> L1;
        typedef layout_map<P1, P2, P3> L2;

        static constexpr int_t N1 = L1::template at<P1>();
        static constexpr int_t N2 = L1::template at<P2>();
        static constexpr int_t N3 = L1::template at<P3>();

        typedef layout_map<N1, N2, N3> type;
    };

    /*
     * metafunction to filter out some of the dimensions of a layout map, determined by the DimSelector
     * Example of use: filter_layout<layout_map<0,1,2,3>, selector<1,1,0,1 > == layout_map<0,1,-1,2>
     */
    template <typename Layout, typename DimSelector>
    struct filter_layout;

    template <typename Layout, bool... Bitmask>
    struct filter_layout<Layout, selector<Bitmask...>> {
        typedef selector<Bitmask...> dim_selector_t;
        GT_STATIC_ASSERT((is_selector<dim_selector_t>::value), "Error: Dimension selector is wrong");
        typedef boost::mpl::vector_c<bool, Bitmask...> dim_selector_vec_t;
        GT_STATIC_ASSERT((is_layout_map<Layout>::value), "Error: need a layout map type");
        GT_STATIC_ASSERT(
            (sizeof...(Bitmask) >= Layout::masked_length), "Error: need to specifiy at least 4 dimensions");

        template <uint_t NumNullDims, typename Seq_>
        struct data_ {
            typedef Seq_ seq_t;
            static constexpr const uint_t num_null_dims = NumNullDims;
        };

        template <typename Data, typename Index>
        struct insert_index_at_pos {

            template <typename Data_, typename Position>
            struct insert_a_null {
                typedef data_<Data_::num_null_dims + 1,
                    typename replace<typename Data_::seq_t, Position, static_int<-1>>::type>
                    type;
            };

            template <typename Data_, typename Position>
            struct insert_a_pos_index {
                typedef data_<Data_::num_null_dims,
                    typename replace<typename Data_::seq_t,
                        Position,
                        static_int<Layout::template at<Position::value>() - Data_::num_null_dims>>::type>
                    type;
            };
            typedef static_int<Layout::template find<Index::value>()> position_t;
            typedef typename boost::mpl::if_c<(boost::mpl::at_c<dim_selector_vec_t, position_t::value>::type::value ==
                                                  false),
                typename insert_a_null<Data, position_t>::type,
                typename insert_a_pos_index<Data, position_t>::type>::type type;
        };

        typedef typename boost::mpl::fold<boost::mpl::range_c<int, 0, Layout::masked_length>,
            boost::mpl::vector0<>,
            boost::mpl::push_back<boost::mpl::_1, static_int<0>>>::type initial_vector;

        typedef data_<0, initial_vector> initial_data;
        typedef typename boost::mpl::fold<boost::mpl::range_c<int, 0, Layout::masked_length>,
            initial_data,
            insert_index_at_pos<boost::mpl::_1, boost::mpl::_2>>::type new_layout_indices_data_t;

        typedef typename new_layout_indices_data_t::seq_t new_layout_indices_t;

        template <typename T>
        struct indices_to_layout;

        template <typename... Args>
        struct indices_to_layout<variadic_typedef<Args...>> {
            using type = layout_map<Args::value...>;
        };

        typedef typename indices_to_layout<typename sequence_unpacker<new_layout_indices_t>::type>::type type;
    };

    namespace impl {
        template <int_t Val, int_t NExtraDim>
        struct inc_ {
            static const int_t value = Val == -1 ? -1 : Val + NExtraDim;
        };
    } // namespace impl

    enum class InsertLocation { pre, post };

    template <typename LayoutMap, uint_t NExtraDim, InsertLocation Location = InsertLocation::post>
    struct extend_layout_map;

    /*
     * metafunction to extend a layout_map with certain number of dimensions.
     * Example of use:
     * a) extend_layout_map< layout_map<0, 1, 3, 2>, 3> == layout_map<3, 4, 6, 5, 0, 1, 2>
     * b) extend_layout_map< layout_map<0, 1, 3, 2>, 3, InsertLocation::pre> == layout_map<0, 1, 2, 3, 4, 6, 5>
     */
    template <uint_t NExtraDim, int_t... Args, InsertLocation Location>
    struct extend_layout_map<layout_map<Args...>, NExtraDim, Location> {

        template <InsertLocation Loc, typename T, int_t... InitialInts>
        struct build_ext_layout;

        // build an extended layout
        template <int_t... Indices, int_t... InitialIndices>
        struct build_ext_layout<InsertLocation::post, meta::integer_sequence<int_t, Indices...>, InitialIndices...> {
            typedef layout_map<InitialIndices..., Indices...> type;
        };
        template <int_t... Indices, int_t... InitialIndices>
        struct build_ext_layout<InsertLocation::pre, meta::integer_sequence<int_t, Indices...>, InitialIndices...> {
            typedef layout_map<Indices..., InitialIndices...> type;
        };

        using seq = meta::make_integer_sequence<int_t, NExtraDim>;

        typedef typename build_ext_layout<Location, seq, impl::inc_<Args, NExtraDim>::value...>::type type;
    };

    template <int_t D>
    struct default_layout_map {
        using type = typename extend_layout_map<layout_map<>, D, InsertLocation::pre>::type;
    };

    template <int_t D>
    using default_layout_map_t = typename default_layout_map<D>::type;

    /** @} */
    /** @} */
} // namespace gridtools


#include <Rcpp.h>

//[[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>

//[[Rcpp::depends(BH)]]
#include <boost/math/special_functions/digamma.hpp>

#include "experimentalSetup.hpp"
#include "logLikelihoods.hpp"
#include "individual.hpp"
#include "estimatePoissonGammaParameters.hpp"
#include "AuxiliaryFunctions.hpp"

#include "nlopt.hpp"

// Allele coverage
EstimatePoissonGammaAlleleParameters::EstimatePoissonGammaAlleleParameters(const Eigen::VectorXd & coverage, const std::vector<Eigen::MatrixXd> & expectedContributionMatrix,
                                                                           const std::vector<Eigen::VectorXd> & alleleIndex, const Eigen::VectorXd & markerImbalances,
                                                                           const Eigen::VectorXd & partialSumAlleles, const double & convexMarkerImbalanceInterpolation,
                                                                           const Eigen::VectorXd & tolerance)
{
    Coverage = coverage;
    ExpectedContributionMatrix = expectedContributionMatrix;
    AlleleIndex = alleleIndex;

    PartialSumAlleles = partialSumAlleles;
    MarkerImbalances = markerImbalances;

    ConvexMarkerImbalanceInterpolation = convexMarkerImbalanceInterpolation;
    Tolerance = tolerance;
    MaximumNumberOfIterations = 2000;

    Counter = 0;

    NumberOfContributors = expectedContributionMatrix[0].cols();
    NumberOfMarkers = MarkerImbalances.size();

    initialiseParameters();
}

void EstimatePoissonGammaAlleleParameters::initialiseParameters()
{
    // Initialising mixture parameters
    Eigen::VectorXd phiCurrent = Eigen::VectorXd::Ones(NumberOfContributors) / NumberOfContributors;
    double sumCoverage = 0.0;
    double sumCount = 0.0;
    Eigen::VectorXd sumCoverageMarker = Eigen::VectorXd::Zero(NumberOfMarkers);
    for (std::size_t m = 0; m < NumberOfMarkers; m++)
    {
        const Eigen::VectorXd & AlleleIndex_m = AlleleIndex[m];
        const Eigen::MatrixXd & ExpectedContributionMatrix_m = ExpectedContributionMatrix[m];
        for (std::size_t a = 0; a < AlleleIndex_m.size(); a++)
        {
            std::size_t n = PartialSumAlleles[m] + AlleleIndex_m[a];
            sumCoverageMarker[m] += Coverage[n];
            sumCount++;

            for (std::size_t c = 0; c < NumberOfContributors; c++)
            {
                Eigen::VectorXd contributor_c = ExpectedContributionMatrix_m.col(c);
                if (contributor_c[a] >= 1)
                {
                    phiCurrent[c] += Coverage[n] / contributor_c[a];
                }
            }
        }

        sumCoverage += sumCoverageMarker[m];
    }

    MixtureParameters = phiCurrent / phiCurrent.sum();

    // Initialising sample and marker parameters
    double averageCoverageDenominator = 0.0;
    Eigen::VectorXd markerImbalancesMoM = sumCoverageMarker;
    for (std::size_t m = 0; m < NumberOfMarkers; m++)
    {
        const Eigen::MatrixXd & ExpectedContributionMatrix_m = ExpectedContributionMatrix[m];
        Eigen::VectorXd EC = ExpectedContributionMatrix_m * MixtureParameters;
        const double & ECSum = EC.sum();
        double markerImbalancesMoM_m = markerImbalancesMoM[m];
        if (!(markerImbalancesMoM_m > 0))
        {
            markerImbalancesMoM_m = 1e-6;
        }

        averageCoverageDenominator += ECSum;
        markerImbalancesMoM[m] = markerImbalancesMoM_m / ECSum;
    }

    SampleParameters = 2.0 * Eigen::VectorXd::Ones(2);
    SampleParameters[0] = sumCoverage / averageCoverageDenominator;

    markerImbalancesMoM = markerImbalancesMoM / SampleParameters[0];
    markerImbalancesMoM = markerImbalancesMoM / markerImbalancesMoM.mean();

    MarkerImbalancesParameters = ConvexMarkerImbalanceInterpolation * markerImbalancesMoM + (1 - ConvexMarkerImbalanceInterpolation) * MarkerImbalances;
}

double logLikelihoodAlleleCoverageNLopt(const std::vector<double> & x, std::vector<double> & grad, void *data)
{
    // Unpacking data
    EstimatePoissonGammaAlleleParameters *EPGA = reinterpret_cast<EstimatePoissonGammaAlleleParameters*>(data);
    const std::vector<Eigen::MatrixXd> & ExpectedContributionMatrix = EPGA->ExpectedContributionMatrix;
    const std::vector<Eigen::VectorXd> & AlleleIndex = EPGA->AlleleIndex;

    const Eigen::VectorXd & MarkerImbalances = EPGA->MarkerImbalancesParameters;
    const Eigen::VectorXd & PartialSumAlleles = EPGA->PartialSumAlleles;
    const Eigen::VectorXd & Coverage = EPGA->Coverage;

    const std::size_t NumberOfMarkers = EPGA->NumberOfMarkers;

    std::size_t & Counter = EPGA->Counter;

    const std::size_t & C = EPGA->NumberOfContributors;
    const std::size_t S = 2;

    const double & referenceMarkerAverage = x[0];
    const double & dispersion = x[1];

    double logLikelihood = 0.0;
    std::vector<double> gradient(C + S - 1, 0.0);
    for (std::size_t m = 0; m < NumberOfMarkers; m++)
    {
        const Eigen::VectorXd & AlleleIndex_m = AlleleIndex[m];
        const Eigen::MatrixXd & ExpectedContributionMatrix_m = ExpectedContributionMatrix[m];
        for (std::size_t a = 0; a < AlleleIndex_m.size(); a++)
        {
            std::size_t n = PartialSumAlleles[m] + AlleleIndex_m[a];
            const Eigen::VectorXd ExpectedContributionMatrix_ma = ExpectedContributionMatrix_m.row(a);

            double EC_n = ExpectedContributionMatrix_ma[C - 1];
            for (std::size_t c = 0; c < C - 1; c++)
            {
                EC_n += (ExpectedContributionMatrix_ma[c] - ExpectedContributionMatrix_ma[C - 1]) * x[S + c];
            }

            double mu_ma = referenceMarkerAverage * MarkerImbalances[m] * EC_n;
            if (!(mu_ma > 0.0))
            {
                mu_ma += 2e-16;
            }

            logLikelihood += logPoissonGammaDistribution(Coverage[n], mu_ma, mu_ma / dispersion);

            if (!grad.empty())
            {
                const double & gradient_common_term = boost::math::digamma(Coverage[n] + mu_ma / dispersion) - boost::math::digamma(mu_ma / dispersion) - std::log(dispersion + 1.0);

                gradient[0] += gradient_common_term * (MarkerImbalances[m] * EC_n) / (dispersion);
                gradient[1] += (Coverage[n] - mu_ma) / (dispersion * (dispersion + 1.0)) - gradient_common_term * (mu_ma / (std::pow(dispersion, 2.0)));

                for (std::size_t c = 0; c < C - 1; c++)
                {
                    const double & EC_nm = ExpectedContributionMatrix_ma[c] - ExpectedContributionMatrix_ma[C - 1];
                    gradient[S + c] +=  gradient_common_term * ((referenceMarkerAverage * MarkerImbalances[m] * EC_nm) / dispersion);
                }

            }
        }
    }

    if (!grad.empty())
    {
        grad = gradient;
    }

    Counter++;
    return logLikelihood;
}

void estimateParametersAlleleCoverage(EstimatePoissonGammaAlleleParameters &EPGA)
{
    std::size_t S = EPGA.SampleParameters.size();
    std::size_t C = EPGA.MixtureParameters.size();

    std::vector<double> parameters(S + C - 1, 0.0);
    for (std::size_t s = 0; s < S; s++)
    {
        parameters[s] = EPGA.SampleParameters[s];
    }

    for (std::size_t c = 0; c < C - 1; c++)
    {
        parameters[S + c] = EPGA.MixtureParameters[c];
    }

    // Optimiser
    // nlopt::opt individualOptimisation(nlopt::LN_BOBYQA, S + C - 1);
    // nlopt::opt individualOptimisation(nlopt::LD_LBFGS, S + C - 1);
    nlopt::opt individualOptimisation(nlopt::LD_SLSQP, S + C - 1);

    // Box-constraints
    std::size_t N = EPGA.Coverage.size();
    std::vector<double> lowerBound(S + C - 1), upperBound(S + C - 1);
    lowerBound[0] = 1;
    lowerBound[1] = 2e-8;
    upperBound[0] = EPGA.Coverage.maxCoeff() + 1;
    upperBound[1] = std::exp(std::log(2.0) + std::log(N) - std::log(2.0 * N - 2.0))  * (std::pow(EPGA.Coverage.maxCoeff(), 2.0) + 1);

    if (C == 1)
    {
        lowerBound[S] = 1 - 2e-16;
        upperBound[S] = 1;
    }
    else
    {
        for (std::size_t j = 0; j < C - 1; j++)
        {
            lowerBound[S + j] = 2e-16;
            upperBound[S + j] = 1.0 - 2e-16;
        }
    }

    individualOptimisation.set_lower_bounds(lowerBound);
    individualOptimisation.set_upper_bounds(upperBound);

    // Objective function
    individualOptimisation.set_max_objective(logLikelihoodAlleleCoverageNLopt, &EPGA);

    individualOptimisation.set_ftol_rel(EPGA.Tolerance[0]);
    individualOptimisation.set_ftol_abs(EPGA.Tolerance[1]);
    individualOptimisation.set_xtol_rel(EPGA.Tolerance[2]);
    individualOptimisation.set_xtol_abs(EPGA.Tolerance[3]);

    individualOptimisation.set_maxeval(EPGA.MaximumNumberOfIterations);

    double logLikelihood;
    try
    {
        nlopt::result result = individualOptimisation.optimize(parameters, logLikelihood);
    }
    catch (...)
    {
        try
        {
            if (EPGA.Tolerance[0] > 0)
                individualOptimisation.set_ftol_rel(std::pow(10, (-2 + std::log10(EPGA.Tolerance[0])) / 2));

            if (EPGA.Tolerance[1] > 0)
                individualOptimisation.set_ftol_abs(std::pow(10, (-2 + std::log10(EPGA.Tolerance[1])) / 2));

            if (EPGA.Tolerance[2] > 0)
                individualOptimisation.set_xtol_rel(std::pow(10, (-2 + std::log10(EPGA.Tolerance[2])) / 2));

            if (EPGA.Tolerance[3] > 0)
                individualOptimisation.set_xtol_abs(std::pow(10, (-2 + std::log10(EPGA.Tolerance[3])) / 2));

            nlopt::result result = individualOptimisation.optimize(parameters, logLikelihood);
        }
        catch (...)
        {
            //
        }
    }

    EPGA.LogLikelihood = logLikelihood;
    Eigen::VectorXd Parameters = STDEigen(parameters);

    EPGA.SampleParameters = Parameters.segment(0, S);

    EPGA.MixtureParameters.segment(0, C - 1) = Parameters.segment(S, C - 1);
    EPGA.MixtureParameters[C - 1] =  1.0 - Parameters.segment(S, C - 1).sum();
}

// Noise coverage
EstimatePoissonGammaNoiseParameters::EstimatePoissonGammaNoiseParameters(const Eigen::VectorXd & coverage,
                                                                         const std::vector<Eigen::VectorXd> & noiseIndex,
                                                                         const Eigen::VectorXd & partialSumAlleles,
                                                                         const Eigen::VectorXd & tolerance,
                                                                         const double & varianceUpperLimit)
{
    Coverage = coverage;
    NoiseIndex = noiseIndex;

    PartialSumAlleles = partialSumAlleles;

    NumberOfMarkers = NoiseIndex.size();
    Tolerance = tolerance;
    MaximumNumberOfIterations = 2000;
    VarianceUpperLimit = varianceUpperLimit;

    Counter = 0;

    initialiseParameters();
}

void EstimatePoissonGammaNoiseParameters::initialiseParameters()
{
    Eigen::VectorXd parameters = Eigen::VectorXd::Ones(3);

    double noiseCoverageSum = 0.0;
    double noiseCoverageSquaredSum = 0.0;
    double noiseCoverageSize = 0.0;
    double noiseCoverageInflation = 0.0;
    for (std::size_t m = 0; m < NumberOfMarkers; m++)
    {
        const Eigen::VectorXd & NoiseIndex_m = NoiseIndex[m];
        for (std::size_t a = 0; a < NoiseIndex_m.size(); a++)
        {
            std::size_t n = PartialSumAlleles[m] + NoiseIndex_m[a];
            noiseCoverageSum += Coverage[n];
            noiseCoverageSquaredSum += std::pow(Coverage[n], 2.0);
            noiseCoverageSize++;

            if (Coverage[n] == 1.0)
                noiseCoverageInflation++;
        }
    }

    double averageNoiseCoverage = std::ceil(0.5 * noiseCoverageSum) / noiseCoverageSize;

    parameters[0] = averageNoiseCoverage * (1 - std::exp(logPoissonGammaDistribution(0, averageNoiseCoverage, averageNoiseCoverage)));
    parameters[1] = 1.0; // std::abs((noiseCoverageSquaredSum / noiseCoverageSize - std::pow(parameters[0], 2.0)) / parameters[0] - 1.0);
    parameters[2] = noiseCoverageInflation / (2.0 * noiseCoverageSize);

    if (parameters[0] < 1.0) {
        parameters[0] = 1.0;
    }

    if (parameters[1] < 1.0) {
        parameters[1] = 1.0;
    }

    if (parameters[1] > VarianceUpperLimit) {
        parameters[1] = VarianceUpperLimit;
    }

    NoiseParameters = parameters;
}

double logLikelihoodNoiseCoverageNLopt(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
    EstimatePoissonGammaNoiseParameters *EPGN = reinterpret_cast<EstimatePoissonGammaNoiseParameters*>(data);
    const Eigen::VectorXd & Coverage = EPGN->Coverage;
    const std::vector<Eigen::VectorXd> & NoiseIndex = EPGN->NoiseIndex;
    const Eigen::VectorXd & PartialSumAlleles = EPGN->PartialSumAlleles;
    const std::size_t & NumberOfMarkers = EPGN->NumberOfMarkers;
    std::size_t & Counter = EPGN->Counter;

    const double & mu_ma = x[0];
    const double & dispersion = x[1];
    const double & p = x[2];

    double logLikelihood = 0.0;
    for (std::size_t m = 0; m < NumberOfMarkers; m++)
    {
        const Eigen::VectorXd & NoiseIndex_m = NoiseIndex[m];
        for (std::size_t a = 0; a < NoiseIndex_m.size(); a++)
        {
            std::size_t n = PartialSumAlleles[m] + NoiseIndex_m[a];
            logLikelihood += logInflatedTruncatedPoissonGammaDistribution(Coverage[n], mu_ma, dispersion, p, 1.0, 0.0);
                //- std::log(1 - std::exp(dispersion * logeta)); // std::log(1.0 - std::exp(gamma * zero_truncation)); //
        }
    }

    Counter++;
    return logLikelihood;
}

void estimateParametersNoiseCoverage(EstimatePoissonGammaNoiseParameters &EPGN)
{
    const std::size_t & N = EPGN.NoiseParameters.size();
    std::vector<double> parameters = EigenSTD(EPGN.NoiseParameters);

    // Optimiser
    // nlopt::opt individualOptimisation(nlopt::LN_BOBYQA, N);
    nlopt::opt individualOptimisation(nlopt::LN_SBPLX, N);

    // Box-constraints
    std::vector<double> lowerBound(N), upperBound(N);
    lowerBound[0] = 1.0;
    lowerBound[1] = 1.0;
    lowerBound[2] = 2e-16;

    upperBound[0] = EPGN.Coverage.maxCoeff();
    upperBound[1] = EPGN.VarianceUpperLimit; // (EPGN.Coverage.size() / (EPGN.Coverage.size() - 1.0)) * std::pow(EPGN.Coverage.maxCoeff(), 2.0);
    upperBound[2] = 1.0 - 2e-16;

    individualOptimisation.set_lower_bounds(lowerBound);
    individualOptimisation.set_upper_bounds(upperBound);

    // Objective function
    individualOptimisation.set_max_objective(logLikelihoodNoiseCoverageNLopt, &EPGN);

    individualOptimisation.set_ftol_rel(EPGN.Tolerance[0]);
    individualOptimisation.set_ftol_abs(EPGN.Tolerance[1]);
    individualOptimisation.set_xtol_rel(EPGN.Tolerance[2]);
    individualOptimisation.set_xtol_abs(EPGN.Tolerance[3]);

    individualOptimisation.set_maxeval(EPGN.MaximumNumberOfIterations);

    double logLikelihood;
    try
    {
        nlopt::result result = individualOptimisation.optimize(parameters, logLikelihood);
    }
    catch (...)
    {
        try
        {
            if (EPGN.Tolerance[0] > 0)
                individualOptimisation.set_ftol_rel(std::pow(10, (-2 + std::log10(EPGN.Tolerance[0])) / 2));

            if (EPGN.Tolerance[1] > 0)
                individualOptimisation.set_ftol_abs(std::pow(10, (-2 + std::log10(EPGN.Tolerance[1])) / 2));

            if (EPGN.Tolerance[2] > 0)
                individualOptimisation.set_xtol_rel(std::pow(10, (-2 + std::log10(EPGN.Tolerance[2])) / 2));

            if (EPGN.Tolerance[3] > 0)
                individualOptimisation.set_xtol_abs(std::pow(10, (-2 + std::log10(EPGN.Tolerance[3])) / 2));

            nlopt::result result = individualOptimisation.optimize(parameters, logLikelihood);
        }
        catch (...)
        {
            //
        }
    }

    EPGN.LogLikelihood = logLikelihood;
    Eigen::VectorXd Parameters = STDEigen(parameters);
    EPGN.NoiseParameters = Parameters;
}