/*
# linalgCpp
Collection of basic tools or examples for linear algebra in c++
Original code from https://github.com/leopoldcambier/linalgCpp
*/

#ifndef MMIO_HPP
#define MMIO_HPP

#include <assert.h>

#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <string>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace mmio {

    /** Read a line real / cplx **/
    template<typename V>
    V read_entry(std::istringstream& vals) {
        V v;
        vals >> v;
        return v;
    };
    template<>
    std::complex<double> read_entry(std::istringstream& vals) {
        double v1, v2;
        vals >> v1 >> v2;
        return std::complex<double>(v1, v2);
    };
    template<>
    std::complex<float> read_entry(std::istringstream& vals) {
        float v1, v2;
        vals >> v1 >> v2;
        return std::complex<float>(v1, v2);
    };

    /** Read a line real / cplx of coordinate values **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> read_line(std::istringstream& vals) {
        I i, j;
        V v;
        vals >> i >> j >> v;
        return Eigen::Triplet<V,I>(i-1,j-1,v);
    };
    template<>
    Eigen::Triplet<std::complex<double>,int> read_line(std::istringstream& vals) {
        int i, j; double v1, v2;
        vals >> i >> j >> v1 >> v2;
        return Eigen::Triplet<std::complex<double>,int>(i-1,j-1,std::complex<double>(v1, v2));
    };
    template<>
    Eigen::Triplet<std::complex<float>,int> read_line(std::istringstream& vals) {
        int i, j; float v1, v2;
        vals >> i >> j >> v1 >> v2;
        return Eigen::Triplet<std::complex<float>,int>(i-1,j-1,std::complex<float>(v1, v2));
    };

    /** Write a line real / cplx of coordinate values **/
    template<typename V, typename I>
    std::string get_line(I i, I j, V v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v);
    };
    template<>
    std::string get_line(int i, int j, std::complex<double> v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v.real()) + " " + std::to_string(v.imag());
    };
    template<>
    std::string get_line(int i, int j, std::complex<float> v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v.real()) + " " + std::to_string(v.imag());
    };

    /** Symmetric (real) / hermitian (cplx) **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> symmetric(Eigen::Triplet<V,I>& a) {
        return Eigen::Triplet<V,I>(a.col(), a.row(), a.value());
    }
    template<>
    Eigen::Triplet<std::complex<double>,int> symmetric(Eigen::Triplet<std::complex<double>,int>& a) {
        return Eigen::Triplet<std::complex<double>,int>(a.col(), a.row(), std::conj(a.value()));
    }
    template<>
    Eigen::Triplet<std::complex<float>,int> symmetric(Eigen::Triplet<std::complex<float>,int>& a) {
        return Eigen::Triplet<std::complex<float>,int>(a.col(), a.row(), std::conj(a.value()));
    }

    /** Skew-symmetric (real only, really) **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> skew_symmetric(Eigen::Triplet<V,I>& a) {
        return Eigen::Triplet<V,I>(a.col(), a.row(), - a.value());
    }

    enum class format {coordinate, array};
    enum class type {real, integer, complex, pattern};
    enum class property {general, symmetric, hermitian, skew_symmetric};

    std::string prop2str(property p) {
        if(p == property::general) return "general";
        else if(p == property::symmetric) return "symmetric";
        else if(p == property::hermitian) return "hermitian";
        else return "skew_symmetric";
    }

    template<typename V>
    struct V2str {
        static std::string value() {
            if (std::is_same<V,std::complex<double>>::value || std::is_same<V,std::complex<float>>::value) {
                return "complex";
            } else if (std::is_integral<V>::value) {
                return "integer";
            } else {
                return "real";
            }
        }
    };

    struct Header {
        bool bannerOK;
        bool objectOK;
        format f;
        type   t;
        property p;
        Header(std::istringstream& header) {
            std::string banner, object, format, type, properties;
            header >> banner >> object >> format >> type >> properties;
            std::transform(object.begin(),      object.end(),       object.begin(),       ::tolower);
            std::transform(format.begin(),      format.end(),       format.begin(),       ::tolower);
            std::transform(type.begin(),        type.end(),         type.begin(),         ::tolower);
            std::transform(properties.begin(),  properties.end(),   properties.begin(),   ::tolower);
            this->bannerOK = ! banner.compare("%%MatrixMarket");
            this->objectOK = ! object.compare("matrix");
            assert(this->bannerOK);
            assert(this->objectOK);
            if(! format.compare("coordinate")) {
                this->f = format::coordinate;
            } else if(! format.compare("array")) {
                this->f = format::array;
            } else {
                assert(false);
            }
            if (! type.compare("real")) {
                this->t = type::real;
            } else if (! type.compare("integer")) {
                this->t = type::integer;
            } else if (! type.compare("complex")) {
                this->t = type::complex;
            } else if (! type.compare("pattern")) {
                this->t = type::pattern;
            } else {
                assert(false);
            }
            if (! properties.compare("general")) {
                this->p = property::general;
            } else if (! properties.compare("symmetric")) {
                this->p = property::symmetric;
            } else if (! properties.compare("skew-symmetric")) {
                this->p = property::skew_symmetric;
            } else if (! properties.compare("hermitian")) {
                this->p = property::hermitian;
            } else {
                assert(false);
            }
        }
    };

    /**
     * Read a sparse matrix in MM format
     */
    template<typename V, typename I>
    Eigen::SparseMatrix<V, Eigen::ColMajor, I> sp_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.f == format::coordinate);
            // assert(h.t != type::pattern);
            /** Find M N K row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            I M, N, K;
            std::istringstream MNK(line);
            MNK >> M >> N >> K;
            std::vector<Eigen::Triplet<V,I>> data;
            if(h.p != property::general) {
                data.reserve(2*K);
            } else {
                data.reserve(K);
            }
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                if (h.t == type::pattern) line = line + " 1";
                std::istringstream vals(line);
                Eigen::Triplet<V,I> dataline = read_line<V,I>(vals);
                data.push_back(dataline);
                if(dataline.row() != dataline.col() && (h.p == property::symmetric || h.p == property::hermitian)) {
                    Eigen::Triplet<V,I> dataline2 = symmetric(dataline);
                    data.push_back(dataline2);
                }
                if(dataline.row() != dataline.col() && (h.p == property::skew_symmetric)) {
                    Eigen::Triplet<V,I> dataline2 = skew_symmetric(dataline);
                    data.push_back(dataline2);
                }
                if(h.p == property::skew_symmetric && dataline.row() == dataline.col()) {
                    assert(false);
                }
                if(h.p != property::general && dataline.row() < dataline.col()) {
                    assert(false);
                }
                lineread ++;
            }
            assert(lineread == K);
            Eigen::SparseMatrix<V, Eigen::ColMajor, I> A(M, N);
            A.setFromTriplets(data.begin(), data.end());
            return std::move(A);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Reads a dense matrix in MM format
     */
    template<typename V>
    Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> dense_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.p == property::general);
            assert(h.f == format::array);
            assert(h.t != type::pattern);
            /** Find M N row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            int M, N;
            std::istringstream MNK(line);
            MNK >> M >> N;
            Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> A(M, N);
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                std::istringstream vals(line);
                V v = read_entry<V>(vals);
                int i = (lineread % M);
                int j = (lineread / M);
                A(i,j) = v;
                lineread ++;
            }
            assert(lineread == M*N);
            return std::move(A);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Reads a dense vector in MM format
     */
    template<typename V>
    Eigen::Matrix<V, Eigen::Dynamic, 1> vector_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.p == property::general);
            assert(h.f == format::array);
            assert(h.t != type::pattern);
            /** Find M N row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            int M, N;
            std::istringstream MNK(line);
            MNK >> M >> N;
            assert(N ==1);
            Eigen::Matrix<V, Eigen::Dynamic, 1> b(M);
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                std::istringstream vals(line);
                V v = read_entry<V>(vals);
                int i = (lineread % M);
                // int j = (lineread / M);
                // A(i,j) = v;
                b(i) = v;
                lineread ++;
            }
            assert(lineread == M*N);
            return std::move(b);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Writes a sparse matrix in MM format, using the optional property p.
     * Wether the matrix satisfies or not p is not verified
     */
    template<typename V, int S, typename I>
    void sp_mmwrite(std::string filename, Eigen::SparseMatrix<V,S,I> mat, property p = property::general) {
        std::ofstream mfile;
        mfile.open (filename);
        if (mfile.is_open()) {
            std::string type = V2str<V>::value();
            std::string prop = prop2str(p);
            mfile << "%%MatrixMarket matrix coordinate " << type << " " << prop << "\n";
            int NNZ = 0;
            for (int k = 0; k < mat.outerSize(); ++k) {
                for (typename Eigen::SparseMatrix<V,S,I>::InnerIterator it(mat,k); it; ++it) {
                    if( (p == property::symmetric || p == property::hermitian) && (it.row() < it.col()) ) continue;
                    if( (p == property::skew_symmetric) && (it.row() <= it.col()) ) continue;
                    NNZ ++;
                }
            }
            mfile << mat.rows() << " " << mat.cols() << " " << NNZ << "\n";
            for (int k = 0; k < mat.outerSize(); ++k) {
                for (typename Eigen::SparseMatrix<V,S,I>::InnerIterator it(mat,k); it; ++it) {
                    if( (p == property::symmetric || p == property::hermitian) && (it.row() < it.col()) ) continue;
                    if( (p == property::skew_symmetric) && (it.row() <= it.col()) ) continue;
                    mfile << get_line(it.row(), it.col(), it.value()) << "\n";
                }
            }
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Writes a dense matrix in MM format, using the optional property p.
     * Wether the matrix satisfies or not p is not verified
     */
    template<typename V>
    void dense_mmwrite(std::string filename, Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> mat, property p = property::general) {
        std::ofstream mfile;
        mfile.open (filename);
        if (mfile.is_open()) {
            std::string type = V2str<V>::value();
            std::string prop = prop2str(p);
            mfile << "%%MatrixMarket matrix array " << type << " " << prop << "\n";
            mfile << mat.rows() << " " << mat.cols() << "\n";
            for(int j = 0; j < mat.cols(); j++) {
                for(int i = 0; i < mat.rows(); i++) {
                    if( (p == property::symmetric || p == property::hermitian) && (i < j) ) continue;
                    if( (p == property::skew_symmetric) && (i <= j) ) continue;
                    mfile << mat(i,j) << "\n";
                }
            }
        } else {
            throw("Couldn't open file");
        }
    }

}

#endif


/*
 [auto_generated]
 boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp

 [begin_description]
 Implementation of the symplectic MacLachlan stepper for separable Hamiltonian system.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_MCLACHLAN_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_MCLACHLAN_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/base/symplectic_rkn_stepper_base.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
namespace detail {
namespace symplectic_rkn_sb3a_mclachlan {

    /*
      exp( a1 t A ) exp( b1 t B )
      exp( a2 t A ) exp( b2 t B )
      exp( a3 t A ) exp( b3 t B ) exp( a3 t A )
      exp( b2 t B ) exp( a2 t A )
      exp( b1 t B ) exp( a1 t A )
    */

    template< class Value >
    struct coef_a_type : public boost::array< Value , 6 >
    {
        coef_a_type( void )
        {
            (*this)[0] = static_cast< Value >( 0.40518861839525227722 );
            (*this)[1] = static_cast< Value >( -0.28714404081652408900 );
            (*this)[2] = static_cast< Value >( 1 ) / static_cast< Value >( 2 ) - ( (*this)[0] + (*this)[1] );
            (*this)[3] = (*this)[2];
            (*this)[4] = (*this)[1];
            (*this)[5] = (*this)[0];

        }
    };

    template< class Value >
    struct coef_b_type : public boost::array< Value , 6 >
    {
        coef_b_type( void )
        {
            (*this)[0] = static_cast< Value >( -3 ) / static_cast< Value >( 73 );
            (*this)[1] = static_cast< Value >( 17 ) / static_cast< Value >( 59 );
            (*this)[2] = static_cast< Value >( 1 ) - static_cast< Value >( 2 ) * ( (*this)[0] + (*this)[1] );
            (*this)[3] = (*this)[1];
            (*this)[4] = (*this)[0];
            (*this)[5] = static_cast< Value >( 0 );
        }
    };

} // namespace symplectic_rkn_sb3a_mclachlan
} // namespace detail
#endif // DOXYGEN_SKIP

template<
    class Coor ,
    class Momentum = Coor ,
    class Value = double ,
    class CoorDeriv = Coor ,
    class MomentumDeriv = Coor ,
    class Time = Value ,
    class Algebra = range_algebra ,
    class Operations = default_operations ,
    class Resizer = initially_resizer
    >
#ifndef DOXYGEN_SKIP
class symplectic_rkn_sb3a_mclachlan :
        public symplectic_nystroem_stepper_base
<
    6 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    >
#else
class symplectic_rkn_sb3a_mclachlan : public symplectic_nystroem_stepper_base
#endif
{
public:
#ifndef DOXYGEN_SKIP
    typedef symplectic_nystroem_stepper_base
    <
    6 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    > stepper_base_type;
#endif
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::value_type value_type;

    symplectic_rkn_sb3a_mclachlan( const algebra_type &algebra = algebra_type() )
        : stepper_base_type(
            detail::symplectic_rkn_sb3a_mclachlan::coef_a_type< value_type >() ,
            detail::symplectic_rkn_sb3a_mclachlan::coef_b_type< value_type >() ,
            algebra )
    { }
};

/************* DOXYGEN ***********/

/**
 * \class symplectic_rkn_sb3a_mclachlan
 * \brief Implement of the symmetric B3A method of Runge-Kutta-Nystroem method of sixth order.
 *
 * The method is of fourth order and has six stages. It is described HERE. This method cannot be used
 * with multiprecision types since the coefficients are not defined analytically.
 *
 * ToDo Add reference to the paper.
 *
 * \tparam Order The order of the stepper.
 * \tparam Coor The type representing the coordinates q.
 * \tparam Momentum The type representing the coordinates p.
 * \tparam Value The basic value type. Should be something like float, double or a high-precision type.
 * \tparam CoorDeriv The type representing the time derivative of the coordinate dq/dt.
 * \tparam MomemtnumDeriv The type representing the time derivative of the momentum dp/dt.
 * \tparam Time The type representing the time t.
 * \tparam Algebra The algebra.
 * \tparam Operations The operations.
 * \tparam Resizer The resizer policy.
 */

    /**
     * \fn symplectic_rkn_sb3a_mclachlan::symplectic_rkn_sb3a_mclachlan( const algebra_type &algebra )
     * \brief Constructs the symplectic_rkn_sb3a_mclachlan. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_MCLACHLAN_HPP_INCLUDED


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP
#define BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Compute the arc length of an ellipse.
*/

template <typename CT, unsigned int Order = 1>
class elliptic_arc_length
{

public :

    struct result
    {
        result()
            : distance(0)
            , meridian(false)
        {}

        CT distance;
        bool meridian;
    };

    template <typename T, typename Spheroid>
    static result apply(T lon1, T lat1, T lon2, T lat2, Spheroid const& spheroid)
    {
        result res;

        CT c0 = 0;
        CT pi = math::pi<CT>();
        CT half_pi = pi/CT(2);
        CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        if (lat1 > lat2)
        {
            std::swap(lat1, lat2);
        }

        if ( math::equals(diff, c0) ||
            (math::equals(lat2, half_pi) && math::equals(lat1, -half_pi)) )
        {
            // single meridian not crossing pole
            res.distance = apply(lat2, spheroid) - apply(lat1, spheroid);
            res.meridian = true;
        }

        if (math::equals(math::abs(diff), pi))
        {
            // meridian crosses pole
            CT lat_sign = 1;
            if (lat1+lat2 < c0)
            {
                lat_sign = CT(-1);
            }
            res.distance = math::abs(lat_sign * CT(2) * apply(half_pi, spheroid)
                               - apply(lat1, spheroid) - apply(lat2, spheroid));
            res.meridian = true;
        }
        return res;
    }

    // Distance computation on meridians using series approximations
    // to elliptic integrals. Formula to compute distance from lattitude 0 to lat
    // https://en.wikipedia.org/wiki/Meridian_arc
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T, typename Spheroid>
    static CT apply(T lat, Spheroid const& spheroid)
    {
        CT const a = get_radius<0>(spheroid);
        CT const f = formula::flattening<CT>(spheroid);
        CT n = f / (CT(2) - f);
        CT M = a/(1+n);
        CT C0 = 1;

        if (Order == 0)
        {
           return M * C0 * lat;
        }

        CT C2 = -1.5 * n;

        if (Order == 1)
        {
            return M * (C0 * lat + C2 * sin(2*lat));
        }

        CT n2 = n * n;
        C0 += .25 * n2;
        CT C4 = 0.9375 * n2;

        if (Order == 2)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat));
        }

        CT n3 = n2 * n;
        C2 += 0.1875 * n3;
        CT C6 = -0.729166667 * n3;

        if (Order == 3)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat));
        }

        CT n4 = n2 * n2;
        C4 -= 0.234375 * n4;
        CT C8 = 0.615234375 * n4;

        if (Order == 4)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat) + C8 * sin(8*lat));
        }

        CT n5 = n4 * n;
        C6 += 0.227864583 * n5;
        CT C10 = -0.54140625 * n5;

        // Order 5 or higher
        return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                  + C6 * sin(6*lat) + C8 * sin(8*lat) + C10 * sin(10*lat));

    }

    // Iterative method to elliptic arc length based on
    // http://www.codeguru.com/cpp/cpp/algorithms/article.php/c5115/
    // Geographic-Distance-and-Azimuth-Calculations.htm
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T1, typename T2, typename Spheroid>
    CT interative_method(T1 lat1,
                         T2 lat2,
                         Spheroid const& spheroid)
    {
        CT result = 0;
        CT const zero = 0;
        CT const one = 1;
        CT const c1 = 2;
        CT const c2 = 0.5;
        CT const c3 = 4000;

        CT const a = get_radius<0>(spheroid);
        CT const f = formula::flattening<CT>(spheroid);

        // how many steps to use

        CT lat1_deg = lat1 * geometry::math::r2d<CT>();
        CT lat2_deg = lat2 * geometry::math::r2d<CT>();

        int steps = c1 + (c2 + (lat2_deg > lat1_deg) ? CT(lat2_deg - lat1_deg)
                                                     : CT(lat1_deg - lat2_deg));
        steps = (steps > c3) ? c3 : steps;

        //std::cout << "Steps=" << steps << std::endl;

        CT snLat1 = sin(lat1);
        CT snLat2 = sin(lat2);
        CT twoF   = 2 * f - f * f;

        // limits of integration
        CT x1 = a * cos(lat1) /
                sqrt(1 - twoF * snLat1 * snLat1);
        CT x2 = a * cos(lat2) /
                sqrt(1 - twoF * snLat2 * snLat2);

        CT dx = (x2 - x1) / (steps - one);
        CT x, y1, y2, dy, dydx;
        CT adx = (dx < zero) ? -dx : dx;    // absolute value of dx

        CT a2 = a * a;
        CT oneF = 1 - f;

        // now loop through each step adding up all the little
        // hypotenuses
        for (int i = 0; i < (steps - 1); i++){
            x = x1 + dx * i;
            dydx = ((a * oneF * sqrt((one - ((x+dx)*(x+dx))/a2))) -
                    (a * oneF * sqrt((one - (x*x)/a2)))) / dx;
            result += adx * sqrt(one + dydx*dydx);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestFunctionalBind
#include <boost/test/unit_test.hpp>

#include <iostream>

#include <boost/compute/function.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/algorithm/find_if.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/bind.hpp>
#include <boost/compute/functional/common.hpp>
#include <boost/compute/functional/operator.hpp>
#include <boost/compute/types/struct.hpp>

// simple test struct
struct data_struct
{
    int int_value;
    float float_value;
};

BOOST_COMPUTE_ADAPT_STRUCT(data_struct, data_struct, (int_value, float_value))

#include "quirks.hpp"
#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

using compute::placeholders::_1;
using compute::placeholders::_2;

BOOST_AUTO_TEST_CASE(transform_plus_two)
{
    int data[] = { 1, 2, 3, 4 };
    compute::vector<int> vector(4, context);
    compute::copy_n(data, 4, vector.begin(), queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(),
        compute::bind(compute::plus<int>(), _1, 2),
        queue
    );

    CHECK_RANGE_EQUAL(int, 4, vector, (3, 4, 5, 6));
}

BOOST_AUTO_TEST_CASE(transform_pow_two)
{
    float data[] = { 2, 3, 4, 5 };
    compute::vector<float> vector(4, context);
    compute::copy_n(data, 4, vector.begin(), queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(),
        compute::bind(compute::pow<float>(), 2.0f, _1),
        queue
    );

    compute::copy(vector.begin(), vector.end(), data, queue);
    BOOST_CHECK_CLOSE(data[0], 4.0f, 1e-4);
    BOOST_CHECK_CLOSE(data[1], 8.0f, 1e-4);
    BOOST_CHECK_CLOSE(data[2], 16.0f, 1e-4);
    BOOST_CHECK_CLOSE(data[3], 32.0f, 1e-4);
}

BOOST_AUTO_TEST_CASE(find_if_equal)
{
    int data[] = { 1, 2, 3, 4 };
    compute::vector<int> vector(4, context);
    compute::copy_n(data, 4, vector.begin(), queue);

    BOOST_CHECK(
        compute::find_if(
            vector.begin(), vector.end(),
            compute::bind(compute::equal_to<int>(), _1, 3),
            queue
        ) == vector.begin() + 2
    );
}

BOOST_AUTO_TEST_CASE(compare_less_than)
{
    int data[] = { 1, 2, 3, 4 };
    compute::vector<int> vector(data, data + 4, queue);

    int count = boost::compute::count_if(
        vector.begin(), vector.end(),
        compute::bind(compute::less<int>(), _1, 3),
        queue
    );
    BOOST_CHECK_EQUAL(count, 2);

    count = boost::compute::count_if(
        vector.begin(), vector.end(),
        compute::bind(compute::less<int>(), 3, _1),
        queue
    );
    BOOST_CHECK_EQUAL(count, 1);
}

BOOST_AUTO_TEST_CASE(subtract_ranges)
{
    int data1[] = { 1, 2, 3, 4 };
    int data2[] = { 4, 3, 2, 1 };

    compute::vector<int> vector1(data1, data1 + 4, queue);
    compute::vector<int> vector2(data2, data2 + 4, queue);

    compute::vector<int> result(4, context);

    compute::transform(
        vector1.begin(),
        vector1.end(),
        vector2.begin(),
        result.begin(),
        compute::bind(compute::minus<int>(), _1, _2),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, result, (-3, -1, 1, 3));

    compute::transform(
        vector1.begin(),
        vector1.end(),
        vector2.begin(),
        result.begin(),
        compute::bind(compute::minus<int>(), _2, _1),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, result, (3, 1, -1, -3));

    compute::transform(
        vector1.begin(),
        vector1.end(),
        vector2.begin(),
        result.begin(),
        compute::bind(compute::minus<int>(), 5, _1),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, result, (4, 3, 2, 1));

    compute::transform(
        vector1.begin(),
        vector1.end(),
        vector2.begin(),
        result.begin(),
        compute::bind(compute::minus<int>(), 5, _2),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, result, (1, 2, 3, 4));
}

BOOST_AUTO_TEST_CASE(clamp_values)
{
    int data[] = { 1, 2, 3, 4 };
    compute::vector<int> vector(data, data + 4, queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(),
        compute::bind(compute::clamp<int>(), _1, 2, 3),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, vector, (2, 2, 3, 3));
}

BOOST_AUTO_TEST_CASE(bind_custom_function)
{
    int data[] = { 1, 2, 3, 4 };
    compute::vector<int> vector(data, data + 4, queue);

    BOOST_COMPUTE_FUNCTION(int, x_if_odd_else_y, (int x, int y),
    {
        if(x & 1)
            return x;
        else
            return y;
    });

    compute::transform(
        vector.begin(), vector.end(), vector.begin(),
        compute::bind(x_if_odd_else_y, _1, 9),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, vector, (1, 9, 3, 9));

    compute::copy(
        data, data + 4, vector.begin(), queue
    );

    compute::transform(
        vector.begin(), vector.end(), vector.begin(),
        compute::bind(x_if_odd_else_y, 2, _1),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, vector, (1, 2, 3, 4));
}

BOOST_AUTO_TEST_CASE(bind_struct)
{
    if(bug_in_struct_assignment(device)){
        std::cerr << "skipping bind_struct test" << std::endl;
        return;
    }

    BOOST_COMPUTE_FUNCTION(int, add_struct_value, (int x, data_struct s),
    {
        return s.int_value + x;
    });

    data_struct data;
    data.int_value = 3;
    data.float_value = 4.56f;

    int input[] = { 1, 2, 3, 4 };
    compute::vector<int> vec(input, input + 4, queue);

    compute::transform(
        vec.begin(), vec.end(), vec.begin(),
        compute::bind(add_struct_value, _1, data),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, vec, (4, 5, 6, 7));
}

BOOST_AUTO_TEST_SUITE_END()


#include "path_finding.h"

#include "error_handling.h"
#include "rover.h"
#include "geometry.h"
#include "robot_configuration.h"

#include <opencv2/opencv.hpp>

#include <vector>
#include <queue>

#include <boost/functional/hash.hpp>
#include <boost/optional.hpp>
#include <boost/range/algorithm/reverse.hpp>

// A* path finding algorithm
template<typename TNode, typename T, typename FMinimalNodeCost, typename FForEachNeighbor, typename FIsGoal>
boost::optional<TNode> GenericAStar(rbt::pose<T> const& poseStart,
    rbt::point<T> const& ptEnd,
    FMinimalNodeCost MinimalNodeCost,
    FForEachNeighbor ForEachNeighbor,
	FIsGoal IsGoal
) {
    auto GreaterDistance = [&](TNode const& lhs, TNode const& rhs) noexcept {
        return (lhs.Position() - ptEnd).Abs() + lhs.m_fCost > (rhs.Position() - ptEnd).Abs() + rhs.m_fCost;
    };

    std::priority_queue<TNode, std::vector<TNode>, decltype(GreaterDistance)> queue(GreaterDistance);

    TNode nodeStart(poseStart);
    queue.push(nodeStart);
    MinimalNodeCost(nodeStart) = nodeStart.m_fCost;

    while(!queue.empty()) {
        auto const nodeTop = queue.top();
        queue.pop();

        if(MinimalNodeCost(nodeTop) < nodeTop.m_fCost) continue;

        if(IsGoal(nodeTop, ptEnd)) return nodeTop;

        ForEachNeighbor(
            nodeTop,
            [&](TNode const& nodeNeighbor) {
                if(rbt::assign_min(MinimalNodeCost(nodeNeighbor), nodeNeighbor.m_fCost)) {
                    queue.push(nodeNeighbor);
                }
            });
    }

    return boost::none;
}

std::vector<rbt::point<double>> FindPath(cv::Mat matn, rbt::pose<double> const& posefStart, rbt::point<double> const& ptfEnd) {
    cv::Mat matnEroded;
    auto const nMaxExtent = std::max(c_nRobotWidth/c_nScale, c_nRobotHeight/c_nScale);
    cv::erode(
        matn,
        matnEroded,
        cv::Mat::ones(nMaxExtent, nMaxExtent, CV_8U)
    );

    // Include costs of traveling close to an obstacle in calculation
    cv::Mat matnGauss;
    auto const nMaxExtentOdd = 2*nMaxExtent + 1;
    cv::GaussianBlur(matnEroded, matnGauss, cv::Size(nMaxExtentOdd, nMaxExtentOdd), 0, 0);

    float aanMinimalCost[c_nMapExtent][c_nMapExtent];
    std::fill_n(&aanMinimalCost[0][0], c_nMapExtent*c_nMapExtent, std::numeric_limits<float>::max());

    struct node {
        node() {}
        node(rbt::pose<int> const& pose) : m_pt(pose.m_pt), m_fCost(0) {}
        node(rbt::point<int> const& pt, float fCost) : m_pt(pt), m_fCost(fCost) {}

		rbt::point<int> Position() const { return m_pt; }

        rbt::point<int> m_pt; // in grid coordinates
        float m_fCost;
    };

    auto MinimalNodeCost = [&](node const& node) noexcept -> float& {
        return aanMinimalCost[node.m_pt.x][node.m_pt.y];
    };

    auto ForEachNeighbor = [&](node const& n, auto fn) noexcept {
        for(int x = -1; x <= 1; ++x) {
            for(int y = -1; y <= 1; ++y) {
                auto const ptNext = n.m_pt + rbt::size<int>(x, y);
                if((0!=x || 0!=y)
                && 128<matnGauss.at<std::uint8_t>(ptNext.y, ptNext.x)) {
                    fn(node(
                        ptNext,
                        n.m_fCost + (0==x || 0==y ? 1.0f : M_SQRT2) * (1 + (255 - matnGauss.at<std::uint8_t>(ptNext.y, ptNext.x))/10)
                    ));
                }
            }
        }
    };
	auto IsGoal = [](node const& n, rbt::point<int> const& ptnEnd) {
		return n.m_pt == ptnEnd;
	};

	auto const posenStart = ToGridCoordinate(posefStart);

    std::vector<rbt::point<double>> vecptfResult;
    if(auto onode = GenericAStar<node>(posenStart, ToGridCoordinate(ptfEnd), MinimalNodeCost, ForEachNeighbor, IsGoal)) {
        vecptfResult.emplace_back(ptfEnd);
        auto const ptnStart = posenStart.m_pt;
        for(auto nodePrev = *onode; nodePrev.m_pt!=ptnStart; ) {
            float fMinCost = std::numeric_limits<float>::max();
            node nodeMin;
            ForEachNeighbor(
                nodePrev,
                [&](node const& node) {
                    if(rbt::assign_min(fMinCost, MinimalNodeCost(node))) {
                        nodeMin = node;
                    }
                }
            );

            nodePrev = nodeMin;
            vecptfResult.emplace_back(ToWorldCoordinate(rbt::point<double>(nodeMin.m_pt)));
        }
    }
    return vecptfResult;
}

namespace {
    struct config_space_node {
		config_space_node() {}
        config_space_node(rbt::pose<double> const& pose) : m_pose(pose) {}

		rbt::point<double> Position() const { return m_pose.m_pt; }

		int RoundedYaw() const { return (int)std::round(m_pose.m_fYaw*100); }
		rbt::point<int> RoundedPosition() const {
			return rbt::point<int>((int)std::round(m_pose.m_pt.x), (int)std::round(m_pose.m_pt.y));
		}
        rbt::pose<double> m_pose;

		// as used for robot commands, encoder ticks / s?
        int m_nSpeedLeft = 0;
        int m_nSpeedRight = 0;

        // path cost
        float m_fCost = 0;

		config_space_node const* m_pnodeParent = nullptr;

        friend bool operator==(config_space_node const& lhs, config_space_node const& rhs) {
            return lhs.RoundedPosition()==rhs.RoundedPosition()
				&& lhs.RoundedYaw()==rhs.RoundedYaw()
                && lhs.m_nSpeedLeft==rhs.m_nSpeedLeft
                && rhs.m_nSpeedRight==rhs.m_nSpeedRight;
        }
    };

	constexpr int c_nSpeedStep = 100;
	constexpr double c_fTimeStep = 0.2; // s
	const float c_fHighTravelDistance = encoderTicksToCm(c_nMaxSpeed*0.8*c_fTimeStep);
}

namespace std {
	template <> struct hash<config_space_node> {
		size_t operator()(config_space_node const& node) const {
			std::size_t seed = 0;
			auto pt = node.RoundedPosition();
			boost::hash_combine(seed, pt.x);
			boost::hash_combine(seed, pt.y);
			boost::hash_combine(seed, node.RoundedYaw());
			boost::hash_combine(seed, node.m_nSpeedLeft);
			boost::hash_combine(seed, node.m_nSpeedRight);
			return seed;
		}
	};
}

std::vector<rbt::pose<double>> PathConfigurationSpace(cv::Mat matn, rbt::pose<double> const& posefStart, rbt::point<double> const& ptfEnd) {
    auto const vecptf = FindPath(matn, posefStart, ptfEnd);

	// TODO: Share code?
	cv::Mat matnEroded;
    auto const nMaxExtent = std::max(c_nRobotWidth/c_nScale, c_nRobotHeight/c_nScale);
    cv::erode(
        matn,
        matnEroded,
        cv::Mat::ones(nMaxExtent, nMaxExtent, CV_8U)
    );

    cv::Mat matnGauss;
    auto const nMaxExtentOdd = 2*nMaxExtent + 1;
    cv::GaussianBlur(matnEroded, matnGauss, cv::Size(nMaxExtentOdd, nMaxExtentOdd), 0, 0);

	cv::Mat matnPath = cv::Mat::zeros(matn.size(), CV_8U);
	rbt::point<int> ptnPrev = ToGridCoordinate(vecptf.front());
	boost::for_each(vecptf, [&](rbt::point<double> const& ptf) {
		auto const ptnGrid = ToGridCoordinate(ptf);
        cv::line(matnPath, ptnPrev, ptnGrid, cv::Scalar(255), 50/c_nScale);
        ptnPrev = ptnGrid;
	});

	int cExpanded = 0;
	std::unordered_map<config_space_node, float> mapnodefCosts;
	auto onode = GenericAStar<config_space_node>(
		posefStart,
		// Calculating a longer path in configuration space takes too much time to compute due to large state space.
		*(vecptf.end() - 80),
		[&](config_space_node const& node) noexcept -> float& {
			return mapnodefCosts.emplace(node, std::numeric_limits<float>::max()).first->second;
		},
		[&](config_space_node const& node, auto fn) noexcept {
			++cExpanded;

			auto const pnodeParent = [&] {
				auto const itpair = mapnodefCosts.find(node);
				ASSERT(itpair!=mapnodefCosts.end());
				return &itpair->first;
			}();

			for(int nStepLeft = -1; nStepLeft <= 1; ++nStepLeft) {
				// [decelerate both, left, right, no change, accelerate right, left, both]
				for(int nStepRight = -1; nStepRight <= 1; ++nStepRight) {
					// -> Calculate new positions / angle
					auto nodeNeighbor = node;
					nodeNeighbor.m_pnodeParent = pnodeParent;
					nodeNeighbor.m_nSpeedLeft += nStepLeft * c_nSpeedStep;
					nodeNeighbor.m_nSpeedRight += nStepRight * c_nSpeedStep;

					if(std::abs(nodeNeighbor.m_nSpeedLeft)<=c_nMaxSpeed
					&& std::abs(nodeNeighbor.m_nSpeedRight)<=c_nMaxSpeed
					&& std::abs(nodeNeighbor.m_nSpeedRight-nodeNeighbor.m_nSpeedLeft)<=400)
					{
						nodeNeighbor.m_pose = UpdatePose(
							node.m_pose,
							nodeNeighbor.m_nSpeedLeft*c_fTimeStep,
							nodeNeighbor.m_nSpeedRight*c_fTimeStep
						);

						cv::LineIterator itpt(
							matn,
							ToGridCoordinate(node.Position()),
							ToGridCoordinate(nodeNeighbor.Position())
						);

						float fWeightedCost = 0;
						for(int i = 0; i < itpt.count; ++i, ++itpt) {
							auto const pt = rbt::point<int>(itpt.pos());
							// -> check if still in range of shortest path
							if(matnPath.at<std::uint8_t>(pt.y, pt.x)<255) {
								goto outside_range;
							}

							// integrate costs over node.m_pt -> nodeNeighbor.m_pt
							fWeightedCost += std::pow((255.0 - matnGauss.at<std::uint8_t>(pt.y, pt.x))/30, 2);
						}
						{
							fWeightedCost = std::max(1.f, fWeightedCost/itpt.count);
							float const fDistance = (nodeNeighbor.Position()-node.Position()).Abs();
							// Penalize several short moves, i.e., slow moves
							nodeNeighbor.m_fCost += fDistance * std::max(1.0, std::pow(c_fHighTravelDistance/fDistance, 2)) * fWeightedCost;
						}

						fn(nodeNeighbor);

outside_range:
						;
					}
				}
			}
		},
		[](config_space_node const& node, rbt::point<double> const ptfEnd) {
			return (node.m_pose.m_pt - ptfEnd).SqrAbs() < c_nScale*c_nScale;
		}
	);

	std::cout << "Discovered " << mapnodefCosts.size() << " states, " << cExpanded << " expanded." << std::endl;
	std::vector<rbt::pose<double>> vecposef;
	if(onode) {
		auto const* pnodeNext = &*onode;
		while(pnodeNext) {
			std::cout << pnodeNext->m_pose <<
			" (" << pnodeNext->m_nSpeedLeft << ", " << pnodeNext->m_nSpeedRight << ") " << std::endl;

			vecposef.emplace_back(pnodeNext->m_pose);
			pnodeNext = pnodeNext->m_pnodeParent;
		}
		boost::reverse(vecposef);
	}
	return vecposef;
}


#include "core/hosvd.hpp"
#include "video/video.hpp"
#include "utils/utils.hpp"
#include "tests/tests.hpp"
#include "core/compression.hpp"
#include <iostream>
#include <random>
#include <chrono>
#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <fstream>
#include <charconv>
#include <args.hxx>
#include <filesystem>
#include <thread>

// CRT's memory leak detection
#ifndef NDEBUG
#if defined(_MSC_VER)
#define _CRTDBG_MAP_ALLOC
#include <crtdbg.h>
#endif
#endif

void benchmarkSVD(int sx, int sy)
{
	std::default_random_engine rng;
	std::normal_distribution<float> dist;

	using namespace Eigen;
	MatrixXf testMat(sx, sy);
	for (int i = 0; i < sx; ++i)
		for (int j = 0; j < sy; ++j)
			testMat(i, j) = dist(rng);

	auto start = std::chrono::high_resolution_clock::now();
	BDCSVD<MatrixXf> svd(testMat);
	auto end = std::chrono::high_resolution_clock::now();

	std::cout << "SVD takes " << std::chrono::duration<float>(end - start).count() << std::endl;

	std::cout << svd.singularValues().size() << std::endl;
}

template<int K, int Dim>
void flattenTest(Tensor<float, Dim>& tensor, float& sum)
{
	auto start = std::chrono::high_resolution_clock::now();
	auto m = tensor.template flatten<K>();
	auto end = std::chrono::high_resolution_clock::now();
	float t = std::chrono::duration<float>(end - start).count();
	std::cout << "Flatten in dimension " << K << "  \t" << std::chrono::duration<float>(end - start).count() << std::endl;
	//	m *= 2.14f;
	//	sum += m.trace();

	start = std::chrono::high_resolution_clock::now();
	tensor.template set<K>(m);
	end = std::chrono::high_resolution_clock::now();
	std::cout << "Unflatten in dimension " << K << "\t" << std::chrono::duration<float>(end - start).count() << std::endl;
	t += std::chrono::duration<float>(end - start).count();
	sum += t;//tensor.norm()
}

template<int Dim>
void benchmarkTensor(const std::array<int, Dim>& _sizeVec)
{
	std::cout << "Benchmarking a tensor with size " << _sizeVec << "^" << Dim << std::endl;

	std::default_random_engine rng;
	std::uniform_real_distribution<float> dist;

	auto start = std::chrono::high_resolution_clock::now();

	Tensor<float, Dim> tensor(_sizeVec);
	tensor.set([&](auto) { return dist(rng); });

	auto end = std::chrono::high_resolution_clock::now();
	std::cout << "Fill with random elements " << std::chrono::duration<float>(end - start).count() << std::endl;

	float sum = 0.f;
	//for (int k = 0; k < Dim; ++k)

	flattenTest<0>(tensor, sum);
	flattenTest<1>(tensor, sum);
	flattenTest<2>(tensor, sum);
	flattenTest<3>(tensor, sum);

	start = std::chrono::high_resolution_clock::now();
	const auto&[U, C] = hosvdInterlaced(tensor, truncation::Tolerance(0.05f));
	end = std::chrono::high_resolution_clock::now();
	std::cout << "hosvd                   \t" << std::chrono::duration<float>(end - start).count() << std::endl;

	start = std::chrono::high_resolution_clock::now();
	auto tensor2 = multilinearProduct(U, C);
	end = std::chrono::high_resolution_clock::now();
	std::cout << "multilinear product     \t" << std::chrono::duration<float>(end - start).count() << std::endl;

	start = std::chrono::high_resolution_clock::now();
	auto tensor3 = tensor - tensor2;
	end = std::chrono::high_resolution_clock::now();
	std::cout << "subtract                \t" << std::chrono::duration<float>(end - start).count() << std::endl;

	start = std::chrono::high_resolution_clock::now();
	sum += tensor3.norm();
	end = std::chrono::high_resolution_clock::now();
	std::cout << "norm                    \t" << std::chrono::duration<float>(end - start).count() << std::endl;

	std::cout << sum + C.norm();
}

enum struct TruncationMode
{
	Zero,
	Rank,
	Tolerance,
	ToleranceSum,
};

const std::unordered_map<std::string, TruncationMode> TRUNCATION_NAMES =
{ {
	{"zero", TruncationMode::Zero},
	{"rank", TruncationMode::Rank},
	{"tolerance", TruncationMode::Tolerance},
	{"tolerance_sum", TruncationMode::ToleranceSum},
} };

enum struct PixelFormat
{
	RGB,
	YUV444
};

const std::unordered_map<std::string, PixelFormat> PIXEL_FORMATS =
{ {
	{"RGB", PixelFormat::RGB},
	{"YUV444", PixelFormat::YUV444}
} };

int main(int argc, char** args)
{
#ifndef NDEBUG
#if defined(_MSC_VER)
	_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
	//	_CrtSetBreakAlloc(12613);
#endif
	Tests tests;
	tests.run();
#endif

	//	benchmarkSVD(1920, 1080);
	//	benchmarkTensor<4>({3,800,600,14});

	args::ArgumentParser parser("HOSVD video compressor.");
	args::HelpFlag help(parser, "help", "display this help menu", { 'h', "help" });

	args::ValueFlag<std::string> inputFile(parser, "input file",
		"path of the video file to process",
		{ 'i', "input" });
	args::ValueFlag<std::string> outputFile(parser, "output file",
		"name of the output file",
		{ 'o', "output" });

	args::MapFlag<std::string, TruncationMode> truncationMode(parser, "truncation mode",
		"rule which is applied to truncate singular values", { "trunc" }, TRUNCATION_NAMES,
		TruncationMode::Tolerance);
	args::MapFlag<std::string, PixelFormat> pixelFormat(parser, "pixel format",
		"pixel format on which tensors are defined; independent of the format used by input/output videos", { "pix_fmt" },
		PIXEL_FORMATS, PixelFormat::YUV444);
	args::PositionalList<float> truncationThreshold(parser, "truncation threshold",
		"values used for truncation in each dimension");
	args::ValueFlag<int> framesPerBlock(parser, "frames per block",
		"number of frames combined to a single tensor; if 0, the whole video is used (larger blocks allow for better compression but reduce encode and decode performance)",
		{ "block_size" }, 24);
	args::ValueFlag<int> numThreads(parser, "max threads",
		"maximum number of threads used during computations",
		{ "num_threads" }, std::thread::hardware_concurrency() / 2);

	try
	{
		parser.ParseCLI(argc, args);
	}
	catch (const args::Help&)
	{
		std::cout << parser;
		return 0;
	}
	catch (const args::ParseError& e)
	{
		std::cerr << e.what() << std::endl;
		return 1;
	}
	catch (const args::ValidationError& e)
	{
		std::cerr << e.what() << std::endl;
		return 1;
	}

	namespace fs = std::filesystem;
	const fs::path inputPath = args::get(inputFile);
	if (!fs::exists(inputPath) || !fs::is_regular_file(inputPath))
	{
		std::cerr << "[Error] The input file " << inputFile << " does not exist.\n";
		return 1;
	}

	Eigen::setNbThreads(args::get(numThreads));

	auto process = [&](const auto& pixelFormat)
	{
		auto compressor = compression::HOSVDCompressor(pixelFormat);
		compressor.setFramesPerBlock(args::get(framesPerBlock));

		std::vector<float> rank = args::get(truncationThreshold);
		if (rank.size() < 4)
		{
			std::cout << "[Warning] Less than 4 truncation threshold values given. Default may not work with every truncation mode.\n";
		}
		switch (args::get(truncationMode))
		{
		case TruncationMode::Zero:
			compressor.setTruncation(truncation::Zero());
			break;
		case TruncationMode::Rank:
			if (rank.empty())
				rank.push_back(1);
			compressor.setTruncation(truncation::Rank(std::vector<int>(rank.begin(), rank.end())));
			break;
		case TruncationMode::Tolerance:
			if (rank.empty())
				rank.push_back(0.1f);
			compressor.setTruncation(truncation::Tolerance(rank));
			break;
		case TruncationMode::ToleranceSum:
			if (rank.empty())
				rank.push_back(0.2f);
			compressor.setTruncation(truncation::ToleranceSum(rank));
			break;
		}

		if (inputPath.extension() == "ten")
		{
			std::cout << "Loading tensor file " << args::get(inputFile) << ".\n";
			compressor.load(args::get(inputFile));
		}
		else
		{
			std::cout << "Loading video " << args::get(inputFile) << ".\n";
			Video video(args::get(inputFile));
			std::cout << "Applying HOSVD.\n";
			compressor.encode(video);
		}

		struct Stats
		{
			int min = std::numeric_limits<int>::max();
			int max = 0;
			int sum = 0;
		};
		const auto& singularValues = compressor.singularValues();
		std::vector<Stats> stats(singularValues.front().order());

		for (auto s : singularValues)
		{
			for (size_t dim = 0; dim < s.size().size(); ++dim)
			{
				const int r = s.size()[dim];
				stats[dim].min = std::min(stats[dim].min, r);
				stats[dim].max = std::max(stats[dim].max, r);
				stats[dim].sum += r;
			}
		}
		std::cout << "Statistics of the resulting tensors: \n dimension\\rank min max mean\n";
		for (auto& stat : stats)
			std::cout << stat.min << " " << stat.max << " " << stat.sum / singularValues.size() << "\n";

		namespace fs = std::filesystem;
		const fs::path outputPath = args::get(outputFile);
		if (outputPath.extension() == "ten")
		{
			std::cout << "Saving tensors as " << args::get(outputFile) << ".\n";
			compressor.save(args::get(outputFile));
		}
		else
		{
			std::cout << "Saving video as " << args::get(outputFile) << ".\n";
			Video video = compressor.decode();
			video.save(args::get(outputFile));
		}
	};

	switch (args::get(pixelFormat))
	{
	case PixelFormat::RGB:
		process(Video::RGB());
		break;
	case PixelFormat::YUV444:
		process(Video::YUV444());
		break;
	}

#if false
	Video video("TestScene.mp4");
	/*	auto tensor = video.asTensor(0, 80, Video::YUV420());
		Video video2(tensor, Video::FrameRate{1,24}, Video::YUV420());
		video2.save("TestSceneRestoredYUV420.avi");*/
	/**/compression::HOSVDCompressor compressor;
	compressor.setTruncation(truncation::Rank{ 2,100,100,10 });
	compressor.encode(video);
	Video video2 = compressor.decode();
	video2.save("TestSceneRestoredYUV4.avi");
#endif

	return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class HPS
{

public:
  void processFrame(const RealVectorView& input, RealVectorView output,
                    index nHarmonics, double minFreq, double maxFreq,
                    double sampleRate)
  {
    using namespace Eigen;
    using namespace std;

    ArrayXd::Index maxIndex;

    ArrayXd mag = _impl::asEigen<Array>(input);
    ArrayXd hps = mag;
    index   nBins = mag.size();
    double  binHz = sampleRate / ((nBins - 1) * 2);
    index   minBin = lrint(minFreq / binHz);
    index   maxBin = lrint(maxFreq / binHz);
    double  f0 = 0;
    double  confidence = 0;
    double hpsSum = 0;

    for (index i = 2; i < nHarmonics; i++)
    {
      index   hBins = nBins / i;
      ArrayXd h = ArrayXd::Zero(hBins);
      for (index j = 0; j < hBins; j++) h(j) = mag(j * i);
      ArrayXd hp = ArrayXd::Zero(nBins);
      hp.segment(0, hBins) = h;
      hps = hps * hp;
    }
    hpsSum = hps.sum();

    if (maxBin > minBin &&  hpsSum > 0)
    {
      hps = hps.segment(minBin, maxBin - minBin);
      double maxVal = hps.maxCoeff(&maxIndex);
      confidence = maxVal / hpsSum;
      f0 = (minBin + maxIndex) * binHz;
    }
    output(0) = f0;
    output(1) = confidence;
  }
};
} // namespace algorithm
} // namespace fluid


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// These functions are largely based off the the Ceres solver polynomial
// functions which are not available through the public interface. The license
// is below:
//
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: moll.markus@arcor.de (Markus Moll)
//         sameeragarwal@google.com (Sameer Agarwal)

#include "theia/math/polynomial.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <cmath>
#include <limits>

#include "theia/math/find_polynomial_roots_companion_matrix.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

bool FindPolynomialRoots(const VectorXd& polynomial,
                         VectorXd* real,
                         VectorXd* imaginary) {
  return FindPolynomialRootsCompanionMatrix(polynomial, real, imaginary);
}

// Remove leading terms with zero coefficients.
VectorXd RemoveLeadingZeros(const VectorXd& polynomial_in) {
  int i = 0;
  while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0) {
    ++i;
  }
  return polynomial_in.tail(polynomial_in.size() - i);
}

VectorXd DifferentiatePolynomial(const VectorXd& polynomial) {
  const int degree = polynomial.rows() - 1;
  CHECK_GE(degree, 0);

  // Degree zero polynomials are constants, and their derivative does
  // not result in a smaller degree polynomial, just a degree zero
  // polynomial with value zero.
  if (degree == 0) {
    return VectorXd::Zero(1);
  }

  VectorXd derivative(degree);
  for (int i = 0; i < degree; ++i) {
    derivative(i) = (degree - i) * polynomial(i);
  }

  return derivative;
}

VectorXd MultiplyPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  VectorXd multiplied_poly = VectorXd::Zero(poly1.size() + poly2.size() - 1);;
  for (int i = 0; i < poly1.size(); i++) {
    for (int j = 0; j < poly2.size(); j++) {
      multiplied_poly.reverse().operator()(i + j) +=
          poly1.reverse()(i) * poly2.reverse()(j);
    }
  }
  return multiplied_poly;
}

void DividePolynomial(const VectorXd& polynomial,
                      const VectorXd& divisor,
                      VectorXd* quotient,
                      VectorXd* remainder) {
  // If the divisor is higher degree than the polynomial then it cannot be
  // divided so we simply return the remainder.
  if (polynomial.size() < divisor.size()) {
    *quotient = VectorXd::Zero(1);
    *remainder = polynomial;
    return;
  }

  VectorXd numerator = RemoveLeadingZeros(polynomial);
  VectorXd denominator;
  *quotient = VectorXd::Zero(numerator.size() - divisor.size() + 1);
  while (numerator.size() >= divisor.size()) {
    denominator = VectorXd::Zero(numerator.size());
    denominator.head(divisor.size()) = divisor;

    const double quotient_scalar = numerator(0) / denominator(0);
    quotient->reverse().operator()(numerator.size() - divisor.size()) =
        quotient_scalar;
    denominator = denominator * quotient_scalar;
    numerator = numerator - denominator;
    // Sometimes there are floating point errors that result in a non-zero first
    // value.
    numerator(0) = 0;
    numerator = RemoveLeadingZeros(numerator);
  }
  *remainder = numerator;
}

VectorXd AddPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  if (poly1.size() > poly2.size()) {
    VectorXd sum = poly1;
    sum.tail(poly2.size()) += poly2;
    return sum;
  } else {
    VectorXd sum = poly2;
    sum.tail(poly1.size()) += poly1;
    return sum;
  }
}

void FindLinearPolynomialRoots(const VectorXd& polynomial,
                               VectorXd* real,
                               VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 2);
  if (real != NULL) {
    real->resize(1);
    (*real)(0) = -polynomial(1) / polynomial(0);
  }

  if (imaginary != NULL) {
    imaginary->setZero(1);
  }
}

void FindQuadraticPolynomialRoots(const VectorXd& polynomial,
                                  VectorXd* real,
                                  VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 3);
  const double a = polynomial(0);
  const double b = polynomial(1);
  const double c = polynomial(2);
  const double D = b * b - 4 * a * c;
  const double sqrt_D = sqrt(fabs(D));
  if (real != NULL) {
    real->setZero(2);
  }
  if (imaginary != NULL) {
    imaginary->setZero(2);
  }

  // Real roots.
  if (D >= 0) {
    if (real != NULL) {
      // Stable quadratic roots according to BKP Horn.
      // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_D) / (2.0 * a);
        (*real)(1) = (2.0 * c) / (-b - sqrt_D);
      } else {
        (*real)(0) = (2.0 * c) / (-b + sqrt_D);
        (*real)(1) = (-b + sqrt_D) / (2.0 * a);
      }
    }
    return;
  }

  // Use the normal quadratic formula for the complex case.
  if (real != NULL) {
    (*real)(0) = -b / (2.0 * a);
    (*real)(1) = -b / (2.0 * a);
  }
  if (imaginary != NULL) {
    (*imaginary)(0) = sqrt_D / (2.0 * a);
    (*imaginary)(1) = -sqrt_D / (2.0 * a);
  }
}

void MinimizePolynomial(const VectorXd& polynomial,
                        const double x_min,
                        const double x_max,
                        double* optimal_x,
                        double* optimal_value) {
  // Find the minimum of the polynomial at the two ends.
  //
  // We start by inspecting the middle of the interval. Technically
  // this is not needed, but we do this to make this code as close to
  // the minFunc package as possible.
  *optimal_x = (x_min + x_max) / 2.0;
  *optimal_value = EvaluatePolynomial(polynomial, *optimal_x);

  const double x_min_value = EvaluatePolynomial(polynomial, x_min);
  if (x_min_value < *optimal_value) {
    *optimal_value = x_min_value;
    *optimal_x = x_min;
  }

  const double x_max_value = EvaluatePolynomial(polynomial, x_max);
  if (x_max_value < *optimal_value) {
    *optimal_value = x_max_value;
    *optimal_x = x_max;
  }

  // If the polynomial is linear or constant, we are done.
  if (polynomial.rows() <= 2) {
    return;
  }

  const VectorXd derivative = DifferentiatePolynomial(polynomial);
  VectorXd roots_real;
  if (!FindPolynomialRoots(derivative, &roots_real, NULL)) {
    LOG(WARNING) << "Unable to find the critical points of "
                 << "the interpolating polynomial.";
    return;
  }

  // This is a bit of an overkill, as some of the roots may actually
  // have a complex part, but its simpler to just check these values.
  for (int i = 0; i < roots_real.rows(); ++i) {
    const double root = roots_real(i);
    if ((root < x_min) || (root > x_max)) {
      continue;
    }

    const double value = EvaluatePolynomial(polynomial, root);
    if (value < *optimal_value) {
      *optimal_value = value;
      *optimal_x = root;
    }
  }
}

// An iterative solver to find the closest root based on an initial guess. We
// use Laguerre's method, which is a polynomial root finding method that
// converges to a root with very high certainty. For multiple roots, the
// convergence is linear, otherwise it is cubic.
double FindRootIterativeLaguerre(const VectorXd& polynomial,
                                 const double x0,
                                 const double epsilon,
                                 const int max_iter) {
  const double kSmallestValue = 1e-10;

  // Constant symbolic derivitives.
  const VectorXd f_prime = DifferentiatePolynomial(polynomial);
  const VectorXd f_prime_prime = DifferentiatePolynomial(f_prime);
  const double k = static_cast<double>(polynomial.size());

  double x = x0;

  for (int i = 0; i < max_iter; i++) {
    const double f_of_x = EvaluatePolynomial(polynomial, x);
    if (std::abs(f_of_x) < kSmallestValue) {
      break;
    }

    const double g = EvaluatePolynomial(f_prime, x) / f_of_x;
    const double h = g * g - EvaluatePolynomial(f_prime_prime, x) / f_of_x;
    const double denom_part = std::sqrt(std::abs((k - 1.0) * (k * h - g * g)));
    const double denom = (g < 0) ? g - denom_part : g + denom_part;
    const double delta =  k / denom;
    if (std::abs(delta) < epsilon) {
      break;
    }

    x -= delta;
  }

  return x;
}

double FindRootIterativeNewton(const Eigen::VectorXd& polynomial,
                               const double x0,
                               const double epsilon,
                               const int max_iterations) {
  double root = x0;
  const Eigen::VectorXd derivative = DifferentiatePolynomial(polynomial);
  double prev = std::numeric_limits<double>::max();
  for (int i = 0; i < max_iterations && std::abs(prev - root) > epsilon; i++) {
    prev = root;
    root -= EvaluatePolynomial(polynomial, root) /
            EvaluatePolynomial(derivative, root);
  }
  return root;
}

}  // namespace theia


#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/numerics/data_out.h>

#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>

#include <deal.II/base/utilities.h>

#include <deal.II/lac/slepc_solver.h>

#include <deal.II/base/timer.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/index_set.h>

#include <deal.II/lac/sparsity_tools.h>

#include <deal.II/distributed/tria.h>
#include <deal.II/distributed/grid_refinement.h>

#include <fstream>
#include <iostream>
#include <math.h>
#include <random>
#include <mpi.h>

using namespace dealii;
class ParallelKL
{
public:
  ParallelKL();
  void run();
private:
  void make_grid();
  void setup_system();
  void assemble_system();
  void solve();
  void create_random_field();
  void stats_for_random_field();
  void output_results() const;
  void output_results_parallel();

  MPI_Comm mpi_communicator;

  parallel::distributed::Triangulation<2> triangulation;

  FE_Q<2>          fe;
  DoFHandler<2>    dof_handler;

  IndexSet         locally_owned_dofs;
  IndexSet         locally_relevant_dofs;

  AffineConstraints<double> constraints;

  SparsityPattern      sparsity_pattern;

  PETScWrappers::MPI::SparseMatrix system_mass_matrix;
  PETScWrappers::MPI::SparseMatrix system_stiffness_matrix;

  PETScWrappers::MPI::Vector randomfield_vector;

  std::vector<double> eigenvalues;
  std::vector<PETScWrappers::MPI::Vector> eigenvectors;

  std::vector<double> normalized_gaussian;
  std::default_random_engine generator;
  std::normal_distribution<double> distribution;

  unsigned int this_mpi_process, n_mpi_processes;

  ConditionalOStream pcout;
  TimerOutput        computing_timer;
};

ParallelKL::ParallelKL()
  : mpi_communicator(MPI_COMM_WORLD)
  , triangulation(mpi_communicator,
                  typename Triangulation<2>::MeshSmoothing(
                  Triangulation<2>::smoothing_on_refinement |
                  Triangulation<2>::smoothing_on_coarsening))
  , fe(1)
  , dof_handler(triangulation)
  , this_mpi_process(Utilities::MPI::this_mpi_process(mpi_communicator))
  , n_mpi_processes(Utilities::MPI::n_mpi_processes(mpi_communicator))
  , pcout(std::cout,
        (Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
  , computing_timer(mpi_communicator,
                    pcout,
                    TimerOutput::summary,
                    TimerOutput::wall_times)
{}

void ParallelKL::make_grid()
{
  GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(5);

  //parallel console output
  pcout << "Number of active cells: " << triangulation.n_active_cells()
        << std::endl;
}

void ParallelKL::setup_system()
{
  TimerOutput::Scope t(computing_timer, "setup");

  dof_handler.distribute_dofs(fe);
  pcout << "Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;

  locally_owned_dofs = dof_handler.locally_owned_dofs();
  DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);

  DynamicSparsityPattern dsp(locally_relevant_dofs);
  DoFTools::make_sparsity_pattern(dof_handler, dsp);

  SparsityTools::distribute_sparsity_pattern(dsp,
                                             locally_owned_dofs,
                                             mpi_communicator,
                                             locally_relevant_dofs);

  system_stiffness_matrix.reinit(locally_owned_dofs,
                                 locally_owned_dofs,
                                 dsp,
                                 mpi_communicator);

  system_mass_matrix.reinit(locally_owned_dofs,
                            locally_owned_dofs,
                            dsp,
                            mpi_communicator);

  randomfield_vector.reinit(locally_owned_dofs, mpi_communicator);

  constraints.clear();
  constraints.reinit(locally_relevant_dofs);
  constraints.close();
}

void ParallelKL::assemble_system()
{
  TimerOutput::Scope t(computing_timer, "assembly");

  system_stiffness_matrix = 0.;
  system_mass_matrix = 0.;

  QGauss<2> quadrature_formula(fe.degree + 1);

  FEValues<2> fe_values(fe,
                        quadrature_formula,
                        update_values | update_gradients | update_JxW_values | update_quadrature_points);

  const unsigned int dofs_per_cell = fe.dofs_per_cell;

  FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);
  FullMatrix<double> cell_stiffness_matrix(dofs_per_cell, dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (const auto &cell : dof_handler.active_cell_iterators())
    { if (cell->is_locally_owned())
       {
          fe_values.reinit(cell);
          cell_mass_matrix = 0.;
          cell_stiffness_matrix = 0.;

          for (const unsigned int q_index : fe_values.quadrature_point_indices())
            {
              Point<2> quad_pointq = fe_values.quadrature_point(q_index);
              for (const unsigned int i : fe_values.dof_indices())
                for (const unsigned int j : fe_values.dof_indices())
                  cell_mass_matrix(i, j) +=
                    (fe_values.shape_value(i, q_index) * // grad phi_i(x_q)
                    fe_values.shape_value(j, q_index) * // grad phi_j(x_q)
                    fe_values.JxW(q_index));           // dx

              for (const unsigned int l_index : fe_values.quadrature_point_indices())
                {
                  Point<2> quad_pointl = fe_values.quadrature_point(l_index);
                  const double point_distance = quad_pointl.distance(quad_pointq);

                  for (const unsigned int i : fe_values.dof_indices())
                    for (const unsigned int j : fe_values.dof_indices())
                        cell_stiffness_matrix(i, j) +=
                        (exp(-0.5*point_distance/(0.05*0.05))*          // R(x,x')
                        fe_values.shape_value(i, q_index) * // grad phi_i(x_q)
                        fe_values.shape_value(j, l_index) * // grad phi_j(x_l)
                        fe_values.JxW(q_index)*fe_values.JxW(l_index));           // dx
                }
            }
          cell->get_dof_indices(local_dof_indices);

          constraints.distribute_local_to_global(cell_stiffness_matrix,
                                         local_dof_indices,
                                         system_stiffness_matrix);

          constraints.distribute_local_to_global(cell_mass_matrix,
                                         local_dof_indices,
                                         system_mass_matrix);
       }
    }

system_mass_matrix.compress(VectorOperation::add);
system_stiffness_matrix.compress(VectorOperation::add);

//system_mass_matrix.print(std::cout);
//system_stiffness_matrix.print(std::cout);
}

void ParallelKL::solve()
{
  const unsigned int num_eigenpairs_requested = 100;

  eigenvalues.resize(num_eigenpairs_requested);
  eigenvectors.resize(num_eigenpairs_requested);

  normalized_gaussian.resize(num_eigenpairs_requested);

  for (unsigned int i = 0; i < num_eigenpairs_requested; ++i)
    eigenvectors[i].reinit(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);

  SolverControl eigen_solver_control (10000, 1e-10);

  SLEPcWrappers::SolverKrylovSchur eigensolver(eigen_solver_control, mpi_communicator);

  eigensolver.set_which_eigenpairs(EPS_LARGEST_REAL);

  eigensolver.set_problem_type(EPS_GHEP);

  pcout << "Beginning Eigensolve..." << std::endl;
  eigensolver.solve(system_stiffness_matrix,
                    system_mass_matrix,
                    eigenvalues,
                    eigenvectors,
                    num_eigenpairs_requested);

  for (unsigned int i = 0; i < num_eigenpairs_requested; i++)
  {
    double temporary_sample = 0.0;

    if (this_mpi_process == 0)
      temporary_sample = distribution(generator);

    double temporary_sum = 0.0;

    MPI_Allreduce (&temporary_sample, &temporary_sum, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);

    normalized_gaussian[i] = temporary_sum;

  }
}

void ParallelKL::create_random_field()
{
  randomfield_vector = 0.0;

  PETScWrappers::MPI::Vector tmp_locally_owned_vector(locally_owned_dofs, mpi_communicator);

  for (unsigned int i = 0; i < eigenvalues.size(); i++)
    {
      const double multiplier = sqrt(eigenvalues[i])*normalized_gaussian[i];
      tmp_locally_owned_vector = eigenvectors[i];
      randomfield_vector.add(multiplier, tmp_locally_owned_vector);
    }
}

void ParallelKL::stats_for_random_field()
{
  PETScWrappers::MPI::Vector relevent_randomfield_vector(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);
  relevent_randomfield_vector = randomfield_vector;

  QGauss<2> quadrature_formula(fe.degree + 1);

  FEValues<2> fe_values(fe,
                        quadrature_formula,
                        update_values | update_JxW_values);

  double vol_on_current_processor = 0.;
  double random_field_volume_integral_on_current_processor = 0.;

  std::vector<double> local_dof_values(fe.dofs_per_cell);
  std::vector<double> current_function_values(quadrature_formula.size());

  const FEValuesExtractors::Scalar r_field(0);

  for (const auto &cell : dof_handler.active_cell_iterators())
  { if (cell->is_locally_owned())
       {
          fe_values.reinit(cell);
          cell->get_dof_values(relevent_randomfield_vector, local_dof_values.begin(), local_dof_values.end());
          fe_values[r_field].get_function_values_from_local_dof_values(local_dof_values, current_function_values);

          for (const unsigned int q_index : fe_values.quadrature_point_indices())
            {
              vol_on_current_processor  += fe_values.JxW(q_index);
              random_field_volume_integral_on_current_processor += current_function_values[q_index]*fe_values.JxW(q_index);
            }
       }
  }

  double total_random_field_volume_integral = 0.0;
  double total_volume = 0.0;

  MPI_Allreduce (&random_field_volume_integral_on_current_processor, &total_random_field_volume_integral, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);
  MPI_Allreduce (&vol_on_current_processor, &total_volume, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);

  double volume_average_of_random_field = total_random_field_volume_integral/total_volume;

  pcout << "Volume Average of Random Field = " << volume_average_of_random_field << std::endl;
  pcout << "Total Volume = " << total_volume << std::endl;

}

void ParallelKL::output_results() const
{
  DataOut<2> data_out;
  data_out.attach_dof_handler(dof_handler);
  /*
  Vector<float> subdomain(triangulation.n_active_cells());
  for (unsigned int i = 0; i < subdomain.size(); ++i)
      subdomain(i) = triangulation.locally_owned_subdomain();
  data_out.add_data_vector(subdomain, "subdomain");
*/
  for (unsigned int i = 0; i < eigenvalues.size(); i++)
  {
    //std::cout << eigenvectors[i] << "\n" << std::endl;
    std::string tmpname = "solution";
    tmpname += Utilities::int_to_string(i, 3);
    //std::cout << "Eigen_val = " << eigenvalues[i] << " " << "Normalized_gaussian = " << normalized_gaussian[i] << "\n"  << std::endl;
    data_out.add_data_vector(eigenvectors[i], tmpname);
  }

  data_out.build_patches();
  std::ofstream output("solution.vtk");
  data_out.write_vtk(output);
}

void ParallelKL::output_results_parallel()
{
    TimerOutput::Scope t(computing_timer, "output parallel");

    PETScWrappers::MPI::Vector relevent_randomfield_vector(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);
    relevent_randomfield_vector = randomfield_vector;

    DataOut<2> data_out;

    // ############################################################
    // #######                   OUTPUT                     #######
    // ############################################################

    data_out.add_data_vector(dof_handler, relevent_randomfield_vector,
                               "RandomField");

  /*
    for (unsigned int i = 0; i < eigenvalues.size(); i++)
      {
        std::string tmpname = "solution";
        tmpname += Utilities::int_to_string(i, 3);
        data_out.add_data_vector(eigenvectors[i], tmpname);
      }
      */

    std::vector<Vector<double>> buckling_eigenmodes_out;

    buckling_eigenmodes_out.resize(eigenvalues.size());
    for (unsigned int i = 0; i < eigenvalues.size(); ++i)
      {
        buckling_eigenmodes_out[i].reinit(dof_handler.n_dofs(), 0.0);
        buckling_eigenmodes_out[i] = eigenvectors[i];
        const std::string buckling_mode_string = std::string("EigenVector") + Utilities::int_to_string(i, 2);

        data_out.add_data_vector(dof_handler, buckling_eigenmodes_out[i],
                                 buckling_mode_string);
      }

    // ############################################################
    // #######                 WRITE VTU FILE               #######
    // ############################################################
    data_out.build_patches ();
    const std::string filename = "Solution." +
                                  Utilities::int_to_string (this_mpi_process, 2) + ".vtu";

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

    // ############################################################
    // #######                 WRITE PVTU FILE              #######
    // ############################################################
    if (this_mpi_process == 0)
    {
      std::vector<std::string> filenames;
      for (unsigned int i=0; i < n_mpi_processes; ++i)
        filenames.push_back ("Solution." +
                                  Utilities::int_to_string (i, 2) + ".vtu");

      std::ofstream master_output ("Solution.pvtu");

      data_out.write_pvtu_record (master_output, filenames);
    }
}

void ParallelKL::run()
{
  pcout << "Running with " << "PETSc" << " on " << Utilities::MPI::n_mpi_processes(mpi_communicator)
          << " MPI rank(s)..." << std::endl;

  make_grid();

  setup_system();

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

  assemble_system();
  solve();
  create_random_field();
  stats_for_random_field();

  if (Utilities::MPI::n_mpi_processes(mpi_communicator) <= 32)
    {
      TimerOutput::Scope t(computing_timer, "output");
      //output_results();
      output_results_parallel();

    }
}

int main(int argc, char* argv[])
{
  Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

  deallog.depth_console(0);

  ParallelKL klexpansion_2d;
  klexpansion_2d.run();
  return 0;
}


#include "testsuite.h"
#include <blitz/array.h>
#include <blitz/array.cc>
#include <blitz/array/stencil-et.h>
#include <blitz/array/stencilops.h>
#include <blitz/array/stencil-et-macros.h>
//#include <blitz/tinyvec-et.h>
//#include <blitz/matrix.h>
//#include <blitz/tinymatexpr.h>
#include <random/uniform.h>

BZ_USING_NAMESPACE(blitz)

// Tests that the various stencil operators work with
// expressions. Does NOT test that the stencils produce the correct
// output, only that they compile and that the output is consistent
// between different identical expressions.

typedef blitz::Array<double,1> array_1;
typedef blitz::Array<double,2> array_2;
typedef blitz::Array<double,3> array_3;
typedef Array<TinyVector<double, 2>, 2> array_2v;
typedef Array<TinyMatrix<double, 2, 2>, 2> array_2m;
typedef Array<TinyVector<double, 3>, 3> array_3v;
typedef Array<TinyMatrix<double, 3, 3>, 3> array_3m;

// test with functors
class doubler {
public:
  double operator()(double x) const {return 2.0*x;}
  BZ_DECLARE_FUNCTOR(doubler);
};

class multiplier {
public:
  double operator()(double a, double b) const {return a*b;}
  BZ_DECLARE_FUNCTOR2(multiplier);
};
/*
// Test two expressions for equality
template<typename T1, typename T2>
void test_expr(const T1& d1, const T2& d2)
{
  BZTEST(all(d1==d2));
}
*/
#define test_expr(d1,d2) BZTEST(all((d1)==(d2)));

// Test two vector expressions for equality
template<typename T1, typename T2>
void test_vexpr(const T1& d1, const T2& d2)
{
  Array<typename T1::T_numtype, T1::rank_> a(d1),b(d2);
  for(int i=0; i<T1::T_numtype::numElements(); ++i)
    BZTEST(all(a[i]==b[i]));
}

// Test two matrix expressions for equality
template<typename T1, typename T2>
void test_mexpr(const T1& d1, const T2& d2)
{
  // there appears to be no way to tell the size of a TinyMatrix... or to compare them.
  // so for now we're happy just to have the call succeed.
}

BZ_DECLARE_DIFF(shifter) {
  return A.shift(1,dim); }

BZ_ET_STENCIL_DIFF(shifter, 1,1)

int main()
{
  // create some arrays to operate on
  const int sz=5;

  ranlib::Uniform<float> rnd;
  rnd.seed(42);

  array_3 field3(sz,sz+1,sz+2), result3(sz,sz+1,sz+2),
    fx(field3.shape()), fy(field3.shape()), fz(field3.shape());
  for(int i=0; i<field3.size();++i) {
    field3.data()[i]=rnd.random();
    fx.data()[i]=rnd.random();
    fy.data()[i]=rnd.random();
    fz.data()[i]=rnd.random();
  }
  array_2 field2(sz,sz+1), result2(field2.shape());
  field2=sin(0.5*(tensor::i+2*tensor::j));
  array_3v vfield3(field3.shape());
  vfield3[0]=fx;
  vfield3[1]=fy;
  vfield3[2]=fz;
  array_2v vfield2(sz,sz+1);
  vfield2[0]=vfield3(Range::all(), Range::all(), 0)[0];
  vfield2[1]=vfield3(Range::all(), Range::all(), 0)[1];

  doubler doubleit;
  multiplier multiplyit;

  // Now apply "all" possible stencil types to arrays and expressions,
  // as well as recursive applications

  // defined with BZ_ET_STENCIL:
  test_expr(Laplacian2D(field2), Laplacian2D(1.0*field2));
  test_expr(Laplacian2D(field2), 1.0*Laplacian2D(field2));
  test_expr(Laplacian2D(const_cast<const array_2&>(field2)),
	    Laplacian2D(1.0*field2));
  test_expr(Laplacian2D(Laplacian2D(field2)),
	    Laplacian2D(Laplacian2D(1.0*field2)));
  test_expr(Laplacian2D(Laplacian2D(field2)),
	    Laplacian2D(1.0*Laplacian2D(field2)));
  test_expr(Laplacian2D(Laplacian2D(field2+field2)),
	    Laplacian2D(Laplacian2D(field2)+Laplacian2D(field2)));
  test_expr(Laplacian2D(field3), Laplacian2D(1.0*field3));

  // and some more complicated expressions and assignments
  result2(_bz_shrinkDomain(result2.domain(),shape(-1,-1),shape(1,1))) =
    Laplacian2D(where(field2>0.5,0.,1.));
  test_expr(result2(_bz_shrinkDomain(result2.domain(),shape(-1,-1),shape(1,1))),
	    Laplacian2D(where(field2>0.5,0.,1.)));
  test_expr(where(Laplacian2D(field2)>0.5,0.,1.),
	    where(Laplacian2D(2*field2)>1,0.,1.));
  test_expr(where(Laplacian2D(field2)>0.5,
		  0.0*field2(_bz_shrinkDomain(result2.domain(),shape(-1,-1),shape(1,1))),
		  0.0*field2(_bz_shrinkDomain(result2.domain(),shape(-1,-1),shape(1,1)))+2.0),
	    2*where(Laplacian2D(2*field2)>1.0, 0., 1.));
  test_expr(Laplacian2D(2.0*field2), Laplacian2D(doubleit(field2)));
  test_expr(Laplacian2D(field2*field3(0,Range(0,sz-1), Range(1,sz+1))),
	    Laplacian2D(multiplyit(field2, field3(0,Range(0,sz-1), Range(1,sz+1)))));

  // reductions of stencil results
  {
    array_2 temp(Laplacian2D(field2));
    BZTEST(sum(temp) == sum(Laplacian2D(field2)));
    test_expr(sum(temp, tensor::j), sum(Laplacian2D(field2), tensor::j));
  }
  {
    array_3 temp(Laplacian2D(field3));
    BZTEST(sum(temp) == sum(Laplacian2D(field3)));
    test_expr(sum(temp, tensor::k), sum(Laplacian2D(field3), tensor::k));
  }

  // and expressions involving index remappings. we do these on arrays
  // with different sizes in all dimensions to make it less likely we
  // don't detect a screwup
  test_expr(shifter(field2,firstDim),
	    shifter(field2(tensor::i, tensor::j),firstDim));
  {
    array_2 temp(field2(tensor::j, tensor::i));
    test_expr(shifter(temp,firstDim),
	      shifter(field2(tensor::j, tensor::i),firstDim));
    test_expr(shifter(temp,secondDim),
	      shifter(field2(tensor::j, tensor::i),secondDim));
  }
  test_expr(shifter(field3,firstDim),
	    shifter(field3(tensor::i, tensor::j, tensor::k),firstDim));
  test_expr(shifter(field3,thirdDim),
	    shifter(field3(tensor::i, tensor::j, tensor::k),thirdDim));

  {
    array_3 temp(shifter(field3,thirdDim));
    test_expr(temp(tensor::i, tensor::k, tensor::j),
	      shifter(field3(tensor::i, tensor::k, tensor::j),secondDim));
	      }
  {
    array_3 temp(Laplacian3D(field3));
    test_expr(temp(tensor::k, tensor::i, tensor::j),
	      Laplacian3D(field3(tensor::k, tensor::i, tensor::j)));
  }
  {
    array_3 temp(field3.shape());
    temp=field3(tensor::i, tensor::j, tensor::k)*field2(tensor::i, tensor::j);
    test_expr(Laplacian3D(temp),
	      Laplacian3D(field3(tensor::i, tensor::j, tensor::k)*
			  field2(tensor::i, tensor::j)));
    test_expr(mixed22(temp, firstDim, secondDim),
	      mixed22(field3(tensor::i, tensor::j, tensor::k)*
		      field2(tensor::i, tensor::j), firstDim, secondDim));
  }
  /* index placeholders don't work
  { array_3 temp(field3.shape());
    temp=100*tensor::k+10*tensor::j+tensor::i;
    test_expr(Laplacian3D(temp), Laplacian3D(100*tensor::k+10*tensor::j+tensor::i));
  }
  */

  // defined with BZ_ET_STENCIL2:
  test_expr(div(vfield2[0],vfield2[1]),
	    div(vfield2[0],1.0*vfield2[1]));
  test_expr(div(vfield2[0],vfield2[1]),
	    div(1.0*vfield2[0],vfield2[1]));
  test_expr(div(vfield2[0],vfield2[1]),
	    div(1.0*vfield2[0],1.0*vfield2[1]));
  test_expr(div(vfield2[0],vfield2[1]),
	    div(1.0*vfield2[0],const_cast<const array_2v&>(vfield2)[1]));
  test_expr(div(vfield2[0],vfield2[1]),
	    div(const_cast<const array_2v&>(vfield2)[0],
		const_cast<const array_2v&>(vfield2)[1]));

  // defined with BZ_ET_STENCILM.
  test_mexpr(Jacobian3D(vfield3),
	     Jacobian3D(const_cast<const array_3v&>(vfield3)));
  test_mexpr(Jacobian3D(vfield3),
	     Jacobian3D(1.0*vfield3));
  test_mexpr(Jacobian3D(const_cast<const array_3v&>(vfield3)),
	     Jacobian3D(1.0*vfield3));

  // defined with BZ_ET_STENCILV
  test_vexpr(grad3D(field3),
	     grad3D(const_cast<const array_3&>(field3)));
  test_vexpr(grad3D(field3),
	     grad3D(1.0*field3));

  // defined with BZ_ET_STENCIL_SCA
  test_expr(div2D(vfield2),
	    div2D(const_cast<const array_2v&>(vfield2)));
  test_expr(div2D(vfield2),
	    div2D(1.0*vfield2));

  // defined with BZ_ET_STENCIL_DIFF
  test_expr(central12(field3, firstDim),
	    central12(const_cast<const array_3&>(field3), firstDim));
  test_expr(central12(field3, firstDim),
	    central12(1.0*field3, firstDim));

  result2(_bz_shrinkDomain(result2.domain(),shape(0,-1),shape(0,1))) =
    central12(where(field2>0.5,0.,1.), secondDim);
  test_expr(result2(_bz_shrinkDomain(result2.domain(),shape(0,-1),shape(0,1))),
	    central12(where(field2>0.5,0.,1.), secondDim));
  test_expr(where(central12(field2,firstDim)>0.5,0.,1.),
	    where(central12(2*field2, firstDim)>1,0.,1.));
  test_expr(where(central12(field2, firstDim)>0.5,
		  0.0*field2(_bz_shrinkDomain(field2.domain(),shape(-1,0),shape(1,0))),
		  0.0*field2(_bz_shrinkDomain(field2.domain(),shape(-1,0),shape(1,0)))+2.0),
	    2*where(central12(2*field2, firstDim)>1.0, 0., 1.));
  test_expr(central12(sin(1.0*field3),thirdDim),
	    central12(1.0*sin(field3), thirdDim));
  result2 = pow(field3(Range::all(), 1, Range(0,sz)), field2);
  test_expr(central12(result2, secondDim),
	    central12(pow(field3(Range::all(), 1, Range(0,sz)), 1.0*field2), secondDim));

  // defined with BZ_ET_STENCIL_MULTIDIFF
  test_expr(central12(vfield3, firstDim, secondDim),
	    central12(const_cast<const array_3v&>(vfield3), firstDim, secondDim));

  array_3v ee(1.0*vfield3);
  test_expr(central12(vfield3, firstDim, secondDim),
	    central12(1.0*vfield3, firstDim, secondDim));
  test_expr(where(central12(vfield2, firstDim, secondDim)>0.5,
		  0.0*field2(_bz_shrinkDomain(field2.domain(),shape(0,-1),shape(0,1))),
		  0.0*field2(_bz_shrinkDomain(field2.domain(),shape(0,-1),shape(0,1)))+2.0),
	    2*where(central12(2*vfield2, firstDim, secondDim)>1.0, 0., 1.));

  // defined with BZ_ET_STENCIL_DIFF2
  test_expr(mixed22(field3, firstDim, secondDim),
	    mixed22(const_cast<const array_3&>(field3), firstDim, secondDim));
  test_expr(mixed22(field3, firstDim, secondDim),
	    mixed22(1.0*field3, firstDim, secondDim));
  test_expr(where(mixed22(field3, thirdDim, secondDim)>0.5,
		  0.0*field3(_bz_shrinkDomain(field3.domain(),shape(0,-1,-1),shape(0,1,1))),
		  0.0*field3(_bz_shrinkDomain(field3.domain(),shape(0,-1,-1),shape(0,1,1)))+2.0),
	    2*where(mixed22(2*field3, thirdDim, secondDim)>1.0, 0., 1.));

    return 0;
}


/**
 * @file radauthreetimestepping_main.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <iostream>
#include <memory>

#include "radauthreetimestepping.h"
#include "radauthreetimesteppingode.h"

using namespace RadauThreeTimestepping;

int main(int /*argc*/, char ** /*argv*/) {
  /* Solving the ODE problem */
  // This function prints to the terminal the convergence rates and average rate
  // of a convergence study performed for the ODE (d/dt)y = -y.
  testConvergenceTwoStageRadauLinScalODE();

  /* Solving the parabolic heat equation */
  // Create a Lehrfem++ square tensor product mesh
  lf::mesh::utils::TPTriagMeshBuilder builder(
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
  // Set mesh parameters following the Builder pattern
  // Domain is the unit square
  builder.setBottomLeftCorner(Eigen::Vector2d{-1.0, -1.0})
      .setTopRightCorner(Eigen::Vector2d{1, 1})
      .setNumXCells(50)
      .setNumYCells(50);
  auto mesh_p = builder.Build();

  /* SAM_LISTING_BEGIN_1 */
  //====================
  // Your code goes here
  //====================

  return 0;
}