/**
 * Copyright (C) 2013 Roman Hiestand
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or substantial
 * portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
 * LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "CommonIncludes.h"
#include "CalculatePIBoost.h"
#include "ProgressIndicatorInterface.h"

//#include <boost/math/constants/info.hpp>
#include <boost/math/constants/constants.hpp>
//#include <boost/multiprecision/detail/functions/trig.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

const wxString CalculatePIBoost::algoName_ = wxT("PI: boost");
const wxString CalculatePIBoost::algoDescr_ = wxT("Calculates PI using the boost::multiprecision library.");
const wxString CalculatePIBoost::copyrightText_ = wxT("Boost Software License - Version 1.0 - August 17th, 2003\n") \
	wxT("\n") \
	wxT("Permission is hereby granted, free of charge, to any person or organization\n") \
	wxT("obtaining a copy of the software and accompanying documentation covered by\n") \
	wxT("this license (the \"Software\") to use, reproduce, display, distribute,\n") \
	wxT("execute, and transmit the Software, and to prepare derivative works of the\n") \
	wxT("Software, and to permit third-parties to whom the Software is furnished to\n") \
	wxT("do so, all subject to the following:\n") \
	wxT("\n") \
	wxT("The copyright notices in the Software and this entire statement, including\n") \
	wxT("the above license grant, this restriction and the following disclaimer,\n") \
	wxT("must be included in all copies of the Software, in whole or in part, and\n") \
	wxT("all derivative works of the Software, unless such copies or derivative\n") \
	wxT("works are solely in the form of machine-executable object code generated by\n") \
	wxT("a source language processor.\n") \
	wxT("\n") \
	wxT("THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\n") \
	wxT("IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\n") \
	wxT("FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT\n") \
	wxT("SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE\n") \
	wxT("FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,\n") \
	wxT("ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER\n") \
	wxT("DEALINGS IN THE SOFTWARE.");
const std::string CalculatePIBoost::maxNumberOfDigits_ = "1000000";

CalculatePIBoost::CalculatePIBoost() :
	pProgressIndicatorInterface_(NULL)
{
}

CalculatePIBoost::~CalculatePIBoost()
{
}

bool CalculatePIBoost::isMulticoreCapable()
{
	return false;
}

bool CalculatePIBoost::enableProgressBar()
{
	return false;
}

const std::string &CalculatePIBoost::getMaxNumberOfDigits()
{
	return maxNumberOfDigits_;
}

const wxString &CalculatePIBoost::getAlgorithmName()
{
	return algoName_;
}

const wxString &CalculatePIBoost::getAlgorithmDescription()
{
	return algoDescr_;
}

const wxString &CalculatePIBoost::getCopyrightText()
{
	return copyrightText_;
}

void CalculatePIBoost::setDigits(const wxString &digits)
{
	digits_ = digits;
}

void CalculatePIBoost::setCores(int c)
{
	// Don't do anything: This Calculator is not multithreaded
}

#define VERSION_1

void CalculatePIBoost::calculate(ProgressIndicatorInterface *pProgressIndicatorInterface)
{
	// The major drawback of boost::multiprecision::cpp_dec_float is that the number of digits needs
	// to be known at compile time
	const int digits_compile_time = 100000;
	long long int d;
	digits_.ToLongLong(&d);
	if(d > digits_compile_time)
		d = digits_compile_time;	// ... or else calc_pi does not terminate
	long long int d_bits = 0;	// The number of bits required for d digits
	double digits_bits_factor = std::log( 10.0 ) / std::log( 2.0 );	// This is log2(10), approx. 3.32
	d_bits = static_cast<long long int>(std::ceil( digits_bits_factor * static_cast<double>(d) ));

	//typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<digits_compile_time, boost::int32_t, std::allocator<unsigned int> > > calc_pi_mptype;
	//typedef boost::multiprecision::number<boost::multiprecision::backends::mpfr_float_backend<digits_compile_time> > calc_pi_mptype;
	typedef boost::multiprecision::number<boost::multiprecision::backends::gmp_float<digits_compile_time> > calc_pi_mptype;
	calc_pi_mptype pi_boost;

#if defined(VERSION_1)
	// Version 1 uses boost::math::constants
//	boost::math::constants::print_info_on_type<mptype>();
	pi_boost = boost::math::constants::pi<calc_pi_mptype>();
#else
	// Version 2 uses an internal and undocumented method of boost::multiprecision which may or may not be present in the future
	boost::multiprecision::default_ops::calc_pi(pi_boost.backend(), d_bits);
#endif

	std::ostringstream ostr;
	ostr << std::setprecision(d+2);
	ostr << pi_boost;
	result_ = ostr.str();
}


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

/*    $Id: step-46.cc 27737 2012-12-03 19:44:54Z heister $       */
/*                                                                */
/*    Copyright (C) 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

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

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

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

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

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

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

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

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

#include <fstream>
#include <sstream>

namespace Step46
{
  using namespace dealii;

  // @sect3{The FluidStructureProblem class template}

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

  private:
    enum
    {
      fluid_domain_id,
      solid_domain_id
    };

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

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

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

    const unsigned int    stokes_degree;
    const unsigned int    elasticity_degree;

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

    ConstraintMatrix      constraints;

    SparsityPattern       sparsity_pattern;
    SparseMatrix<double>  system_matrix;

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

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

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

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

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

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

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

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

    return 0;
  }

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

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

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

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

  };

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

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

  // @sect3{The FluidStructureProblem implementation}

  // @sect4{Constructors and helper functions}

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

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

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

  // @sect4{Meshes and assigning subdomains}

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

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

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

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

  // @sect4{FluidStructureProblem::setup_dofs}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    system_matrix.reinit (sparsity_pattern);

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

  // @sect4{FluidStructureProblem::assemble_system}

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

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

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

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

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

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

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

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

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

    const RightHandSide<dim>  right_hand_side;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

                  }
              }
      }
  }

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

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

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

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

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

  // @sect4{FluidStructureProblem::solve}

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

    constraints.distribute (solution);
  }

  // @sect4{FluidStructureProblem::output_results}

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

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

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

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

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

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

  // @sect4{FluidStructureProblem::refine_mesh}

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

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

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

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

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

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

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

    estimated_error_per_cell += stokes_estimated_error_per_cell;
    estimated_error_per_cell += elasticity_estimated_error_per_cell;

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

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

  // @sect4{FluidStructureProblem::run}

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

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

        if (refinement_cycle > 0)
          refine_mesh ();

        setup_dofs ();

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

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

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

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

// @sect4{The main() function}

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

      deallog.depth_console (0);

      FluidStructureProblem<2> flow_problem(1, 1);
      flow_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include "map/pathfinder.h"

#include "database/defines.h"
#include "map/province.h"
#include "util/container_util.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>

namespace metternich {

class pathfinder::impl
{
	using cost = int;
	using graph = boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, cost>>;
	using vertex = graph::vertex_descriptor;
	using edge = graph::edge_descriptor;

public:
	impl(const std::set<province *> &provinces);

	find_trade_path_result find_trade_path(const province *start_province, const province *goal_province) const;

private:
	size_t get_province_index(const province *province) const
	{
		return this->province_to_index.find(province)->second;
	}

	cost get_trade_cost(const edge e) const;

private:
	std::vector<province *> provinces;
	std::map<const province *, size_t> province_to_index;
	graph province_graph;
};

template <class graph, class cost>
class trade_path_heuristic final : public boost::astar_heuristic<graph, cost>
{
public:
	using vertex = typename boost::graph_traits<graph>::vertex_descriptor;

	trade_path_heuristic(vertex goal, const std::vector<province *> &provinces)
		: goal(goal), provinces(provinces)
	{}

	cost operator()(vertex v)
	{
		return this->provinces[v]->get_kilometers_distance_to(this->provinces[this->goal]) * 100 / province::base_distance * defines::get()->get_trade_cost_modifier_per_distance() / 100;
	}

private:
	vertex goal;
	const std::vector<province *> &provinces;
};

template <class vertex>
class astar_visitor final : public boost::default_astar_visitor
{
public:
	astar_visitor(vertex goal) : goal(goal)
	{}

	template <class graph>
	void examine_vertex(vertex v, graph &g) {
		Q_UNUSED(g)

		if (v == this->goal) {
			throw found_goal();
		}
	}

private:
	vertex goal;
};

struct found_goal
{
};

pathfinder::pathfinder(const std::set<province *> &provinces)
{
	this->implementation = std::make_unique<impl>(provinces);
}

pathfinder::~pathfinder()
{
}

find_trade_path_result pathfinder::find_trade_path(const province *start_province, const province *goal_province) const
{
	return this->implementation->find_trade_path(start_province, goal_province);
}

pathfinder::impl::impl(const std::set<province *> &provinces)
	: provinces(container::to_vector(provinces)), province_graph(provinces.size())
{
	for (size_t i = 0; i < this->provinces.size(); ++i) {
		const province *province = this->provinces[i];
		this->province_to_index[province] = i;
	}

	for (province *province : this->provinces) {
		for (metternich::province *border_province : province->get_border_provinces()) {
			edge e;
			bool inserted;
			boost::tie(e, inserted) = boost::add_edge(this->province_to_index[province], this->province_to_index[border_province], this->province_graph);
		}
	}
}

find_trade_path_result pathfinder::impl::find_trade_path(const province *start_province, const province *goal_province) const
{
	vertex start = this->get_province_index(start_province);
	vertex goal = this->get_province_index(goal_province);

	std::vector<vertex> vertex_predecessors(boost::num_vertices(this->province_graph));
	std::vector<cost> vertex_costs(boost::num_vertices(this->province_graph));

	auto weight_function = boost::make_function_property_map<edge, cost>([this](edge e) {
		return this->get_trade_cost(e);
	});

	try {
		astar_search_tree(this->province_graph, start, trade_path_heuristic<graph, cost>(goal, this->provinces),
			weight_map(weight_function).
			predecessor_map(make_iterator_property_map(vertex_predecessors.begin(), get(boost::vertex_index, this->province_graph))).
			distance_map(make_iterator_property_map(vertex_costs.begin(), get(boost::vertex_index, this->province_graph))).
			visitor(astar_visitor<vertex>(goal)));
	} catch (found_goal) {
		find_trade_path_result result(true);
		for (vertex v = goal;; v = vertex_predecessors[v]) {
			result.path.push_back(this->provinces[v]);
			if (vertex_predecessors[v] == v) {
				break;
			}
		}
		std::reverse(result.path.begin(), result.path.end());
		result.trade_cost = vertex_costs[goal];
		return result;
	}

	return find_trade_path_result(false);
}

pathfinder::impl::cost pathfinder::impl::get_trade_cost(const edge e) const
{
	const province *source_province = this->provinces[e.m_source];
	const province *target_province = this->provinces[e.m_target];

	int trade_cost = source_province->get_kilometers_distance_to(target_province) * 100 / province::base_distance * defines::get()->get_trade_cost_modifier_per_distance() / 100;

	if (source_province->is_water() != target_province->is_water()) {
		trade_cost += defines::get()->get_base_port_trade_cost_modifier();
	}

	return trade_cost;
}

}


#include "Trajectory.h"
#include "promp.h"
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SVD>
#include <cmath>

#include <random>

using namespace promp;
int example_counter = 0;

typedef Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> Matrix;
typedef Eigen::Map<Eigen::Matrix<double, 1, Eigen::Dynamic, Eigen::RowMajor>> Vector;
typedef Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> constMatrix;
typedef Eigen::Map<const Eigen::Matrix<double, 1, Eigen::Dynamic, Eigen::RowMajor>> constVector;

std::vector<ConditionPoint> ConditionPoint::fromMatrix(MatrixXd pointMatrix)
{
  std::vector<ConditionPoint> ret;

  for (int i = 0; i < pointMatrix.rows(); i++)
  {
    ret.push_back(ConditionPoint());
    ret.back().timestamp = pointMatrix(i, 0);
    ret.back().dimension = pointMatrix(i, 1);
    ret.back().derivative = pointMatrix(i, 2);
    ret.back().mean = pointMatrix(i, 3);
    ret.back().variance = pointMatrix(i, 4);
  }

  return ret;
}

Trajectory::Trajectory(const TrajectoryData &data) : numWeights_(data.numBF_), numDim_(data.numDim_), overlap_(data.overlap_)
{
  type_ = data.isStroke_ ? Stroke : Periodic;
  weightMean_ = constVector(data.mean_.data(), data.numDim_ * data.numBF_);
  weightCovars_ = constMatrix(data.covariance_.data(), data.numDim_ * data.numBF_, data.numDim_ * data.numBF_);
  conditionPoints_ = ConditionPoint::fromMatrix(constMatrix(data.conditions_.data(), data.conditions_.size() / ConditionPoint::NUM_FIELDS, ConditionPoint::NUM_FIELDS));
  setBF();
  condition(weightMean_, weightCovars_);
}

Trajectory::Trajectory(const int numWeights, const VectorXd &weights, const double overlap, const MatrixXd &covars,
                       const TrajectoryType type)
    : numWeights_(numWeights), numDim_(weights.size() / numWeights), overlap_(overlap), weightMean_(weights),
      weightCovars_(covars),
      type_(type)
{
  setBF();
}

Trajectory::Trajectory(const std::vector<VectorXd> &timestamps, const std::vector<MatrixXd> &values,
                       const double overlap, int numWeights,
                       const int iterationLimit, const TrajectoryType type)
    : numWeights_(numWeights), numDim_(values.front().cols()), overlap_(overlap), type_(type)
{
  setBF();
  imitate(timestamps, values, iterationLimit);
}

void Trajectory::imitate(const std::vector<VectorXd> &timestamps, const std::vector<MatrixXd> &values, const int iterationLimit)
{
  weightMean_ = VectorXd::Zero(numDim_ * numWeights_);
  weightCovars_ = MatrixXd::Identity(numDim_ * numWeights_, numDim_ * numWeights_);
  standardDev_ = 1.;

  MatrixXd means(values.size(), numDim_ * numWeights_);
  std::vector<MatrixXd> covs(values.size(), MatrixXd::Identity(numFunc_ * numWeights_, numFunc_ * numWeights_));

  std::vector<MatrixXd> H(values.size());
  for (size_t i = 0; i < values.size(); i++)
  {
    MatrixXd H_partial = MatrixXd::Identity(values[i].rows(), values[i].rows());

    for (int y = 0; y < values[i].rows(); y++)
    {
      for (int preview = 1; preview < 2 && (y + preview) < values[i].rows(); preview++)
      {
        H_partial(y, y + preview) = -std::pow(0.7, preview);
      }
    }

    H[i] = (MatrixXd::Zero(values[i].size(), values[i].size()));
    for (int j = 0; j < numDim_; j++)
      H[i].block(H_partial.rows() * j, H_partial.rows() * j, H_partial.rows(), H_partial.rows()) = H_partial;
  }

  // value restructuring
  std::vector<MatrixXd> val(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    val[i] = MatrixXd(values[i].size(), 1);
    for (int j = 0; j < numDim_; j++)
      val[i].block(timestamps[i].size() * j, 0, timestamps[i].size(), 1) = values[i].block(0, j, timestamps[i].size(), 1);
  }

  std::vector<MatrixXd> BF(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    BF[i] = basisFunctions_->getValue(timestamps[i], numDim_).transpose();
  }

  std::vector<MatrixXd> R(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    R[i] = H[i] * val[i];
  }

  std::vector<MatrixXd> RR(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    RR[i] = R[i].transpose() * R[i];
  }

  std::vector<MatrixXd> BH(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    BH[i] = BF[i] * H[i].transpose();
  }

  std::vector<MatrixXd> mean_eStep(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    mean_eStep[i] = (BH[i] * R[i]);
  }

  std::vector<MatrixXd> cov_eStep(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    cov_eStep[i] = (BH[i] * BH[i].transpose());
  }

  int sampleCount = 0;
  for (size_t i = 0; i < values.size(); i++)
  {
    sampleCount += values[i].rows();
  }

  int counter = 0;
  VectorXd weightMean_old;
  MatrixXd weightCovars_old;
  do
  {
    counter++;
    weightMean_old = weightMean_;
    weightCovars_old = weightCovars_;

    for (size_t i = 0; i < values.size(); i++)
    {
      E_Step(1. / standardDev_ * mean_eStep[i], 1. / standardDev_ * cov_eStep[i], means.row(i), covs[i]);
    }

    M_Step(means, covs, RR, mean_eStep, cov_eStep, sampleCount);
  } while (counter < iterationLimit && !weightMean_old.isApprox(weightMean_) && !weightCovars_old.isApprox(weightCovars_));
}

MatrixXd Trajectory::getWeightMean() const
{
  return weightMean_;
};

MatrixXd Trajectory::getWeightCovars() const
{
  return weightCovars_;
};

MatrixXd Trajectory::getValueMean(const VectorXd &time) const
{
  MatrixXd out(numDim_ * numFunc_, time.size());

  MatrixXd bf = basisFunctions_->getValue(time);
  MatrixXd bfd = basisFunctions_->getValueDeriv(time);
  for (int dimension = 0; dimension < numDim_; dimension++)
  {
    out.row(numFunc_ * dimension) = bf * weightMean_.segment(numWeights_ * dimension, numWeights_);
    out.row((numFunc_ * dimension) + 1) = bfd * weightMean_.segment(numWeights_ * dimension, numWeights_);
  }

  return out;
}

VectorXd Trajectory::getValueMean(const double time) const
{
  VectorXd timeVec(1);
  timeVec << time;
  return getValueMean(timeVec).col(0);
}

MatrixXd Trajectory::getValueCovars(const VectorXd &time) const
{
  MatrixXd out(time.size(), numDim_ * numFunc_ * numDim_ * numFunc_);
  MatrixXd value = basisFunctions_->getValue(time);
  MatrixXd valueDeriv = basisFunctions_->getValueDeriv(time);

  for (int y = 0; y < numDim_; y++)
  {
    for (int x = 0; x < numDim_; x++)
    {

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x)) = (value *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                value.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + 1) = (value *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                valueDeriv.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + (numDim_ * numFunc_)) = (valueDeriv *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                value.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + (numDim_ * numFunc_) + 1) = (valueDeriv *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                valueDeriv.transpose())
                                                                                .diagonal();
    }
  }
  return out;
}

VectorXd Trajectory::getValueCovars(const double time) const
{
  VectorXd timeVec(1);
  timeVec << time;
  return getValueCovars(timeVec).row(0);
}

void Trajectory::condition(VectorXd &weightMean, MatrixXd &weightCovars) const
{
  if (conditionPoints_.empty())
    return;
  MatrixXd basisFunc_tmp = MatrixXd::Zero(conditionPoints_.size(), numDim_ * numWeights_);
  VectorXd means(conditionPoints_.size());
  VectorXd variances(conditionPoints_.size());

  for (unsigned i = 0; i < conditionPoints_.size(); i++)
  {
    const ConditionPoint &point = conditionPoints_[i];
    if (point.derivative == 0)
    {
      basisFunc_tmp.block(i, numWeights_ * point.dimension, 1, numWeights_).row(0) = basisFunctions_->getValue(
                                                                                point.timestamp)
                                                                                .row(0);
    }
    else
    {
      basisFunc_tmp.block(i, numWeights_ * point.dimension, 1, numWeights_).row(0) = basisFunctions_->getValueDeriv(point.timestamp).row(0);
    }

    means(i) = point.mean;
    variances(i) = point.variance;
  }

  MatrixXd basisFunc = basisFunc_tmp.transpose();

  MatrixXd cov = variances.asDiagonal();
  cov = (cov + (basisFunc.transpose() * weightCovars * basisFunc)).inverse();

  VectorXd weightMeanNew =
      weightMean + weightCovars * basisFunc * cov * (means - (basisFunc.transpose() * weightMean));
  MatrixXd weightCovarsNew =
      weightCovars - (weightCovars * basisFunc * cov * basisFunc.transpose() * weightCovars);
  weightMean = weightMeanNew;
  weightCovars = weightCovarsNew;
}

void Trajectory::getData(TrajectoryData &data) const
{
  std::memcpy(data.mean_.data(), weightMean_.data(), weightMean_.size() * sizeof(double));
  std::memcpy(data.covariance_.data(), weightCovars_.data(), weightCovars_.size() * sizeof(double));
}

Trajectory Trajectory::sampleTrajectoty(unsigned &seed) const
{
  std::default_random_engine generator(seed);
  MatrixXd A = weightCovars_.selfadjointView<Lower>().llt().matrixL();
  VectorXd z(weightMean_.size());

  std::normal_distribution<double> normalDist(0, 1);

  for (int i = 0; i < weightMean_.size(); i++)
  {
    z(i) = normalDist(generator);
  }

  VectorXd newMean = weightMean_ + (A * z);
  MatrixXd newCovars = MatrixXd::Zero(weightMean_.size(), weightMean_.size());
  seed = generator();
  return Trajectory(numWeights_, newMean, overlap_, newCovars, type_);
  ;
}
Trajectory Trajectory::sampleTrajectoty() const
{
  unsigned seed = time(0);
  return sampleTrajectoty(seed);
}

void Trajectory::E_Step(const MatrixXd &mean_eStep, const MatrixXd &cov_eStep, Ref<VectorXd, 0, InnerStride<>> mean,
                        MatrixXd &cov)
{
  cov = (cov_eStep + weightCovars_.inverse()).inverse();
  mean = (cov * (mean_eStep + (weightCovars_.inverse() * weightMean_))).col(0);
}

void Trajectory::M_Step(const MatrixXd &mean, const std::vector<MatrixXd> &cov, const std::vector<MatrixXd> &RR,
                        const std::vector<MatrixXd> &RH, const std::vector<MatrixXd> &HH, const int sampleCount)
{

  weightMean_ = mean.colwise().mean().row(0);
  MatrixXd centered = mean.rowwise() - mean.colwise().mean();
  weightCovars_ = centered.transpose() * centered;

  for (int i = 0; i < mean.rows(); i++)
  {
    weightCovars_ += cov[i];
  }

  weightCovars_ /= mean.rows();

  standardDev_ = 0;
  for (int i = 0; i < mean.rows(); i++)
  {
    standardDev_ += (HH[i] * cov[i]).trace();
    standardDev_ += RR[i](0, 0);
    standardDev_ -= 2 * (RH[i].transpose() * mean.row(i).transpose())(0, 0);
    standardDev_ += (mean.row(i) * HH[i] * mean.row(i).transpose())(0, 0);
  }

  standardDev_ /= mean.norm() * mean.rows() * numDim_ * sampleCount + 2; // magic number from the paper
}

void Trajectory::setBF()
{
  if (type_ == Stroke)
  {
    basisFunctions_ = std::shared_ptr<BasisFunctions>(new StrokeBasisFunctions(numWeights_, overlap_));
  }
  else
  {
    basisFunctions_ = std::shared_ptr<BasisFunctions>(new PeriodicBasisFunctions(numWeights_, overlap_));
  }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_CONJ_INCLUDE
#define MTL_CONJ_INCLUDE

#include <complex>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/algebraic_category.hpp>
#include <boost/numeric/mtl/utility/is_what.hpp>
#include <boost/numeric/mtl/utility/view_code.hpp>
#include <boost/numeric/mtl/utility/viewed_collection.hpp>
#include <boost/numeric/mtl/utility/compose_view.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/matrix/view_ref.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl {

namespace sfunctor {

    template <typename Value, typename AlgebraicCategory>
    struct conj_aux
    {
	typedef Value result_type;

	static inline result_type apply(const Value& v)
	{
	    return v;
	}

	result_type operator() (const Value& v) const
	{
	    return v;
	}
    };

    template <typename Value, typename AlgebraicCategory>
    struct conj_aux<std::complex<Value>, AlgebraicCategory>
    {
	typedef std::complex<Value> result_type;

	static inline result_type apply(const std::complex<Value>& v)
	{
	    return std::conj(v);
	}

	result_type operator() (const std::complex<Value>& v) const
	{
	    return std::conj(v);
	}
    };

    // Only declarations here, definitions in mat::map_view (map_view)
    template <typename Matrix>
    struct conj_aux<Matrix, tag::matrix>;

    template <typename Vector>
    struct conj_aux<Vector, tag::vector>;

    // Short cut for result type
    template <typename Value>
    struct conj
	: public conj_aux<Value, typename mtl::traits::algebraic_category<Value>::type>
    {};

} // namespace sfunctor

    namespace vec {

	/// Conjugate of an vector
	template <typename Vector>
	typename mtl::traits::enable_if_vector<Vector, conj_view<Vector> >::type
	inline conj(const Vector& v)
	{
	    return conj_view<Vector>(v);
	}
    }

    namespace mat {

	namespace detail {

	    template <typename Matrix>
	    struct conj_trait
	    {
		static const unsigned code= mtl::traits::view_toggle_conj<mtl::traits::view_code<Matrix> >::value;
		typedef typename mtl::traits::compose_view<code, typename mtl::traits::viewed_collection<Matrix>::type>::type type;

		static inline type apply(const Matrix& A)
		{
		    return type(view_ref(A));
		}
	    };

	}

	/// Conjugate of a matrix
	template <typename Matrix>
	typename mtl::traits::lazy_enable_if_matrix<Matrix, detail::conj_trait<Matrix> >::type
	inline conj(const Matrix& A)
	{
	    return detail::conj_trait<Matrix>::apply(A);
	}
    }

    namespace scalar {

	// Only scalar values remain here
	template <typename Value>
	typename mtl::traits::enable_if_scalar<
	    Value
	  , typename sfunctor::conj<Value>::result_type
	>::type
	inline conj(const Value& v)
	{
	    return mtl::sfunctor::conj<Value>::apply(v);
	}

	float inline conj(float v) { return v; }
	double inline conj(double v) { return v; }
	long double inline conj(long double v) { return v; }
    }

    /// Conjugate of vector, matrix, or scalar
    using vec::conj;
    using mat::conj;
    using scalar::conj;
    // using std::conj;

} // namespace mtl

#endif // MTL_CONJ_INCLUDE


// Copyright John Maddock 2006, 2007.
// Copyright Paul A. Bristow 2008.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP
#define BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete beta.
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp>

#include <utility>

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class chi_squared_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   chi_squared_distribution(RealType i) : m_df(i)
   {
      RealType result;
      detail::check_df(
         "boost::math::chi_squared_distribution<%1%>::chi_squared_distribution", m_df, &result, Policy());
   } // chi_squared_distribution

   RealType degrees_of_freedom()const
   {
      return m_df;
   }

   // Parameter estimation:
   static RealType find_degrees_of_freedom(
      RealType difference_from_variance,
      RealType alpha,
      RealType beta,
      RealType variance,
      RealType hint = 100);

private:
   //
   // Data member:
   //
   RealType m_df;  // degrees of freedom are a real number.
}; // class chi_squared_distribution

typedef chi_squared_distribution<double> chi_squared;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const chi_squared_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(0, max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const chi_squared_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   return std::pair<RealType, RealType>(0, tools::max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
RealType pdf(const chi_squared_distribution<RealType, Policy>& dist, const RealType& chi_square)
{
   BOOST_MATH_STD_USING  // for ADL of std functions
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;

   static const char* function = "boost::math::pdf(const chi_squared_distribution<%1%>&, %1%)";

   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   if(chi_square == 0)
   {
      // Handle special cases:
      if(degrees_of_freedom < 2)
      {
         return policies::raise_overflow_error<RealType>(
            function, 0, Policy());
      }
      else if(degrees_of_freedom == 2)
      {
         return 0.5f;
      }
      else
      {
         return 0;
      }
   }

   return gamma_p_derivative(degrees_of_freedom / 2, chi_square / 2, Policy()) / 2;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const chi_squared_distribution<RealType, Policy>& dist, const RealType& chi_square)
{
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;
   static const char* function = "boost::math::cdf(const chi_squared_distribution<%1%>&, %1%)";

   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   return boost::math::gamma_p(degrees_of_freedom / 2, chi_square / 2, Policy());
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const chi_squared_distribution<RealType, Policy>& dist, const RealType& p)
{
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   static const char* function = "boost::math::quantile(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy())
         && detail::check_probability(
            function, p, &error_result, Policy()))
      return error_result;

   return 2 * boost::math::gamma_p_inv(degrees_of_freedom / 2, p, Policy());
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   RealType const& degrees_of_freedom = c.dist.degrees_of_freedom();
   RealType const& chi_square = c.param;
   static const char* function = "boost::math::cdf(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   return boost::math::gamma_q(degrees_of_freedom / 2, chi_square / 2, Policy());
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   RealType const& degrees_of_freedom = c.dist.degrees_of_freedom();
   RealType const& q = c.param;
   static const char* function = "boost::math::quantile(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy())
         && detail::check_probability(
            function, q, &error_result, Policy()))
      return error_result;

   return 2 * boost::math::gamma_q_inv(degrees_of_freedom / 2, q, Policy());
}

template <class RealType, class Policy>
inline RealType mean(const chi_squared_distribution<RealType, Policy>& dist)
{ // Mean of Chi-Squared distribution = v.
  return dist.degrees_of_freedom();
} // mean

template <class RealType, class Policy>
inline RealType variance(const chi_squared_distribution<RealType, Policy>& dist)
{ // Variance of Chi-Squared distribution = 2v.
  return 2 * dist.degrees_of_freedom();
} // variance

template <class RealType, class Policy>
inline RealType mode(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   static const char* function = "boost::math::mode(const chi_squared_distribution<%1%>&)";
   // Most sources only define mode for df >= 2,
   // but for 0 <= df <= 2, the pdf maximum actually occurs at random variate = 0;
   // So one could extend the definition of mode thus:
   //if(df < 0)
   //{
   //   return policies::raise_domain_error<RealType>(
   //      function,
   //      "Chi-Squared distribution only has a mode for degrees of freedom >= 0, but got degrees of freedom = %1%.",
   //      df, Policy());
   //}
   //return (df <= 2) ? 0 : df - 2;

   if(df < 2)
      return policies::raise_domain_error<RealType>(
         function,
         "Chi-Squared distribution only has a mode for degrees of freedom >= 2, but got degrees of freedom = %1%.",
         df, Policy());
   return df - 2;
}

//template <class RealType, class Policy>
//inline RealType median(const chi_squared_distribution<RealType, Policy>& dist)
//{ // Median is given by Quantile[dist, 1/2]
//   RealType df = dist.degrees_of_freedom();
//   if(df <= 1)
//      return tools::domain_error<RealType>(
//         BOOST_CURRENT_FUNCTION,
//         "The Chi-Squared distribution only has a mode for degrees of freedom >= 2, but got degrees of freedom = %1%.",
//         df);
//   return df - RealType(2)/3;
//}
// Now implemented via quantile(half) in derived accessors.

template <class RealType, class Policy>
inline RealType skewness(const chi_squared_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING // For ADL
   RealType df = dist.degrees_of_freedom();
   return sqrt (8 / df);  // == 2 * sqrt(2 / df);
}

template <class RealType, class Policy>
inline RealType kurtosis(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   return 3 + 12 / df;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   return 12 / df;
}

//
// Parameter estimation comes last:
//
namespace detail
{

template <class RealType, class Policy>
struct df_estimator
{
   df_estimator(RealType a, RealType b, RealType variance, RealType delta)
      : alpha(a), beta(b), ratio(delta/variance) {}

   RealType operator()(const RealType& df)
   {
      if(df <= tools::min_value<RealType>())
         return 1;
      chi_squared_distribution<RealType, Policy> cs(df);

      RealType result;
      if(ratio > 0)
      {
         RealType r = 1 + ratio;
         result = cdf(cs, quantile(complement(cs, alpha)) / r) - beta;
      }
      else
      {
         RealType r = 1 + ratio;
         result = cdf(complement(cs, quantile(cs, alpha) / r)) - beta;
      }
      return result;
   }
private:
   RealType alpha, beta, ratio;
};

} // namespace detail

template <class RealType, class Policy>
RealType chi_squared_distribution<RealType, Policy>::find_degrees_of_freedom(
   RealType difference_from_variance,
   RealType alpha,
   RealType beta,
   RealType variance,
   RealType hint)
{
   static const char* function = "boost::math::chi_squared_distribution<%1%>::find_degrees_of_freedom(%1%,%1%,%1%,%1%,%1%)";
   // Check for domain errors:
   RealType error_result;
   if(false == detail::check_probability(
         function, alpha, &error_result, Policy())
         && detail::check_probability(function, beta, &error_result, Policy()))
      return error_result;

   if(hint <= 0)
      hint = 1;

   detail::df_estimator<RealType, Policy> f(alpha, beta, variance, difference_from_variance);
   tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   std::pair<RealType, RealType> r = tools::bracket_and_solve_root(f, hint, RealType(2), false, tol, max_iter, Policy());
   RealType result = r.first + (r.second - r.first) / 2;
   if(max_iter >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
         " either there is no answer to how many degrees of freedom are required"
         " or the answer is infinite.  Current best guess is %1%", result, Policy());
   }
   return result;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP


//=======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Authors: Ireneusz Szcześniak <www.irkos.org>
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

//=======================================================================
// This is the implementation of the Yen algorithm:
//
// Jin Y. Yen, Finding the k shortest loopless paths in a network,
// Management Science, vol. 17, no. 11, July 1971, pages 712-716
//
// But actually, I found the following explanation better:
//
// https://en.wikipedia.org/wiki/Yen%27s_algorithm
//=======================================================================

#ifndef BOOST_GRAPH_YEN_KSP
#define BOOST_GRAPH_YEN_KSP

#include <list>
#include <set>
#include <map>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/utility/value_init.hpp>

#include "custom_dijkstra_call.hpp"
#include "exclude_filter.hpp"

namespace boost {

  template <typename Graph, typename WeightMap, typename IndexMap>
  std::list<std::pair<typename WeightMap::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(const Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          WeightMap wm, IndexMap im, optional<unsigned> K)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;
    typedef typename WeightMap::value_type weight_type;
    typedef exclude_filter<vertex_descriptor> evf_type;
    typedef exclude_filter<edge_descriptor> eef_type;
    typedef std::list<edge_descriptor> path_type;
    typedef std::pair<weight_type, path_type> kr_type;

    // The results.
    std::list<kr_type> A;
    // The tentative results.
    std::multimap<weight_type, path_type> C;

    // The set of excluded edges.
    std::set<edge_descriptor> exe;
    // The set of excluded vertexes.
    std::set<vertex_descriptor> exv;

    // The filter which excludes edges.
    eef_type ef(&exe);
    // The filter which excludes vertexes.
    evf_type vf(&exv);

    // The filtered graph type.
    typedef boost::filtered_graph<Graph, eef_type, evf_type> fg_type;

    // The filtered graph.
    fg_type fg(g, ef, vf);

    optional<kr_type> okr = custom_dijkstra_call(g, s, t, wm, im);

    if (okr)
      {
        A.push_back(okr.get());

        for (int k = 1; !K || k < K.get(); ++k)
          {
            // The previous shortest result.
            const kr_type &psr = A.back();
            const path_type &psp = psr.second;

            // Iterate over the edges of the previous shortest path.
            for(auto i = psp.begin(); i != psp.end(); ++i)
              {
                // The spur vertex.
                vertex_descriptor sv = source(*i, g);
                // The root path.
                path_type rp = path_type(psp.begin(), i);

                // Iterate over the previous shortest paths.
                for(const auto &kr: A)
                  {
                    const path_type &kp = kr.second;
                    typename path_type::const_iterator i1, i2;

                    // Iterate as long as possible, and as long as
                    // paths are equal.
                    for(tie(i1, i2) = std::make_pair(kp.begin(), rp.begin());
                        i1 != kp.end() && i2 != rp.end() && *i1 == *i2;
                        ++i1, ++i2);

                    // Make sure we didn't reach the end of kp.  If we
                    // did, there is no next edge in kp, which we
                    // could exclude.  Also, make sure we reached the
                    // end of rp, i.e., the kp begins with rp.
                    if (i1 != kp.end() && i2 == rp.end())
                      exe.insert(*i1);
                  }

                // Remove the vertexes that belong to the root path,
                // except the last vertex, i.e., the spur node.
                for (const auto &e: rp)
                  exv.insert(source(e, g));

                // Optional spur result.
                optional<kr_type> osr = custom_dijkstra_call(fg, sv, t, wm, im);

                if (osr)
                  {
                    // The tentative result.
                    kr_type tr = osr.get();
                    for(const auto &e: boost::adaptors::reverse(rp))
                      {
                        tr.second.push_front(e);
                        tr.first += get(wm, e);
                      }
                    C.insert(tr);
                  }

                // Clear the excluded edges and vertexes.
                exe.clear();
                exv.clear();
              }

            // Stop searching when there are no tentative paths.
            if (C.empty())
              break;

            // Take the shortest tentative path and make it the next
            // shortest path.
            A.push_back(*C.begin());
            C.erase(C.begin());
          }
      }

    return A;
  }

  template <typename Graph>
  std::list<std::pair<typename property_map<Graph, edge_weight_t>::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          optional<unsigned> K = optional<unsigned>())
  {
    return yen_ksp(g, s, t, get(edge_weight_t(), g),
                   get(vertex_index_t(), g), K);
  }

} // boost

#endif /* BOOST_GRAPH_YEN_KSP */


// Copyright (C) by Josh Blum. See LICENSE.txt for licensing information.

#include <gras/time_tag.hpp>
#include <PMC/Containers.hpp>
#include <boost/cstdint.hpp> //uint64
#include <boost/math/special_functions/round.hpp>

#define TIME_TAG_TPS time_ticks_t(1000000000UL)

using namespace gras;

static TimeTag &normalize(TimeTag &t)
{
    int num = int(t._ticks/TIME_TAG_TPS);
    if (num < 0) num--; //stops negative ticks
    t._fsecs += num;
    t._ticks -= num*TIME_TAG_TPS;
    return t;
}

TimeTag::TimeTag(void):
    _fsecs(0), _ticks(0)
{/*NOP*/}

TimeTag TimeTag::from_ticks(const time_ticks_t ticks)
{
    TimeTag t;
    t._ticks = ticks;
    return normalize(t);
}

TimeTag TimeTag::from_ticks(const time_ticks_t ticks, const double rate)
{
    TimeTag t;
    t._fsecs = time_ticks_t(ticks/rate);
    const double error = ticks - (t._fsecs*rate);
    t._ticks = boost::math::llround((error*TIME_TAG_TPS)/rate);
    return normalize(t);
}

TimeTag TimeTag::from_pmc(const PMCC &p)
{
    TimeTag t;
    const PMCTuple<2> &tuple = p.as<PMCTuple<2> >();
    t._fsecs = tuple[0].as<boost::uint64_t>();
    t._ticks = boost::math::llround(tuple[1].as<double>()*TIME_TAG_TPS);
    return normalize(t);
}

time_ticks_t TimeTag::to_ticks(void) const
{
    return _fsecs*TIME_TAG_TPS + _ticks;
}

time_ticks_t TimeTag::to_ticks(const double rate) const
{
    const time_ticks_t full = time_ticks_t(_fsecs*rate);
    const double error = _fsecs - (full/rate);
    return full + boost::math::llround(_ticks*rate/TIME_TAG_TPS + error*rate);
}

PMCC TimeTag::to_pmc(void) const
{
    PMCTuple<2> tuple;
    tuple[0] = PMC_M<boost::uint64_t>(_fsecs);
    tuple[1] = PMC_M<double>(_ticks/double(TIME_TAG_TPS));
    return PMC_M(tuple);
}

TimeTag &TimeTag::operator+=(const TimeTag &rhs)
{
    _fsecs += rhs._fsecs;
    _ticks += rhs._ticks;
    return normalize(*this);
}

TimeTag &TimeTag::operator-=(const TimeTag &rhs)
{
    _fsecs -= rhs._fsecs;
    _ticks -= rhs._ticks;
    return normalize(*this);
}

bool gras::operator<(const TimeTag &lhs, const TimeTag &rhs)
{
    if (lhs._fsecs == rhs._fsecs) return lhs._ticks < rhs._ticks;
    return lhs._fsecs < rhs._fsecs;
}

bool gras::operator==(const TimeTag &lhs, const TimeTag &rhs)
{
    return (lhs._fsecs == rhs._fsecs) and (lhs._ticks == rhs._ticks);
}