#ifndef EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP
#define EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP

#include <armadillo>

#include "fftw3/shared_plan.hpp"

namespace expsum
{
/*!
 * Fast matrix-vector product for generalized Hankel matrix.
 *
 * A general Hankel matrix is a matrix of special form give as
 *
 * \f[
 *   A = \left[ \begin{array}{cccccc}
 *     h_0 & h_1 & h_2 & h_3 & \cdots & h_{n-1} \\
 *     h_1 & h_2 & h_3 & h_4 & \cdots & h_{n}   \\
 *     h_2 & h_3 & h_4 & h_5 & \cdots & h_{n+1} \\
 *     h_3 & h_4 & h_5 & h_6 & \cdots & h_{n+2} \\
 *     \vdots & \vdots & \vdots &\vdots &\ddots & \vdots \\
 *     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1} \\
 *   \end{array} \right],
 * \f]
 *
 * where \f$ m \f$ and \f$ n \f$ are the number of rows and colums of matrix.
 * From the definition above, a general Hankel matrix \f$ A \f$ can fully be
 * determined by a vector \f$ h = [h_0,h_1,...,h_{n+m-1}]^{T}\f$ composed of the
 * elements of first column and last row where \f$ A_{ij} = h_{i+j}.\f$
 *
 * This class compute the matrix-vector product,
 *
 * \f[
 *   \bm{y} = A \bm{x},
 * \f]
 *
 * where \f$ \bm{x} = [x_0,x_1,\dots,x_{n-1}]^{T} \f$ and \f$ \bm{y} =
 * [y_0,y_1,\dots,y_{m-1}]^{T}. \f$ This product can be efficiently computed by
 * the fast Fourier transfor (FFT), as follows.
 *
 * Let define a new vector \f$ \hat{\bm{c}} \f$ of size \f$ n + m - 1 \f$ as
 *
 * \f[
 *   \hat{\bm{c}}=[h_{n-1},\dots,h_{n+m-2},h_{0},\dots,h_{n-2}]^{T},
 * \f]
 *
 * and corresponding circulant matrix
 *
 * \f[
 *   C = \left[ \begin{array}{}
 *         c_0     & c_{n+m-2} & \cdots    & c_{2}  & c_{1} \\
 *         c_1     & c_{0}     & c_{n+m-2} &        & c_{2} \\
 *         \vdots  & c_{1}     & c_{0}     & \ddots & \vdots    \\
 *         c_{n+m-3} &         & \ddots    & \ddots & c_{n+m-2} \\
 *         c_{n+m-2} & c_{n+m-3} & \cdots  & c_{1}  & c_{0}
 *       \end{array} \right].
 * \f]
 *
 * For a given vector \f$ \bm{x} \f$ of length \f$ n, \f$ we define a auxilialy
 * vector of length \f$ n + m - 1 \f$ as
 *
 * \f[
 *   \hat{\bm{x}}=[x_{n-1},x_{n-2},\dots,x_{0},0,\dots,0]^{T}.
 * \f]
 *
 * Then, the result vector \f$ \bm{y} \f$ can be obtained as the first
 * \f$ m \f$-elemets of the vector \f$ \hat{\bm{y}}\equiv C\hat{\bm{x}}. \f$
 * The produt \f$ C\hat{\bm{x}} \f$ can be evaluated as,
 *
 * \f[
 *   \hat{\bm{y}} = \text{IFFT}(\text{FFT}(\hat{\bm{c}}) \odot
 *                              \text{FFT}(\hat{\bm{x}})).
 * \f]
 *
 * Here, \$f \text{FFT}(\bm{v}) \f$ and \$f \text{IFFT}(\bm{v}) \f$ denote
 * one-dimensional FFT and inverse FFT of vector \bm{v}, and \f$ \odot \f$
 * denotes a element-wise multiplication of two vectors.
 *
 * The computational complexity of this algorithm is \f$
 * \mathcal{O}((m+n-1)\log(m+n-1)) \f$, rather than \f$ \mathcal{O}(mn) \f$
 * for ordinary dense matrix-vector operation in BLAS2.
 *
 * For the computational efficiency, the FFT of the vector \f$ \hat{\bm{c}} \f$
 * is pre-computed and stored internally.
 *
 */

template <typename T>
class hankel_gemv
{
public:
    using size_type   = arma::uword;
    using value_type  = T;
    using vector_type = arma::Col<value_type>;

    using real_type           = typename vector_type::pod_type;
    using complex_type        = std::complex<real_type>;
    using complex_vector_type = arma::Col<complex_type>;

private:
    using fft  = fftw3::fft<real_type>;
    using ifft = fftw3::ifft<real_type>;

    typename fft::plan_pointer fft_plan_;
    typename ifft::plan_pointer ifft_plan_;

    size_type nrows_;
    size_type ncols_;

    mutable vector_type work_;
    complex_vector_type caux_;
    mutable complex_vector_type xaux_;

public:
    /// Default constructor
    hankel_gemv() = default;

    /// Create an Hankel matrix operator with memory preallocation.
    hankel_gemv(size_type nrows, size_type ncols, size_type fft_size = 0)
        : fft_plan_(),
          ifft_plan_(),
          nrows_(nrows),
          ncols_(ncols),
          work_(std::max(fft_size, nrows + ncols - 1)),
          caux_(arma::is_complex<value_type>::value ? work_.size()
                                                    : work_.size() / 2 + 1),
          xaux_(caux_.size())
    {
        set_fft_plans();
    }

    /// Copy constructor (default)
    hankel_gemv(const hankel_gemv&) = default;

    /// Move constructor (default)
    hankel_gemv(hankel_gemv&&) = default;

    /// Destructor (default)
    ~hankel_gemv() = default;

    /// Copy assignment operator
    hankel_gemv& operator=(const hankel_gemv&) = default;

    /// Move assignment operator
    hankel_gemv& operator=(hankel_gemv&&) = default;

    /// @return the number of rows of the Hankel matrix
    size_type nrows() const
    {
        return nrows_;
    }
    /// @return the number of columns of the Hankel matrix
    size_type ncols() const
    {
        return ncols_;
    }
    /// @return number of coefficients that defines this Hankel matrix
    size_type size() const
    {
        return nrows() + ncols() - 1;
    }

    /// Reallocate internal memory space
    void resize(size_type nrows, size_type ncols, size_type fft_size = 0)
    {
        fft_size = std::max(fft_size, nrows + ncols - 1);

        nrows_ = nrows;
        ncols_ = ncols;
        work_.set_size(fft_size);
        caux_.set_size(arma::is_complex<value_type>::value
                           ? work_.size()
                           : work_.size() / 2 + 1);
        xaux_.set_size(caux_.size());

        set_fft_plans();
    }
    ///
    /// Set coefficients that defines the Hankel matrix.
    ///
    template <typename T1>
    typename std::enable_if<arma::is_arma_type<T1>::value>::type
    set_coeffs(const T1& coeffs)
    {
        assert(coeffs.is_vec() && coeffs.n_elem == size());
        //
        // Set first column of circulant matrix C. Then compute the discrete
        // Fourier transform this vector and store the result into \c caux.
        //
        const auto nhead    = nrows();
        const auto ntail    = ncols() - 1;
        const auto npadding = work_.size() - nhead - ntail;

        work_.head(nhead) = coeffs.tail(nhead);
        if (npadding > size_type())
        {
            work_.subvec(nhead, nhead + npadding - 1).zeros();
        }
        work_.tail(ntail) = coeffs.head(ntail);

        // caux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), caux_.memptr());
        caux_ *= real_type(1) / work_.size();
    }

    ///
    /// Compute `y = A * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply(const U1& x, value_type beta, U2& y) const
    {
        assert(x.is_vec() && x.n_elem == ncols());
        assert(y.is_vec() && y.n_elem == nrows());
        //
        // Form new vector x' = [x(n-1),x(n-2),...,x(0),0....0] of length
        // n + m - 1, and compute FFT.
        //
        work_.head(ncols()) = arma::flipud(x);
        work_.tail(work_.size() - ncols()).zeros();
        // xaux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), xaux_.memptr());
        //
        // y[0:nrows] = IFFT(FFT(c') * FFT(x'))[0:nrows]
        //
        xaux_ %= caux_;
        ifft::run(ifft_plan_, xaux_.memptr(), work_.memptr());
        if (beta == value_type())
        {
            y = work_.head(nrows());
        }
        else
        {
            y = work_.head(nrows()) + beta * y;
        }
    }

    ///
    /// Compute `y = A.t() * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply_trans(const U1& x, value_type beta, U2& y) const
    {
        assert(x.is_vec() && x.n_rows == nrows());
        assert(y.is_vec() && y.n_rows == ncols());
        //
        // Form new vector x' = [0,0,...,0,x(m-1),x(m-2),...,x(0)] of length
        // n + m - 1, and compute FFT.
        //
        work_.head(work_.size() - nrows()).zeros();
        work_.tail(nrows()) = arma::conj(arma::flipud(x));
        // xaux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), xaux_.memptr());
        //
        // y[0:nrows-1] = IFFT(FFT(c') * FFT(x'))[0:nrows-1]
        //
        xaux_ %= caux_;
        ifft::run(ifft_plan_, xaux_.memptr(), work_.memptr());
        if (beta == value_type())
        {
            y = arma::conj(work_.tail(ncols()));
        }
        else
        {
            y = arma::conj(work_.tail(ncols())) + beta * y;
        }
    }

private:
    void set_fft_plans()
    {
        const int n       = static_cast<int>(work_.size());
        const int howmany = 1;
        fft_plan_ = fft::make_plan(n, howmany, work_.memptr(), xaux_.memptr());
        ifft_plan_ =
            ifft::make_plan(n, howmany, xaux_.memptr(), work_.memptr());
    }
};

/*!
 * Create a Hankel matrix in dense form from the sequence of elements.
 *
 * This function creates a \f$ m \times n \f$ Hankel matrix \f$ A \f$ in the
 * dense form from a given vector of matrix element \f$ h =
 * [h_0,h_1,...,h_{n+m-1}]^{T}\f$ such that,
 *
 * \f[
 *   A = \left[ \begin{array}{}
 *     h_0 & h_1 & h_2 & h_3 & \cdots & h_{n-1} \\
 *     h_1 & h_2 & h_3 & h_4 & \cdots & h_{n}   \\
 *     h_2 & h_3 & h_4 & h_5 & \cdots & h_{n+1} \\
 *     h_3 & h_4 & h_5 & h_6 & \cdots & h_{n+2} \\
 *     \vdots & \vdots & \vdots &\vdots &\ddots & \vdots \\
 *     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1} \\
 *   \end{array} \right]
 * \f]
 *
 * \param[in] nrows number of rows, \f$ m \f$
 * \param[in] ncols number of columns, \f$ n \f$
 * \param[in] h vector of elments of Hankel matrix with length \c nrows+ncols-1
 * \return \c arma::Mat with same scalar type of input vector type \c T1.
 */
template <typename T1>
typename std::enable_if<arma::is_arma_type<T1>::value,
                        arma::Mat<typename T1::elem_type>>::type
make_dense_hankel(arma::uword nrows, arma::uword ncols, const T1& h)
{
    assert(h.n_elem == nrows + ncols - 1);
    arma::Mat<typename T1::elem_type> A(nrows, ncols);

    for (arma::uword col = 0; col < ncols; ++col)
    {
        for (arma::uword row = 0; row < nrows; ++row)
        {
            A(row, col) = h(row + col);
        }
    }

    return A;
}

namespace detail
{
template <typename T>
inline T abs2(T x)
{
    return x * x;
}

template <typename T>
inline T abs2(std::complex<T> x)
{
    return std::real(x) * std::real(x) + std::imag(x) * std::imag(x);
}
} // namespace: detail

/*!
 * Compute the Frobenius norm of general Hankel matrix.
 *
 * This function computes the Frobenius norm of \c nrows-by-ncols general Hankel
 * matrix defined by the given vector of elements.
 *
 * \param[in] nrows number of rows
 * \param[in] ncols number of columns
 * \param[in] h a vector that determines the Hankel matrix \f$ A \f$.  If <tt>
 * h.size() >= N </tt> with <tt> N = nrows + ncols - 1, </tt> first \c N elemnts
 * are refered as the elements of Hankel matrix. If <tt> h.size() < N, </tt>
 * rest of elements are assumed to be zero.
 */

template <typename T1>
typename T1::pod_type fnorm_hankel(arma::uword nrows, arma::uword ncols,
                                   const T1& h)
{
    arma::uword m, n;
    std::tie(m, n) = std::minmax(nrows, ncols);
    arma::uword l = m + n - 1;

    auto sqsum = typename T1::pod_type();

    if (h.n_elem > m)
    {
        for (arma::uword i = 0; i < m; ++i)
        {
            sqsum += (i + 1) * std::norm(h(i));
        }

        if (h.n_elem > n)
        {
            for (arma::uword i = m; i < n; ++i)
            {
                sqsum += m * std::norm(h(i));
            }

            for (arma::uword i = n; i < std::min(l, h.n_elem); ++i)
            {
                sqsum += (l - i) * std::norm(h(i));
            }
        }
        else
        {
            for (arma::uword i = m; i < h.n_elem; ++i)
            {
                sqsum += m * std::norm(h(i));
            }
        }
    }
    else
    {
        for (arma::uword i = 0; i < h.n_elem; ++i)
        {
            sqsum += (i + 1) * std::norm(h(i));
        }
    }

    return std::sqrt(sqsum);
}
} // namespace: expsum

#endif /* EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP */


// Driver program for solution to Advent of Code, Day 22
// Jeff Trull <edaskel@att.net>

#include <iostream>
#include <fstream>
#include <regex>
#include <algorithm>

#include <boost/coroutine2/all.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/properties.hpp>

#include "graph.h"

struct goal_reached {
    server_state_t state;
};

// specialize an astar visitor to detect when we've reached our goal
struct goal_state_finder : public boost::default_astar_visitor {

    void examine_vertex( server_state_t state, move_graph_t const& g) {
        if ((g.servers()[state.data_offset()].x == 0) &&
            (g.servers()[state.data_offset()].y == 0)) {
            throw goal_reached{state};
        }
    }
};

// a heuristic to guide the A* search
struct server_move_heuristic_t {
    server_move_heuristic_t(std::vector<server_t> const& servers) : servers_(servers) {}

    using vertex_t = move_graph_t::vertex_t;

    int operator()(const vertex_t& v) const {

        // Heuristic plan:
        // We need enough steps to move the original data to the origin
        // That ends up being about 5 times the Manhattan distance due to the need to move
        // the "blank tile" (server with sufficient capacity) back into place between the
        // target data and the origin each time.
        // In addition, we need to move the "blank tile" into position in the first place.

        // Manhattan distance to goal
        // we must make at least this many moves to get the original data home
        server_t current_server = servers_[v.data_offset()];
        int mdist = current_server.x + current_server.y;

        // if mdist is 0, we are at the target, so simply return 0
        if (mdist == 0) {
            return 0;
        }

        // Finding the distance to the "blank tile"
        // First, find the nearest (to the origin) server of sufficient reserve capacity
        // to hold the target data

        std::vector<server_t> eligible_servers;
        for (size_t i = 0; i < servers_.size(); ++i) {
            if ((servers_[i].capacity - v.usage(i)) >= v.usage(v.data_offset())) {
                eligible_servers.push_back(servers_[i]);
            }
        }
        // take the one with the minimum Manhattan distance to the server with our data
        auto min_it = min_element(eligible_servers.begin(), eligible_servers.end(),
                                  [&current_server](server_t const& a, server_t const& b) {
                                      return ((abs(current_server.x - a.x) + abs(current_server.y - a.y)) <
                                              (abs(current_server.x - b.x) + abs(current_server.y - b.y)));
                                  });
        assert(min_it != eligible_servers.end());   // insoluble!

        // calculate distance to point above or to left of target data, whichever is shorter
        int hole_dist = std::numeric_limits<int>::max();
        if (current_server.y > 0) {
            // distance to point above target
            hole_dist = abs((current_server.y - 1) - min_it->y) + abs(current_server.x - min_it->x);
        }
        if (current_server.x > 0) {
            // distance to point left of target
            hole_dist = std::min(hole_dist,
                                 abs((current_server.x - 1) - min_it->x) + abs(current_server.y - min_it->y));
        }

        // each move of the target data requires 5 moves overall, except for the last one
        return (5*(mdist-1)+ 1) + hole_dist;
    }

private:

    std::vector<server_t> const & servers_;

};

int main(int argc, char **argv) {
    using namespace std;

    if (argc != 2) {
        cerr << "usage: day22 input.txt\n";
        return 1;
    }

    ifstream input(argv[1]);
    if (!input.is_open()) {
        cerr << "error opening " << argv[1] << "\n";
        return 1;
    }

    regex df_re(R"(^/dev/grid/node-x(\d+)-y(\d+)\s+(\d+)T\s+(\d+)T\s.*)");
    vector<server_t> servers;
    using capacity_t = server_t::capacity_t;
    std::vector<capacity_t> usages;
    while (!input.eof()) {
        string instr;
        getline(input, instr);
        match_results<string::iterator> matches;
        if (regex_match(instr.begin(), instr.end(), matches, df_re)) {
            // collect info on this server
            int capacity = stoi(matches.str(3));
            assert(capacity <= std::numeric_limits<capacity_t>::max());
            servers.push_back({
                    stoi(matches.str(1)),
                    stoi(matches.str(2)),
                    static_cast<capacity_t>(capacity)});
            int usage = stoi(matches.str(4));
            assert(usage <= std::numeric_limits<capacity_t>::max());
            usages.push_back(usage);
        }
    }
    move_graph_t move_graph(servers);

    server_state_t   initial_state(move_graph.ur_corner(),
                                   usages.begin(), usages.end());

    // now see how many viable pairs there are
    // create a generator from the pair calculation:

    using namespace boost::coroutines2;
    using viable_pair_coro_t = boost::coroutines2::coroutine<std::pair<int, int>>;
    auto viable_pair_generator =
        [&](viable_pair_coro_t::push_type & sink) {
        // faster way is to sort by capacity but this is good enough for now
        for (size_t i = 0; i < servers.size(); ++i) {
            for (size_t j = 0; j < servers.size(); ++j) {
                if (initial_state.usage(i) == 0) {
                    continue;
                }

                if (i == j) {
                    continue;
                }

                if (initial_state.usage(i) <= (servers[j].capacity - initial_state.usage(j))) {
                    // room to move there, if there is a path
                    sink(make_pair(i, j));
                }
            }
        }};

    // create a sequence from the generator
    viable_pair_coro_t::pull_type viable_pairs(viable_pair_generator);

    // count pairs in sequence
    size_t viable_pair_count = 0;
    for (auto const& v : viable_pairs) {
        (void)v;
        ++viable_pair_count;
    }

    cout << viable_pair_count << " viable pairs\n";

    // next, find a sequence of moves of data that will result in the data in the
    // upper right being in the upper left

    // requirements for A* search
    using vertex_t = move_graph_t::vertex_t;
    map<vertex_t, size_t> vertex_index_map;
    map<vertex_t, size_t> rank_map;
    map<vertex_t, boost::default_color_type> color_map;
    map<vertex_t, vertex_t> predecessor_map;  // results (path) storage
    // the distance map needs to default to a large number instead of 0
    // because initially all vertices have unknown paths to them
    map<vertex_t, size_t> distance_map;
    auto distance_lookup =
        [&distance_map](vertex_t const& v) -> size_t& {
        if (distance_map.find(v) == distance_map.end()) {
            distance_map[v] = numeric_limits<size_t>::max();
        }
        return distance_map[v];
    };
    auto distance_pmap =
        boost::make_function_property_map<vertex_t, size_t&, decltype(distance_lookup)>(
            distance_lookup);

    // set up initial state
    distance_map[initial_state] = 0;
    predecessor_map[initial_state] = initial_state;

    using namespace boost;
    try {
        // no_init is the appropriate variant for implicit graphs like ours
        astar_search_no_init(
            move_graph,
            initial_state,
            server_move_heuristic_t(servers),
            // named params
            weight_map(make_static_property_map<vertex_t, size_t>(1)).
            vertex_index_map(associative_property_map<map<vertex_t, size_t>>(vertex_index_map)).
            rank_map(associative_property_map<map<vertex_t, size_t>>(rank_map)).
            distance_map(distance_pmap).
            color_map(associative_property_map<map<vertex_t, default_color_type>>(color_map)).
            visitor(goal_state_finder()).
            predecessor_map(associative_property_map<map<vertex_t, vertex_t>>(predecessor_map))
            );
    } catch (goal_reached const& e) {
        // reverse path for display
        vector<server_state_t> soln_path;
        auto next_state = e.state;
        do {
            soln_path.push_back(next_state);
            next_state = predecessor_map[next_state];
        } while (!(soln_path.back() == next_state));
        reverse(soln_path.begin(), soln_path.end());

        // describe the path
        cout << "solution: " << (soln_path.size() - 1) << " steps to goal state:\n";
        copy(soln_path.begin(), soln_path.end(),
                  ostream_iterator<server_state_t>(cout, "\n"));

        return 0;
    }
    cerr << "could not find solution\n";
    return 1;
}


#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>


/**
 * @file locallaplaceqfe.cc
 * @brief NPDE homework ParametricElementMatrices code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include "locallaplaceqfe.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>

namespace DebuggingFEM {

Eigen::Matrix<double, 6, 6> LocalLaplaceQFE1::Eval(
    const lf::mesh::Entity &cell) {
  // Query (topological) type of cell/reference element
  const lf::base::RefEl ref_el{cell.RefEl()};
  // Verify that the cell is a triangle
  LF_ASSERT_MSG(ref_el == lf::base::RefEl::kTria(),
                "Implemented for triangles only not for " << ref_el);
  // The final element matrix has size 6x6
  Eigen::Matrix<double, 6, 6> result{};
  // Obtain the vertex coordinates of the triangle
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  LF_ASSERT_MSG(geo_ptr != nullptr, "Invalid geometry!");
  // Matrix storing corner coordinates in its columns
  Eigen::Matrix<double, 2, 3> vertices{geo_ptr->Global(ref_el.NodeCoords())};
  // Comopute element matrix for negative Laplacian and lowest-order Lgrangian
  // finite elements as in Remark 2.4.5.9. in the course notes
  Eigen::Matrix<double, 3, 3> X;  // temporary matrix
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = vertices.transpose();
  const double area = 0.5 * std::abs(X.determinant());
  // Initialize gradients!
  auto grad_bary_coords{X.inverse().block<2, 3>(1, 0)};

  // Returns all gradients of the local shape functions for quadatic Lagrangian
  // finite elements in the columns of a matrix. The gradients are evaluated at
  // a point specified by its reference coordinates.
  auto gradientsLocalShapeFunctions =
      [&grad_bary_coords](
          const Eigen::Vector2d xh) -> Eigen::Matrix<double, 2, 6> {
    Eigen::Matrix<double, 2, 6> gradients;
    // barycentric coordinate functions
    const std::array<double, 3> l{1.0 - xh[0] - xh[1], xh[0], xh[1]};
    gradients.col(0) = grad_bary_coords.col(0) * (4 * l[0] - 1);
    gradients.col(1) = grad_bary_coords.col(1) * (4 * l[1] - 1);
    gradients.col(2) = grad_bary_coords.col(2) * (4 * l[2] - 1);
    gradients.col(3) =
        4 * (grad_bary_coords.col(0) * l[1] + grad_bary_coords.col(1) * l[0]);
    gradients.col(4) =
        4 * (grad_bary_coords.col(1) * l[2] + grad_bary_coords.col(2) * l[1]);
    gradients.col(5) =
        4 * (grad_bary_coords.col(0) * l[2] + grad_bary_coords.col(2) * l[0]);
    return gradients;
  };

  const auto grad_vt_0{gradientsLocalShapeFunctions(Eigen::Vector2d(0, 0))};
  const auto grad_vt_1{gradientsLocalShapeFunctions(Eigen::Vector2d(1, 0))};
  const auto grad_vt_2{gradientsLocalShapeFunctions(Eigen::Vector2d(0, 1))};
  result =
      area / 3.0 *
      (grad_vt_0.transpose() * grad_vt_0 + grad_vt_1.transpose() * grad_vt_1 +
       grad_vt_2.transpose() * grad_vt_2);
  return result;
}

Eigen::Matrix<double, 6, 6> LocalLaplaceQFE2::Eval(
    const lf::mesh::Entity &cell) {
  // Query (topological) type of cell/reference element
  const lf::base::RefEl ref_el{cell.RefEl()};
  // Verify that the cell is a triangle
  LF_ASSERT_MSG(ref_el == lf::base::RefEl::kTria(),
                "Implemented for triangles only not for " << ref_el);
  // The final element matrix has size 6x6
  Eigen::Matrix<double, 6, 6> result{};
  // Obtain the vertex coordinates of the triangle
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  LF_ASSERT_MSG(geo_ptr != nullptr, "Invalid geometry!");
  // Matrix storing corner coordinates in its columns
  Eigen::Matrix<double, 2, 3> vertices{geo_ptr->Global(ref_el.NodeCoords())};
  // Comopute element matrix for negative Laplacian and lowest-order Lgrangian
  // finite elements as in Remark 2.4.5.9. in the course notes
  Eigen::Matrix<double, 3, 3> X;  // temporary matrix
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = vertices.transpose();
  const double area = 0.5 * std::abs(X.determinant());
  auto grad_bary_coords{X.inverse().block<2, 3>(1, 0)};
  auto L{grad_bary_coords.transpose() * grad_bary_coords};

  // See Example 2.7.5.7 in course notes for derivation of the formulas
  result << 3. * L(0, 0), -L(0, 1), -L(0, 2), 4. * L(0, 1), 0, 4. * L(0, 2),
      -L(0, 1), 3. * L(1, 1), -L(1, 2), 4. * L(0, 1), 4. * L(1, 2), 0, -L(0, 2),
      -L(1, 2), 3. * L(2, 2), 0, 4. * L(2, 1), 4. * L(2, 0), 4. * L(0, 1),
      4. * L(0, 1), 0, 8. * (L(0, 0) + L(0, 1) + L(1, 1)), 8 * L(0, 2),
      8 * L(1, 2), 0, 4. * L(1, 2), 4. * L(2, 1), 8. * L(0, 2),
      8. * (L(1, 1) + L(1, 2) + L(2, 2)), 8 * L(0, 1), 4 * L(0, 2), 0,
      4. * L(2, 0), 8. * L(1, 2), 8. * L(0, 1),
      8. * (L(0, 0) + L(0, 2) + L(2, 2));
  result *= (area / 3.);
  return result;
}

// implementation
Eigen::Matrix<double, 6, 6> LocalLaplaceQFE3::Eval(
    const lf::mesh::Entity &cell) {
  // Obtain the element matrix for piecewise linear Lagrangian FEM by using
  // a built-in class of LehrFEM++
  auto linear_lapl_element_matrix = lf::uscalfe::LinearFELaplaceElementMatrix();
  Eigen::Matrix4d L = linear_lapl_element_matrix.Eval(cell);
  // Variable for returning the final 6x6 element matrix
  Eigen::Matrix<double, 6, 6> result{};

  // The element matrix for quadratic finite elements can be constructed from
  // the element matrix for linear FEM, see Example 2.7.5.7. in the lecture
  // notes.
  result << 3. * L(0, 0), -L(0, 1), -L(0, 2), 4. * L(0, 1), 0, 4. * L(0, 2),
      -L(0, 1), 3. * L(1, 1), -L(1, 2), 4. * L(0, 1), 4. * L(1, 2), 0, -L(0, 2),
      -L(1, 2), 3. * L(2, 2), 0, 4. * L(2, 1), 4. * L(2, 0), 4. * L(0, 1),
      4. * L(0, 1), 0, 8. * (L(0, 0) + L(0, 1) + L(1, 1)), 8 * L(0, 2),
      8 * L(1, 2), 0, 4. * L(1, 2), 4. * L(2, 1), 8. * L(0, 2),
      8. * (L(1, 1) + L(1, 2) + L(2, 2)), 8 * L(0, 1), 4 * L(0, 2), 0,
      4. * L(2, 0), 8. * L(1, 2), 8. * L(0, 1),
      8. * (L(0, 0) + L(0, 2) + L(2, 2));
  // A hideous manipulation introducces an error !
  result(3, 3) *= 1.000001;
  return (result / 3.0);
}

}  // namespace DebuggingFEM


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#include <boost/math/special_functions/hypergeometric_1F0.hpp>

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class T>
void test_spots(T)
{
   using std::pow;
   //
   // basic sanity checks, tolerance is 10 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 1000;

   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(2)), T(-1), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(4)), T(-27), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(0.5)), T(0.125), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(0.5)), T(8), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(2)), T(-1), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(4)), T(T(-1) / 27), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(-0.5)), pow(T(1.5), -3), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(-2)), T(1 / T(27)), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(3), T(-4)), T(T(1) / 125), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(-0.5)), pow(T(1.5), 3), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(-2)), T(27), tolerance);
   BOOST_CHECK_CLOSE(boost::math::hypergeometric_1F0(T(-3), T(-4)), T(125), tolerance);

   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(3), T(1)), std::domain_error);
   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(-3), T(1)), std::domain_error);
   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(3.25), T(1)), std::domain_error);
   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(-3.25), T(1)), std::domain_error);
   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(3.25), T(2)), std::domain_error);
   BOOST_CHECK_THROW(boost::math::hypergeometric_1F0(T(-3.25), T(2)), std::domain_error);
}


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file geometry_tools.cpp
 * \brief Source file for the ASRL vision package
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <opencv2/opencv.hpp>

#include <vtr_logging/logging.hpp>
#include <vtr_vision/geometry/geometry_tools.hpp>

namespace vtr {
namespace vision {

/////////////////////////////////////////////////////////////////////////////////
// @brief Triangulates a point from a rig and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromRig(const RigCalibration &rig_calibration,
                                   const std::vector<cv::Point2f> &keypoints,
                                   const FeatureInfos &kp_infos,
                                   double *covariance) {
  return triangulateFromCameras(rig_calibration.intrinsics,
                                rig_calibration.extrinsics, keypoints, kp_infos,
                                covariance);
}

/////////////////////////////////////////////////////////////////////////////////
/// @brief Triangulates a point (linearly) from a set of cameras and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromCameras(
    const CameraIntrinsics &intrinsics, const Transforms &extrinsics,
    const std::vector<cv::Point2f> &keypoints,
    const vision::FeatureInfos &kp_infos, double *covariance) {
  // sanity check
  assert(intrinsics.size() == keypoints.size());
  assert(extrinsics.size() == keypoints.size());

  // make up the solution matrix
  Eigen::MatrixXd Z = Eigen::MatrixXd::Zero(keypoints.size() * 2, 4);
  for (unsigned ii = 0; ii < keypoints.size(); ii++) {
    // grab each observation and add it to the matrix to solve
    Eigen::Matrix<double, 3, 4> P =
        intrinsics[ii] * extrinsics[ii].matrix().block(0, 0, 3, 4);
    Z.row(ii * 2) = keypoints[ii].x * P.row(2) - P.row(0);
    Z.row(ii * 2 + 1) = keypoints[ii].y * P.row(2) - P.row(1);
  }

  // solve the linear triangulation problem using SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      Z, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd V = svd.matrixV();

  // extract the 3D point
  Eigen::Vector3d X = V.col(3).hnormalized();

  // calculate the covariance if required
  if (covariance) {
    // wrap the covariance and zero-initialize
    Eigen::Map<Eigen::Matrix3d> cov_map(covariance);
    cov_map.setZero();

    // loop over the contribution of each measurement
    for (unsigned ii = 0; ii < keypoints.size(); ii++) {
      // Reproject the solved point for linearization
      Eigen::Matrix<double, 3, 4> P =
          intrinsics[ii] * extrinsics[ii].matrix().block(0, 0, 3, 4);
      Eigen::Vector3d xii = P * X.homogeneous();
      const auto &xii2 = xii(2);
      auto xii2_2 = xii2 * xii2;  // helpers

      // homogeneous to cartesian Jacobian for image points
      Eigen::Matrix<double, 2, 3> h2c_jac;
      h2c_jac << 1 / xii2, 0, -xii(0) / xii2_2, 0, 1 / xii2, -xii(1) / xii2_2;

      // full camera projection Jacobian
      auto jac = h2c_jac * P.leftCols<3>();
      // sum the (linearized) precision contributions of each measurement
      cov_map += jac.transpose() * kp_infos[ii].covariance.inverse() * jac;
    }
    // from precision matrix to covariance matrix
    cov_map = cov_map.inverse().eval();
  }

  // return the linearly triangulated 3D point
  return X;
}
#if 0
/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimates a plane from a PCL point cloud
/////////////////////////////////////////////////////////////////////////////////
bool estimatePlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                              const double distance_thresh,
                              pcl::ModelCoefficients &coefficients,
                              pcl::PointIndices &inliers) {

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (distance_thresh);
  seg.setInputCloud (cloud);
  seg.segment (inliers, coefficients);

  return !inliers.indices.empty();
}
#endif
/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimate the distance from a plane
/////////////////////////////////////////////////////////////////////////////////
double estimatePlaneDepth(const Eigen::Vector3d &point,
                          const Eigen::Vector4f &coefficients) {
  // rename for clarity
  const double &a = point(0);
  const double &b = point(1);
  const double &c = point(2);
  const float &pa = coefficients(0);
  const float &pb = coefficients(1);
  const float &pc = coefficients(2);
  const float &pd = coefficients(3);

  // numerator
  double num = std::fabs(a * pa + b * pb + c * pc + pd);

  // denominator
  double den = std::sqrt(pa * pa + pb * pb + pc * pc);

  // result
  double dist = num / den;

  return dist;
}

}  // namespace vision
}  // namespace vtr


#include <boost/test/unit_test.hpp>

#include "geometry.h"
#include "logs.h"
#include "route.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_maxGradient_N0697611 )

const bool isFileName = true;

BOOST_AUTO_TEST_CASE( elev_zero_throughout )//checks that the output is 0 if all elevations are 0
{
   Route route = Route(LogFiles::GPXRoutesDir + "ABCD.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.maxGradient(), 0 );
}

BOOST_AUTO_TEST_CASE( One_Point )//checks that the output is 0 when there is only 1 position
{
   Route route = Route(LogFiles::GPXRoutesDir + "N0697611_M_OnePoint.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.maxGradient(), 0 );
}

BOOST_AUTO_TEST_CASE(  typical_flat)//Checks that a flat route still returns 0 at an elevation other than 0
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_ACE_CliftonCampus.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), 0,0.15) ;

}

BOOST_AUTO_TEST_CASE( typical_uphill)//Checks that a correct uphill gradient is calculated
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_CHM_CliftonCampus.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE(  typical_downhill)//Checks that a correct downhill gradient is calculated
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_MRW_CliftonCampus.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(-100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( typical_uturn)//Checks that if two gradients of equal magnitude, but one positive and one negative, the positive one is the one returned
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_JIGQST_CliftonCampus.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

//
BOOST_AUTO_TEST_CASE( Flat_near_equator)//Checks that 0 is returned for a flat route
{                                         //this test is repeated at the equator
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_ACE_Pontianak.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), 0,0.15) ;

}

BOOST_AUTO_TEST_CASE( Equatorial_uphill)//Checks that a correct uphill gradient is calculated
{                                         //this test is repeated at the equator
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_CHM_Pontianak.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( Equatorial_downhill)//Checks that a correct downhill gradient is calculated
{                                         //this test is repeated at the equator
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_MRW_Pontianak.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(-100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( Equatorial_uturn)//Checks that if two gradients of equal magnitude, but one positive and one negative, the positive one is the one returned
{                                       //this test repeats this at the equator
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_JIGQST_Pontianak.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

//

BOOST_AUTO_TEST_CASE( EquatorialMeridian_uturn)//Checks that if two gradients of equal magnitude, but one positive and one negative, the positive one is the one returned
{                                               //This test repeats this at the equatorial meridian
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_JIGQST_EquatorialMeridian.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

//

BOOST_AUTO_TEST_CASE( EquatorialAntiMeridian_uturn)//Checks that if two gradients of equal magnitude, but one positive and one negative, the positive one is the one returned
{                                                   //this test repeats this at the equatorial anti meridian
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_JIGQST_EquatorialAntiMeridian.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

//

BOOST_AUTO_TEST_CASE( Northpole_flat)//Tests a flat route near the north pole
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_ACE_NorthPole.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), 0,0.15) ;

}
BOOST_AUTO_TEST_CASE( NorthPole_uphill)//Checks the gradient is accurate when going uphill near the northpole
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_CHM_NorthPole.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( NorthPole_downhill)//Checks the gradient is accurate when going downhill near the northpole
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_MRW_NorthPole.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(-100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( Flat_then_up) //checks that the correct gradient is returned when choosing between 0 and a positive gradient
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_VWXS_FlatThenUp.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(100,1000)),0.15) ;

}

BOOST_AUTO_TEST_CASE( Flat_then_down)//checks that the correct gradient is returned when choosing between 0 and a negative gradient
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_GHID_FlatThenDown.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), 0,0.15) ;

}

BOOST_AUTO_TEST_CASE( Steep_up) //checks that an extreme positive gradient is returned correctly
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_LM_Steepup.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(10000000,100)),0.15) ;

}

BOOST_AUTO_TEST_CASE( Steep_down)//checks that an extreme negative gradient is returned correctly
{
Route route = Route(LogFiles::GPXRoutesDir + "N0697611_MN_Steepdown.gpx", isFileName);

BOOST_CHECK_CLOSE(route.maxGradient(), radToDeg(atan2(-10000000,100)),0.15) ;

}

BOOST_AUTO_TEST_SUITE_END()


#include <chrono>
#include <iostream>
#include <vector>
#include <utility> // std::pair
#include <algorithm> // std::max_element()
#include <string> // std::to_string()

#include <armadillo>

#include "AudioFFT/AudioFFT.h"

using std::chrono::high_resolution_clock;
using std::chrono::duration;
using AudioVec = std::vector<float>;
using audiofft::AudioFFT;
using FftwData = std::tuple<AudioFFT& // fft class
                           ,AudioVec& // input vector
						   ,AudioVec& // real vector
						   ,AudioVec& // imag vector
						   ,AudioVec&>; // output vector

namespace {
	uint32_t num = 1;
	uint32_t fft_size = 48000;
	uint32_t conv_ir_size = 72000;
	uint32_t conv_sig_size = 5760000;
}

std::pair<duration<double>, arma::fvec> Convolution(arma::fvec sig, arma::fvec ir) {
	size_t size = (::conv_sig_size+::conv_ir_size-1);
	ir = arma::flipud(ir);
	arma::fvec sig_new = arma::zeros<arma::fvec>(::conv_sig_size + 2*(::conv_ir_size-1));
	sig_new.subvec(::conv_ir_size - 1, ::conv_sig_size + ::conv_ir_size -2) = sig;
	arma::fvec output (::conv_sig_size + ::conv_ir_size - 1, arma::fill::zeros);

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		for (uint32_t sample_cnt=0;sample_cnt<size;++sample_cnt) {
			for (uint32_t ir_cnt=0;ir_cnt<::conv_ir_size;++ir_cnt) {
				output[sample_cnt] += sig_new[sample_cnt+ir_cnt] * ir[ir_cnt];
			}
		}
	}
	AudioVec output_copy = arma::conv_to<AudioVec>::from(output);
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, output);
}

std::pair<duration<double>, arma::fvec> ArmadilloConv(arma::fvec sig, arma::fvec ir) {
	arma::fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::conv(sig, ir);
	}
	AudioVec output_copy = arma::conv_to<AudioVec>::from(output);
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, output);
}

duration<double> FftwConv(FftwData sig, FftwData ir) {
	auto sig_in = std::get<1>(sig);
	auto sig_re = std::get<2>(sig);
	auto sig_im = std::get<3>(sig);
	auto ir_in = std::get<1>(ir);
	auto ir_re = std::get<2>(ir);
	auto ir_im = std::get<3>(ir);
	AudioVec re (sig_re.size());
	AudioVec im (sig_re.size());

	auto begin = high_resolution_clock::now();
	std::get<0>(sig).fft(sig_in.data(), sig_re.data(), sig_im.data());
	std::get<0>(ir).fft(ir_in.data(), ir_re.data(), ir_im.data());
	for (size_t cnt=0;cnt<sig_re.size();++cnt) {
		re[cnt] = sig_re[cnt]*ir_re[cnt] - sig_im[cnt]*ir_im[cnt];
		im[cnt] = sig_re[cnt]*ir_im[cnt] + sig_im[cnt]*ir_re[cnt];
	}
	std::get<0>(sig).ifft(std::get<4>(sig).data(), re.data(), im.data());
	auto end = high_resolution_clock::now();

	return end - begin;
}

std::pair<duration<double>, arma::fvec> ArmadilloFftConv(arma::fvec sig, arma::fvec ir) {
	arma::cx_fvec output;
	size_t size = sig.size() + ir.size() - 1;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig, size) % arma::fft(ir, size));
	}
	AudioVec output_copy = arma::conv_to<AudioVec>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, arma::real(output));
}

std::pair<duration<double>, arma::fvec> ArmadilloFftPow2Conv(arma::fvec sig, arma::fvec ir) {
	uint32_t size = pow(2,ceil(log2(::conv_sig_size + ::conv_ir_size - 1)));
	arma::cx_fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig,size) % arma::fft(ir,size));
	}
	AudioVec output_copy = arma::conv_to<AudioVec>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, arma::real(output.subvec(0, ::conv_sig_size + ::conv_ir_size - 2)));
}

std::pair<duration<double>,arma::fvec> ArmadilloFft(arma::fvec input) {
	arma::cx_fvec output_fd;
	arma::cx_fvec output_td;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output_fd = arma::fft(input);
		output_td = arma::ifft(output_fd);

	}
	AudioVec output_copy = arma::conv_to<AudioVec>::from(arma::real(output_td));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin,arma::real(output_td));
}

int main(int argc, char* argv[]) {

	std::cout << "generating input signals" << std::endl;
	arma::fvec sig {arma::randn<arma::fvec>(::conv_sig_size)};
	arma::fvec ir {arma::randn<arma::fvec>(::conv_ir_size)};
	size_t out_size = ::conv_sig_size + ::conv_ir_size - 1;

	std::cout << "initializing FFTW" << std::flush;
	size_t fft_size (pow(2,ceil(log2(out_size))));
	AudioVec sig_vec  (fft_size, 0);
	std::copy_n(sig.begin(), ::conv_sig_size, sig_vec.begin());
	AudioVec ir_vec (fft_size, 0);
	std::copy_n(ir.begin(), ::conv_ir_size, ir_vec.begin());
	AudioVec sig_re(AudioFFT::ComplexSize(fft_size));
	AudioVec ir_re(AudioFFT::ComplexSize(fft_size));
	AudioVec sig_im(AudioFFT::ComplexSize(fft_size));
	AudioVec ir_im(AudioFFT::ComplexSize(fft_size));
	AudioVec output(fft_size);
	AudioFFT fft_sig;
	auto begin = high_resolution_clock::now();
	fft_sig.init(fft_size);
	auto end = high_resolution_clock::now();
	std::cout << "\n\tduration: " << (std::chrono::duration_cast<duration<double>>(end - begin)).count() << std::endl;
	AudioFFT fft_ir;
	fft_ir.init(fft_size);
	FftwData sig_data = std::forward_as_tuple(fft_sig, sig_vec, sig_re, sig_im, output);
	FftwData ir_data = std::forward_as_tuple(fft_ir, ir_vec, ir_re, ir_im, output);

	std::cout << "Armadillo FFT: " << std::flush;
	auto result_arma_fft = ArmadilloFft(sig);
	std::cout << "\n\tduration: " << result_arma_fft.first.count() << std::endl;

	// normal convolution, this is our reference output
	std::cout << "convolution: " << std::flush;
	auto result_conv = Convolution(sig, ir);
	std::cout << "\n\tduration: " << result_conv.first.count() << std::endl;

	std::cout << "Armadillo FFT-Pow2-convolution: " << std::flush;
	auto result_arma_fft_pow2_conv = ArmadilloFftPow2Conv(sig, ir);
	std::cout << "\n\tduration: " << result_arma_fft_pow2_conv.first.count();
	std::cout << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_fft_pow2_conv.second).max()
			  << std::endl;

	std::cout << "FFTW FFT-Pow2-convolution: " << std::flush;
	auto result_fftw_pow2_conv = FftwConv( sig_data, ir_data);
	AudioVec diff;
	std::transform(
			result_conv.second.begin()
			, result_conv.second.end()
			, std::get<4>(sig_data).begin()
			, std::back_inserter(diff)
			, [](float a, float b) { return fabs(a-b); }
			);
	std::cout << "\n\tduration: " << result_fftw_pow2_conv.count();
	std::cout << "\n\tmaximum difference of result: "
			  << std::to_string(*std::max_element(diff.begin(), diff.end()))
			  << std::endl;

	std::cout << "Armadillo FFT-convolution: " << std::flush;
	auto result_arma_fft_conv = ArmadilloFftConv(sig, ir);
	std::cout << "\n\tduration: " << result_arma_fft_conv.first.count();
	std::cout << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_fft_conv.second).max()
			  << std::endl;

	std::cout << "Armadillo convolution: " << std::flush;
	auto result_arma_conv = ArmadilloConv(sig, ir);
	std::cout << "\n\tduration: " << result_arma_conv.first.count();
	std::cout << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_conv.second).max()
			  << std::endl;
}


#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::Vector<3u, tiny::float_traits>  Vf;
typedef tiny::Vector<3u, tiny::double_traits> Vd;

BOOST_AUTO_TEST_SUITE(tiny_truncate);

  BOOST_AUTO_TEST_CASE(double_testing)
  {
    double const tol = 0.1;

    Vd const A  = Vd::make( 0.2, 0.01, 0.09999 );

    Vd const tB = tiny::truncate(A, tol);

    BOOST_CHECK_CLOSE(tB(0), 0.2, 0.01);
    BOOST_CHECK_CLOSE(tB(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(tB(2), 0.0, 0.01);
  }

BOOST_AUTO_TEST_SUITE_END();


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
// 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)
//

// fixed_point_detail_hypergeometric.hpp implements templates
// for computing hypergeometric series used for Taylor-series-like
// expansions. These are used by certain <cmath> functions to
// simplify typing and reduce the complexity of the code.

#ifndef FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_
  #define FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_

  #include <cmath>
  #include <cstdint>
  #include <limits>

  #include <boost/config.hpp>

  namespace boost { namespace fixed_point { namespace detail {

  template<typename NumericType>
  NumericType hypergeometric_0f0(const NumericType& x)
  {
    // Compute the series representation of hypergeometric_0f0.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_0f0.

    NumericType term(x);
    NumericType h0f0(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_0f0(; ; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h0f0 += term;
    }

    return h0f0;
  }

  template<typename NumericType>
  NumericType hypergeometric_0f1(const NumericType& b,
                                 const NumericType& x)
  {
    // Compute the series representation of hypergeometric_0f1.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_0f1.

    NumericType bp(b);

    NumericType term(x / bp);
    NumericType h0f1(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_0f1(; b; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      ++bp;

      term /= bp;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h0f1 += term;
    }

    return h0f1;
  }

  template<typename NumericType>
  NumericType hypergeometric_2f1(const NumericType& a,
                                 const NumericType& b,
                                 const NumericType& c,
                                 const NumericType& x)
  {
    // Compute the series representation of hypergeometric_2f1 taken from
    // Abramowitz and Stegun 15.1.1.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_2f1.

    NumericType ap(a);
    NumericType bp(b);
    NumericType cp(c);

    NumericType term(((ap * bp) / cp) * x);
    NumericType h2f1(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_2f1(a, b; c; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      ++ap;
      term *= ap;

      ++cp;
      term /= cp;

      ++bp;
      term *= bp;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h2f1 += term;
    }

    return h2f1;
  }

  template<typename NumericType>
  NumericType two_to_the_power_of_x(const NumericType& x,
                                    const NumericType& my_ln_two)
  {
    // Compute the series representation of (2^x),
    // which is very closely related to a hypergeometric
    // series.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of (2^x).

    const NumericType ln_two_times_x(my_ln_two * x);

    NumericType term(ln_two_times_x);
    NumericType sum (1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of (2^x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= ln_two_times_x;
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      sum += term;
    }

    return sum;
  }

  } } } // namespace boost::fixed_point::detail

#endif // FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_


#pragma once

#include <Eigen/Core>

#ifdef USE_FLOAT
typedef float Real;
typedef Eigen::MatrixXf MatD;
typedef Eigen::VectorXf VecD;
#else
typedef double Real;
typedef Eigen::MatrixXd MatD;
typedef Eigen::VectorXd VecD;
#endif

typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
#define REAL_MAX std::numeric_limits<Real>::max()


/**
 * @file main.cpp
 * @brief Entry-point for dynamic notch filter example
 * @author Parker Lusk <parkerclusk@gmail.com>
 * @date 8 Dec 2020
 */

#include <chrono>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

#include <plot.hpp>

#include <adaptnotch/adaptnotch.h>

#include "csv.h"

static constexpr int DATUMS = 4; // number of columns to be extracted from CSV
using Data = Eigen::Matrix<double, Eigen::Dynamic, DATUMS>;

void usage(int argc, char const *argv[])
{
  std::cout << argv[0] << " <input csv data> [axis] [plot]" << std::endl << std::endl;
  std::cout << "\tRun adaptive notching algorithm on gyro data stored in CSV.";
  std::cout << std::endl << "\t";
  std::cout << "CSV file expected to have been generated from sfpro or to be ";
  std::cout << std::endl << "\t";
  std::cout << "in same format. A gyro axis to analyze may be specified as";
  std::cout << std::endl << "\t";
  std::cout << "(1, 2, 3) which corresponds to axes (x, y, z).";
  std::cout << std::endl << std::endl;
  std::cout << "\tIf a 3rd argument is specified (e.g., '1'), an ASCII plot";
  std::cout << std::endl << "\t";
  std::cout << "of the pre- and post-spectrum is shown in the terminal.";
  std::cout << std::endl << std::endl;
}

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

Data parseCSV(const std::string& file)
{

  // count number of entries (estimate)
  std::ifstream ifile(file);
  const int N = std::count(std::istreambuf_iterator<char>(ifile),
                            std::istreambuf_iterator<char>(), '\n');
  ifile.close();

  io::CSVReader<DATUMS> in(file);
  in.next_line(); // ignore "dsp clock" message

  // we only care about these four (DATUMS) columns
  in.read_header(io::ignore_extra_column,
                  "timestamp(us)", "ang_x", "ang_y", "ang_z");

  Data D = Data::Zero(N, DATUMS);
  int i = 0;
  int time_us;
  double wx, wy, wz;
  while (in.read_row(time_us, wx, wy, wz)) {
    D.row(i++) << time_us*1e-6, wx, wy, wz;
  }

  // resize to however many valid entries there were
  D.conservativeResize(i, DATUMS);

  return D;
}

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

void plotResults(const adaptnotch::AdaptiveNotch& filter,
            const Eigen::VectorXd& gyro, const Eigen::VectorXd& gyrof, int n)
{
  static const int N = filter.params().NFFT;
  static Eigen::FFT<double> fft;
  static plot::TerminalInfo term;
  static bool init = false;
  if (!init) {
    term.detect();
    fft.SetFlag(Eigen::FFT<double>::Unscaled);
    fft.SetFlag(Eigen::FFT<double>::HalfSpectrum);
    init = true;
  }
  constexpr float ymax = 0.2f; // arbitrary FFT mag scaling
  static plot::RealCanvas<plot::BrailleCanvas> prefilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term);
  static plot::RealCanvas<plot::BrailleCanvas> postfilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term);

  // Build block layout
  auto layout =
      plot::alignment(
          { term.size().x, 0 },
          plot::margin(
                  plot::vbox(
                      plot::frame(u8"Original Spectrum", plot::Align::Center, &prefilter),
                      plot::frame(u8"Filtered Spectrum", plot::Align::Center, &postfilter))));

  // pre-filter spectrum
  const Eigen::VectorXd Y = filter.spectrum() / N;

  // select N most recent filtered measurements
  const size_t s = (n-N<0) ? 0 : n-N;
  const Eigen::VectorXd yf = gyrof.segment(s,N);

  // post-filter spectrum
  Eigen::VectorXcd Yfc;
  fft.fwd(Yfc, yf);
  const Eigen::VectorXd Yf = Yfc.array().abs() / N;

  // Plot spectrum pre-filtering
  prefilter.clear();
  for (size_t i=1; i<N/2; i++) {
    prefilter.path(plot::palette::royalblue,{{static_cast<float>(i-1), static_cast<float>(Y(i-1))},
                                          {static_cast<float>(i), static_cast<float>(Y(i))}});
  }

  // Plot spectrum post-filtering
  postfilter.clear();
  for (size_t i=1; i<N/2; i++) {
    postfilter.path(plot::palette::royalblue,{{static_cast<float>(i-1), static_cast<float>(Yf(i-1))},
                                          {static_cast<float>(i), static_cast<float>(Yf(i))}});
  }

  for (auto const& line: layout)
    std::cout << term.clear_line() << line << std::endl;
  std::cout << term.move_up(layout.size().y) << std::flush;
}

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

int main(int argc, char const *argv[])
{

  int axis = 1;     ///< x, y, or z axis of gyro to analyze
  std::string file; ///< input data from IMU
  bool shouldPlot = false; ///< show FFT plots in terminal

  if (argc < 2) {
    std::cerr << "Not enough input arguments." << std::endl << std::endl;
    usage(argc, argv);
    return -1;
  } else if (argc >= 2) {
    file = std::string(argv[1]);
  }

  if (argc >= 3) {
    axis = std::stoi(argv[2]);
    if (axis < 1 || axis > 3) axis = 1;
  }

  if (argc >= 4) shouldPlot = true;

  //
  // Process raw IMU data
  //

  Data D = parseCSV(file);

  const Eigen::VectorXd diff = D.col(0).bottomRows(D.rows()-1) - D.col(0).topRows(D.rows()-1);
  const double Ts = diff.mean();
  const double Fs = 1./Ts;
  const int N = D.rows();

  //
  // Adaptive notch filter setup
  //

  adaptnotch::AdaptiveNotch::Params params;
  adaptnotch::AdaptiveNotch filter(params);

  //
  // Main loop - simulated gyro sampling
  //

  Eigen::VectorXd gyrof = Eigen::VectorXd::Zero(N);
  const auto start = std::chrono::steady_clock::now();

  for (size_t n=0; n<N; n++) {
    const double gyro = D(n, axis);
    gyrof(n) = filter.apply(gyro);
    if (shouldPlot) plotResults(filter, D.col(axis), gyrof, n);
  }

  //
  // Timing stats
  //

  const auto end = std::chrono::steady_clock::now();
  const double duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 1e-6;

  const double timu = D(N-1,0) - D(0,0);

  std::cout << "Processed " << timu << " seconds (" << N << " samples) of IMU";
  std::cout << " data in " << duration << " seconds" << std::endl;
  std::cout << "Real-time factor: " << timu / duration << std::endl;
  std::cout << "Estimated peak freq: " << filter.peakFreq() << std::endl;

  //
  // Write data to file
  //

  Eigen::MatrixXd out(N, 3);
  out << Eigen::VectorXd::LinSpaced(N, 0, N*Ts), D.col(axis), gyrof;

  std::ofstream of("data_processed.txt");
  of << out << std::endl;
  of.close();

  return 0;
}


//  Copyright (c) 2019 AUTHORS
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include "octotiger/grid.hpp"

#include <hpx/runtime/threads/run_as_os_thread.hpp>

#include "octotiger/test_problems/blast.hpp"

#include <algorithm>
#include <functional>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <vector>

#if !defined(OCTOTIGER_HAVE_BOOST_MULTIPRECISION)
#include <quadmath.h>
using sed_real = __float128;
#else
#include <boost/multiprecision/cpp_bin_float.hpp>
using sed_real = boost::multiprecision::cpp_bin_float_quad;
#endif

/*extern "C" {*/
/* Subroutine */int sed_1d__(sed_real *time, int *nstep, sed_real *xpos, sed_real *eblast, sed_real *omega_in__, sed_real *xgeom_in__, sed_real *rho0,
		sed_real *vel0, sed_real *ener0, sed_real *pres0, sed_real *cs0, sed_real *gam0, sed_real *den, sed_real *ener, sed_real *pres, sed_real *vel,
		sed_real *cs);
//}

constexpr real blast_wave_t0 = 7e-4;

std::vector<real> blast_wave_analytic(real x, real y, real z, real t) {
	static const auto dxmin = 2.0 * opts().xscale / INX / double(1 << opts().max_level);
	real r = std::sqrt(x * x + y * y + z * z);
	r = std::max(r, dxmin * 1.0e-3);
	t += blast_wave_t0;
	real rmax = 3.0 * opts().xscale;
	real d, v, p;
	sedov::solution(t, r, rmax, d, v, p);
	std::vector<real> u(opts().n_fields, 0.0);
	u[rho_i] = u[spc_i] = std::max(d, 1.0e-20);
	real s = d * v;
	u[sx_i] = s * x / r;
	u[sy_i] = s * y / r;
	u[sz_i] = s * z / r;
	real e = std::max(p / (grid::get_fgamma() - 1), 1.0e-20);
	u[egas_i] = e + s * v * 0.5;
	u[tau_i] = std::pow(e, 1 / grid::get_fgamma());
	return u;
}

std::vector<real> blast_wave(real x, real y, real z, real dx) {
	std::vector<real> u(opts().n_fields, 0.0);
	u[rho_i] = u[spc_i] = 1.0;
	const auto r2 = x * x + y * y + z * z;
	const auto rmax = dx * 3.5;
	if (r2 < rmax * rmax) {
		u[egas_i] = opts().eblast0 / (dx * dx * dx)  / 160.0;
	} else {
		u[egas_i] = 1.0e-20;
	}
	u[tau_i] = std::pow(u[egas_i], 1.0 / grid::get_fgamma());
	return u;

}


// c10e14ExprWithSimbolTable.cpp: определяет точку входа для консольного приложения.
//

#include "stdafx.h"
#include <boost/regex.hpp>
#include <iostream>
#include <sstream>
typedef std::string str;
class Expr
{
public:
	Expr(const char* a) { s0 = a;reed(); }
	int Eval()
	{
		std::vector<str> ter = terms;
		std::vector<str> sig = signs;
		boost::smatch sm;
		for (auto &t : ter)
		{
			while (boost::regex_search(t, sm, boost::regex("([0-9]+)(" + ops[0] + ")([0-9]+)")))
			{
				t.replace(sm.position(),sm.length(),f(sm[1], sm[2], sm[3]) );
				std::cout << t << std::endl;
			}
		}
		for (auto t : ter)
			std::cout << t << std::endl;
		if (ter.size() == sig.size()) ter.insert(ter.begin(), "0");
		while (ter.size() > 1)
		{
			ter[0] = f(ter[0], sig[0], ter[1]);
			ter.erase(ter.begin());
			sig.erase(sig.begin());
		}
/*
		str s = s0;

		boost::smatch f00;
		for (auto o : ops)
		{
			while (boost::regex_search(s, f00, boost::regex(str("((?:^-)?([0-9]+))([") + o + "])((?2))")))
			{
				s.replace(f00.position(), f00.length(),f(f00[1], f00[3], f00[4]));
			}
		}
		i = s_to_i(s);*/
		i = s_to_i(ter[0]);
		return i;
	}
	//void print() { std::cout << i << std::endl; }
	void printBrackets()
	{

		std::vector<str> ter = terms;
		int ts = ter.size();
		int ss = signs.size();
		if (ts > ss) { std::cout << '(' << ter[0] << ')'; ter.erase(ter.begin()); }
		for (int i = 0; i < ts; i++)
		{
			std::cout << signs[i] << '(' << ter[i] << ')';
		}
		std::cout << std::endl;
	}

private:
	str s0;
	int s_to_i(str z)
	{
		std::stringstream io;
		io << z;
		int i;
		io >> i;
		return i;
	}
	int i;
	std::vector<str> ops = { "\\*|/","+-" };
	std::vector<str> terms;
	std::vector<str> signs;
	void reed()
	{
		boost::regex e("([+-])?([^+-]+)");
		boost::sregex_iterator it(s0.begin(), s0.end(), e);
		boost::sregex_iterator itend;
		while (it != itend)
		{
			str z = (*it)[1];
			if(z.size()>0) signs.push_back(z);
			terms.push_back((*it)[2]);
			it++;
		}
	}
	str f(str x0, str op0, str y0)
	{
		int x = s_to_i(x0);
		int y = s_to_i(y0);
		char op = op0[0];
		int r;
		switch (op)
		{
		case '+':
			r= x + y;
			break;
		case '-':
			r = x - y;
			break;
		case '*': r = x*y;
			break;
		case '/':r = x / y;
			break;
		}
		str st = (std::to_string(int(r)));
		std::cout <<x0<<op0<<y0<<'='<< r << std::endl;
		return st;
	}
};
int main()
{
	Expr ex("-3-2+4*3/2");
	//ex.Eval();
	ex.printBrackets();
	std::cout << ex.Eval() << std::endl;

	/*str s = "3-2+4*3/2";
	boost::smatch sm;
	boost::regex e("^([\\+\\-])?([^\\+\\-]+)(?:([\\+\\-])([^\\+\\-]+))*");
	boost::regex_match(s, sm, e, boost::match_extra);
	for(auto i:sm)
		std::cout << i << std::endl;
	boost::regex e1("([+-])?([^+-]+)");
	boost::sregex_iterator it(s.begin(),s.end(), e1);
	boost::sregex_iterator itend;
	while (it != itend)
	{
		std::cout << (*it)[1]<<'\n'<< (*it)[2] << std::endl;
		it++;
	}*/
}


#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <aikido/common/RNG.hpp>

#include "PolynomialConstraint.hpp"

using aikido::statespace::R1;

TEST(PolynomialConstraint, Constructor)
{
  PolynomialConstraint<1> p(Eigen::Vector3d(1, 2, 3));
  EXPECT_THROW(
      PolynomialConstraint<1>(Eigen::Vector3d(1, 2, 0)), std::invalid_argument);
}

TEST(PolynomialConstraint, GetValue)
{
  PolynomialConstraint<1> p(Eigen::Vector3d(1, 2, 3));

  Eigen::VectorXd v(1);
  v(0) = -2;

  R1 rvss;
  auto s1 = rvss.createState();
  s1.setValue(v);

  Eigen::VectorXd value;
  p.getValue(s1, value);

  EXPECT_DOUBLE_EQ(value(0), 9);
}

TEST(PolynomialConstraint, GetJacobian)
{
  PolynomialConstraint<1> p(Eigen::Vector3d(1, 2, 3));

  Eigen::VectorXd v(1);
  v(0) = -2;

  R1 rvss;
  auto s1 = rvss.createState();
  s1.setValue(v);

  Eigen::MatrixXd jac;
  p.getJacobian(s1, jac);
  EXPECT_DOUBLE_EQ(-10, jac(0, 0));
}


//          Copyright Alain Miniussi 2014.
// 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)

// Authors: Alain Miniussi

#include <vector>
#include <iostream>

#include <boost/mpicxx/communicator.hpp>
#include <boost/mpicxx/collectives.hpp>
#include <boost/mpicxx/environment.hpp>
#include <boost/mpicxx/cartesian_communicator.hpp>

#include "check_test.hpp"

namespace mpi = boost::mpicxx;
int main(int argc, char* argv[])
{
  mpi::environment  env;
  mpi::communicator world;

  if (world.size() != 24)  return -1;
  mpi::cartesian_dimension dims[] = {{2, true}, {3,true}, {4,true}};
  mpi::cartesian_communicator cart(world, mpi::cartesian_topology(dims));
  for (int r = 0; r < cart.size(); ++r) {
    cart.barrier();
    if (r == cart.rank()) {
      std::vector<int> c = cart.coordinates(r);
      std::cout << "rk :" << r << " coords: "
                << c[0] << ' ' << c[1] << ' ' << c[2] << '\n';
    }
  }
  return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <eve/detail/diff_div.hpp>

EVE_TEST_TYPES( "Check return types of sph_bessel_j0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::sph_bessel_j0(T(0)), T);
  TTS_EXPR_IS(eve::sph_bessel_j0(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of sph_bessel_j0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__sph_bessel_j0 =  [](auto x) { return eve::sph_bessel_j0(x); };
  auto std__sph_bessel_j0 =  [](auto x)->v_t { return boost::math::sph_bessel(0u, double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_j0(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j0(eve::inf(eve::as< T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_j0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(500)), std__sph_bessel_j0(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(10)), std__sph_bessel_j0(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(5)),  std__sph_bessel_j0(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(2)),  std__sph_bessel_j0(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(1.5)),std__sph_bessel_j0(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(0.5)),std__sph_bessel_j0(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(1)),  std__sph_bessel_j0(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(v_t(0)),  eve::one(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(500)),  T(std__sph_bessel_j0(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(10)) ,  T(std__sph_bessel_j0( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(5))  ,  T(std__sph_bessel_j0( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(2))  ,  T(std__sph_bessel_j0( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(1.5)),  T(std__sph_bessel_j0( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(0.5)),  T(std__sph_bessel_j0( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(1))  ,  T(std__sph_bessel_j0( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0( T(0))  , eve::one(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_j0(a0), map(std__sph_bessel_j0, a0), 10.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(a1), map(std__sph_bessel_j0, a1), 10.0);
  TTS_ULP_EQUAL(eve__sph_bessel_j0(a2), map(std__sph_bessel_j0, a2), 10.0);

};

EVE_TEST( "Check behavior of diff(sph_bessel_j0) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(1.0, 10.0))
        )
  <typename T>(T a0 )
{
  auto eve__diff_bessel_j0 =  [](auto x) { return eve::diff(eve::sph_bessel_j0)(x); };
  auto df = [](auto x){return eve::detail::centered_diffdiv(eve::sph_bessel_j0, x); };

  TTS_RELATIVE_EQUAL(eve__diff_bessel_j0(a0),   df(a0), 2.0e-2);
};


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

double
error_H ( std::size_t Nx , std::size_t Nv , double dt )
{
  const double Tf = 5.;

  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.compute_steps();

  ublas::vector<double> v(Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
    for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  const double alpha = 0.2 , ui = 2.;
  auto tb_M1 = maxwellian( 0.5*alpha , ui , 1. ) , tb_M2 = maxwellian( 0.5*alpha , -ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }

  std::size_t iter = 0;
  double current_time = 0.;

  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);

  std::vector<double> H; H.reserve(int(std::ceil(Tf/dt))+1);

  const double rho_c = 1.-alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(Nx,fh.range.len_x());
    ublas::vector<double> rho(Nx,0.); rho = fh.density(); // compute density from init hot data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += (1.-alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(Nx,0.);
  fft::spectrum_ d(Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , uc5(Nx) , uc6(Nx) , uc7(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , E5 (Nx) , E6 (Nx) , E7 (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfh5(boost::extents[Nv][Nx]) , hfh6(boost::extents[Nv][Nx]) ,
                          hfh7(boost::extents[Nv][Nx]) ;
  fh.write("init.dat");

  while (  current_time < Tf ) {

///////////////////////////////////////////////////////////////////////////////
// DP4(3) /////////////////////////////////////////////////////////////////////

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*dt*sqrt_rho_c), s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) - 0.5*dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*dt*sqrt_rho_c), s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) - 0.5*dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) - dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + hfh3[k][i]/3. - (1./6.)*dt*d[i];
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*dt*d[i];
        }
      }
    } // end stage 5

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    // SAVE TIME STEP
    std::copy(  uc4.begin() ,  uc4.end() ,  uc.begin() );
    std::copy(   E4.begin() ,   E4.end() ,   E.begin() );
    std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    double total_energy = energy(fh,E);
    {
      auto rhoh = fh.density();
      fft::spectrum_ hrhoh(Nx); hrhoh.fft(&rhoh[0]);
      fft::spectrum_ hE(Nx); hE.fft(&E[0]);
      fft::spectrum_ hrhoc(Nx);
      hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
      for ( auto i=1 ; i<Nx ; ++i )
        { hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i]; }
      ublas::vector<double> rhoc (Nx,0.); hrhoc.ifft(rhoc.begin());

      for ( auto i=0 ; i<Nx ; ++i )
        { total_energy += rhoc[i]*uc[i]*uc[i]; }
    }
    H.push_back( total_energy );

    // increment time
    current_time += dt;

    ++iter;
    if ( current_time+dt > Tf ) { dt = Tf - current_time; }
  } // while (  current_time < Tf ) // end of time loop

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
  fh.write("vp.dat");

  double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  return std::abs((h-H[0])/std::abs(H[0]));
}

int
main ( int argc , char const * argv[] )
{
  const std::size_t Nx = 75 , Nv = 1024;
  const double dt_max = 3.*16./Nv;

  for ( auto i=1 ; i<6 ; ++i ) {
    double dt = dt_max/double(i);
    std::cout << dt << " " << std::flush;
    double h = error_H(Nx,Nv,dt);
    std::cout << h << std::endl;
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::normal::normalizing_constant.hpp         //
//                                                                               //
//  (C) Copyright 2009 Erwann Rogard                                             //
//  Use, modification and distribution are subject to the                        //
//  Boost Software License, Version 1.0. (See accompanying file                  //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_NORMALIZING_CONSTANT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_NORMALIZING_CONSTANT_HPP_ER_2009
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/distributions/normal.hpp>

namespace boost{
namespace math{

template<typename T,typename P>
T normalizing_constant(const boost::math::normal_distribution<T,P>& d){
    static T pi = boost::math::constants::pi<T>;
    static T two = static_cast<T>(2);
    return sqrt(two * pi) * d.scale();
}

}// math
}// boost

#endif


/*
* Authors:
* Nicolas Stoiber (nicolas.stoi@gmail.com)
*
* 2016
*/

#ifndef _DENSE_IM_REG_CPU_HPP
#define _DENSE_IM_REG_CPU_HPP

// ************************** EIGEN SPECIFIC DEFS *****************************
// setting the flag below could help see if there are hidden allocations.
//#define EIGEN_NO_AUTOMATIC_RESIZING
//#define EIGEN_RUNTIME_NO_MALLOC
#ifdef EIGEN_RUNTIME_NO_MALLOC
#define malloc_allowed(v) Eigen::internal::set_is_malloc_allowed(v)
#else
#define malloc_allowed(v)
#endif

#define EIGEN_DEFAULT_TO_ROW_MAJOR // comment out this line to use ColMajor
// ****************************************************************************

#undef Success //(X11 and Eigen both define the Success macro)

// match our own structure orders with Eigen internal's
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
    #define MY_STORAGE_ORDER Eigen::RowMajor
#else
    #define MY_STORAGE_ORDER Eigen::ColMajor
#endif

#include <Eigen/Dense>

#include <vector>

#include <CImg.h>

#include "errCodes.h"

#include "im_processing_utils.hpp"
#include "optimization_utils.hpp"

template <typename FloatPrec>
struct DenseImageRegistrationSolver
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
    DenseImageRegistrationSolver() {}
    ~DenseImageRegistrationSolver() {}
public:
    /*
    * initialize registration solver
    * This preallocates most internal variables used in registration
    */
    Common::ErrCode
    init(
            const uint32_t  template_width,
            const uint32_t  template_height,
            const uint32_t  nb_levels,
            const FloatPrec lvl_resz_ratio);

    /*
    * record the template to be registered in images
    * i_normz_factor is a normalization factor to map image values to a favorable
    * floating point range (typically [0., 1.]
    */
    Common::ErrCode
    set_template(
            const cimg_library::CImg<unsigned char> & i_ref_image,
            std::vector<FloatPrec> &                  i_annot_pts,
            FloatPrec                                 i_normz_factor = 1./255.);

    /*
    * register the recorded template (set with 'set_template' in the input
    * image.
    */
    Common::ErrCode
    register_image(
            const cimg_library::CImg<unsigned char> & i_reg_image,
            uint32_t                                  i_nb_iterations,
            std::vector<FloatPrec> &                  io_reg_pts);

    /*
    * get the level templates as images (mainly for debug purposes)
    */
    Common::ErrCode
    get_template_image(
            std::vector<cimg_library::CImg<unsigned char> > & o_lvl_templims) const;

    /*
    * get nb levels
    */
    inline uint32_t nb_levels() const {return m_nb_levels;}

public: // public typedefs
    typedef Eigen::Matrix<FloatPrec, Eigen::Dynamic, Eigen::Dynamic, MY_STORAGE_ORDER>
            MatrixNN;
    typedef Eigen::Matrix<FloatPrec, Eigen::Dynamic, 2, MY_STORAGE_ORDER>
            MatrixN2;
    typedef Eigen::Matrix<FloatPrec, Eigen::Dynamic, 4, MY_STORAGE_ORDER>
            MatrixN4;
    typedef Eigen::Matrix<FloatPrec, Eigen::Dynamic, 6, MY_STORAGE_ORDER>
            MatrixN6;
    typedef Eigen::Matrix<FloatPrec, 1, Eigen::Dynamic, MY_STORAGE_ORDER>
            VecN;
    typedef Eigen::Matrix<FloatPrec, 1, 6, MY_STORAGE_ORDER>
            Vec6;
    typedef Eigen::Matrix<FloatPrec, 4, 2, MY_STORAGE_ORDER>
            Matrix42;
private: // private typedefs
    typedef Common::ImDim<uint32_t>                         ImDim;
    typedef std::vector<ImDim>                              LvlList_ImDim;
    typedef std::vector<FloatPrec>                          LvlList_Ratio;
    typedef std::vector<VecN>                               LvlList_VecN;
    typedef std::vector<std::vector<FloatPrec> >            LvlList_StdN4;
    typedef std::vector<MatrixN4>                           LvlList_MatN4;
    typedef std::vector<MatrixN2>                           LvlList_MatN2;
    typedef std::vector<cimg_library::CImg<unsigned char> > LvlList_Images;
    typedef std::vector<MatrixNN>                           LvlList_MatNN;
// public:
private:
    bool           m_is_init = false;
    bool           m_template_is_set = false;
    uint32_t       m_nb_levels = 3;
    FloatPrec      m_lvl_resz_ratio = 0.5;
    FloatPrec      m_normz_factor = 1./255.;
    ImDim          m_ref_imdim;
    LvlList_Ratio  m_lvl_abs_resz_ratio;
    LvlList_ImDim  m_lvl_templdims;
    LvlList_VecN   m_lvl_templates;
    LvlList_StdN4  m_lvl_Ws;
    LvlList_MatN4  m_lvl_Ws_eigen;
    LvlList_MatN2  m_lvl_gridpts_eigen;
    LvlList_Images m_reg_im_pyr; // registration image resolution pyramid
    // solver containers
    VecN           m_delta_vars;
    LvlList_VecN   m_lvl_errs;
    LvlList_MatNN  m_lvl_jacos;
    LvlList_MatNN  m_lvl_jTj;
    LvlList_VecN   m_lvl_jTb;
    VecN           m_mr_errs; // mr: multi resolution
    MatrixNN       m_mr_jaco;
    MatrixNN       m_mr_jTj;
    VecN           m_mr_jTb;
    VecN           m_curr_pts;
private:
    // The following makes the copy contructor and the assignment operator
    // private to emulate a "non-copyable" class.
    DenseImageRegistrationSolver(DenseImageRegistrationSolver const &);
    DenseImageRegistrationSolver & operator = (DenseImageRegistrationSolver const &);
private:
    /*
    * compute multi-resolution pixel error vector for a given configuration of
    * points
    */
    void
    compute_multires_pix_error(
            const VecN & i_pts,
            VecN &       o_mr_pix_err);

    /*
    * compute pixel error vector for a given configuration of
    * points for a given resoltion level
    */
    void
    compute_lvl_pix_error(
            const VecN & i_pts,
            uint32_t     i_lvl,
            VecN &       o_lvl_pix_err);

    /*
    * compute multi-resolution pixel jacobian matrix for a given configuration of
    * points
    */
    void
    compute_multires_pix_jacobian(
            const VecN & i_pts,
            MatrixNN &   o_mr_pix_err);

    /*
    * compute pixel jacobian matrix for a given configuration of
    * points for a given resoltion level
    */
    void
    compute_lvl_pix_jacobian(
            const VecN & i_pts,
            uint32_t     i_lvl,
            MatrixNN &   o_mr_pix_err);
};

#include "dense_im_reg_cpu.inl.hpp"

#endif // #ifndef _DENSE_IM_REG_CPU_HPP