/*
 * adjointness.cpp
 *
 *  Created on: 22.07.2017
 *      Author: thies
 */

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/SpaceTimeMesh.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/tria.h>
#include <forward/L2RightHandSide.h>
#include <forward/VectorRightHandSide.h>
#include <forward/WaveEquation.h>
#include <forward/WaveEquationAdjoint.h>
#include <forward/WaveEquationBase.h>
#include <gtest/gtest.h>
#include <norms/H1L2.h>
#include <norms/L2L2.h>
#include <stddef.h>
#include <fstream>
#include <iostream>
#include <memory>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::forward;
using namespace wavepi::base;
using namespace wavepi;

/*****
NOTE: This module is used for automatic figure generation
and should therefore only be changed together with the thesis!
*****/

template <int dim>
class TestF : public LightFunction<dim> {
 public:
  virtual ~TestF() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const {
    // if (p.norm() < 0.5)
    return std::sin(t * 2 * numbers::PI);
    // else
    // return 0.0;
  }
};

template <int dim>
class TestG : public LightFunction<dim> {
 public:
  virtual ~TestG() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const {
    Point<dim> pc = Point<dim>::unit_vector(0);
    pc *= 0.5;

    return t * std::sin(p.distance(pc) * 2 * numbers::PI);
  }
};

template <int dim>
class TestH : public LightFunction<dim> {
 public:
  virtual ~TestH() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const { return p.norm() * t; }
};

template <int dim>
class TestC : public LightFunction<dim> {
 public:
  virtual ~TestC() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const { return p[0] * cos(t) + 1.5; }
};

template <int dim>
class TestRho : public LightFunction<dim> {
 public:
  virtual ~TestRho() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const { return p.norm() + sin(t) + 1.5; }
};

template <int dim>
class TestNu : public LightFunction<dim> {
 public:
  virtual ~TestNu() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const {
    // if (t > 1.0) return 0.0;

    return std::abs(p[1]) * sin(t);
  }
};

template <int dim>
class TestQ : public LightFunction<dim> {
 public:
  virtual ~TestQ() = default;
  virtual double evaluate(const Point<dim> &p, const double t) const {
    return p.norm() < 0.5 ? std::sin(t / 2 * 2 * numbers::PI) : 0.0;
  }

  static const Point<dim> q_position;
};

template <int dim>
void run_wave_adjoint_test(int fe_order, int quad_order, int refines, int n_steps,
                           typename WaveEquationBase<dim>::L2AdjointSolver adjoint_solver, bool trivial, double tol,
                           std::shared_ptr<std::ofstream> log = nullptr) {
  AssertThrow(adjoint_solver == WaveEquationBase<dim>::WaveEquationAdjoint ||
                  adjoint_solver == WaveEquationBase<dim>::WaveEquationBackwards,
              ExcInternalError());

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 2.0, dt = t_end / (n_steps - 1);
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  std::shared_ptr<SpaceTimeMesh<dim>> mesh =
      std::make_shared<ConstantMesh<dim>>(times, FE_Q<dim>(fe_order), QGauss<dim>(quad_order), triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  WaveEquation<dim> wave_eq(mesh);

  if (!trivial) {
    wave_eq.set_param_rho(std::make_shared<TestRho<dim>>());
    wave_eq.set_param_c(std::make_shared<TestC<dim>>());
    wave_eq.set_param_q(std::make_shared<TestQ<dim>>());
    wave_eq.set_param_nu(std::make_shared<TestNu<dim>>());
  }

  WaveEquationAdjoint<dim> wave_eq_adj(wave_eq);

  bool use_adj   = adjoint_solver == WaveEquationBase<dim>::WaveEquationAdjoint;
  double err_avg = 0.0;
  double err_simple;

  for (size_t i = 0; i < 1 + 10; i++) {
    std::shared_ptr<DiscretizedFunction<dim>> f, g;

    if (i == 0) {
      TestF<dim> f_cont;
      f = std::make_shared<DiscretizedFunction<dim>>(mesh, f_cont);

      TestG<dim> g_cont;
      g = std::make_shared<DiscretizedFunction<dim>>(mesh, g_cont);
    } else {
      f = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));
      g = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));

      if (n_steps > 7) {
        // make f and g a slightly smoother, random noise might be a too harsh

        f->set_norm(std::make_shared<norms::H1L2<dim>>(0.1));
        f->dot_transform_inverse();

        g->set_norm(std::make_shared<norms::H1L2<dim>>(0.1));
        g->dot_transform_inverse();
      }
    }

    f->set_norm(std::make_shared<norms::L2L2<dim>>());
    *f *= 1.0 / f->norm();

    g->set_norm(std::make_shared<norms::L2L2<dim>>());
    *g *= 1.0 / g->norm();

    DiscretizedFunction<dim> sol_f = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(f), WaveEquation<dim>::Forward);
    sol_f.throw_away_derivative();
    sol_f.set_norm(std::make_shared<norms::L2L2<dim>>());
    EXPECT_GT(sol_f.norm(), 0.0);

    auto g_time_mass = std::make_shared<DiscretizedFunction<dim>>(*g);
    g_time_mass->set_norm(std::make_shared<norms::L2L2<dim>>());

    DiscretizedFunction<dim> adj_g(mesh);
    if (use_adj) {
      // if L2RightHandSide would be used, we would also need a call to solve_mass
      g_time_mass->dot_transform();

      adj_g = wave_eq_adj.run(std::make_shared<VectorRightHandSide<dim>>(g_time_mass));

      // wave_eq_adj does everything except the multiplication with the mass matrix (to allow for optimization)
      adj_g.set_norm(std::make_shared<norms::L2L2<dim>>());
      adj_g.dot_mult_mass_and_transform_inverse();
    } else {
      // dot_transforms not needed here, wave_eq backwards should be the L^2([0,T], L^2)-Adjoint

      adj_g = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(g_time_mass), WaveEquation<dim>::Backward);
      adj_g.throw_away_derivative();
      adj_g.set_norm(std::make_shared<norms::L2L2<dim>>());
    }

    EXPECT_GT(adj_g.norm(), 0.0);

    double dot_solf_g = sol_f * (*g);
    double dot_f_adjg = (*f) * adj_g;
    double fg_err     = std::abs(dot_solf_g - dot_f_adjg) / (std::abs(dot_solf_g) + 1e-300);

    if (i == 0) {
      // deallog << "simple f,g: " << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg
      //         << std::endl;
      err_simple = fg_err;
      deallog << std::scientific << "        relative error for simple f,g = " << fg_err << std::endl;
    } else
      err_avg = ((i - 1) * err_avg + fg_err) / i;

    // deallog << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg
    //        << ", rel. error = " << fg_err << std::endl;

    // EXPECT_LT(zz_err, tol);
  }

  deallog << std::scientific << "average relative error for random f,g = " << err_avg << std::endl;

  double h = dealii::GridTools::maximal_cell_diameter(*mesh->get_triangulation(0));

  if (log)
    *log << std::scientific << mesh->length() << " " << dt << " " << refines << " " << h << " " << err_avg << std::endl;

  EXPECT_LT(err_simple, tol);
  EXPECT_LT(err_avg, tol);
}
}  // namespace

// TEST(WaveEquationAdjointness, Adjoint1DFE1) {
//   for (int i = 3; i < 10; i++)
//     run_wave_adjoint_test<1>(1, 5, 6, 1 << i, WaveEquationBase<1>::WaveEquationAdjoint, false, 1e-4);
// }

// TEST(WaveEquationAdjointness, Adjoint1DFE2) {
//   for (int i = 3; i < 10; i++)
//     run_wave_adjoint_test<1>(2, 5, 4, 1 << i, WaveEquationBase<1>::WaveEquationAdjoint, false, 1e-4);
// }

TEST(WaveEquationAdjointness, Backwards2DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_Backwards2DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 10; i++)
    run_wave_adjoint_test<2>(1, 5, 5, 1 << i, WaveEquationBase<2>::WaveEquationBackwards, false, 1e+2, f);
}

TEST(WaveEquationAdjointness, BackwardsTrivial2DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_BackwardsTrivial2DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 10; i++)
    run_wave_adjoint_test<2>(1, 5, 5, 1 << i, WaveEquationBase<2>::WaveEquationBackwards, true, 1e+2, f);
}

TEST(WaveEquationAdjointness, Adjoint2DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_Adjoint2DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 10; i++)
    run_wave_adjoint_test<2>(1, 5, 5, 1 << i, WaveEquationBase<2>::WaveEquationAdjoint, false, 1e-4, f);
}

TEST(WaveEquationAdjointness, AdjointTrivial2DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_AdjointTrivial2DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 10; i++)
    run_wave_adjoint_test<2>(1, 5, 5, 1 << i, WaveEquationBase<2>::WaveEquationAdjoint, true, 1e-4, f);
}

// TEST(WaveEquationAdjointness, Adjoint2DFE2) {
//   auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_Adjoint2DFE2.dat", std::ios_base::trunc);
//   ASSERT_TRUE(*f) << "could not open file for output";

//   for (int i = 3; i < 10; i++)
//     run_wave_adjoint_test<2>(2, 5, 5, 1 << i, WaveEquationBase<2>::WaveEquationAdjoint, false, 1e-4, f);
// }

TEST(WaveEquationAdjointness, Adjoint3DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_Adjoint3DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 9; i++)
    run_wave_adjoint_test<3>(1, 5, 3, 1 << i, WaveEquationBase<3>::WaveEquationAdjoint, false, 1e-4, f);
}

TEST(WaveEquationAdjointness, Backwards3DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_Backwards3DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 9; i++)
    run_wave_adjoint_test<3>(1, 5, 3, 1 << i, WaveEquationBase<3>::WaveEquationBackwards, false, 1e+2, f);
}

TEST(WaveEquationAdjointness, AdjointTrivial3DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_AdjointTrivial3DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 9; i++)
    run_wave_adjoint_test<3>(1, 5, 3, 1 << i, WaveEquationBase<3>::WaveEquationAdjoint, true, 1e-4, f);
}

TEST(WaveEquationAdjointness, BackwardsTrivial3DFE1) {
  auto f = std::make_shared<std::ofstream>("./WaveEquationAdjointness_BackwardsTrivial3DFE1.dat", std::ios_base::trunc);
  ASSERT_TRUE(*f) << "could not open file for output";

  for (int i = 3; i < 9; i++)
    run_wave_adjoint_test<3>(1, 5, 3, 1 << i, WaveEquationBase<3>::WaveEquationBackwards, true, 1e+2, f);
}


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <random>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "../include/core/config.hpp"
#include "../include/stokes/assembly.hpp"
#include "../include/incompNS/test_cases_factory.hpp"
#include "../include/incompNS/coefficients.hpp"
#include "../include/incompNS/pobserver.hpp"
#include "../include/incompNS/putilities.hpp"
#include "../include/uq/scheduler/communicator.hpp"
#include "../include/uq/scheduler/scheduler.hpp"
#include "../include/uq/sampler/sampler.hpp"

TEST (IncompNSUtils, parDivgFreeVelQuadMesh)
{
    int nprocs, myrank;
    MPI_Comm global_comm = MPI_COMM_WORLD;
    MPI_Comm_size(global_comm, &nprocs);
    MPI_Comm_rank(global_comm, &myrank);

    // config
    std::string filename
            = "../config_files/unit_tests/"
              "pincompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"quad_mesh_l0.mesh";

    const int nparams = 10;
    const int nsamples = 3;
    if (nprocs == 6)
    {
        int nprocsPerGroup = 2;

        // make communicators
        MPI_Comm intra_group_comm, inter_group_comm;
        std::tie (intra_group_comm, inter_group_comm)
                = make_communicators(global_comm,
                                     nprocsPerGroup);
        int mygroup, mygrouprank;
        MPI_Comm_rank(inter_group_comm, &mygroup);
        MPI_Comm_rank(intra_group_comm, &mygrouprank);

        // make schedule
        int mynsamples;
        Array<int> dist_nsamples;
        std::tie(mynsamples, dist_nsamples)
                = make_schedule(inter_group_comm, nsamples);

        // make samples
        std::string samplerType = "uniform";
        DenseMatrix all_myomegas;
        Array<int> all_mysampleIds;
        std::tie(all_myomegas, all_mysampleIds)
                = make_samples(intra_group_comm,
                               inter_group_comm,
                               samplerType,
                               nparams, mynsamples,
                               dist_nsamples);

        // test case
        std::shared_ptr<IncompNSTestCases> testCase
            = make_incompNS_test_case(config);

        // mesh
        const int lx = config["level_x"];
        Mesh *mesh = new Mesh(mesh_file.c_str());
        for (int k=0; k<lx; k++) {
            mesh->UniformRefinement();
        }
        std::shared_ptr<ParMesh> pmesh
                = std::make_shared<ParMesh>
                (intra_group_comm, *mesh);
        delete mesh;

        // FE spaces
        int deg = config["deg_x"];
        int ndim = pmesh->Dimension();
        FiniteElementCollection *hdiv_coll
                = new RT_FECollection(deg, ndim);
        ParFiniteElementSpace *R_space
                = new ParFiniteElementSpace(pmesh.get(),
                                            hdiv_coll);

        FiniteElementCollection *l2_coll
                = new L2_FECollection(deg, ndim);
        ParFiniteElementSpace *W_space
                = new ParFiniteElementSpace(pmesh.get(),
                                            l2_coll);

        // Divergence operator
        // \int_{\Omega} div(u_h) q_h d_{\Omega}
        ParMixedBilinearForm *div_form
                = new ParMixedBilinearForm(R_space,
                                           W_space);
        ConstantCoefficient one(-1.0);
        div_form->AddDomainIntegrator
                (new VectorFEDivergenceIntegrator(one));
        div_form->Assemble();
        div_form->Finalize();
        HypreParMatrix *div = div_form->ParallelAssemble();
        delete div_form;

        // observer
        std::shared_ptr<IncompNSParObserver> observer
                = std::make_shared<IncompNSParObserver>
                (intra_group_comm, config, lx);

        // velocity
        std::shared_ptr <ParGridFunction> v
                = std::make_shared<ParGridFunction>(R_space);
        Vector omegas;
        all_myomegas.GetColumn(0, omegas);
        IncompNSInitialVelocityCoeff v0_coeff(testCase);
        testCase->set_perturbations(omegas);
        v0_coeff.SetTime(0);
        v->ProjectCoefficient(v0_coeff);
        //(*observer) (v);

        double div_old = measure_divergence(div, v.get());
        std::cout << "My group: " << mygroup
                  << "\tMy group rank: " << mygrouprank
                  << "\tWeak divergence before cleaning: "
                  << div_old << std::endl;

        // make divergence free
        ParDivergenceFreeVelocity divFreeVel (config, pmesh);
        divFreeVel (v.get());

        double div_new = measure_divergence(div, v.get());
        std::cout << "My group: " << mygroup
                  << "\tMy group rank: " << mygrouprank
                  << "\tWeak divergence after cleaning: "
                  << div_new << std::endl;

        double TOL=1E-5;
        ASSERT_LE(div_new, TOL);
    }
}

TEST (IncompNSUtils, parDivgFreeVelTriMesh)
{
    int nprocs, myrank;
    MPI_Comm global_comm = MPI_COMM_WORLD;
    MPI_Comm_size(global_comm, &nprocs);
    MPI_Comm_rank(global_comm, &myrank);

    // config
    std::string filename
            = "../config_files/unit_tests/"
              "pincompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"tri_mesh_l0.mesh";

    const int nparams = 10;
    const int nsamples = 3;
    if (nprocs == 6)
    {
        int nprocsPerGroup = 2;

        // make communicators
        MPI_Comm intra_group_comm, inter_group_comm;
        std::tie (intra_group_comm, inter_group_comm)
                = make_communicators(global_comm,
                                     nprocsPerGroup);
        int mygroup, mygrouprank;
        MPI_Comm_rank(inter_group_comm, &mygroup);
        MPI_Comm_rank(intra_group_comm, &mygrouprank);

        // make schedule
        int mynsamples;
        Array<int> dist_nsamples;
        std::tie(mynsamples, dist_nsamples)
                = make_schedule(inter_group_comm, nsamples);

        // make samples
        std::string samplerType = "uniform";
        DenseMatrix all_myomegas;
        Array<int> all_mysampleIds;
        std::tie(all_myomegas, all_mysampleIds)
                = make_samples(intra_group_comm,
                               inter_group_comm,
                               samplerType,
                               nparams, mynsamples,
                               dist_nsamples);

        // test case
        std::shared_ptr<IncompNSTestCases> testCase
            = make_incompNS_test_case(config);

        // mesh
        const int lx = config["level_x"];
        Mesh *mesh = new Mesh(mesh_file.c_str());
        for (int k=0; k<lx; k++) {
            mesh->UniformRefinement();
        }
        std::shared_ptr<ParMesh> pmesh
                = std::make_shared<ParMesh>
                (intra_group_comm, *mesh);
        delete mesh;

        // FE spaces
        int deg = config["deg_x"];
        int ndim = pmesh->Dimension();
        FiniteElementCollection *hdiv_coll
                = new RT_FECollection(deg, ndim);
        ParFiniteElementSpace *R_space
                = new ParFiniteElementSpace(pmesh.get(),
                                            hdiv_coll);

        FiniteElementCollection *l2_coll
                = new L2_FECollection(deg, ndim);
        ParFiniteElementSpace *W_space
                = new ParFiniteElementSpace(pmesh.get(),
                                            l2_coll);

        // Divergence operator
        // \int_{\Omega} div(u_h) q_h d_{\Omega}
        ParMixedBilinearForm *div_form
                = new ParMixedBilinearForm(R_space,
                                           W_space);
        ConstantCoefficient one(-1.0);
        div_form->AddDomainIntegrator
                (new VectorFEDivergenceIntegrator(one));
        div_form->Assemble();
        div_form->Finalize();
        HypreParMatrix *div = div_form->ParallelAssemble();
        delete div_form;

        // observer
        std::shared_ptr<IncompNSParObserver> observer
                = std::make_shared<IncompNSParObserver>
                (intra_group_comm, config, lx);

        // velocity
        std::shared_ptr <ParGridFunction> v
                = std::make_shared<ParGridFunction>(R_space);
        Vector omegas;
        all_myomegas.GetColumn(0, omegas);
        IncompNSInitialVelocityCoeff v0_coeff(testCase);
        testCase->set_perturbations(omegas);
        v0_coeff.SetTime(0);
        v->ProjectCoefficient(v0_coeff);
        //(*observer) (v);

        double div_old = measure_divergence(div, v.get());
        std::cout << "My group: " << mygroup
                  << "\tMy group rank: " << mygrouprank
                  << "\tWeak divergence before cleaning: "
                  << div_old << std::endl;

        // make divergence free
        ParDivergenceFreeVelocity divFreeVel (config, pmesh);
        divFreeVel (v.get());

        double div_new = measure_divergence(div, v.get());
        std::cout << "My group: " << mygroup
                  << "\tMy group rank: " << mygrouprank
                  << "\tWeak divergence after cleaning: "
                  << div_new << std::endl;

        double TOL=1E-4;
        ASSERT_LE(div_new, TOL);
    }
}


#ifndef OSRM_LOCATION_DEPENDENT_DATA_HPP
#define OSRM_LOCATION_DEPENDENT_DATA_HPP

#include <boost/filesystem/path.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <osmium/osm/way.hpp>

#include <string>
#include <unordered_map>

namespace osrm
{
namespace extractor
{

struct LocationDependentData
{
    using point_t = boost::geometry::model::d2::
        point_xy<double, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;
    using segment_t = boost::geometry::model::segment<point_t>;
    using polygon_t = boost::geometry::model::polygon<point_t>;
    using polygon_bands_t = std::vector<std::vector<segment_t>>;
    using box_t = boost::geometry::model::box<point_t>;

    using polygon_position_t = std::size_t;
    using rtree_t = boost::geometry::index::rtree<std::pair<box_t, polygon_position_t>,
                                                  boost::geometry::index::rstar<8>>;

    using property_t = boost::variant<boost::blank, double, std::string, bool>;
    using properties_t = std::unordered_map<std::string, property_t>;

    LocationDependentData(const std::vector<boost::filesystem::path> &file_paths);

    bool empty() const { return rtree.empty(); }

    std::vector<std::size_t> GetPropertyIndexes(const point_t &point) const;

    property_t FindByKey(const std::vector<std::size_t> &property_indexes, const char *key) const;

  private:
    void loadLocationDependentData(const boost::filesystem::path &file_path,
                                   std::vector<rtree_t::value_type> &bounding_boxes);

    rtree_t rtree;
    std::vector<std::pair<polygon_bands_t, std::size_t>> polygons;
    std::vector<properties_t> properties;
};
}
}

#endif


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef GEOMETRY_RECTANGLE_2_INTEGRATED_FLUX_HPP
#define GEOMETRY_RECTANGLE_2_INTEGRATED_FLUX_HPP

#include "rjmcmc/geometry/Rectangle_2.hpp"
#include "rjmcmc/geometry/Segment_2_iterator.hpp"
#include "rjmcmc/geometry/Iso_rectangle_2_Segment_2_clip.hpp"
#include <boost/gil/image.hpp>
#include <boost/gil/extension/matis/float_images.hpp>

template<typename K, typename OrientedImage, typename Segment, typename Functor>
void integrated_flux(const OrientedImage& v, const Segment& s0, Functor& f)
{
    typedef typename OrientedImage::view_t view_t;
    typedef typename view_t::xy_locator xy_locator;

    view_t view = v.view();
    int x0 = v.x0();
    int y0 = v.y0();

    int x1 = x0+view.width();
    int y1 = y0+view.height();
    Segment s(s0);
    typename K::Iso_rectangle_2 bbox(x0,y0,x1,y1);
    if(!clip(bbox,s)) return;

    geometry::Segment_2_iterator<K> it(s);

    xy_locator loc = view.xy_at(
            (typename xy_locator::x_coord_t) (it.x()-x0),
            (typename xy_locator::y_coord_t) (it.y()-y0)
            );

    boost::gil::point2<std::ptrdiff_t> movement[2] = {
        boost::gil::point2<std::ptrdiff_t> (it.step(0), 0),
        boost::gil::point2<std::ptrdiff_t> (0, it.step(1))
    };
    typename K::Vector_2 edge(s.target()-s.source());
    typename K::Vector_2 normal(edge.y(),-edge.x());
    for (; !it.end() ; ++it)
    {
        if(it.x()>=x0 && it.x()<x1 && it.y()>=y0 && it.y()<y1) {
            typename K::RT length = it.length();
            f(length,normal,loc);
        }
        loc += movement[it.axis()];
    }
}

class Flux_functor
{
public:
    template<typename RT, typename Vector, typename Locator>
    void operator()(RT length, const Vector& normal, const Locator& loc)
    {
        Vector g(boost::gil::at_c<0> (*loc), boost::gil::at_c<1> (*loc));
        double dot = length*geometry::to_double(normal * g);
        // res += std::abs(dot)-2.;
        m_value += std::max(0.,dot);
    }
    Flux_functor() : m_value(0) {}
    double value() const { return m_value; }
private:
    double m_value;
};

template<typename OrientedImage, typename K>
double integrated_flux(const OrientedImage& view, const geometry::Rectangle_2<K>& r)
{
    Flux_functor f;
    integrated_flux<K>(view,r.segment(0),f);
    integrated_flux<K>(view,r.segment(1),f);
    integrated_flux<K>(view,r.segment(2),f);
    integrated_flux<K>(view,r.segment(3),f);
    return f.value();
}

#endif // GEOMETRY_RECTANGLE_2_INTEGRATED_FLUX_HPP


#include <fstream>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>

#include <vtkMath.h>
#include <vtkSmartPointer.h>
#include <vtkNew.h>
#include <vtkObjectFactory.h>
#include <vtkTransform.h>
#include <vtkMath.h>

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

#include "vtkConversions.h"
#include "vtkCustomTransformInterpolator.h"
#include "vtkTimeCalibration.h"
#include "statistics.h"
#include "eigenFFTCorrelation.h"
#include "interpolator1D.h"

std::string ToString(CorrelationStrategy correlationStrategy)
{
  switch (correlationStrategy)
  {
    case CorrelationStrategy::DPOS:
      return "dpos";
    case CorrelationStrategy::SPEED_WINDOW:
      return "speed_window";
    case CorrelationStrategy::ACC_WINDOW:
      return "acc_window";
    case CorrelationStrategy::JERK_WINDOW:
      return "jerk_window";
    case CorrelationStrategy::LENGTH:
      return "length";
    case CorrelationStrategy::DERIVATED_LENGTH:
      return "derivated_length";
    case CorrelationStrategy::DROT:
      return "drot";
    case CorrelationStrategy::TRAJECTORY_ANGLE:
      return "trajectory_angle";
    case CorrelationStrategy::ORIENTATION_ANGLE:
      return "orientation_angle";
    case CorrelationStrategy::DERIVATED_ORIENTATION_ARC:
      return "derivated_orientation_arc";
    default:
      return "unkown";
  }
}

Interpolator1D<double> compute_speed_window(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  std::vector<double> times = std::vector<double>();
  std::vector<double> speeds = std::vector<double>();
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();

  double minMidWindowTime = transform->GetMinimumT() + 0.5 * window_width;
  double maxMidWindowTime = transform->GetMaximumT() - 0.5 * window_width;
  double period = transform->GetPeriod();
  double time = minMidWindowTime;
  while (time < maxMidWindowTime)
  {
    transform->InterpolateTransform(time - 0.5 * window_width, prev);
    transform->InterpolateTransform(time + 0.5 * window_width, next);
    speeds.push_back((PositionVectorFromTransform(next)
			    - PositionVectorFromTransform(prev)).norm() / window_width);
    times.push_back(time);
    time = time + period;
  }

  return Interpolator1D<double>(times, speeds);
}

Interpolator1D<double> compute_acc_window(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  std::vector<double> times = std::vector<double>();
  std::vector<double> accs = std::vector<double>();
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();

  double minMidWindowTime = transform->GetMinimumT() + 0.5 * window_width;
  double maxMidWindowTime = transform->GetMaximumT() - 0.5 * window_width;
  double period = transform->GetPeriod();
  double time = minMidWindowTime;
  while (time < maxMidWindowTime)
  {
    transform->InterpolateTransform(time - 0.5 * window_width, prev);
    transform->InterpolateTransform(time, curr);
    transform->InterpolateTransform(time + 0.5 * window_width, next);
    Eigen::Vector3d a = (PositionVectorFromTransform(next)
		    + PositionVectorFromTransform(prev)
		    - 2 * PositionVectorFromTransform(curr)) / (window_width * window_width);
    accs.push_back(a.norm());
    times.push_back(time);
    time = time + period;
  }

  return Interpolator1D<double>(times, accs);
}

Interpolator1D<double> compute_jerk_window(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  std::vector<double> times = std::vector<double>();
  std::vector<double> jerks = std::vector<double>();
  vtkSmartPointer<vtkTransform> t1 = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> t2 = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> t3 = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> t4 = vtkSmartPointer<vtkTransform>::New();

  double minMidWindowTime = transform->GetMinimumT() + 0.5 * window_width;
  double maxMidWindowTime = transform->GetMaximumT() - 0.5 * window_width;
  double period = transform->GetPeriod();
  double time = minMidWindowTime;
  while (time < maxMidWindowTime)
  {
    transform->InterpolateTransform(time - 0.5 * window_width, t1);
    transform->InterpolateTransform(time + (- 0.5 + 1.0/3.0) * window_width, t2);
    transform->InterpolateTransform(time + (- 0.5 + 2.0/3.0) * window_width, t3);
    transform->InterpolateTransform(time + (- 0.5 + 3.0/3.0) * window_width, t4);
    Eigen::Vector3d j = (PositionVectorFromTransform(t4)
		   - 3 * PositionVectorFromTransform(t3)
		   + 3 * PositionVectorFromTransform(t2)
		   - PositionVectorFromTransform(t1))
	    / std::pow(window_width, 3.0);
    jerks.push_back(j.norm());
    times.push_back(time);
    time = time + period;
  }

  return Interpolator1D<double>(times, jerks);
}

Interpolator1D<double> compute_dPos(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform)
{
  std::vector<std::vector<double>> transforms = transform->GetTransformList();
  std::vector<double> t = std::vector<double>(transforms.size() - 1);
  std::vector<double> x = std::vector<double>(transforms.size() - 1);
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();
  for (unsigned int i = 0; i < transforms.size() - 1; i++)
  {
    double t0 = transforms[i][0];
    double t1 = transforms[i+1][0];
    t[i] = 0.5 * (t0 + t1);
    transform->InterpolateTransform(t0, prev);
    transform->InterpolateTransform(t1, curr);
    if (std::abs(t1 - t0) < 0.0001) {
      x[i] = 0.0;
    } else {
      x[i] = (PositionVectorFromTransform(curr)
		      - PositionVectorFromTransform(prev)).norm()
	      / (t1 - t0);
    }
  }
  return Interpolator1D<double>(t, x);
}

Interpolator1D<double> compute_length(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform)
{
  std::vector<std::vector<double>> transforms = transform->GetTransformList();
  std::vector<double> t = std::vector<double>(transforms.size());
  std::vector<double> x = std::vector<double>(transforms.size());
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();
  t[0] = transforms[0][0];
  x[0] = 0.0;
  for (unsigned int i = 1; i < transforms.size(); i++)
  {
    t[i] = transforms[i][0];
    transform->InterpolateTransform(t[i-1], prev);
    transform->InterpolateTransform(t[i], curr);
    x[i] = x[i - 1] + (PositionVectorFromTransform(curr)
		    - PositionVectorFromTransform(prev)).norm();
  }

  return Interpolator1D<double>(t, x);
}

Interpolator1D<double> compute_derivated_length(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  Interpolator1D<double> length = compute_length(transform);
  double tMin = transform->GetMinimumT() + 0.5 * window_width;
  double tMax = transform->GetMaximumT() - 0.5 * window_width;
  int steps = (tMax - tMin) / transform->GetPeriod() + 1;
  std::vector<double> derivated_length = std::vector<double>(steps);
  std::vector<double> times = std::vector<double>(steps);
  for (int i = 0; i < steps; i++)
  {
    double time = tMin + i * transform->GetPeriod();
    times[i] = time;
    // length is an interpolator so no need to check that the sample instants
    // are not the same (they are not, even if the interpolation mode of
    // this->Reference/Aligned is "NEAREST")
    derivated_length[i] = (length.Get(time + 0.5 * window_width)
                           - length.Get(time - 0.5 * window_width))
                    / window_width;
  }

  return Interpolator1D<double>(times, derivated_length);
}

Interpolator1D<double> compute_dRot(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform)
{
  std::vector<std::vector<double>> transforms = transform->GetTransformList();
  std::vector<double> t = std::vector<double>(transforms.size() - 1);
  std::vector<double> x = std::vector<double>(transforms.size() - 1);
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();
  for (int i = 0; i < static_cast<int>(transforms.size()) - 1; i++)
  {
    double t0 = transforms[i][0];
    double t1 = transforms[i+1][0];
    transform->InterpolateTransform(t0, prev);
    transform->InterpolateTransform(t1, curr);
    Eigen::AngleAxisd aa = Eigen::AngleAxisd(RotationMatrixFromTransform(curr)
		    * RotationMatrixFromTransform(prev).transpose());
    t[i] = 0.5 * (t0 + t1);
    if (std::abs(t1 - t0) < 0.0001) {
      x[i] = 0.0;
    } else {
      x[i] = std::abs(aa.angle()) / (t1 - t0);
    }
  }
  return Interpolator1D<double>(t, x);
}

// TODO: handle case where real_sample_time(next) == real_sample_time(prev)
Interpolator1D<double> compute_trajectory_angle(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();
  double tMin = transform->GetMinimumT() + 0.5 * window_width;
  double tMax = transform->GetMaximumT() - 0.5 * window_width;
  int steps = (tMax - tMin) / transform->GetPeriod() + 1;
  std::vector<double> trajectory_angle = std::vector<double>(steps);
  std::vector<double> times = std::vector<double>(steps);
  for (int i = 0; i < steps; i++)
  {
    double time = tMin + i * transform->GetPeriod();
    transform->InterpolateTransform(time - 0.5 * window_width, prev);
    transform->InterpolateTransform(time, curr);
    transform->InterpolateTransform(time + 0.5 * window_width, next);
    times[i] = time;
    trajectory_angle[i] = SignedAngle(PositionVectorFromTransform(curr)
		    - PositionVectorFromTransform(prev),
		    PositionVectorFromTransform(next)
		    - PositionVectorFromTransform(curr));
  }

  return Interpolator1D<double>(times, trajectory_angle);
}

Interpolator1D<double> compute_orientation_arc(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform)
{
  std::vector<std::vector<double>> transforms = transform->GetTransformList();
  std::vector<double> t = std::vector<double>(transforms.size());
  std::vector<double> x = std::vector<double>(transforms.size());
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> curr = vtkSmartPointer<vtkTransform>::New();
  t[0] = transforms[0][0];
  x[0] = 0.0;
  for (unsigned int i = 1; i < transforms.size(); i++)
  {
    t[i] = transforms[i][0];
    transform->InterpolateTransform(t[i-1], prev);
    transform->InterpolateTransform(t[i], curr);
    Eigen::AngleAxisd aa = Eigen::AngleAxisd(RotationMatrixFromTransform(curr)
		    * RotationMatrixFromTransform(prev).transpose());
    x[i] = x[i - 1] + std::abs(aa.angle());
  }

  return Interpolator1D<double>(t, x);
}

Interpolator1D<double> compute_derivated_orientation_arc(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  Interpolator1D<double> orientation_arc = compute_orientation_arc(transform);
  double tMin = transform->GetMinimumT() + 0.5 * window_width;
  double tMax = transform->GetMaximumT() - 0.5 * window_width;
  int steps = (tMax - tMin) / transform->GetPeriod() + 1;
  std::vector<double> derivated_orientation_arc = std::vector<double>(steps);
  std::vector<double> times = std::vector<double>(steps);
  for (int i = 0; i < steps; i++)
  {
    double time = tMin + i * transform->GetPeriod();
    times[i] = time;
    // length is an interpolator so no need to check that the sample instants
    // are not the same (they are not, even if the interpolation mode of
    // this->Reference/Aligned is "NEAREST")
    derivated_orientation_arc[i] =
                    (orientation_arc.Get(time + 0.5 * window_width)
                     - orientation_arc.Get(time - 0.5 * window_width))
                    / window_width;
  }

  return Interpolator1D<double>(times, derivated_orientation_arc);
}

// TODO: handle case where real_sample_time(next) == real_sample_time(prev)
Interpolator1D<double> compute_orientation_angle(
    const vtkSmartPointer<vtkCustomTransformInterpolator>& transform,
    double window_width)
{
  vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
  vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();
  double tMin = transform->GetMinimumT() + 0.5 * window_width;
  double tMax = transform->GetMaximumT() - 0.5 * window_width;
  int steps = (tMax - tMin) / transform->GetPeriod() + 1;
  std::vector<double> orientation_angle = std::vector<double>(steps);
  std::vector<double> times = std::vector<double>(steps);
  for (int i = 0; i < steps; i++)
  {
    double time = tMin + i * transform->GetPeriod();
    transform->InterpolateTransform(time - 0.5 * window_width, prev);
    transform->InterpolateTransform(time + 0.5 * window_width, next);
    times[i] = time;
    Eigen::AngleAxisd angleAxis(RotationMatrixFromTransform(next)
		    * RotationMatrixFromTransform(prev).transpose());
    orientation_angle[i] = angleAxis.angle();
  }

  return Interpolator1D<double>(times, orientation_angle);
}

double ComputeTimeShift(vtkSmartPointer<vtkTemporalTransforms> reference,
                      vtkSmartPointer<vtkTemporalTransforms> aligned,
                      CorrelationStrategy correlationStrategy,
                      double time_window_width,
                      bool substract_mean)
{
  vtkSmartPointer<vtkCustomTransformInterpolator> referenceInterpolator
      = reference->CreateInterpolator();
  referenceInterpolator->SetInterpolationTypeToLinear();
  vtkSmartPointer<vtkCustomTransformInterpolator> alignedInterpolator
      = aligned->CreateInterpolator();
  alignedInterpolator->SetInterpolationTypeToLinear();
  Interpolator1D<double> sig_reference;
  Interpolator1D<double> sig_aligned;
  // first, compute the signals using the chosen method
  switch (correlationStrategy)
  {
    case CorrelationStrategy::DPOS:
      sig_reference = compute_dPos(referenceInterpolator);
      sig_aligned = compute_dPos(alignedInterpolator);
      break;
    case CorrelationStrategy::SPEED_WINDOW:
      sig_reference = compute_speed_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_speed_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::ACC_WINDOW:
      sig_reference = compute_acc_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_acc_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::JERK_WINDOW:
      sig_reference = compute_jerk_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_jerk_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::LENGTH:
      sig_reference = compute_length(referenceInterpolator);
      sig_aligned = compute_length(alignedInterpolator);
      break;
    case CorrelationStrategy::DERIVATED_LENGTH:
      sig_reference = compute_derivated_length(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_derivated_length(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::DROT:
      sig_reference = compute_dRot(referenceInterpolator);
      sig_aligned = compute_dRot(alignedInterpolator);
      break;
    case CorrelationStrategy::TRAJECTORY_ANGLE:
      sig_reference = compute_trajectory_angle(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_trajectory_angle(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::ORIENTATION_ANGLE:
      sig_reference = compute_orientation_angle(referenceInterpolator,
                                                time_window_width);
      sig_aligned = compute_orientation_angle(alignedInterpolator,
                                              time_window_width);
      break;
    case CorrelationStrategy::DERIVATED_ORIENTATION_ARC:
      sig_reference = compute_derivated_orientation_arc(
                              referenceInterpolator,
                              time_window_width);
      sig_aligned = compute_derivated_orientation_arc(
                              alignedInterpolator,
                              time_window_width);
      break;
    default:
      std::cerr << "unknown correlation strategy" << std::endl;
      return 0.0;
  }

  if (substract_mean)
  {
    sig_reference.ApplyValueShift(- sig_reference.Mean());
    sig_aligned.ApplyValueShift(- sig_aligned.Mean());
  }

  // We prefere the two signals to start at t = 0, so we time shift them,
  // but before that we save the information that we would lose otherwise.
  double pre_resample = sig_aligned.GetMinimumT() - sig_reference.GetMinimumT();
  sig_aligned.ApplyTimeShift(- sig_aligned.GetMinimumT());
  sig_reference.ApplyTimeShift(- sig_reference.GetMinimumT());

  // by construction we now have tMin == 0.0;
  double tMax = std::max(sig_reference.GetMaximumT(), sig_aligned.GetMaximumT());
  double period = std::min(sig_reference.GetAveragePeriod(),
		  sig_aligned.GetAveragePeriod());
  int steps = std::floor(tMax / period);
  std::vector<double> reference_resampled = std::vector<double>(steps);
  std::vector<double> aligned_resampled = std::vector<double>(steps);
  for (int i = 0; i < steps; i++)
  {
    double time = 0.0 + i * period;
    reference_resampled[i] = sig_reference.Get(time);
    aligned_resampled[i] = sig_aligned.Get(time);
  }

  std::vector<double> test = fftcorrelate(reference_resampled, aligned_resampled);

  int correlation = max_fftcorrelation(reference_resampled, aligned_resampled);
  double correction = correlation * period;
  double delta_t = pre_resample - correction;
  return delta_t;
}

void ShowTrajectoryInfo(vtkSmartPointer<vtkTemporalTransforms> reference, vtkSmartPointer<vtkTemporalTransforms> aligned)
{
  vtkSmartPointer<vtkCustomTransformInterpolator> referenceI = reference->CreateInterpolator();
  vtkSmartPointer<vtkCustomTransformInterpolator> alignedI = aligned->CreateInterpolator();
  std::cout << std::fixed;
  std::cout << std::setprecision(4);
  std::cout << "reference:                 " << referenceI->GetMaximumT() - referenceI->GetMinimumT()
            << "s from " << referenceI->GetMinimumT() << " to " << referenceI->GetMaximumT()
            << ", period is " << referenceI->GetPeriod() << std::endl;
  std::cout << "aligned:                   " << alignedI->GetMaximumT() - alignedI->GetMinimumT()
            << "s from " << alignedI->GetMinimumT() << " to " << alignedI->GetMaximumT()
            << ", period is " << alignedI->GetPeriod() << std::endl;
}

void DemoAllTimesyncMethods(vtkSmartPointer<vtkTemporalTransforms> reference, vtkSmartPointer<vtkTemporalTransforms> aligned) {
  vtkSmartPointer<vtkCustomTransformInterpolator> referenceI = reference->CreateInterpolator();
  vtkSmartPointer<vtkCustomTransformInterpolator> alignedI = aligned->CreateInterpolator();

  std::cout << std::fixed;
  std::cout << std::setprecision(4);
  ShowTrajectoryInfo(reference, aligned);
  std::cout << std::endl;
  std::cout << "dPos:                      " << ComputeTimeShift(reference, aligned, CorrelationStrategy::DPOS, 1.0) << std::endl;
  std::cout << "speed window:              " << ComputeTimeShift(reference, aligned, CorrelationStrategy::SPEED_WINDOW, 1.0) << std::endl;
  std::cout << "acceleration window:       " << ComputeTimeShift(reference, aligned, CorrelationStrategy::ACC_WINDOW, 3) << std::endl;
  std::cout << "jerk window:               " << ComputeTimeShift(reference, aligned, CorrelationStrategy::JERK_WINDOW, 6) << std::endl;
  std::cout << "derivated length:          " << ComputeTimeShift(reference, aligned, CorrelationStrategy::DERIVATED_LENGTH, 1.0) << std::endl;
  std::cout << "dRot:                      " << ComputeTimeShift(reference, aligned, CorrelationStrategy::DROT, 1.0) << std::endl;
  std::cout << "trajectory angle:          " << ComputeTimeShift(reference, aligned, CorrelationStrategy::TRAJECTORY_ANGLE, 10.0) << std::endl;
  std::cout << "orientation angle:         " << ComputeTimeShift(reference, aligned, CorrelationStrategy::ORIENTATION_ANGLE, 1.0) << std::endl;
  std::cout << "derivated orientation arc: " << ComputeTimeShift(reference, aligned, CorrelationStrategy::DERIVATED_ORIENTATION_ARC, 1.0) << std::endl;
}

double ComputeScale(vtkSmartPointer<vtkTemporalTransforms> reference,
                  vtkSmartPointer<vtkTemporalTransforms> aligned,
                  CorrelationStrategy correlationStrategy,
                  double time_window_width)
{
  const double div_epsilon = 1e-4;
  vtkSmartPointer<vtkCustomTransformInterpolator> referenceInterpolator = reference->CreateInterpolator();
  referenceInterpolator->SetInterpolationTypeToLinear();
  vtkSmartPointer<vtkCustomTransformInterpolator> alignedInterpolator = aligned->CreateInterpolator();
  alignedInterpolator->SetInterpolationTypeToLinear();
  Interpolator1D<double> sig_reference;
  Interpolator1D<double> sig_aligned;
  switch (correlationStrategy)
  {
    case CorrelationStrategy::DPOS:
      sig_reference = compute_dPos(referenceInterpolator);
      sig_aligned = compute_dPos(alignedInterpolator);
      break;
    case CorrelationStrategy::SPEED_WINDOW:
      sig_reference = compute_speed_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_speed_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::ACC_WINDOW:
      sig_reference = compute_acc_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_acc_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::JERK_WINDOW:
      sig_reference = compute_jerk_window(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_jerk_window(alignedInterpolator,
                                             time_window_width);
      break;
    case CorrelationStrategy::LENGTH:
      sig_reference = compute_length(referenceInterpolator);
      sig_aligned = compute_length(alignedInterpolator);
      break;
    case CorrelationStrategy::DERIVATED_LENGTH:
      sig_reference = compute_derivated_length(referenceInterpolator,
                                               time_window_width);
      sig_aligned = compute_derivated_length(alignedInterpolator,
                                             time_window_width);
      break;
    default:
      std::cerr << "unsuported correlation strategy" << std::endl;
      return 0.0;
  }

  double tMin = std::min(sig_reference.GetMinimumT(), sig_aligned.GetMaximumT());
  double tMax = std::max(sig_reference.GetMaximumT(), sig_aligned.GetMaximumT());
  double period = std::min(sig_reference.GetAveragePeriod(),
		  sig_aligned.GetAveragePeriod());
  int steps = std::floor((tMax - tMin) / period);
  std::vector<double> ratios;
  for (int i = 0; i < steps; i++)
  {
    double time = tMin + i * period;
    double reference_resampled = sig_reference.Get(time);
    double aligned_resampled = sig_aligned.Get(time);
    if (reference_resampled < div_epsilon)
    {
      continue;
    }
    else
    {
      ratios.push_back(aligned_resampled / reference_resampled);
    }
  }

  return ComputeMedian(ratios);
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_IROUND_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_IROUND_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/toints.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
 BOOST_DISPATCH_OVERLOAD ( iround_
                         , (typename A0)
                         , bd::cpu_
                         , bd::generic_<bd::arithmetic_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( iround_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 inc = if_else(is_ltz(a0), Mhalf<A0>(), Half<A0>());
      return toints(a0+inc);
    }
  };
} } }

#endif


static char help[] = "Advance_ small CMEs to benchmark intranode performance.\n\n";

#include <fstream>
#include <iomanip>
#include <petscmat.h>
#include <petscvec.h>
#include <petscviewer.h>
#include <armadillo>
#include <cmath>
#include <sys/stat.h>
#include "pacmensl_all.h"

namespace hog1p_cme {
// stoichiometric matrix of the toggle switch model
arma::Mat<PetscInt> SM{{1,-1,-1,0,0,0,0,0,0},
                       {0,0,0,1,0,-1,0,0,0},
                       {0,0,0,0,1,0,-1,0,0},
                       {0,0,0,0,0,1,0,-1,0},
                       {0,0,0,0,0,0,1,0,-1},};

// reaction parameters
const PetscReal k12{1.29},k23{0.0067},k34{0.133},k32{0.027},k43{0.0381},k21{1.0e0},kr21{0.005},kr31{
    0.45},      kr41{0.025},kr22{0.0116},kr32{0.987},kr42{0.0538},trans{0.01},gamma1{0.001},gamma2{0.0049},
// parameters for the time-dependent factors
                r1{6.9e-5},r2{7.1e-3},eta{3.1},Ahog{9.3e09},Mhog{6.4e-4};

// propensity function
inline PetscReal hog_propensity(int *X,int k)
{
  switch (k)
  {
    case 0:return k12 * double(X[0] == 0) + k23 * double(X[0] == 1) + k34 * double(X[0] == 2);
    case 1:return k32 * double(X[0] == 2) + k43 * double(X[0] == 3);
    case 2:return k21 * double(X[0] == 1);
    case 3:return kr21 * double(X[0] == 1) + kr31 * double(X[0] == 2) + kr41 * double(X[0] == 3);
    case 4:return kr22 * double(X[0] == 1) + kr32 * double(X[0] == 2) + kr42 * double(X[0] == 3);
    case 5:return trans * double(X[1]);
    case 6:return trans * double(X[2]);
    case 7:return gamma1 * double(X[3]);
    case 8:return gamma2 * double(X[4]);
    default:return 0.0;
  }
}

int propensity(const int reaction,
               const int num_species,
               const int num_states,
               const int *states,
               PetscReal *outputs,
               void *args)
{
  int (*X)[5] = ( int (*)[5] ) states;
  for (int i = 0; i < num_states; ++i)
  {
    outputs[i] = hog_propensity(X[i],reaction);
  }
  return 0;
}

// function to compute the time-dependent coefficients of the propensity functions
int t_fun(double t,int num_coefs,PetscReal *outputs,void *args)
{
  if (num_coefs != 9) return -1;

  arma::Row<double> u(outputs,9,false,true);
  u.fill(1.0);

  double h1 = (1.0 - exp(-r1 * t)) * exp(-r2 * t);

  double hog1p = pow(h1 / (1.0 + h1 / Mhog),eta) * Ahog;

  u(2) = std::max(0.0,3200.0 - 7710.0 * (hog1p));

  return 0;
}

// Function to constraint the shape of the Fsp
int lhs_constr(PetscInt num_species,PetscInt num_constrs,PetscInt num_states,PetscInt *states,int *vals,
               void *args)
{
  if (num_species != 5)
  {
    return -1;
  }
  if (num_constrs != 7)
  {
    return -1;
  }
  for (int j{0}; j < num_states; ++j)
  {
    for (int i{0}; i < 5; ++i)
    {
      vals[num_constrs * j + i] = (states[num_species * j + i]);
    }
    vals[num_constrs * j + 5] = ((states[num_species * j + 1]) + (states[num_species * j + 3]));
    vals[num_constrs * j + 6] = ((states[num_species * j + 2]) + (states[num_species * j + 4]));
  }
  return 0;
}

arma::Row<int>    rhs_constr_hyperrec{3,10,10,10,10};
arma::Row<double> expansion_factors_hyperrec{0.0,0.25,0.25,0.25,0.25};
arma::Row<int>    rhs_constr{3,10,10,10,10,10,10};
arma::Row<double> expansion_factors{0.0,0.2,0.2,0.2,0.2,0.2,0.2};
}

using arma::dvec;
using arma::Col;
using arma::Row;

using std::cout;
using std::endl;

using namespace hog1p_cme;
using namespace pacmensl;

void output_marginals(MPI_Comm comm,std::string model_name,PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach,std::string constraint_type,
                      DiscreteDistribution &solution,arma::Row<int> constraints);

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver);

int ParseOptions(MPI_Comm comm,
                 PartitioningType &fsp_par_type,
                 PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal,
                 PetscBool &fsp_log_events,
                 ODESolverType &ode_solver);

int main(int argc,char *argv[])
{
  Environment my_env(&argc,&argv,help);

  PetscMPIInt    ierr,myRank,num_procs;
  PetscErrorCode petsc_err;
  MPI_Comm       comm;
  MPI_Comm_dup(PETSC_COMM_WORLD,&comm);
  MPI_Comm_size(comm,&num_procs);
  PetscPrintf(comm,"Solving with %d processors.\n",num_procs);

  // Register PETSc stages
  PetscLogStage stages[2];
  petsc_err = PetscLogStageRegister("Solve with adaptive default state set shape",&stages[0]);
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStageRegister("Solve with fixed default state set shape",&stages[1]);
  CHKERRQ(petsc_err);

  // Set up CME
  std::string         model_name = "hog1p";
  Model               hog1p_model(hog1p_cme::SM,hog1p_cme::t_fun,hog1p_cme::propensity,
                                  nullptr,nullptr,{2});
  PetscReal           t_final    = 60 * 3.0;
  PetscReal           fsp_tol    = 1.0e-4;
  arma::Mat<PetscInt> X0         = {0,0,0,0,0};
  X0 = X0.t();
  arma::Col<PetscReal> p0 = {1.0};

  // Default options
  PartitioningType     fsp_par_type        = PartitioningType::GRAPH;
  PartitioningApproach fsp_repart_approach = PartitioningApproach::REPARTITION;
  ODESolverType        fsp_odes_type       = CVODE;
  PetscBool            output_marginal     = PETSC_FALSE;
  PetscBool            fsp_log_events      = PETSC_FALSE;

  ierr = ParseOptions(comm,fsp_par_type,fsp_repart_approach,output_marginal,fsp_log_events,fsp_odes_type);
  CHKERRQ(ierr);

  FspSolverMultiSinks fsp_solver(comm,fsp_par_type,fsp_odes_type);
  fsp_solver.SetModel(hog1p_model);
  fsp_solver.SetInitialDistribution(X0,p0);
  fsp_solver.SetFromOptions();
  DiscreteDistribution solution;

  petsc_err = PetscLogStagePush(stages[0]);
  CHKERRQ(petsc_err);

  // Solve using adaptive default constraints
  fsp_solver.SetInitialBounds(rhs_constr_hyperrec);
  fsp_solver.SetExpansionFactors(expansion_factors_hyperrec);
  fsp_solver.SetFromOptions();
  solution = fsp_solver.Solve(t_final,fsp_tol,0);
  std::shared_ptr<const StateSetConstrained>
                 fss                   = std::static_pointer_cast<const StateSetConstrained>(fsp_solver.GetStateSet());
  arma::Row<int> final_hyperrec_constr = fss->GetShapeBounds();
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("adaptive_default"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("adaptive_default"),solution,final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");

  petsc_err = PetscLogStagePop();
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStagePush(stages[1]);
  CHKERRQ(petsc_err);
  // Solve using fixed default constraints
  fsp_solver.SetInitialBounds(final_hyperrec_constr);
  solution = fsp_solver.Solve(t_final,fsp_tol,0);
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("fixed_default"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("fixed_default"),solution,final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");
  return ierr;
}

int ParseOptions(MPI_Comm comm,
                 PartitioningType &fsp_par_type,
                 PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal,
                 PetscBool &fsp_log_events,
                 ODESolverType &ode_solver)
{
  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Read options for fsp
  char      opt[100];
  PetscBool opt_set;
  int       ierr;
  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_partitioning_type",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    fsp_par_type = str2part(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_repart_approach",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    fsp_repart_approach = str2partapproach(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_output_marginal",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"1") == 0 || strcmp(opt,"true") == 0)
    {
      output_marginal = PETSC_TRUE;
    }
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_log_events",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"1") == 0 || strcmp(opt,"true") == 0)
    {
      fsp_log_events = PETSC_TRUE;
    }
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_use_solver",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"krylov") == 0)
    {
      ode_solver = KRYLOV;
    } else
    {
      ode_solver = CVODE;
    }
  }

  PetscPrintf(comm,"Partitiniong option %s \n",part2str(fsp_par_type).c_str());
  PetscPrintf(comm,"Repartitoning option %s \n",partapproach2str(fsp_repart_approach).c_str());
  return 0;
}

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver)
{
  int myRank,num_procs;
  MPI_Comm_rank(comm,&myRank);
  MPI_Comm_size(comm,&num_procs);

  std::string ode;
  if (ode_type == KRYLOV)
  {
    ode = "krylov";
  } else
  {
    ode = "cvode";
  }

  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Output time breakdowns
  FspSolverComponentTiming    sum_times, min_times, max_times;
  sum_times = fsp_solver.ReduceComponentTiming("sum");
  min_times = fsp_solver.ReduceComponentTiming("min");
  max_times = fsp_solver.ReduceComponentTiming("max");

  if (myRank == 0)
  {
    struct stat buffer;
    int fstat;

    std::string   filename =
                      model_name + "_time_breakdown.dat";

    fstat = stat (filename.c_str(), &buffer);

    std::ofstream file;
    file.open(filename,std::ios_base::app);

    if (fstat != 0){
      file << "ncpu,partitioner,fsp_shape,ode_solver,min_cput,max_cput,avg_cput,mat_gen_time,ode_time,state_expand_time,min_flops,max_flops,avg_flops \n";
    }

    file << num_procs << ","
         << part_type << ","
         << constraint_type << ","
         << ode << ","
         << min_times.TotalTime << ","
         << max_times.TotalTime << ","
         << sum_times.TotalTime/num_procs << ","
         << sum_times.MatrixGenerationTime/num_procs << ","
         << sum_times.ODESolveTime/num_procs << ","
         << sum_times.StatePartitioningTime/num_procs  << ","
         << min_times.TotalFlops << ","
         << max_times.TotalFlops << ","
         << sum_times.TotalFlops/num_procs << "\n"
        ;
    file.close();
  }

  FiniteProblemSolverPerfInfo perf_info   = fsp_solver.GetSolverPerfInfo();

  if (myRank == 0){
    std::string filename =
                    model_name + "_perf_info_" + std::to_string(num_procs) + "_" + part_type + "_" + part_approach + "_" +
                        constraint_type + ".dat";
    std::ofstream file;
    file.open(filename);
    file << "Model time, ODEs size, Average processor time (sec) \n";
    for (auto i{0}; i < perf_info.n_step; ++i)
    {
      file << perf_info.model_time[i] << "," << perf_info.n_eqs[i] << "," << perf_info.cpu_time[i] << "\n";
    }
    file.close();
  }
}

void output_marginals(MPI_Comm comm,std::string model_name,PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach,std::string constraint_type,
                      DiscreteDistribution &solution,arma::Row<int> constraints)
{
  int myRank,num_procs;
  MPI_Comm_rank(comm,&myRank);
  MPI_Comm_size(comm,&num_procs);

  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  /* Compute the marginal distributions */
  std::vector<arma::Col<PetscReal>> marginals(solution.states_.n_rows);
  for (PetscInt                     i{0}; i < marginals.size(); ++i)
  {
    marginals[i] = Compute1DMarginal(solution,i);
  }

  MPI_Comm_rank(PETSC_COMM_WORLD,&myRank);
  if (myRank == 0)
  {
    for (PetscInt i{0}; i < marginals.size(); ++i)
    {
      std::string filename =
                      model_name + "_marginal_" + std::to_string(i) + "_" + std::to_string(num_procs) + "_" +
                          part_type + "_" + part_approach + "_" + constraint_type + ".dat";
      marginals[i].save(filename,arma::raw_ascii);
    }
    std::string   filename =
                      model_name + "_constraint_bounds_" + std::to_string(num_procs) + "_" + part_type + "_"
                          + part_approach +
                          "_" + constraint_type + ".dat";
    constraints.save(filename,arma::raw_ascii);
  }
}


#include <memory>
#include <random>
#include <vector>

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

#include "test/utils.hpp"

#include "ldaplusplus/Parameters.hpp"
#include "ldaplusplus/events/ProgressEvents.hpp"
#include "ldaplusplus/em/FastOnlineSupervisedMStep.hpp"
#include "ldaplusplus/em/FastSupervisedEStep.hpp"

using namespace Eigen;
using namespace ldaplusplus;

// T will be available as TypeParam in TYPED_TEST functions
template <typename T>
class TestOnlineMaximizationStep : public ParameterizedTest<T> {};

TYPED_TEST_CASE(TestOnlineMaximizationStep, ForFloatAndDouble);

TYPED_TEST(TestOnlineMaximizationStep, Maximization) {
    // Build the corpus
    std::mt19937 rng;
    rng.seed(0);
    MatrixXi X(100, 50);
    VectorXi y(50);
    std::uniform_int_distribution<> class_generator(0, 5);
    std::exponential_distribution<> words_generator(0.1);
    for (int d=0; d<50; d++) {
        for (int w=0; w<100; w++) {
            X(w, d) = static_cast<int>(words_generator(rng));
        }
        y(d) = class_generator(rng);
    }

    // Create the corpus and the model
    auto corpus = std::make_shared<corpus::EigenClassificationCorpus>(X, y);
    MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(10, 100);
    beta.array() -= beta.minCoeff();
    beta.array().rowwise() /= beta.array().colwise().sum();
    auto model = std::make_shared<parameters::SupervisedModelParameters<TypeParam> >(
        VectorX<TypeParam>::Constant(10, 0.1),
        beta,
        MatrixX<TypeParam>::Zero(10, 6)
    );

    em::FastSupervisedEStep<TypeParam> e_step(10, 1e-2, 10);
    em::FastOnlineSupervisedMStep<TypeParam> m_step(
        6,
        1e-2,
        25
    );

    std::vector<TypeParam> progress;
    m_step.get_event_dispatcher()->add_listener(
        [&progress](std::shared_ptr<events::Event> event) {
            if (event->id() == "MaximizationProgressEvent") {
                auto prog_ev = std::static_pointer_cast<events::MaximizationProgressEvent<TypeParam> >(event);
                progress.push_back(prog_ev->likelihood());
            }
        }
    );

    size_t N = 4;
    for (size_t n=0; n<N; n++) {
        corpus->shuffle();

        for (size_t i=0; i<corpus->size(); i++) {
            m_step.doc_m_step(
                corpus->at(i),
                e_step.doc_e_step(
                    corpus->at(i),
                    model
                ),
                model
            );
        }

        m_step.m_step(
            model
        );
    }

    ASSERT_EQ(progress.size(), 2*N);
    for (size_t i=0; i<(2*N - 2); i+=2) {
        EXPECT_LT(
            progress[i] + progress[i+1],
            progress[i+2] + progress[i+3]
        );
    }
}