// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include <dlib/matrix.h>
#include <dlib/geometry.h>
#include <dlib/image_transforms.h>
#include <pybind11/stl_bind.h>
#include "indexing.h"

using namespace dlib;
using namespace std;

typedef matrix<double,0,1> cv;

void cv_set_size(cv& m, long s)
{
    m.set_size(s);
    m = 0;
}

double dotprod ( const cv& a, const cv& b)
{
    return dot(a,b);
}

string cv__str__(const cv& v)
{
    ostringstream sout;
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << "\n";
    }
    return sout.str();
}

string cv__repr__ (const cv& v)
{
    std::ostringstream sout;
    sout << "dlib.vector([";
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << ", ";
    }
    sout << "])";
    return sout.str();
}

std::shared_ptr<cv> cv_from_object(py::object obj)
{
    try {
        long nr = obj.cast<long>();
        auto temp = std::make_shared<cv>(nr);
        *temp = 0;
        return temp;
    } catch(py::cast_error&) {
        py::list li = obj.cast<py::list>();
        const long nr = len(obj);
        auto temp = std::make_shared<cv>(nr);
        for ( long r = 0; r < nr; ++r)
        {
            (*temp)(r) = li[r].cast<double>();
        }
        return temp;
    }
}

long cv__len__(cv& c)
{
    return c.size();
}

void cv__setitem__(cv& c, long p, double val)
{
    if (p < 0) {
        p = c.size() + p; // negative index
    }
    if (p > c.size()-1) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        throw py::error_already_set();
    }
    c(p) = val;
}

double cv__getitem__(cv& m, long r)
{
    if (r < 0) {
        r = m.size() + r; // negative index
    }
    if (r > m.size()-1 || r < 0) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        throw py::error_already_set();
    }
    return m(r);
}

cv cv__getitem2__(cv& m, py::slice r)
{
    size_t start, stop, step, slicelength;
    if (!r.compute(m.size(), &start, &stop, &step, &slicelength))
        throw py::error_already_set();

    cv temp(slicelength);

    for (size_t i = 0; i < slicelength; ++i) {
         temp(i) = m(start); start += step;
    }
    return temp;
}

py::tuple cv_get_matrix_size(cv& m)
{
    return py::make_tuple(m.nr(), m.nc());
}

// ----------------------------------------------------------------------------------------

string point_transform_projective__repr__ (const point_transform_projective& tform)
{
    std::ostringstream sout;
    sout << "point_transform_projective(\n" << csv << tform.get_m() << ")";
    return sout.str();
}

string point_transform_projective__str__(const point_transform_projective& tform)
{
    std::ostringstream sout;
    sout << "(" << csv << tform.get_m() << ")";
    return sout.str();
}

point_transform_projective init_point_transform_projective (
    const numpy_image<double>& m_
)
{
    const_image_view<numpy_image<double>> m(m_);
    DLIB_CASSERT(m.nr() == 3 && m.nc() == 3,
        "The matrix used to construct a point_transform_projective object must be 3x3.");

    return point_transform_projective(mat(m));
}

// ----------------------------------------------------------------------------------------

string point__repr__ (const point& p)
{
    std::ostringstream sout;
    sout << "point(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string point__str__(const point& p)
{
    std::ostringstream sout;
    sout << "(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string dpoint__repr__ (const dpoint& p)
{
    std::ostringstream sout;
    sout << "dpoint(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string dpoint__str__(const dpoint& p)
{
    std::ostringstream sout;
    sout << "(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

long point_x(const point& p) { return p.x(); }
long point_y(const point& p) { return p.y(); }
double dpoint_x(const dpoint& p) { return p.x(); }
double dpoint_y(const dpoint& p) { return p.y(); }

// ----------------------------------------------------------------------------------------

template <typename T>
dlib::vector<T,2> numpy_to_dlib_vect (
    const py::array_t<T>& v
)
/*!
    ensures
        - converts a numpy array with 2 elements into a dlib::vector<T,2>
!*/
{
    DLIB_CASSERT(v.size() == 2, "You can only convert a numpy array to a dlib point or dpoint if it has just 2 elements.");
    DLIB_CASSERT(v.ndim() == 1 || v.ndim() == 2, "The input needs to be interpretable as a row or column vector.");
    dpoint temp;
    if (v.ndim() == 1)
    {
        temp.x() = v.at(0);
        temp.y() = v.at(1);
    }
    else if (v.shape(0) == 2)
    {
        temp.x() = v.at(0,0);
        temp.y() = v.at(1,0);
    }
    else
    {
        temp.x() = v.at(0,0);
        temp.y() = v.at(0,1);
    }
    return temp;
}

// ----------------------------------------------------------------------------------------

point_transform_projective py_find_projective_transform (
    const std::vector<dpoint>& from_points,
    const std::vector<dpoint>& to_points
)
{
    DLIB_CASSERT(from_points.size() == to_points.size(),
        "from_points and to_points must have the same number of points.");
    DLIB_CASSERT(from_points.size() >= 4,
        "You need at least 4 points to find a projective transform.");
    return find_projective_transform(from_points, to_points);
}

template <typename T>
point_transform_projective py_find_projective_transform2 (
    const numpy_image<T>& from_points_,
    const numpy_image<T>& to_points_
)
{
    const_image_view<numpy_image<T>> from_points(from_points_);
    const_image_view<numpy_image<T>> to_points(to_points_);

    DLIB_CASSERT(from_points.nc() == 2 && to_points.nc() == 2,
        "Both from_points and to_points must be arrays with 2 columns.");
    DLIB_CASSERT(from_points.nr() == to_points.nr(),
        "from_points and to_points must have the same number of rows.");
    DLIB_CASSERT(from_points.nr() >= 4,
        "You need at least 4 rows in the input matrices to find a projective transform.");

    std::vector<dpoint> from, to;
    for (long r = 0; r < from_points.nr(); ++r)
    {
        from.push_back(dpoint(from_points[r][0], from_points[r][1]));
        to.push_back(dpoint(to_points[r][0], to_points[r][1]));
    }

    return find_projective_transform(from, to);
}

// ----------------------------------------------------------------------------------------

void register_point_transform_projective(
    py::module& m
)
{

    py::class_<point_transform_projective>(m, "point_transform_projective",
        "This is an object that takes 2D points and applies a projective transformation to them.")
            .def(py::init<>(),
"ensures \n\
    - This object will perform the identity transform.  That is, given a point \n\
      as input it will return the same point as output.  Therefore, self.m == a 3x3 identity matrix."
        /*!
            ensures
                - This object will perform the identity transform.  That is, given a point
                  as input it will return the same point as output.  Therefore, self.m == a 3x3 identity matrix.
        !*/
                )
            .def(py::init<>(&init_point_transform_projective), py::arg("m"),
"ensures \n\
    - self.m == m"
                )
            .def("__repr__", &point_transform_projective__repr__)
            .def("__str__", &point_transform_projective__str__)
            .def("__call__", [](const point_transform_projective& tform, const dpoint& p){return tform(p);}, py::arg("p"),
"ensures \n\
    - Applies the projective transformation defined by this object's constructor \n\
      to p and returns the result.  To define this precisely: \n\
        - let p_h == the point p in homogeneous coordinates.  That is: \n\
            - p_h.x == p.x \n\
            - p_h.y == p.y \n\
            - p_h.z == 1  \n\
        - let x == m*p_h  \n\
        - Then this function returns the value x/x.z"
        /*!
            ensures
                - Applies the projective transformation defined by this object's constructor
                  to p and returns the result.  To define this precisely:
                    - let p_h == the point p in homogeneous coordinates.  That is:
                        - p_h.x == p.x
                        - p_h.y == p.y
                        - p_h.z == 1
                    - let x == m*p_h
                    - Then this function returns the value x/x.z
        !*/
                )
            .def_property_readonly("m", [](const point_transform_projective& tform){numpy_image<double> tmp; assign_image(tmp,tform.get_m()); return tmp;},
                "m is the 3x3 matrix that defines the projective transformation.")
            .def(py::pickle(&getstate<point_transform_projective>, &setstate<point_transform_projective>));

    m.def("inv", [](const point_transform_projective& tform){return inv(tform); }, py::arg("trans"),
"ensures \n\
    - If trans is an invertible transformation then this function returns a new \n\
      transformation that is the inverse of trans. "
    /*!
        ensures
            - If trans is an invertible transformation then this function returns a new
              transformation that is the inverse of trans.
    !*/
        );

    m.def("find_projective_transform", &py_find_projective_transform, py::arg("from_points"), py::arg("to_points"),
"requires \n\
    - len(from_points) == len(to_points) \n\
    - len(from_points) >= 4 \n\
ensures \n\
    - returns a point_transform_projective object, T, such that for all valid i: \n\
        length(T(from_points[i]) - to_points[i]) \n\
      is minimized as often as possible.  That is, this function finds the projective \n\
      transform that maps points in from_points to points in to_points.  If no \n\
      projective transform exists which performs this mapping exactly then the one \n\
      which minimizes the mean squared error is selected. "
    /*!
        requires
            - len(from_points) == len(to_points)
            - len(from_points) >= 4
        ensures
            - returns a point_transform_projective object, T, such that for all valid i:
                length(T(from_points[i]) - to_points[i])
              is minimized as often as possible.  That is, this function finds the projective
              transform that maps points in from_points to points in to_points.  If no
              projective transform exists which performs this mapping exactly then the one
              which minimizes the mean squared error is selected.
    !*/
        );

    const char* docs =
"requires \n\
    - from_points and to_points have two columns and the same number of rows. \n\
      Moreover, they have at least 4 rows. \n\
ensures \n\
    - returns a point_transform_projective object, T, such that for all valid i: \n\
        length(T(dpoint(from_points[i])) - dpoint(to_points[i])) \n\
      is minimized as often as possible.  That is, this function finds the projective \n\
      transform that maps points in from_points to points in to_points.  If no \n\
      projective transform exists which performs this mapping exactly then the one \n\
      which minimizes the mean squared error is selected. ";
    /*!
        requires
            - from_points and to_points have two columns and the same number of rows.
              Moreover, they have at least 4 rows.
        ensures
            - returns a point_transform_projective object, T, such that for all valid i:
                length(T(dpoint(from_points[i])) - dpoint(to_points[i]))
              is minimized as often as possible.  That is, this function finds the projective
              transform that maps points in from_points to points in to_points.  If no
              projective transform exists which performs this mapping exactly then the one
              which minimizes the mean squared error is selected.
    !*/
    m.def("find_projective_transform", &py_find_projective_transform2<float>, py::arg("from_points"), py::arg("to_points"), docs);
    m.def("find_projective_transform", &py_find_projective_transform2<double>, py::arg("from_points"), py::arg("to_points"), docs);

}

// ----------------------------------------------------------------------------------------

double py_polygon_area(
    const std::vector<dpoint>& pts
)
{
    return polygon_area(pts);
}

double py_polygon_area2(
    const py::list& pts
)
{
    std::vector<dpoint> temp(len(pts));
    for (size_t i = 0; i < temp.size(); ++i)
        temp[i] = pts[i].cast<dpoint>();

    return polygon_area(temp);
}

// ----------------------------------------------------------------------------------------

void bind_vector(py::module& m)
{
    {
    py::class_<cv, std::shared_ptr<cv>>(m, "vector", "This object represents the mathematical idea of a column vector.")
        .def(py::init())
        .def("set_size", &cv_set_size)
        .def("resize", &cv_set_size)
        .def(py::init(&cv_from_object))
        .def("__repr__", &cv__repr__)
        .def("__str__", &cv__str__)
        .def("__len__", &cv__len__)
        .def("__getitem__", &cv__getitem__)
        .def("__getitem__", &cv__getitem2__)
        .def("__setitem__", &cv__setitem__)
        .def_property_readonly("shape", &cv_get_matrix_size)
        .def(py::pickle(&getstate<cv>, &setstate<cv>));

    m.def("dot", &dotprod, "Compute the dot product between two dense column vectors.");
    }
    {
    typedef point type;
    py::class_<type>(m, "point", "This object represents a single point of integer coordinates that maps directly to a dlib::point.")
            .def(py::init<long,long>(), py::arg("x"), py::arg("y"))
            .def(py::init<dpoint>(), py::arg("p"))
            .def(py::init<>(&numpy_to_dlib_vect<long>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<float>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<double>), py::arg("v"))
            .def("__repr__", &point__repr__)
            .def("__str__", &point__str__)
            .def(py::self + py::self)
            .def(py::self - py::self)
            .def(py::self / double())
            .def(py::self * double())
            .def(double() * py::self)
            .def("normalize", &type::normalize, "Returns a unit normalized copy of this vector.")
            .def_property("x", &point_x, [](point& p, long x){p.x()=x;}, "The x-coordinate of the point.")
            .def_property("y", &point_y, [](point& p, long y){p.y()=y;}, "The y-coordinate of the point.")
            .def(py::pickle(&getstate<type>, &setstate<type>));
    }
    {
    typedef std::vector<point> type;
    py::bind_vector<type>(m, "points", "An array of point objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<point>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    {
    typedef dpoint type;
    py::class_<type>(m, "dpoint", "This object represents a single point of floating point coordinates that maps directly to a dlib::dpoint.")
            .def(py::init<double,double>(), py::arg("x"), py::arg("y"))
            .def(py::init<point>(), py::arg("p"))
            .def(py::init<>(&numpy_to_dlib_vect<long>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<float>), py::arg("v"))
            .def(py::init<>(&numpy_to_dlib_vect<double>), py::arg("v"))
            .def("__repr__", &dpoint__repr__)
            .def("__str__", &dpoint__str__)
            .def("normalize", &type::normalize, "Returns a unit normalized copy of this vector.")
            .def_property("x", &dpoint_x, [](dpoint& p, double x){p.x()=x;}, "The x-coordinate of the dpoint.")
            .def_property("y", &dpoint_y, [](dpoint& p, double y){p.y()=y;}, "The y-coordinate of the dpoint.")
            .def(py::self + py::self)
            .def(py::self - py::self)
            .def(py::self / double())
            .def(py::self * double())
            .def(double() * py::self)
            .def(py::pickle(&getstate<type>, &setstate<type>));
    }
    {
    typedef std::vector<dpoint> type;
    py::bind_vector<type>(m, "dpoints", "An array of dpoint objects.")
        .def(py::init<size_t>(), py::arg("initial_size"))
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def("extend", extend_vector_with_python_list<dpoint>)
        .def(py::pickle(&getstate<type>, &setstate<type>));
    }

    m.def("length", [](const point& p){return length(p); },
        "returns the distance from p to the origin, i.e. the L2 norm of p.", py::arg("p"));
    m.def("length", [](const dpoint& p){return length(p); },
        "returns the distance from p to the origin, i.e. the L2 norm of p.", py::arg("p"));

    m.def("dot", [](const point& a, const point& b){return dot(a,b); },  "Returns the dot product of the points a and b.", py::arg("a"), py::arg("b"));
    m.def("dot", [](const dpoint& a, const dpoint& b){return dot(a,b); },  "Returns the dot product of the points a and b.", py::arg("a"), py::arg("b"));

    register_point_transform_projective(m);

    m.def("polygon_area", &py_polygon_area, py::arg("pts"));
    m.def("polygon_area", &py_polygon_area2, py::arg("pts"),
"ensures \n\
    - If you walk the points pts in order to make a closed polygon, what is its \n\
      area?  This function returns that area.  It uses the shoelace formula to \n\
      compute the result and so works for general non-self-intersecting polygons."
    /*!
        ensures
            - If you walk the points pts in order to make a closed polygon, what is its
              area?  This function returns that area.  It uses the shoelace formula to
              compute the result and so works for general non-self-intersecting polygons.
    !*/
        );

}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussKronrodIntegrator.hpp
//! \author Luke Kersting
//! \brief  Gauss-Kronrod integrator
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP
#define UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP

// std Includes
#include <queue>

// Boost Includes
#include <boost/numeric/odeint.hpp>

// FRENSIE Includes
#include "Utility_Vector.hpp"
#include "Utility_ArrayView.hpp"

namespace Utility{

template<typename T>
struct BinTraits
{
  T lower_limit;
  T upper_limit;
  T result;
  T error;

  bool operator <( const BinTraits<T>& bins ) const
  {
    return error < bins.error;
  }
};

template<typename T>
struct ExtrapolatedBinTraits
{
  T lower_limit;
  T upper_limit;
  T result;
  T error;
  int level;

  bool operator <( const ExtrapolatedBinTraits<T>& bins ) const
  {
    return error < bins.error;
  }
};

//! The Gauss-Kronrod integrator
template<typename T>
class GaussKronrodIntegrator
{

public:

typedef std::priority_queue<BinTraits<T>> BinQueue;
typedef std::vector<ExtrapolatedBinTraits<T>> BinArray;

  //! Constructor
  GaussKronrodIntegrator( const T relative_error_tol,
                          const T absolute_error_tol = 0.0,
                          const size_t subinterval_limit = 1000 );

  //! Destructor
  ~GaussKronrodIntegrator()
  { /* ... */ }

  //! Throw exception on dirty integration
  void throwExceptionOnDirtyIntegration();

  //! Check if an exception will be thrown on dirty integration
  bool isExceptionThrownOnDirtyIntegration() const;

  //! Warn on dirty integration (default)
  void warnOnDirtyIntegration();

  //! Use heuristic roundoff error estimator (default)
  void estimateRoundoff();

  //! Don't use heuristic roundoff error estimator
  void dontEstimateRoundoff();

  //! Set the realtive error tolerance
  void setRelativeErrorTolerance( const double relative_error_tol );

  //! Get the relative error tolerance
  double getRelativeErrorTolerance() const;

  //! Set the absolute error tolerance
  void setAbsoluteErrorTolerance( const double absolute_error_tol );

  //! Get the absolute error tolerance
  double getAbsoluteErrorTolerance() const;

/*
  //! Integrate the function
  template<typename Functor>
  void integrate( Functor& integrand,
		  T lower_limit,
		  T upper_limit,
		  T& result,
		  T& absolute_error,
		  size_t& number_of_function_eval ) const;
*/
  //! Integrate the function adaptively with BinQueue
  template<int Points, typename FunctorType = double, typename Functor>
  void integrateAdaptively( Functor& integrand,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error ) const;

  //! Integrate the function adaptively with BinQueue
  template<int Points, typename FunctorType = double,
                       typename ParameterType = double, typename Functor>
  void integrateAdaptively( Functor& integrand,
                            ParameterType integrand_parameter,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error ) const;
/*
  //! Integrate the function over a semi-infinite interval (+infinity)
  template<typename Functor>
  void integrateSemiInfiniteIntervalUpper( Functor& integrand,
					   T lower_limit,
					   T& result,
					   T& absolute_error ) const;

  //! Integrate the function over a semi-infinite interval (-infinity)
  template<typename Functor>
  void integrateSemiInfiniteIntervalLower( Functor& integrand,
					   T upper_limit,
					   T& result,
					   T& absolute_error ) const;

  //! Integrate the function over an infinite interval (-infinity,+infinity)
  template<typename Functor>
  void integrateInfiniteInterval( Functor& integrand,
				  T& result,
				  T& absolute_error ) const;

  //! Integrate a function with integrable singularities adaptively
  template<typename Functor>
  void integrateAdaptivelyWynnEpsilon( Functor& integrand,
				       T lower_limit,
				       T upper_limit,
				       T& result,
				       T& absolute_error ) const;
*/
  //! Integrate a function with known integrable singularities adaptively
  template<typename FunctorType = double, typename Functor>
  void integrateAdaptivelyWynnEpsilon(
			  Functor& integrand,
			  const Utility::ArrayView<T>& points_of_interest,
			  T& result,
			  T& absolute_error ) const;

  //! Integrate a function with known integrable singularities adaptively
  template<typename FunctorType = double, typename ParameterType = double, typename Functor>
  void integrateAdaptivelyWynnEpsilon(
			  Functor& integrand,
                          ParameterType integrand_parameter,
			  const Utility::ArrayView<T>& points_of_interest,
			  T& result,
			  T& absolute_error ) const;

  //! Integrate the function with point rule
  template<int Points, typename FunctorType = double, typename Functor>
  void integrateWithPointRule(
                Functor& integrand,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error,
                T& result_abs,
                T& result_asc ) const;

  //! Integrate the function with point rule
  template<int Points, typename FunctorType = double,
                       typename ParameterType = double, typename Functor>
  void integrateWithPointRule(
                Functor& integrand,
                ParameterType integrand_parameter,
			    T lower_limit,
			    T upper_limit,
			    T& result,
			    T& absolute_error,
                T& result_abs,
                T& result_asc ) const;

protected:

  // Calculate the quadrature upper and lower integrand values at an abscissa
  template<typename FunctorType = double, typename Functor>
  void calculateQuadratureIntegrandValuesAtAbscissa(
    Functor& integrand,
    T abscissa,
    T half_length,
    T midpoint,
    T& integrand_value_lower,
    T& integrand_value_upper ) const;

  // Calculate the quadrature upper and lower integrand values at an abscissa
  template<typename FunctorType = double, typename ParameterType = double, typename Functor>
  void calculateQuadratureIntegrandValuesAtAbscissa(
    Functor& integrand,
    ParameterType functor_parameter,
    T abscissa,
    T half_length,
    T midpoint,
    T& integrand_value_lower,
    T& integrand_value_upper ) const;

  // Bisect and integrate the given bin interval
  template<int Points, typename FunctorType = double, typename Functor, typename Bin>
  void bisectAndIntegrateBinInterval(
    Functor& integrand,
    const Bin& bin,
    Bin& bin_1,
    Bin& bin_2,
    T& bin_1_asc,
    T& bin_2_asc ) const;

  // Bisect and integrate the given bin interval
  template<int Points, typename FunctorType = double, typename ParameterType = double, typename Functor, typename Bin>
  void bisectAndIntegrateBinInterval(
    Functor& integrand,
    ParameterType functor_parameter,
    const Bin& bin,
    Bin& bin_1,
    Bin& bin_2,
    T& bin_1_asc,
    T& bin_2_asc ) const;

  // Rescale absolute error from integration
  void rescaleAbsoluteError(
    T& absolute_error,
    T result_abs,
    T result_asc ) const;

  // Test if subinterval is too small
  template<int Points>
  bool subintervalTooSmall( T& lower_limit_1,
                                   T& lower_limit_2,
                                   T& upper_limit_2 ) const;

  // check the roundoff error
  void checkRoundoffError(
                       const BinTraits<T>& bin,
                       const BinTraits<T>& bin_1,
                       const BinTraits<T>& bin_2,
                       const T& bin_1_asc,
                       const T& bin_2_asc,
                       int& round_off_1,
                       int& round_off_2,
                       const int number_of_iterations ) const;

  // check the roundoff error
  void checkRoundoffError(
                       const ExtrapolatedBinTraits<T>& bin,
                       const ExtrapolatedBinTraits<T>& bin_1,
                       const ExtrapolatedBinTraits<T>& bin_2,
                       const T& bin_1_asc,
                       const T& bin_2_asc,
                       int& round_off_1,
                       int& round_off_2,
                       int& round_off_3,
                       const bool extrapolate,
                       const int number_of_iterations ) const;

  // Sort the bin order from highest to lowest error
  void sortBins(
        std::vector<int>& bin_order,
        BinArray& bin_array,
        const ExtrapolatedBinTraits<T>& bin_1,
        const ExtrapolatedBinTraits<T>& bin_2,
        const int& number_of_intervals,
        int& nr_max ) const;

  // get the Wynn Epsilon-Algorithm extrapolated value
  void getWynnEpsilonAlgorithmExtrapolation(
        std::vector<T>& bin_extrapolated_result,
        std::vector<T>& last_three_results,
        T& extrapolated_result,
        T& extrapolated_error,
        int& number_of_extrapolated_intervals,
        int& number_of_extrapolated_calls  ) const;

private:
  // The relative error tolerance
  T d_relative_error_tol;

  // The absolute error tolerance
  T d_absolute_error_tol;

  // The subinterval limit
  size_t d_subinterval_limit;

  // Throw exception on dirty integration
  bool d_throw_exceptions;

  // Estimate roundoff error and throw exceptions/warnings
  bool d_estimate_roundoff;

  // return epsilon numerical limit for type T
  T getLimitEpsilon() const;

  // return min numerical limit for type T
  T getLimitMin() const;

  // return max numerical limit for type T
  T getLimitMax() const;

  // return max of two variables of type T
  T getMax( T variable_1, T variable_2 ) const;
};

} // end Utility namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_GaussKronrodIntegrator_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_GAUSS_KRONROD_INTEGRATOR_HPP

//---------------------------------------------------------------------------//
// end Utility_GaussKronrodIntegrator.hpp
//---------------------------------------------------------------------------//


#ifndef MIRRORPLASMA_HPP
#define MIRRORPLASMA_HPP

#include "PlasmaPhysics.hpp"
#include "Species.hpp"
#include "NetCDFIO.hpp"
#include <toml.hpp>

#include <boost/math/interpolators/barycentric_rational.hpp>

#include <memory>
#include <cmath>

class MirrorPlasma {
	public:
		class VacuumMirrorConfiguration {
			public:
				VacuumMirrorConfiguration( toml::value const& );
				/*
				VacuumMirrorConfiguration( const& VacuumMirrorConfiguration other ) :
					IonSpecies( other.IonSpecies )
				{
					MirrorRatio = other.MirrorRatio;
					PlasmaColumnWidth = other.PlasmaColumnWidth;
					PlasmaLength = other.PlasmaLength;
					AxialGapDistance = other.AxialGapDistance;
					WallRadius = other.WallRadius;
					CentralCellFieldStrength = other.CentralCellFieldStrength;
					AuxiliaryHeating = other.AuxiliaryHeating;
				}
				*/
				Species_t IonSpecies;
				double MirrorRatio;
				double PlasmaColumnWidth;
				double PlasmaLength;
				double AxialGapDistance;
				double WallRadius;
				double CentralCellFieldStrength;
				double AuxiliaryHeating;

				double ParallelFudgeFactor;
				double PerpFudgeFactor;
				bool AmbipolarPhi;

				bool AlphaHeating;
				bool ReportNuclearDiagnostics;
				bool ReportThrust;

				double InitialTemp;
				double InitialMach;
				bool Collisional;

				std::string OutputFile;
				std::string NetcdfOutputFile;

				double PlasmaVolume() const {
					return M_PI * ( PlasmaColumnWidth + 2 * AxialGapDistance ) * PlasmaColumnWidth * PlasmaLength;
				};
				double WallArea() const {
					return 2.0 * M_PI * WallRadius * WallRadius * PlasmaLength;
				};

				double ImposedVoltage;
				double PlasmaInnerRadius() const { return AxialGapDistance; };
				double PlasmaOuterRadius() const { return AxialGapDistance + PlasmaColumnWidth; };
				double PlasmaCentralRadius() const { return AxialGapDistance + PlasmaColumnWidth / 2.0; };
			private:

		};

		// Copy Constructor
		/*
		MirrorPlasma( MirrorPlasma const& other ) :
			pVacuumConfig( other.pVacuumConfig )
		{
			FuellingRate = other.FuellingRate;
			IonDensity = other.IonDensity;
			IonTemperature = other.IonTemperature;
			ElectronDensity = other.ElectronDensity;
			ElectronTemperature = other.ElectronTemperature;
		   NeutralDensity = other.NeutralDensity;
			NeutralSource = other.NeutralSource;
			Zeff = other.Zeff;
		   MachNumber = other.MachNumber;
		};
		*/

		MirrorPlasma( MirrorPlasma const& ) = delete;

		MirrorPlasma( toml::value const& configSection );

		double FuellingRate;
		double IonDensity,IonTemperature;
		double ElectronDensity,ElectronTemperature;
		double NeutralDensity,NeutralSource;
		double Zeff;

		double MachNumber; // Sonic Mach number defined with c_s^2 = Z T_e/m_i

		double SoundSpeed() const {
			// We *define* c_s^2 = Z_i T_e / m_i.
			double cs = ::sqrt( pVacuumConfig->IonSpecies.Charge * ElectronTemperature*ReferenceTemperature / ( pVacuumConfig->IonSpecies.Mass * ProtonMass ) );
			return cs;
		};

		// Defining (3/2) d( n_i T_i )/dt = IonHeating - IonHeatLosses
		//
		double ElectronHeatLosses() const;
		double IonHeatLosses() const;
		double ElectronHeating() const;
		double IonHeating() const;

		double EnergyConfinementTime() const {
			double StoredEnergy = 1.5 * ( ElectronDensity * ElectronTemperature + IonDensity * IonTemperature ) * ReferenceDensity * ReferenceTemperature;
			double TotalHeatLosses = ElectronHeatLosses() + IonHeatLosses();
			return StoredEnergy / TotalHeatLosses;
		};

		// RHS of dn/dt = <stuff>
		double IonParticleLosses() const;
		double ElectronParticleLosses() const;

		double ElectricPotential() const {
			return pVacuumConfig->CentralCellFieldStrength * MachNumber * SoundSpeed() * pVacuumConfig->PlasmaColumnWidth;
		};
		double IonLarmorRadius() const {
			return 1.02 * ::sqrt( pVacuumConfig->IonSpecies.Mass * IonTemperature * 1000 ) / ( pVacuumConfig->IonSpecies.Charge * pVacuumConfig->CentralCellFieldStrength * 10000 );
		};

		double Beta() const;
		double DebyeLength() const;

		double NuStar() const;
		double KineticEnergy() const;
		double ThermalEnergy() const;

		void PrintReport();

		void InitialiseNetCDF();
		void WriteTimeslice( double T );
		void FinaliseNetCDF();

		std::shared_ptr< VacuumMirrorConfiguration > pVacuumConfig;

		void SetMachFromVoltage();
		double AmbipolarPhi() const;

		void ComputeSteadyStateNeutrals();

		double ParallelMomentumLossRate() const;
		double initialTemperature() const { return pVacuumConfig->InitialTemp; };
		double initialMach() const { return pVacuumConfig->InitialMach; };
	private:
		NetCDFIO nc_output;

		double LogLambdaElectron() const;
		double LogLambdaIon() const;
		double LogLambdaAlphaElectron() const;

		double ElectronCollisionTime() const;
		double IonCollisionTime() const;
		double CollisionalTemperatureEquilibrationTime() const;

		double SlowingDownTime() const;

		double IonToElectronHeatTransfer() const;

		double BremsstrahlungLosses() const;
		double NeutralLosses() const;
		double ClassicalHeatLosses() const;
		double ParallelHeatLosses() const;

		double ParallelElectronPastukhovLossRate( double Phi ) const;
		double ParallelElectronParticleLoss() const;
		double ParallelElectronHeatLoss() const;

		double Chi_i( double ) const;
		double Chi_i() const;

		double ParallelIonPastukhovLossRate( double Phi ) const;
		double ParallelIonParticleLoss() const;
		double ParallelIonHeatLoss() const;

		double ParallelKineticEnergyLoss() const;

		double ClassicalIonHeatLoss() const;
		double ClassicalElectronHeatLoss() const;

		double ClassicalElectronParticleLosses() const;
		double ClassicalIonParticleLosses() const;

		double ClassicalViscosity() const;
		double AlfvenMachNumber() const;

		double ViscousHeating() const;

		double CentrifugalPotential() const;

		double IonCyclotronFrequency() const
		{
			double MagneticField = pVacuumConfig->CentralCellFieldStrength;
			return pVacuumConfig->IonSpecies.Charge * ElectronCharge * MagneticField / ( pVacuumConfig->IonSpecies.Mass * ProtonMass );
		};

		double ElectronCyclotronFrequency() const
		{
			double MagneticField = pVacuumConfig->CentralCellFieldStrength;
			return ElectronCharge * MagneticField / ElectronMass;
		};

		double FusionAlphaPowerDensity() const;
		double AlphaProductionRate() const;
		double NeutronOutput() const;
		double ThermalPowerOutput() const;
		double NeutronWallLoading() const;
		double DDNeutronRate() const;

		using interpolant = boost::math::barycentric_rational<double>;
		std::unique_ptr<interpolant> VoltageFunction;
		void ReadVoltageFile( std::string const& );
		double time;
		bool isTimeDependent;
	public:
		double AlphaHeating() const;
		double PromptAlphaLossFraction() const;
		double PromptAlphaThrust() const;
		double AlphaPromptLosses() const;
		double AlphaParallelLossRate() const;
		double ViscousTorque() const;
		double ParallelAngularMomentumLossRate() const;
		double RadialCurrent() const;
		double ParallelIonThrust() const;
		double ParallelCurrent(double) const;
		void UpdateVoltage();
		void SetTime( double );
};

#endif // MIRRORPLASMA_HPP


/*
 * DiscretizedFunction.cpp
 *
 *  Created on: 16.06.2017
 *      Author: thies
 */

#include <deal.II/base/data_out_base.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>

#include <base/DiscretizedFunction.h>

using namespace dealii;

namespace wavepi {
namespace base {

inline double square(const double x) { return x * x; }

inline double pow4(const double x) { return x * x * x * x; }

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm,
                                              bool store_derivative)
    : mesh(mesh), norm_(norm), store_derivative(store_derivative), cur_time_idx(0) {
  Assert(mesh && norm, ExcNotInitialized());

  function_coefficients.reserve(mesh->length());

  if (store_derivative) derivative_coefficients.reserve(mesh->length());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients.emplace_back(mesh->n_dofs(i));

    if (store_derivative) derivative_coefficients.emplace_back(mesh->n_dofs(i));
  }
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm)
    : DiscretizedFunction(mesh, norm, false) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh)
    : DiscretizedFunction(mesh, std::make_shared<InvalidNorm<DiscretizedFunction<dim>>>()) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh, Function<dim>& function,
                                              std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm)
    : mesh(mesh), norm_(norm), store_derivative(false), cur_time_idx(0) {
  Assert(mesh && norm, ExcNotInitialized());

  function_coefficients.reserve(mesh->length());

  for (size_t i = 0; i < mesh->length(); i++) {
    auto dof_handler = mesh->get_dof_handler(i);

    Vector<double> tmp(dof_handler->n_dofs());
    function.set_time(mesh->get_time(i));

    VectorTools::interpolate(*dof_handler, function, tmp);
    mesh->get_constraint_matrix(i)->distribute(tmp);

    function_coefficients.push_back(std::move(tmp));
  }
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(std::shared_ptr<SpaceTimeMesh<dim>> mesh, Function<dim>& function)
    : DiscretizedFunction(mesh, function, std::make_shared<InvalidNorm<DiscretizedFunction<dim>>>()) {}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(DiscretizedFunction<dim>&& o)
    : LightFunction<dim>(),
      mesh(std::move(o.mesh)),
      norm_(o.norm_),
      store_derivative(o.store_derivative),
      cur_time_idx(o.cur_time_idx),
      function_coefficients(std::move(o.function_coefficients)),
      derivative_coefficients(std::move(o.derivative_coefficients)) {
  Assert(mesh, ExcNotInitialized());

  o.mesh = std::shared_ptr<SpaceTimeMesh<dim>>();
}

template <int dim>
DiscretizedFunction<dim>::DiscretizedFunction(const DiscretizedFunction<dim>& o)
    : LightFunction<dim>(),
      mesh(o.mesh),
      norm_(o.norm_),
      store_derivative(o.store_derivative),
      cur_time_idx(o.cur_time_idx),
      function_coefficients(o.function_coefficients),
      derivative_coefficients(o.derivative_coefficients) {
  Assert(mesh, ExcNotInitialized());
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(DiscretizedFunction<dim>&& o) {
  mesh                    = std::move(o.mesh);
  norm_                   = o.norm_;
  store_derivative        = o.store_derivative;
  cur_time_idx            = o.cur_time_idx;
  function_coefficients   = std::move(o.function_coefficients);
  derivative_coefficients = std::move(o.derivative_coefficients);

  o.mesh = std::shared_ptr<SpaceTimeMesh<dim>>();

  Assert(mesh, ExcNotInitialized());
  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(const DiscretizedFunction<dim>& o) {
  mesh                    = o.mesh;
  norm_                   = o.norm_;
  store_derivative        = o.store_derivative;
  cur_time_idx            = o.cur_time_idx;
  function_coefficients   = o.function_coefficients;
  derivative_coefficients = o.derivative_coefficients;

  Assert(mesh, ExcNotInitialized());
  return *this;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::derivative() const {
  AssertThrow(store_derivative, ExcInternalError());
  AssertThrow(mesh, ExcNotInitialized());

  DiscretizedFunction<dim> result(mesh);
  result.function_coefficients = this->derivative_coefficients;

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_derivative() const {
  AssertThrow(mesh, ExcNotInitialized());
  AssertThrow(mesh->length() > 1, ExcInternalError());
  // AssertThrow(!store_derivative, ExcInternalError()); // why would you want to calculate it in this case?

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   if (i < mesh->length() - 1)
   AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(),
   ExcNotImplemented());

   if (i == 0) {
   result.function_coefficients[i] = function_coefficients[i + 1];
   result.function_coefficients[i] -= function_coefficients[i];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i);
   } else if (i == mesh->length() - 1) {
   result.function_coefficients[i] = function_coefficients[i];
   result.function_coefficients[i] -= function_coefficients[i - 1];
   result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
   } else {
   result.function_coefficients[i] = function_coefficients[i + 1];
   result.function_coefficients[i] -= function_coefficients[i - 1];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i - 1);
   }
   }
   */

  /* naive, but working implementation for non-constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   if (i == 0) {
   Vector<double> next_coefficients = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, { &next_coefficients });

   result.function_coefficients[i] = next_coefficients;
   result.function_coefficients[i] -= function_coefficients[i];
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i);
   } else if (i == mesh->length() - 1) {
   Vector<double> last_coefficients = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, { &last_coefficients });

   result.function_coefficients[i] = function_coefficients[i];
   result.function_coefficients[i] -= last_coefficients;
   result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
   } else {
   Vector<double> last_coefficients = function_coefficients[i - 1];
   Vector<double> next_coefficients = function_coefficients[i + 1];

   mesh->transfer(i - 1, i, { &last_coefficients });
   mesh->transfer(i + 1, i, { &next_coefficients });

   result.function_coefficients[i] = next_coefficients;
   result.function_coefficients[i] -= last_coefficients;
   result.function_coefficients[i] /= mesh->get_time(i + 1) - mesh->get_time(i - 1);
   }
   }
   */

  /* better: forward- and backward sweep */
  // forward sweep
  for (size_t i = 0; i < mesh->length(); i++) {
    if (i == 0) {
      result.function_coefficients[i].equ(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
    } else if (i == mesh->length() - 1) {
      Vector<double> last_coefficients = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&last_coefficients});

      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i] -= last_coefficients;
      result.function_coefficients[i] /= mesh->get_time(i) - mesh->get_time(i - 1);
    } else {
      Vector<double> last_coefficients = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&last_coefficients});

      result.function_coefficients[i].equ(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i - 1)), last_coefficients);
    }
  }

  // backward sweep
  for (size_t j = 0; j < mesh->length(); j++) {
    size_t i = mesh->length() - 1 - j;

    if (i == 0) {
      Vector<double> next_coefficients = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&next_coefficients});

      result.function_coefficients[i].add(1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), next_coefficients);
    } else if (i == mesh->length() - 1) {
      // nothing to be done
    } else {
      Vector<double> next_coefficients = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&next_coefficients});

      result.function_coefficients[i].add(1.0 / (mesh->get_time(i + 1) - mesh->get_time(i - 1)), next_coefficients);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_second_derivative() const {
  AssertThrow(mesh, ExcNotInitialized());
  AssertThrow(mesh->length() > 1, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */

  for (size_t i = 0; i < mesh->length(); i++) {
    if (i < mesh->length() - 1)
      AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(), ExcNotImplemented());

    if (i == 0) {
      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i].add(-2, function_coefficients[i + 1]);
      result.function_coefficients[i].add(1, function_coefficients[i + 2]);
      result.function_coefficients[i] /= square(mesh->get_time(i + 1) - mesh->get_time(i));
    } else if (i == mesh->length() - 1) {
      result.function_coefficients[i] = function_coefficients[i];
      result.function_coefficients[i].add(-2, function_coefficients[i - 1]);
      result.function_coefficients[i].add(1, function_coefficients[i - 2]);
      result.function_coefficients[i] /= square(mesh->get_time(i) - mesh->get_time(i - 1));
    } else {
      result.function_coefficients[i].equ(-2, function_coefficients[i]);
      result.function_coefficients[i].add(1, function_coefficients[i + 1]);
      result.function_coefficients[i].add(1, function_coefficients[i - 1]);
      result.function_coefficients[i] /= square(mesh->get_time(i + 1) - mesh->get_time(i - 1)) / 4;
    }
  }

  return result;
}

template <int dim>
double DiscretizedFunction<dim>::absolute_error(DiscretizedFunction<dim>& other, double* norm_out) const {
  LogStream::Prefix p("calculate_error");
  AssertThrow(other.mesh == mesh, ExcInternalError());

  DiscretizedFunction<dim> tmp = other;
  tmp.set_norm(norm_);

  if (norm_out) *norm_out = tmp.norm();

  tmp -= *this;

  return tmp.norm();
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_derivative_transpose() const {
  AssertThrow(mesh, ExcNotInitialized());

  // because of the special cases
  AssertThrow(mesh->length() > 3, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */
  /*
   for (size_t i = 0; i < mesh->length(); i++) {
   auto dest = &result.function_coefficients[i];

   if (i < mesh->length() - 1)
   AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(),
   ExcNotImplemented());

   if (i == 0) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   -1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
   } else if (i == 1) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i - 1]);
   } else if (i == mesh->length() - 1) {
   *dest = function_coefficients[i];
   dest->sadd(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   } else if (i == mesh->length() - 2) {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   } else {
   *dest = function_coefficients[i + 1];
   dest->sadd(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)),
   1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
   }
   }
   */

  /* naive, but working implementation for non-constant mesh */
  /*for (size_t i = 0; i < mesh->length(); i++) {
   auto& dest = result.function_coefficients[i];

   if (i == 0) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
   } else if (i == 1) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), tmp);
   } else if (i == mesh->length() - 1) {
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);

   Vector<double> tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, &tmp);
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   } else if (i == mesh->length() - 2) {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   } else {
   Vector<double> tmp = function_coefficients[i + 1];
   mesh->transfer(i + 1, i, {&tmp});
   dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);

   tmp = function_coefficients[i - 1];
   mesh->transfer(i - 1, i, {&tmp});
   dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
   }
   }
   */

  /* better: forward- and backward sweep */
  // forward sweep
  for (size_t i = 0; i < mesh->length(); i++) {
    auto& dest = result.function_coefficients[i];

    if (i == 0) {
      dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
    } else if (i == 1) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), tmp);
    } else if (i == mesh->length() - 1) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);

      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);
    } else if (i == mesh->length() - 2) {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
    } else {
      Vector<double> tmp = function_coefficients[i - 1];
      mesh->transfer(i - 1, i, {&tmp});
      dest.add(1.0 / (mesh->get_time(i) - mesh->get_time(i - 2)), tmp);
    }
  }

  // backward sweep
  for (size_t j = 0; j < mesh->length(); j++) {
    size_t i   = mesh->length() - 1 - j;
    auto& dest = result.function_coefficients[i];

    if (i == 0) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    } else if (i == 1) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    } else if (i == mesh->length() - 1) {
      // nothing to be done
    } else if (i == mesh->length() - 2) {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 1) - mesh->get_time(i)), tmp);
    } else {
      Vector<double> tmp = function_coefficients[i + 1];
      mesh->transfer(i + 1, i, {&tmp});
      dest.add(-1.0 / (mesh->get_time(i + 2) - mesh->get_time(i)), tmp);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::calculate_second_derivative_transpose() const {
  AssertThrow(mesh, ExcNotInitialized());

  // because of the special cases
  AssertThrow(mesh->length() > 3, ExcInternalError());

  DiscretizedFunction<dim> result(mesh, norm_);

  /* implementation for constant mesh */

  for (size_t i = 0; i < mesh->length(); i++) {
    auto dest = &result.function_coefficients[i];

    if (i < mesh->length() - 1)
      AssertThrow(function_coefficients[i + 1].size() == function_coefficients[i].size(), ExcNotImplemented());

    if (i == 0) {
      dest->equ(1.0 / square(mesh->get_time(i + 1) - mesh->get_time(i)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    } else if (i == 1) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(-2.0 / square(mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    } else if (i == 2) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
      dest->add(1.0 / square(mesh->get_time(i - 2) - mesh->get_time(i - 1)), function_coefficients[i - 2]);
    } else if (i == mesh->length() - 1) {
      dest->equ(1.0 / square(mesh->get_time(i) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
    } else if (i == mesh->length() - 2) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(-2.0 / square(mesh->get_time(i) - mesh->get_time(i + 1)), function_coefficients[i + 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i - 2) - mesh->get_time(i)), function_coefficients[i - 1]);
    } else if (i == mesh->length() - 3) {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
      dest->add(1.0 / square(mesh->get_time(i + 2) - mesh->get_time(i + 1)), function_coefficients[i + 2]);
    } else {
      dest->equ(-2.0 * 4 / square(mesh->get_time(i + 1) - mesh->get_time(i - 1)), function_coefficients[i]);
      dest->add(1.0 * 4 / square(mesh->get_time(i) - mesh->get_time(i - 2)), function_coefficients[i - 1]);
      dest->add(1.0 * 4 / square(mesh->get_time(i + 2) - mesh->get_time(i)), function_coefficients[i + 1]);
    }
  }

  return result;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator=(double x) {
  AssertThrow(mesh, ExcNotInitialized());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i] = x;

    if (store_derivative) derivative_coefficients[i] = 0.0;
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator+=(double offset) {
  // note that this is correct independent of store_derivative.

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i].add(offset);
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator+=(const DiscretizedFunction<dim>& V) {
  AssertThrow(norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::operator+= : Norms not compatible"));
  this->add(1.0, V);

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator-=(const DiscretizedFunction<dim>& V) {
  AssertThrow(norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::operator-= : Norms not compatible"));
  this->add(-1.0, V);

  return *this;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator*=(const double factor) {
  AssertThrow(mesh, ExcNotInitialized());

  for (size_t i = 0; i < mesh->length(); i++) {
    function_coefficients[i] *= factor;

    if (store_derivative) derivative_coefficients[i] *= factor;
  }

  return *this;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(const DiscretizedFunction<dim>& like) {
  Assert(!like.store_derivative, ExcInternalError());

  DiscretizedFunction<dim> res = noise(like.mesh);
  res.set_norm(like.get_norm());

  return res;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  Assert(mesh, ExcNotInitialized());

  DiscretizedFunction<dim> res(mesh);

  // auto time = std::chrono::high_resolution_clock::now();
  // std::default_random_engine generator(time.time_since_epoch().count() % 1000000);
  std::default_random_engine generator(2307);
  std::uniform_real_distribution<double> distribution(-1, 1);

  for (size_t i = 0; i < res.mesh->length(); i++)
    for (size_t j = 0; j < res.function_coefficients[i].size(); j++)
      res.function_coefficients[i][j] = distribution(generator);

  return res;
}

template <int dim>
DiscretizedFunction<dim> DiscretizedFunction<dim>::noise(const DiscretizedFunction<dim>& like, double norm) {
  DiscretizedFunction<dim> result = noise(like);

  result *= norm / result.norm();

  return result;
}

template <int dim>
DiscretizedFunction<dim>& DiscretizedFunction<dim>::operator/=(const double factor) {
  return this->operator*=(1.0 / factor);
}

template <int dim>
void DiscretizedFunction<dim>::pointwise_multiplication(const DiscretizedFunction<dim>& V) {
  Assert(mesh, ExcNotInitialized());
  Assert(mesh == V.mesh, ExcInternalError());
  Assert(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].scale(V.function_coefficients[i]);

      Vector<double> tmp = function_coefficients[i];
      tmp.scale(V.derivative_coefficients[i]);

      derivative_coefficients[i] += tmp;
    }

    function_coefficients[i].scale(V.function_coefficients[i]);
  }
}

template <int dim>
void DiscretizedFunction<dim>::add(const double a, const DiscretizedFunction<dim>& V) {
  AssertThrow(mesh && norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(mesh == V.mesh, ExcInternalError());
  AssertThrow(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::add : Norms not compatible"));

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    function_coefficients[i].add(a, V.function_coefficients[i]);

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].add(a, V.derivative_coefficients[i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::sadd(const double s, const double a, const DiscretizedFunction<dim>& V) {
  AssertThrow(mesh && norm_ && V.norm_, ExcNotInitialized());
  AssertThrow(mesh == V.mesh, ExcInternalError());
  AssertThrow(!store_derivative || (store_derivative == V.store_derivative), ExcInternalError());
  AssertThrow(*norm_ == *V.norm_, ExcMessage("DiscretizedFunction<dim>::sadd : Norms not compatible"));

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(function_coefficients[i].size() == V.function_coefficients[i].size(),
           ExcDimensionMismatch(function_coefficients[i].size(), V.function_coefficients[i].size()));

    function_coefficients[i].sadd(s, a, V.function_coefficients[i]);

    if (store_derivative) {
      Assert(derivative_coefficients[i].size() == V.derivative_coefficients[i].size(),
             ExcDimensionMismatch(derivative_coefficients[i].size(), V.derivative_coefficients[i].size()));

      derivative_coefficients[i].sadd(s, a, V.derivative_coefficients[i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::throw_away_derivative() {
  store_derivative        = false;
  derivative_coefficients = std::vector<Vector<double>>();
}

template <int dim>
double DiscretizedFunction<dim>::norm() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->norm(*this);
}

template <int dim>
double DiscretizedFunction<dim>::operator*(const DiscretizedFunction<dim>& V) const {
  Assert(mesh && norm_ && V.norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());
  AssertThrow(*V.norm_ == *norm_, ExcMessage("DiscretizedFunction<dim>::operator* : Norms not compatible"));

  return norm_->dot(*this, V);
}

template <int dim>
double DiscretizedFunction<dim>::dot(const DiscretizedFunction<dim>& V) const {
  return (*this) * V;
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping_lp(double p) {
  AssertThrow(p > 1, ExcMessage("duality_mapping_lp: p has to be larger than 1!"));

  for (size_t i = 0; i < mesh->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (function_coefficients[i][j] != 0.0)
        function_coefficients[i][j] =
            std::pow(std::abs(function_coefficients[i][j]), p - 2) * function_coefficients[i][j];
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping(double p) {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->duality_mapping(*this, p);
}

template <int dim>
void DiscretizedFunction<dim>::duality_mapping_dual(double q) {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->duality_mapping_dual(*this, q);
}

template <int dim>
double DiscretizedFunction<dim>::norm_dual() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->norm_dual(*this);
}

template <int dim>
double DiscretizedFunction<dim>::norm_p(double p) {
  AssertThrow(p >= 1, ExcMessage("norm_p: p has to be >= 1!"));
  double result = 0.0;

  for (size_t i = 0; i < mesh->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      result += std::pow(std::abs(function_coefficients[i][j]), p);

  return std::pow(result, 1 / p);
}

template <int dim>
bool DiscretizedFunction<dim>::hilbert() const {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  return norm_->hilbert();
}

template <int dim>
void DiscretizedFunction<dim>::dot_transform() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_transform(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_transform_inverse() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_transform_inverse(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_solve_mass_and_transform() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_solve_mass_and_transform(*this);
}

template <int dim>
void DiscretizedFunction<dim>::dot_mult_mass_and_transform_inverse() {
  Assert(mesh && norm_, ExcNotInitialized());
  Assert(!store_derivative, ExcInternalError());

  norm_->dot_mult_mass_and_transform_inverse(*this);
}

template <int dim>
void DiscretizedFunction<dim>::mult_mass() {
  Assert(!store_derivative, ExcInternalError());

  for (size_t i = 0; i < mesh->length(); i++) {
    Vector<double> tmp(function_coefficients[i].size());
    mesh->get_mass_matrix(i)->vmult(tmp, function_coefficients[i]);
    function_coefficients[i] = tmp;
  }
}

template <int dim>
void DiscretizedFunction<dim>::solve_mass() {
  Assert(!store_derivative, ExcInternalError());

  LogStream::Prefix p("solve_mass");
  Timer timer;
  timer.start();

  // PreconditionIdentity precondition;
  PreconditionSSOR<SparseMatrix<double>> precondition;
  precondition.initialize(*mesh->get_mass_matrix(0), PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

  for (size_t i = 0; i < mesh->length(); i++) {
    LogStream::Prefix p("step-" + Utilities::int_to_string(i, 4));

    Vector<double> tmp(function_coefficients[i].size());

    SolverControl solver_control(2000, 1e-10 * function_coefficients[i].l2_norm());
    SolverCG<> cg(solver_control);

    cg.solve(*mesh->get_mass_matrix(i), tmp, function_coefficients[i], precondition);
    function_coefficients[i] = tmp;
  }

  deallog << "solved space-time-mass matrices in " << timer.wall_time() << "s" << std::endl;
}

template <int dim>
void DiscretizedFunction<dim>::write_pvd(std::string path, std::string filename, std::string name) const {
  write_pvd(path, filename, name, name + "_prime");
}

template <int dim>
void DiscretizedFunction<dim>::write_pvd(std::string path, std::string filename, std::string name,
                                         std::string name_deriv) const {
  Assert(mesh, ExcNotInitialized());

  LogStream::Prefix p("write_pvd");
  deallog << "Writing " << path << filename << ".pvd" << std::endl;

  Assert(mesh->length() < 10000, ExcNotImplemented());  // 4 digits are ok
  std::vector<std::pair<double, std::string>> times_and_names(mesh->length(), std::pair<double, std::string>(0.0, ""));

  for (size_t i = 0; i < mesh->length(); i++) {
    const std::string vtuname = filename + "-" + Utilities::int_to_string(i, 4) + ".vtu";
    times_and_names[i]        = std::pair<double, std::string>(mesh->get_time(i), vtuname);

    write_vtu(name, name_deriv, path + vtuname, i);
  }

  std::ofstream pvd_output(path + filename + ".pvd");
  AssertThrow(pvd_output, ExcMessage("write_pvd :: output handle invalid"));

  DataOutBase::write_pvd_record(pvd_output, times_and_names);
  // deallog << "Wrote " << filename << std::endl;
}

template <int dim>
void DiscretizedFunction<dim>::write_vtu(const std::string name, const std::string name_deriv,
                                         const std::string filename, size_t i) const {
  DataOut<dim> data_out;

  data_out.attach_dof_handler(*mesh->get_dof_handler(i));
  data_out.add_data_vector(function_coefficients[i], name);

  if (store_derivative) data_out.add_data_vector(derivative_coefficients[i], name_deriv);

  data_out.build_patches();

  deallog << "Writing " << filename << std::endl;

  std::ofstream output(filename.c_str());
  AssertThrow(output, ExcMessage("write_vtk :: output handle invalid"));

  data_out.write_vtu(output);
  // deallog << "Wrote " << filename << std::endl;
}

template <int dim>
const std::shared_ptr<Norm<DiscretizedFunction<dim>>> DiscretizedFunction<dim>::get_norm() const {
  return norm_;
}

template <int dim>
void DiscretizedFunction<dim>::set_norm(std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm) {
  this->norm_ = norm;
}

template <int dim>
double DiscretizedFunction<dim>::evaluate(const Point<dim>& p, const double time) const {
  const size_t time_idx = mesh->find_time(time);

  return VectorTools::point_value(*mesh->get_dof_handler(time_idx), function_coefficients[time_idx], p);
}

template <int dim>
double DiscretizedFunction<dim>::value(const Point<dim>& p, const unsigned int component) const {
  Assert(component == 0, ExcIndexRange(component, 0, 1));
  Assert(cur_time_idx >= 0 && cur_time_idx < mesh->length(), ExcIndexRange(cur_time_idx, 0, mesh->length()));

  return VectorTools::point_value(*mesh->get_dof_handler(cur_time_idx), function_coefficients[cur_time_idx], p);
}

template <int dim>
Tensor<1, dim, double> DiscretizedFunction<dim>::gradient(const Point<dim>& p, const unsigned int component) const {
  Assert(component == 0, ExcIndexRange(component, 0, 1));
  Assert(cur_time_idx >= 0 && cur_time_idx < mesh->length(), ExcIndexRange(cur_time_idx, 0, mesh->length()));

  return VectorTools::point_gradient(*mesh->get_dof_handler(cur_time_idx), function_coefficients[cur_time_idx], p);
}

template <int dim>
double DiscretizedFunction<dim>::get_time_index() const {
  return cur_time_idx;
}

template <int dim>
void DiscretizedFunction<dim>::set_time(const double new_time) {
  Function<dim>::set_time(new_time);
  cur_time_idx = mesh->find_time(new_time);
}

template <int dim>
std::shared_ptr<SpaceTimeMesh<dim>> DiscretizedFunction<dim>::get_mesh() const {
  return mesh;
}

template <int dim>
void DiscretizedFunction<dim>::min_max_value(double* min_out, double* max_out) const {
  *min_out = std::numeric_limits<double>::infinity();
  *max_out = -std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++) {
      if (*min_out > function_coefficients[i][j]) *min_out = function_coefficients[i][j];

      if (*max_out < function_coefficients[i][j]) *max_out = function_coefficients[i][j];
    }
}

template <int dim>
double DiscretizedFunction<dim>::min_value() const {
  double tmp = std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (tmp > function_coefficients[i][j]) tmp = function_coefficients[i][j];

  return tmp;
}

template <int dim>
double DiscretizedFunction<dim>::max_value() const {
  double tmp = -std::numeric_limits<double>::infinity();

  for (size_t i = 0; i < this->length(); i++)
    for (size_t j = 0; j < function_coefficients[i].size(); j++)
      if (tmp < function_coefficients[i][j]) tmp = function_coefficients[i][j];

  return tmp;
}

template <int dim>
double DiscretizedFunction<dim>::relative_error(const DiscretizedFunction<dim>& other) const {
  DiscretizedFunction<dim> tmp(*this);
  tmp -= other;

  double denom = this->norm();
  return tmp.norm() / (denom == 0.0 ? 1.0 : denom);
}

#ifdef WAVEPI_MPI
template <int dim>
void DiscretizedFunction<dim>::mpi_irecv(size_t source, std::vector<MPI_Request>& reqs) {
  AssertThrow(reqs.size() == 0, ExcInternalError());

  reqs.reserve(function_coefficients.size());

  for (size_t i = 0; i < mesh->length(); i++) {
    reqs.emplace_back();
    MPI_Irecv(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, source, 1, MPI_COMM_WORLD,
              &reqs[i]);
  }

  if (store_derivative) {
    reqs.reserve(function_coefficients.size() + derivative_coefficients.size());

    for (size_t i = 0; i < mesh->length(); i++) {
      reqs.emplace_back();
      MPI_Irecv(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, source, 1,
                MPI_COMM_WORLD, &reqs[function_coefficients.size() + i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_send(size_t destination) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Send(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Send(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, destination, 1,
               MPI_COMM_WORLD);
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_bcast(size_t root) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Bcast(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, root, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Bcast(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, root, MPI_COMM_WORLD);
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_isend(size_t destination, std::vector<MPI_Request>& reqs) {
  AssertThrow(reqs.size() == 0, ExcInternalError());

  reqs.reserve(function_coefficients.size());

  for (size_t i = 0; i < mesh->length(); i++) {
    reqs.emplace_back();
    MPI_Isend(&function_coefficients[i][0], function_coefficients[i].size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD,
              &reqs[i]);
  }

  if (store_derivative) {
    reqs.reserve(function_coefficients.size() + derivative_coefficients.size());

    for (size_t i = 0; i < mesh->length(); i++) {
      reqs.emplace_back();
      MPI_Isend(&derivative_coefficients[i][0], derivative_coefficients[i].size(), MPI_DOUBLE, destination, 1,
                MPI_COMM_WORLD, &reqs[function_coefficients.size() + i]);
    }
  }
}

template <int dim>
void DiscretizedFunction<dim>::mpi_all_reduce(DiscretizedFunction<dim> source, MPI_Op op) {
  for (size_t i = 0; i < mesh->length(); i++)
    MPI_Allreduce(&source.function_coefficients[i][0], &function_coefficients[i][0], function_coefficients[i].size(),
                  MPI_DOUBLE, op, MPI_COMM_WORLD);

  if (store_derivative) {
    for (size_t i = 0; i < mesh->length(); i++)
      MPI_Allreduce(&source.derivative_coefficients[i][0], &derivative_coefficients[i][0],
                    derivative_coefficients[i].size(), MPI_DOUBLE, op, MPI_COMM_WORLD);
  }
}
#endif

template class DiscretizedFunction<1>;
template class DiscretizedFunction<2>;
template class DiscretizedFunction<3>;

}  // namespace base
}  // namespace wavepi


// External libraries
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/physics.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>
#include <fparameters/SpaceIterator.h>

// Local headers
#include "State.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "modelParameters.h"
#include "messages.h"

using namespace std;

State::State(boost::property_tree::ptree& cfg):
    photon{ "photon" },
	ntPhoton{ "ntPhoton" },
    ntProton{ "ntProton" },
    ntElectron{ "ntElectron" },
    ntNeutron{ "ntNeutron" },
	ntChargedPion{ "ntChargedPion" },
	ntMuon{ "ntMuon" },
	neutrino{ "neutrino" },
	ntPair{ "ntPair" },
	tau_gg(ntPhoton.ps, false),
    magf(ntPhoton.ps, false),
    denf_i(ntPhoton.ps, false),
    denf_e(ntPhoton.ps, false),
    tempElectrons(ntPhoton.ps, false),
    tempIons(ntPhoton.ps, false),
    thetaH(ntPhoton.ps, false),
    height(ntPhoton.ps, false)
{
    show_message(msgStart, Module_state);

	particles.push_back(&photon);
	particles.push_back(&ntPhoton);
    particles.push_back(&ntElectron);
    particles.push_back(&ntProton);
    particles.push_back(&ntNeutron);
	particles.push_back(&ntChargedPion);
	particles.push_back(&ntMuon);
	particles.push_back(&neutrino);
	particles.push_back(&ntPair);
    for (auto p : particles) {
        initializeParticle(*p, cfg);
    }
    magf.initialize();
    magf.fill([&](const SpaceIterator& i){
        double r = i.val(DIM_R);
        return magneticField(r);
    });
    denf_i.initialize();
    denf_i.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        double dens = massDensityADAF(r)/(atomicMassUnit*iMeanMolecularWeight);
		return dens;
    });
    denf_e.initialize();
    denf_e.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return massDensityADAF(r)/(atomicMassUnit*eMeanMolecularWeight);
    });
    tempElectrons.initialize();
    tempElectrons.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return electronTemp(r);
    });
    tempIons.initialize();
    tempIons.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return ionTemp(r);
    });
    thetaH.initialize();
    thetaH.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return acos(costhetaH(r));
    });
    height.initialize();
    height.fill([&](const SpaceIterator& i) {
        double r = i.val(DIM_R);
        return height_fun(r);
    });
	tau_gg.initialize();

    show_message(msgEnd, Module_state);
}

Dimension* State::createDimension(Particle& p, string dimid,
		function<void(Vector&, double, double)> initializer, function<double(double)> to_linear,  function<double(double)> from_linear, boost::property_tree::ptree& cfg)
{
	int samples = p.getpar<int>(cfg, "dim."+dimid+".samples");
	double min = p.getpar<double>(cfg, "dim."+dimid+".min");
	double max = p.getpar<double>(cfg, "dim."+dimid+".max");
	return new Dimension(samples, bind(initializer, placeholders::_1, min, max), to_linear, from_linear);
}

auto l10 = [](double x) { return (x > 0.0) ? log10(x) : -300.0;};
auto e10 = [](double x) { return exp10(x); };

void State::initializeParticle(Particle& p, boost::property_tree::ptree& cfg)
{
	using std::bind;
	p.configure(cfg.get_child("particle.default"));
	p.configure(cfg.get_child("particle."+p.id));

	// add dimension for energies
	p.ps.add(
		createDimension(
			p,
			"energy",
			initEnergyPoints,
			l10, e10,
			cfg
		)
	);

	// add dimension for r
	double innerRadius = exp(logr.front()) * schwRadius;
	double edgeRadius = exp(logr.back()) * schwRadius;
	p.ps.add(new Dimension(nR, bind(initGridLogarithmically, placeholders::_1, innerRadius, edgeRadius), l10, e10));

	// add dimension for rcd
	p.ps.add(new Dimension(nRcd,bind(initGridLogarithmically,placeholders::_1,
							rTr, rOutCD),l10,e10));

	p.initialize();
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP

#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class NelderMeadOptTriangulation: public NelderMeadOptBase<double>
{
public:

    std::vector< Eigen::Matrix34d > M;
    std::vector< Eigen::Vector2f > p;

    /**
     * @brief NelderMeadOptTriangulation
     * @param M0
     * @param M1
     */
    NelderMeadOptTriangulation(Eigen::Matrix34d &M0, Eigen::Matrix34d &M1) : NelderMeadOptBase()
    {
        this->M.push_back(M0);
        this->M.push_back(M1);
    }

    /**
     * @brief NelderMeadOptTriangulation
     * @param M0
     * @param M1
     */
    NelderMeadOptTriangulation(std::vector< Eigen::Matrix34d> &M) : NelderMeadOptBase()
    {
        this->M.assign(M.begin(), M.end());
    }

    /**
     * @brief update
     * @param p0
     * @param p1
     */
    void update(Eigen::Vector2f &p0, Eigen::Vector2f &p1)
    {
        this->p.clear();
        this->p.push_back(p0);
        this->p.push_back(p1);
    }

    /**
     * @brief update
     * @param p0
     * @param p1
     */
    void update(std::vector< Eigen::Vector2f> &p)
    {
        this->p.clear();
        this->p.assign(p.begin(), p.end());
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    double function(double *x, unsigned int n)
    {
        Eigen::Vector4d point(x[0], x[1], x[2], 1.0);

        double err = 0.0;
        for(unsigned int i = 0; i < M.size(); i++) {
            Eigen::Vector3d proj = M[i] * point;

            proj[0] /= proj[2];
            proj[1] /= proj[2];

            double dx = p[i][0] - proj[0];
            double dy = p[i][1] - proj[1];

            err += (dx * dx) + (dy * dy);
        }

        return err;
    }
};

#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_TRIANGULATION_HPP


#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

#include <iostream>
#include "species.h"

using namespace std;
namespace p = boost::python;
namespace np = boost::python::numpy;
namespace bm = boost::math;

template<typename T>
static T det(const array<array<T, 3>, 3> &p) {
    T sum{};
    for (size_t i = 0; i < 3; i++) {
        sum += p[1][(i + 1) % 3] * p[2][(i + 2) % 3] * p[0][i];
        sum -= p[1][(i + 2) % 3] * p[2][(i + 1) % 3] * p[0][i];
    }
    return sum;
};

class Solver {

    double kperp, kpara;

    vector<Species> species;

public:

    Solver(const p::list &list, const double B) {
        Py_Initialize();
        np::initialize();
        for (int i = 0; i < len(list); i++) {
            species.push_back(Species(list[i], B));
        }
    }

    void push_kperp(const double kperp) {
        this->kperp = kperp;
        for (Species &spec: species) {
            spec.push_kperp(kperp);
        }
    }

    void push_kpara(const double kpara) {
        this->kpara = kpara;
    }

    array<array<cdouble, 3>, 3> evaluateM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M{};
        for (Species &spec: species) {
            M += spec.push_omega(kpara, kperp, wr, wi);
        }
        double kperp2 = kperp*kperp;
        double kperp3 = kperp2*kperp;
        double kperp4 = kperp2*kperp2;
        double kpara2 = kpara*kpara;
        double kpara4 = kpara2*kpara2;
        double kpara5 = kpara4*kpara;
        double kpara6 = kpara4*kpara2;

        M[0][0] += kperp2*kpara4;
        M[1][1] += kperp2*kpara4;
        M[2][2] += kperp2*kpara4;

        cdouble c2_w2 = pow(cl / cdouble{wr, wi}, 2);

        M[0][0] -= c2_w2*kperp2*kpara6;
        M[0][2] += c2_w2*kperp3*kpara5;
        M[1][1] -= c2_w2*kperp2*kpara6;
        M[1][1] -= c2_w2*kperp4*kpara4;
        M[2][0] += c2_w2*kperp3*kpara5;
        M[2][2] -= c2_w2*kperp4*kpara4;

        M *= (1.0/(kpara4*kperp2));

        return M;
    };

    array<array<cdouble, 3>, 3> evaluateEps(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> Eps{};
        for (Species &spec: species) {
            Eps += spec.push_omega(kpara, kperp, wr, wi);
        }

        double kperp2 = kperp*kperp;
        double kperp3 = kperp2*kperp;
        double kperp4 = kperp2*kperp2;
        double kpara2 = kpara*kpara;
        double kpara4 = kpara2*kpara2;

        Eps[0][0] += kperp2*kpara4;
        Eps[1][1] += kperp2*kpara4;
        Eps[2][2] += kperp2*kpara4;

        Eps *= (1.0/(kpara4*kperp2));

        return Eps;
    };

    np::ndarray convertM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);

        p::tuple shape = p::make_tuple(3, 3);
        np::ndarray npM = np::zeros(shape, np::dtype::get_builtin<cdouble>());

        for (size_t i=0;i<3;i++){
            for (size_t j=0;j<3;j++){
                npM[i][j] = M[i][j];
            }
        }

        return npM;
    }

    np::ndarray convertEps(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateEps(wr, wi);

        p::tuple shape = p::make_tuple(3, 3);
        np::ndarray npM = np::zeros(shape, np::dtype::get_builtin<cdouble>());

        for (size_t i=0;i<3;i++){
            for (size_t j=0;j<3;j++){
                npM[i][j] = M[i][j];
            }
        }

        return npM;
    }

    cdouble evaluateDetM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);
        cdouble detM = det(M);
        double wa = sqrt(wr*wr + wi*wi);
        return detM * pow(wa, 4) / pow((kpara * kpara + kperp * kperp) * cl * cl, 2);
    }

    cdouble evaluateDetMLongitudinal(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);
        array<cdouble, 3> ML{};
        ML[0] = kperp*M[0][0] + kpara*M[0][2];
        ML[2] = kperp*M[2][0] + kpara*M[2][2];

        cdouble detML = (ML[0]*kperp + ML[2]*kpara)/(kperp*kperp + kpara*kpara);
        return detML;
    }

    np::ndarray marginalize(const np::ndarray &arr) {
        int nd = arr.get_nd();
        Py_intptr_t const *shape = arr.get_shape();
        Py_intptr_t const *stride = arr.get_strides();
        np::ndarray result = np::zeros(nd, shape, arr.get_dtype());
        if (nd == 1) {
            for (size_t i = 0; i < (size_t) shape[0]; i++) {
                cdouble w = *reinterpret_cast<cdouble const *>(arr.get_data() + i * stride[0]);
                cdouble res = evaluateDetM(w.real(), w.imag());
                *reinterpret_cast<cdouble *>(result.get_data() + i * stride[0]) = res;
            }
        } else if (nd == 2) {
            for (size_t i = 0; i < (size_t) shape[0]; i++) {
                for (size_t j = 0; j < (size_t) shape[1]; j++) {
                    cdouble w = *reinterpret_cast<cdouble const *>(arr.get_data() + i * stride[0] + j * stride[1]);
                    cdouble res = evaluateDetM(w.real(), w.imag());
                    *reinterpret_cast<cdouble *>(result.get_data() + i * stride[0] + j * stride[1]) = res;
                }
            }
        } else
            throw std::runtime_error("Unsupported marginalization dimensionality. ");
        return result;
    }

};

BOOST_PYTHON_MODULE (libSolver) {
    p::class_<Solver>("Solver", p::init<const boost::python::list, double>())
            .def("push_kperp", &Solver::push_kperp)
            .def("push_kpara", &Solver::push_kpara)
            .def("evaluateDetM", &Solver::evaluateDetM)
            .def("evaluateDetMLongitudinal", &Solver::evaluateDetMLongitudinal)
            .def("evaluateM", &Solver::convertM)
            .def("evaluateEps", &Solver::convertEps)
            .def("marginalize", &Solver::marginalize);
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "KrylovDiagonalizer.h"
#include "../MathUtils.h"
#include "DiagonalizerSettings.h"
#include "PreconditionerEvaluator.h"
#include "SigmaVectorEvaluator.h"
#include "SubspaceCollapser.h"
#include "SubspaceOrthogonalizer.h"
#include <Core/Log.h>
#include <Eigen/Eigenvalues>
#include <numeric>

namespace Scine {
namespace Utils {

KrylovDiagonalizer::KrylovDiagonalizer(int eigenvaluesToCompute, int totalDimension)
  : IterativeDiagonalizer(eigenvaluesToCompute, totalDimension) {
  settings_ = std::make_unique<KrylovSettings>(eigenvaluesToCompute, totalDimension);
  initialize(totalDimension);
  notConvergedRoots_.resize(eigenvaluesToCompute_);
  std::iota(notConvergedRoots_.begin(), notConvergedRoots_.end(), 0);
}

void KrylovDiagonalizer::applySettings() {
  IterativeDiagonalizer::applySettings();
  subspaceCollapser_ = std::make_unique<SubspaceCollapser>();
  subspaceCollapser_->setMaxSubspaceDimension(settings_->getInt(subspaceCollapseDimensionOption));
  subspaceCollapser_->setEigenvaluesToCompute(eigenvaluesToCompute_);
}

void KrylovDiagonalizer::performIteration(Core::Log& log) {
  // Handle for, for instance, orthogonalizing
  onIterationStart();

  const Eigen::MatrixXd& projector = guessVectors_.leftCols(subspaceDimension_);
  // Handle for, for instance, calculating basis overlap
  onSigmaMatrixEvaluation(projector);
  // Sigma vectors evaluation + Hamiltonian projection
  const Eigen::MatrixXd& sigmaMatrix = sigmaVectorEvaluator_->evaluate(projector);
  Eigen::MatrixXd projectedMatrix = (projector.transpose() * sigmaMatrix).selfadjointView<Eigen::Lower>();

  // Solve eigenproblem in subspace
  subspaceEigenpairs_ = eigenDecomposition(projectedMatrix);

  calculateResiduals(sigmaMatrix, projector);

  checkConvergence();

  if (converged_) {
    ritzEstimate_ = {ritzEstimate_.eigenValues.head(eigenvaluesToCompute_),
                     ritzEstimate_.eigenVectors.leftCols(eigenvaluesToCompute_)};
    return;
  }
  // Collapse subspace
  collapse(projector, log);
}

inline void KrylovDiagonalizer::calculateResiduals(const Eigen::MatrixXd& sigmaMatrix, const Eigen::MatrixXd& projector) {
  ritzEstimate_.eigenVectors = projector * subspaceEigenpairs_.eigenVectors.leftCols(eigenvaluesToCompute_);
  ritzEstimate_.eigenValues = subspaceEigenpairs_.eigenValues.head(eigenvaluesToCompute_);
  // Calculate residuals
  // TODO: Allow for expansion of more than just not converged roots (f.i. 2 new vectors per root)
  residualVectors_.resize(sigmaMatrix.rows(), notConvergedRoots_.size());
  int index = 0;
  for (int notConvergedRoot : notConvergedRoots_) {
    residualVectors_.col(index) = sigmaMatrix * subspaceEigenpairs_.eigenVectors.col(notConvergedRoot);
    residualVectors_.col(index).noalias() -=
        ritzEstimate_.eigenVectors.col(notConvergedRoot) * ritzEstimate_.eigenValues(notConvergedRoot);
    ++index;
  }
}

inline void KrylovDiagonalizer::checkConvergence() {
  residualNorms_ = residualVectors_.colwise().norm();
  numberConvergedRoots_ = 0;
  // Maps the index in the residual norms to the actual root number
  std::vector<int> oldNotConvergedRoots = notConvergedRoots_;
  notConvergedRoots_.clear();

  // Keep track of converged roots
  for (int i = 0; i < residualVectors_.cols(); ++i) {
    if (residualNorms_(i) < settings_->getDouble(residualNormToleranceOption)) {
      rootConverged_[oldNotConvergedRoots[i]] = true;
    }
    else {
      rootConverged_[oldNotConvergedRoots[i]] = false;
      notConvergedRoots_.push_back(oldNotConvergedRoots[i]);
    }
  }
  numberConvergedRoots_ = std::count(rootConverged_.begin(), rootConverged_.end(), true);
  converged_ = std::count(rootConverged_.begin(), rootConverged_.end(), false) == 0;

  assert(numberConvergedRoots_ <= eigenvaluesToCompute_);
}

void KrylovDiagonalizer::collapse(const Eigen::MatrixXd& projector, Core::Log& log) {
  subspaceCollapser_->setMaxSubspaceDimension(
      SubspaceCollapser::calculateSubspaceCollapserIterations(eigenvaluesToCompute_, numberConvergedRoots_, maxDimension_));
  if (!subspaceCollapser_->collapseNeeded(ritzEstimate_, subspaceDimension_, notConvergedRoots_)) {
    expandSubspace(projector);
  }
  else {
    callCollapserImpl();
    // If total space is small, one-shot this
    if (guessVectors_.cols() >= maxDimension_) {
      guessVectors_ = Eigen::MatrixXd::Identity(maxDimension_, maxDimension_);
      subspaceDimension_ = maxDimension_;
    }
    sigmaVectorEvaluator_->collapsed(subspaceDimension_);
    std::fill(rootConverged_.begin(), rootConverged_.end(), false);
    numberConvergedRoots_ = 0;
    converged_ = false;
    notConvergedRoots_.resize(eigenvaluesToCompute_);
    std::iota(notConvergedRoots_.begin(), notConvergedRoots_.end(), 0);
    log.output << Core::Log::nl << "Subspace collapsed. New dimension is " << subspaceDimension_ << Core::Log::nl
               << Core::Log::endl;

    printHeader(log);
  }
}

inline void KrylovDiagonalizer::expandSubspace(const Eigen::MatrixXd& projector) {
  // look at each residual vector
  Eigen::MatrixXd newGuessVectors(guessVectors_.rows(), notConvergedRoots_.size());
  int index = 0;
  for (int notConvergedRoot : notConvergedRoots_) {
    newGuessVectors.col(index) =
        preconditionerEvaluator_->evaluate(residualVectors_.col(index), ritzEstimate_.eigenValues(notConvergedRoot));
    ++index;
  }
  filterCorrectionVectors(projector, newGuessVectors);

  addVectorsToGuessBasis(newGuessVectors);
}

void KrylovDiagonalizer::filterCorrectionVectors(const Eigen::MatrixXd& /*projector*/,
                                                 Eigen::MatrixXd& /*newGuessVectors*/) const {
}

void KrylovDiagonalizer::addVectorsToGuessBasis(const Eigen::MatrixXd& newVectors) {
  int vectorsAdded = newVectors.cols();
  if (subspaceDimension_ + vectorsAdded < maxDimension_) {
    subspaceDimension_ += vectorsAdded;
    guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
    guessVectors_.rightCols(vectorsAdded) = newVectors;
  }
  else {
    guessVectors_.conservativeResize(Eigen::NoChange, maxDimension_);
    guessVectors_.rightCols(maxDimension_ - subspaceDimension_) = newVectors.leftCols(maxDimension_ - subspaceDimension_);
    subspaceDimension_ = maxDimension_;
  }
}

void KrylovDiagonalizer::onIterationStart() {
}

void KrylovDiagonalizer::onSigmaMatrixEvaluation(const Eigen::MatrixXd& /*projector*/) {
}

KrylovDiagonalizer::~KrylovDiagonalizer() = default;

} // namespace Utils
} // namespace Scine


//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@imes.mavt.ethz.ch)
// ETH Zurich, Center of Structure Technologies (www.imes.ethz.ch/st)
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
//

#ifndef BOOST_GRAPH_PROFILE_HPP
#define BOOST_GRAPH_PROFILE_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/detail/numeric_traits.hpp>
#include <boost/graph/bandwidth.hpp>

namespace boost {

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  profile(const Graph& g, VertexIndexMap index)
  {
    typename graph_traits<Graph>::vertices_size_type b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (tie(i, end) = vertices(g); i != end; ++i){
      b += ith_bandwidth(*i, g, index) + 1;
    }

    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  profile(const Graph& g)
  {
    return profile(g, get(vertex_index, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_PROFILE_HPP