/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_PHYSICS_TEMPLATES_HH
#define THERMAL_PHYSICS_TEMPLATES_HH

#include <ThermalOperator.hh>
#if defined(ADAMANTINE_HAVE_CUDA) && defined(__CUDACC__)
#include <ThermalOperatorDevice.hh>
#endif
#include <CubeHeatSource.hh>
#include <ElectronBeamHeatSource.hh>
#include <GoldakHeatSource.hh>
#include <ThermalPhysics.hh>

#include <deal.II/base/geometry_info.h>
#include <deal.II/distributed/cell_data_transfer.templates.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_nothing.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/hp/fe_values.h>
#include <deal.II/hp/q_collection.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>

#ifdef ADAMANTINE_WITH_CALIPER
#include <caliper/cali.h>
#endif

#include <algorithm>
#include <execution>

namespace adamantine
{
namespace
{
template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
dealii::LA::distributed::Vector<double, MemorySpaceType>
evaluate_thermal_physics_impl(
    std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> thermal_operator,
    double const t, double const current_source_height,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
    std::vector<Timer> &timers)
{
  timers[evol_time_eval_mat_prop].start();
  thermal_operator->evaluate_material_properties(y);
  timers[evol_time_eval_mat_prop].stop();

  timers[evol_time_eval_th_ph].start();
  thermal_operator->set_time_and_source_height(t, current_source_height);

  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> value(
      y.get_partitioner());
  value = 0.;
  // Apply the Thermal Operator.
  thermal_operator->vmult_add(value, y);

  // Multiply by the inverse of the mass matrix.
  value.scale(*thermal_operator->get_inverse_mass_matrix());

  timers[evol_time_eval_th_ph].stop();

  return value;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
void init_dof_vector(
    double const value,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType> &vector)
{
  unsigned int const local_size = vector.locally_owned_size();
  for (unsigned int i = 0; i < local_size; ++i)
    vector.local_element(i) = value;
}

#ifdef ADAMANTINE_HAVE_CUDA
template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
dealii::LA::distributed::Vector<double, MemorySpaceType> vmult_and_scale(
    std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> const
        &thermal_operator,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> &value,
    std::vector<Timer> &timers)
{
  dealii::LA::distributed::Vector<double, MemorySpaceType> value_dev(
      value.get_partitioner());
  value_dev.import(value, dealii::VectorOperation::insert);

  // Apply the Thermal Operator.
  thermal_operator->vmult_add(value_dev, y);

  // Multiply by the inverse of the mass matrix.
  value_dev.scale(*thermal_operator->get_inverse_mass_matrix());

  timers[evol_time_eval_th_ph].stop();

  return value_dev;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
dealii::LA::distributed::Vector<double, MemorySpaceType>
evaluate_thermal_physics_impl(
    std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> const
        &thermal_operator,
    dealii::hp::FECollection<dim> const &fe_collection, double const t,
    dealii::DoFHandler<dim> const &dof_handler,
    std::vector<std::shared_ptr<HeatSource<dim>>> const &heat_sources,
    double current_source_height, BoundaryType boundary_type,
    std::shared_ptr<MaterialProperty<dim>> const &material_properties,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
    std::vector<Timer> &timers)
{
  timers[evol_time_eval_mat_prop].start();
  // TODO do this on the GPU
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> y_host(
      y.get_partitioner());
  y_host.import(y, dealii::VectorOperation::insert);
  thermal_operator->evaluate_material_properties(y_host);
  timers[evol_time_eval_mat_prop].stop();

  timers[evol_time_eval_th_ph].start();
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> source(
      y.get_partitioner());
  source = 0.;

  // Compute the source term.
  dealii::hp::QCollection<dim> source_q_collection;
  source_q_collection.push_back(dealii::QGauss<dim>(fe_degree + 1));
  source_q_collection.push_back(dealii::QGauss<dim>(1));
  dealii::hp::FEValues<dim> hp_fe_values(fe_collection, source_q_collection,
                                         dealii::update_quadrature_points |
                                             dealii::update_values |
                                             dealii::update_JxW_values);
  unsigned int const dofs_per_cell = fe_collection.max_dofs_per_cell();
  unsigned int const n_q_points = source_q_collection.max_n_quadrature_points();
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::QGauss<dim - 1> face_quadrature(fe_degree + 1);
  dealii::FEFaceValues<dim> fe_face_values(
      fe_collection[0], face_quadrature,
      dealii::update_values | dealii::update_quadrature_points |
          dealii::update_JxW_values);
  unsigned int const n_face_q_points = face_quadrature.size();
  dealii::Vector<double> cell_source(dofs_per_cell);

  // Loop over the locally owned cells with an active FE index of zero
  for (auto const &cell : dealii::filter_iterators(
           dof_handler.active_cell_iterators(),
           dealii::IteratorFilters::LocallyOwnedCell(),
           dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
  {
    cell_source = 0.;
    hp_fe_values.reinit(cell);
    dealii::FEValues<dim> const &fe_values =
        hp_fe_values.get_present_fe_values();

    for (unsigned int i = 0; i < dofs_per_cell; ++i)
    {
      for (unsigned int q = 0; q < n_q_points; ++q)
      {
        double const inv_rho_cp = thermal_operator->get_inv_rho_cp(cell, q);
        double quad_pt_source = 0.;
        dealii::Point<dim> const &q_point = fe_values.quadrature_point(q);
        for (auto &beam : heat_sources)
          quad_pt_source += beam->value(q_point, t, current_source_height);

        cell_source[i] += inv_rho_cp * quad_pt_source *
                          fe_values.shape_value(i, q) * fe_values.JxW(q);
      }
    }

    // If we don't have a adiabatic boundary conditions, we need to add boundary
    // conditions
    if (!(boundary_type & BoundaryType::adiabatic))
    {
      for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell;
           ++f)
      {
        // We need to add the boundary conditions on the faces on the boundary
        // but also on the faces at the interface with FE_Nothing
        auto const &face = cell->face(f);
        if ((face->at_boundary()) &&
            ((!face->at_boundary()) &&
             (cell->neighbor(f)->active_fe_index() != 0)))
        {
          double conv_temperature_infty = 0.;
          double conv_heat_transfer_coef = 0.;
          double rad_temperature_infty = 0.;
          double rad_heat_transfer_coef = 0.;
          if (boundary_type & BoundaryType::convective)
          {
            conv_temperature_infty = material_properties->get_cell_value(
                cell, Property::convection_temperature_infty);
            conv_heat_transfer_coef = material_properties->get_cell_value(
                cell, StateProperty::convection_heat_transfer_coef);
          }
          if (boundary_type & BoundaryType::radiative)
          {
            rad_temperature_infty = material_properties->get_cell_value(
                cell, Property::radiation_temperature_infty);
            rad_heat_transfer_coef = material_properties->get_cell_value(
                cell, StateProperty::radiation_heat_transfer_coef);
          }

          fe_face_values.reinit(cell, face);
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int q = 0; q < n_face_q_points; ++q)
            {
              double const inv_rho_cp =
                  thermal_operator->get_inv_rho_cp(cell, q);
              cell_source[i] +=
                  inv_rho_cp *
                  (conv_heat_transfer_coef * conv_temperature_infty +
                   rad_heat_transfer_coef * rad_temperature_infty) *
                  fe_face_values.shape_value(i, q) * fe_face_values.JxW(q);
            }
          }
        }
      }
    }
    cell->get_dof_indices(local_dof_indices);
    affine_constraints.distribute_local_to_global(cell_source,
                                                  local_dof_indices, source);
  }

  source.compress(dealii::VectorOperation::add);

  return vmult_and_scale<dim, fe_degree, MemorySpaceType>(thermal_operator, y,
                                                          source, timers);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
void init_dof_vector(
    double const value,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType> &vector)
{
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
      vector_host(vector.get_partitioner());
  unsigned int const local_size = vector_host.local_size();
  for (unsigned int i = 0; i < local_size; ++i)
    vector_host.local_element(i) = value;

  vector.import(vector_host, dealii::VectorOperation::insert);
}
#endif
} // namespace

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::ThermalPhysics(
    MPI_Comm const &communicator, boost::property_tree::ptree const &database,
    Geometry<dim> &geometry)
    : _boundary_type(BoundaryType::invalid), _geometry(geometry),
      _dof_handler(_geometry.get_triangulation())
{
  // Create the FECollection
  _fe_collection.push_back(dealii::FE_Q<dim>(fe_degree));
  _fe_collection.push_back(dealii::FE_Nothing<dim>());

  // Create the QCollection
  _q_collection.push_back(QuadratureType(fe_degree + 1));
  _q_collection.push_back(QuadratureType(1));

  // Create the material properties
  boost::property_tree::ptree const &material_database =
      database.get_child("materials");
  _material_properties.reset(new MaterialProperty<dim>(
      communicator, _geometry.get_triangulation(), material_database));

  // Create the heat sources
  boost::property_tree::ptree const &source_database =
      database.get_child("sources");
  // PropertyTreeInput sources.n_beams
  unsigned int const n_beams = source_database.get<unsigned int>("n_beams");
  _heat_sources.resize(n_beams);
  for (unsigned int i = 0; i < n_beams; ++i)
  {
    // PropertyTreeInput sources.beam_X.type
    boost::property_tree::ptree const &beam_database =
        source_database.get_child("beam_" + std::to_string(i));
    std::string type = beam_database.get<std::string>("type");
    if (type == "goldak")
    {
      _heat_sources[i] = std::make_shared<GoldakHeatSource<dim>>(beam_database);
    }
    else if (type == "electron_beam")
    {
      _heat_sources[i] =
          std::make_shared<ElectronBeamHeatSource<dim>>(beam_database);
    }
    else if (type == "cube")
    {
      _heat_sources[i] = std::make_shared<CubeHeatSource<dim>>(beam_database);
    }
    else
    {
      ASSERT_THROW(false, "Error: Beam type '" +
                              beam_database.get<std::string>("type") +
                              "' not recognized.");
    }
  }

  // Create the boundary condition type
  // PropertyTreeInput boundary.type
  std::string boundary_type_str = database.get<std::string>("boundary.type");
  // Parse the string
  size_t pos_str = 0;
  std::string boundary;
  std::string delimiter = ",";
  auto parse_boundary_type = [&](std::string const &boundary) {
    if (boundary == "adiabatic")
    {
      ASSERT_THROW(_boundary_type == BoundaryType::invalid,
                   "Adiabatic condition cannot be combined with another type.");
      _boundary_type = BoundaryType::adiabatic;
    }
    else
    {
      ASSERT_THROW(_boundary_type != BoundaryType::adiabatic,
                   "Adiabatic condition cannot be combined with another type.");

      if (boundary == "radiative")
      {
        _boundary_type |= BoundaryType::radiative;
      }
      else if (boundary == "convective")
      {
        _boundary_type |= BoundaryType::convective;
      }
      else
      {
        ASSERT_THROW(false, "Unknown boundary type.");
      }
    }
  };
  while ((pos_str = boundary_type_str.find(delimiter)) != std::string::npos)
  {
    boundary = boundary_type_str.substr(0, pos_str);
    parse_boundary_type(boundary);
    boundary_type_str.erase(0, pos_str + delimiter.length());
  }
  parse_boundary_type(boundary_type_str);

  // Create the thermal operator
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
    _thermal_operator =
        std::make_shared<ThermalOperator<dim, fe_degree, MemorySpaceType>>(
            communicator, _boundary_type, _material_properties, _heat_sources);
#if defined(ADAMANTINE_HAVE_CUDA) && defined(__CUDACC__)
  else
    _thermal_operator = std::make_shared<
        ThermalOperatorDevice<dim, fe_degree, MemorySpaceType>>(
        communicator, _material_properties);
#endif

  // Create the time stepping scheme
  boost::property_tree::ptree const &time_stepping_database =
      database.get_child("time_stepping");
  // PropertyTreeInput time_stepping.method
  std::string method = time_stepping_database.get<std::string>("method");
  std::transform(method.begin(), method.end(), method.begin(),
                 [](unsigned char c) { return std::tolower(c); });
  if (method.compare("forward_euler") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::FORWARD_EULER);
  else if (method.compare("rk_third_order") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::RK_THIRD_ORDER);
  else if (method.compare("rk_fourth_order") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::RK_CLASSIC_FOURTH_ORDER);
  else if (method.compare("heun_euler") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::HEUN_EULER);
    _embedded_method = true;
  }
  else if (method.compare("bogacki_shampine") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::BOGACKI_SHAMPINE);
    _embedded_method = true;
  }
  else if (method.compare("dopri") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::DOPRI);
    _embedded_method = true;
  }
  else if (method.compare("fehlberg") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::FEHLBERG);
    _embedded_method = true;
  }
  else if (method.compare("cash_karp") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::CASH_KARP);
    _embedded_method = true;
  }
  else if (method.compare("backward_euler") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::BACKWARD_EULER);
    _implicit_method = true;
  }
  else if (method.compare("implicit_midpoint") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::IMPLICIT_MIDPOINT);
    _implicit_method = true;
  }
  else if (method.compare("crank_nicolson") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::CRANK_NICOLSON);
    _implicit_method = true;
  }
  else if (method.compare("sdirk2") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::SDIRK_TWO_STAGES);
    _implicit_method = true;
  }

  if (_embedded_method == true)
  {
    // PropertyTreeInput time_steppping.coarsening_parameter
    double coarsen_param =
        time_stepping_database.get("coarsening_parameter", 1.2);
    // PropertyTreeInput time_steppping.refining_parameter
    double refine_param = time_stepping_database.get("refining_parameter", 0.8);
    // PropertyTreeInput time_stepping.min_time_step
    double min_delta = time_stepping_database.get("min_time_step", 1e-14);
    // PropertyTreeInput time_stepping.max_time_step
    double max_delta = time_stepping_database.get("max_time_step", 1e100);
    // PropertyTreeInput time_stepping.refining_tolerance
    double refine_tol = time_stepping_database.get("refining_tolerance", 1e-8);
    // PropertyTreeInput time_stepping.coarsening_tolerance
    double coarsen_tol =
        time_stepping_database.get("coarsening_tolerance", 1e-12);
    dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *embedded_rk =
        static_cast<
            dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    embedded_rk->set_time_adaptation_parameters(coarsen_param, refine_param,
                                                min_delta, max_delta,
                                                refine_tol, coarsen_tol);
  }

  // If the time stepping scheme is implicit, set the parameters for the solver
  // and create the implicit operator.
  if (_implicit_method == true)
  {
    // PropertyTreeInput time_stepping.max_iteration
    _max_iter = time_stepping_database.get("max_iteration", 1000);
    // PropertyTreeInput time_stepping.tolerance
    _tolerance = time_stepping_database.get("tolerance", 1e-12);
    // PropertyTreeInput time_stepping.right_preconditioning
    _right_preconditioning =
        time_stepping_database.get("right_preconditioning", false);
    // PropertyTreeInput time_stepping.n_tmp_vectors
    _max_n_tmp_vectors = time_stepping_database.get("n_tmp_vectors", 30);
    // PropertyTreeInput time_stepping.newton_max_iteration
    unsigned int newton_max_iter =
        time_stepping_database.get("newton_max_iteration", 100);
    // PropertyTreeInput time_stepping.newton_tolerance
    double newton_tolerance =
        time_stepping_database.get("newton_tolerance", 1e-6);
    dealii::TimeStepping::ImplicitRungeKutta<LA_Vector> *implicit_rk =
        static_cast<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    implicit_rk->set_newton_solver_parameters(newton_max_iter,
                                              newton_tolerance);

    // PropertyTreeInput time_stepping.jfnk
    bool jfnk = time_stepping_database.get("jfnk", false);
    _implicit_operator = std::make_unique<ImplicitOperator<MemorySpaceType>>(
        _thermal_operator, jfnk);
  }

  // Set material on part of the domain
  // PropertyTreeInput geometry.material_height
  double const material_height = database.get("geometry.material_height", 1e9);
  for (auto const &cell :
       dealii::filter_iterators(_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    // If the center of the cell is below material_height, it contains material
    // otherwise it does not.
    if (cell->center()[axis<dim>::z] < material_height)
      cell->set_active_fe_index(0);
    else
      cell->set_active_fe_index(1);
  }

  // Set the initial height of the heat source. Right now this is just the
  // maximum heat source height, which can lead to unexpected behavior for
  // different sources with different heights.
  double temp_height = std::numeric_limits<double>::lowest();
  for (auto const &source : _heat_sources)
  {
    temp_height = std::max(temp_height, source->get_current_height(0.0));
  }
  _current_source_height = temp_height;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::setup_dofs()
{
  _dof_handler.distribute_dofs(_fe_collection);
  dealii::IndexSet locally_relevant_dofs;
  dealii::DoFTools::extract_locally_relevant_dofs(_dof_handler,
                                                  locally_relevant_dofs);
  _affine_constraints.clear();
  _affine_constraints.reinit(locally_relevant_dofs);
  dealii::DoFTools::make_hanging_node_constraints(_dof_handler,
                                                  _affine_constraints);
  _affine_constraints.close();

  _thermal_operator->reinit(_dof_handler, _affine_constraints, _q_collection);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::compute_inverse_mass_matrix()
{
  _thermal_operator->compute_inverse_mass_matrix(
      _dof_handler, _affine_constraints, _fe_collection);
  if (_implicit_method == true)
    _implicit_operator->set_inverse_mass_matrix(
        _thermal_operator->get_inverse_mass_matrix());
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    add_material(
        std::vector<
            typename dealii::DoFHandler<dim>::active_cell_iterator> const
            &elements_to_activate,
        double new_material_temperature,
        dealii::LA::distributed::Vector<double, MemorySpaceType> &solution)
{
#ifdef ADAMANTINE_WITH_CALIPER
  CALI_CXX_MARK_FUNCTION;
#endif
  std::vector<dealii::Vector<double>> data_to_transfer;
  unsigned int const dofs_per_cell = _dof_handler.get_fe().n_dofs_per_cell();
  dealii::Vector<double> cell_solution(dofs_per_cell);
  dealii::Vector<double> dummy_cell_solution(dofs_per_cell);
  for (auto &val : dummy_cell_solution)
  {
    val = std::numeric_limits<double>::infinity();
  }
  solution.update_ghost_values();
  std::vector<dealii::types::global_dof_index> dof_indices(dofs_per_cell);
  for (auto const &cell : _dof_handler.active_cell_iterators())
  {
    if ((cell->is_locally_owned()) && (cell->active_fe_index() == 0))
    {
      cell->get_dof_values(solution, cell_solution);
      data_to_transfer.push_back(cell_solution);
    }
    else
    {
      data_to_transfer.push_back(dummy_cell_solution);
    }
  }

  // Activate elements by updating the fe_index
  for (auto const &cell : elements_to_activate)
  {
    cell->set_active_fe_index(0);
  }

  dealii::parallel::distributed::Triangulation<dim> &triangulation =
      dynamic_cast<dealii::parallel::distributed::Triangulation<dim> &>(
          const_cast<dealii::Triangulation<dim> &>(
              _dof_handler.get_triangulation()));
  triangulation.prepare_coarsening_and_refinement();
  dealii::parallel::distributed::CellDataTransfer<
      dim, dim, std::vector<dealii::Vector<double>>>
      cell_data_trans(triangulation);
  cell_data_trans.prepare_for_coarsening_and_refinement(data_to_transfer);
  triangulation.execute_coarsening_and_refinement();

  setup_dofs();

  // Update MaterialProperty DoFHandler and resize the state vectors
  _material_properties->reinit_dofs();

  // Recompute the inverse of the mass matrix
  compute_inverse_mass_matrix();

  initialize_dof_vector(new_material_temperature, solution);
  std::vector<dealii::Vector<double>> transferred_data(
      triangulation.n_active_cells(), dealii::Vector<double>(dofs_per_cell));
  cell_data_trans.unpack(transferred_data);

  unsigned int cell_i = 0;
  for (auto const &cell : _dof_handler.active_cell_iterators())
  {
    if ((cell->is_locally_owned()) && (transferred_data[cell_i][0] !=
                                       std::numeric_limits<double>::infinity()))
    {
      cell->set_dof_values(transferred_data[cell_i], solution);
    }
    ++cell_i;
  }

  // Communicate the results.
  solution.compress(dealii::VectorOperation::min);

  // Set the value to the newly create DoFs. Here we need to be careful with the
  // hanging nodes. When there is a hanging node, the dofs at the vertices are
  // "doubled": there is a dof associated to the coarse cell and a dof
  // associated to the fine cell. The final value is decided by
  // AffineConstraints. Thus, we need to make sure that the newly activated
  // cells are at the same level than their neighbors.
  std::for_each(std::execution::par_unseq, solution.begin(), solution.end(),
                [&](double &val) {
                  if (val == std::numeric_limits<double>::infinity())
                  {
                    val = new_material_temperature;
                  }
                });
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
double ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    evolve_one_time_step(
        double t, double delta_t,
        dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
        std::vector<Timer> &timers)
{

  // Update the height of the heat source. Right now this is just the
  // maximum heat source height, which can lead to unexpected behavior for
  // different sources with different heights.
  double temp_height = std::numeric_limits<double>::lowest();
  for (auto const &source : _heat_sources)
  {
    temp_height = std::max(temp_height, source->get_current_height(t));
  }
  _current_source_height = temp_height;

  auto eval = [&](double const t, LA_Vector const &y) {
    return evaluate_thermal_physics(t, y, timers);
  };
  auto id_m_Jinv = [&](double const t, double const tau, LA_Vector const &y) {
    return id_minus_tau_J_inverse(t, tau, y, timers);
  };

  double time = _time_stepping->evolve_one_time_step(eval, id_m_Jinv, t,
                                                     delta_t, solution);

  // If the method is embedded, get the next time step. Otherwise, just use the
  // current time step.
  if (_embedded_method == false)
    _delta_t_guess = delta_t;
  else
  {
    dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *embedded_rk =
        static_cast<
            dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    _delta_t_guess = embedded_rk->get_status().delta_t_guess;
  }

  // Return the time at the end of the time step. This may be different than
  // t+delta_t for embedded methods.
  return time;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    initialize_dof_vector(
        dealii::LA::distributed::Vector<double, MemorySpaceType> &vector) const
{
  _thermal_operator->initialize_dof_vector(vector);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    initialize_dof_vector(
        double const value,
        dealii::LA::distributed::Vector<double, MemorySpaceType> &vector) const
{
  // Resize the vector
  _thermal_operator->initialize_dof_vector(vector);

  init_dof_vector<dim, fe_degree, MemorySpaceType>(value, vector);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::get_state_from_material_properties()
{
  _thermal_operator->get_state_from_material_properties();
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::set_state_to_material_properties()
{
  _thermal_operator->set_state_to_material_properties();
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
dealii::LA::distributed::Vector<double, MemorySpaceType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    evaluate_thermal_physics(
        double const t,
        dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
        std::vector<Timer> &timers) const
{
#ifdef ADAMANTINE_WITH_CALIPER
  CALI_CXX_MARK_FUNCTION;
#endif
  if constexpr (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
  {
    _thermal_operator->evaluate_material_properties(y);
    return evaluate_thermal_physics_impl<dim, fe_degree, MemorySpaceType>(
        _thermal_operator, t, _current_source_height, y, timers);
  }
  else
  {
    // TODO do this on the GPU
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> y_host(
        y.get_partitioner());
    y_host.import(y, dealii::VectorOperation::insert);
    _thermal_operator->evaluate_material_properties(y_host);
    return evaluate_thermal_physics_impl<dim, fe_degree, MemorySpaceType>(
        _thermal_operator, _fe_collection, t, _dof_handler, _heat_sources,
        _current_source_height, _boundary_type, _material_properties,
        _affine_constraints, y, timers);
  }

  // Dummy to silence warning
  return dealii::LA::distributed::Vector<double, MemorySpaceType>();
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
dealii::LA::distributed::Vector<double, MemorySpaceType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    id_minus_tau_J_inverse(
        double const /*t*/, double const tau,
        dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
        std::vector<Timer> &timers) const
{
  timers[evol_time_J_inv].start();
  _implicit_operator->set_tau(tau);
  dealii::LA::distributed::Vector<double, MemorySpaceType> solution(
      y.get_partitioner());

  // TODO Add a geometric multigrid preconditioner.
  dealii::PreconditionIdentity preconditioner;

  dealii::SolverControl solver_control(_max_iter, _tolerance * y.l2_norm());
  // We need to inverse (I - tau M^{-1} J). While M^{-1} and J are SPD,
  // (I - tau M^{-1} J) is symmetric indefinite in the general case.
  typename dealii::SolverGMRES<
      dealii::LA::distributed::Vector<double, MemorySpaceType>>::AdditionalData
      additional_data(_max_n_tmp_vectors, _right_preconditioning);
  dealii::SolverGMRES<dealii::LA::distributed::Vector<double, MemorySpaceType>>
      solver(solver_control, additional_data);
  solver.solve(*_implicit_operator, solution, y, preconditioner);

  timers[evol_time_J_inv].stop();

  return solution;
}
} // namespace adamantine

#endif


/*
 * do_learning.cpp
 *
 * incrementally learning of objects by
 * transfering object indices from initial cloud to the remaining clouds
 * using given camera poses
 *
 *  Created on: June, 2015
 *      Author: Thomas Faeulhammer
 */

#ifndef EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#endif

#include "v4r/object_modelling/incremental_object_learning.h"

#include <stdlib.h>
#include <thread>
#include <iostream>

#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/segmentation/supervoxel_clustering.h>

#include <v4r/common/convertCloud.h>
#include <v4r/common/convertNormals.h>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/registration/metrics.h>
#include <v4r/common/binary_algorithms.h>
#include <v4r/common/normals.h>
#include <v4r/common/noise_models.h>
#include <v4r/common/pcl_visualization_utils.h>
#include <v4r/io/filesystem.h>
#include <v4r/io/eigen.h>
#include <v4r/common/zbuffering.h>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#ifdef HAVE_SIFTGPU
    #include <v4r/features/sift_local_estimator.h>
#else
    #include <v4r/features/opencv_sift_local_estimator.h>
#endif

namespace v4r
{
namespace object_modelling
{

bool
IOL::calcSiftFeatures (const pcl::PointCloud<PointT> &cloud_src,
                       pcl::PointCloud<PointT> &sift_keypoints,
                       std::vector< size_t > &sift_keypoint_indices,
                       std::vector<std::vector<float> > &sift_signatures,
                       std::vector<float> &sift_keypoint_scales)
{
    std::vector<int> sift_kp_indices;

#ifdef HAVE_SIFTGPU
    (void) sift_keypoint_indices;
    boost::shared_ptr < SIFTLocalEstimation<PointT> > estimator (new SIFTLocalEstimation<PointT>(sift_));
    bool ret = estimator->estimate (cloud_src, sift_keypoints, sift_signatures, sift_keypoint_scales);
    estimator->getKeypointIndices( sift_kp_indices );
#else
    (void)sift_keypoint_scales; //silences compiler warning of unused variable
    boost::shared_ptr < OpenCVSIFTLocalEstimation<PointT> > estimator (new OpenCVSIFTLocalEstimation<PointT>);
    pcl::PointCloud<PointT> processed_foo;
    bool ret = estimator->estimate (cloud_src, processed_foo, sift_keypoints, sift_signatures);
    estimator->getKeypointIndices( sift_kp_indices );
#endif
    sift_keypoint_indices = convertVecInt2VecSizet(sift_kp_indices);
    return ret;
}

void
IOL::estimateViewTransformationBySIFT(const pcl::PointCloud<PointT> &src_cloud,
                                      const pcl::PointCloud<PointT> &dst_cloud,
                                      const std::vector<size_t> &src_sift_keypoint_indices,
                                      const std::vector<size_t> &dst_sift_keypoint_indices,
                                      const std::vector<std::vector<float> > &src_sift_signatures,
                                      boost::shared_ptr< flann::Index<DistT> > &dst_flann_index,
                                      std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations,
                                      bool use_gc )
{
    const int K = 1;
    flann::Matrix<int> indices = flann::Matrix<int> ( new int[K], 1, K );
    flann::Matrix<float> distances = flann::Matrix<float> ( new float[K], 1, K );

    boost::shared_ptr< pcl::PointCloud<PointT> > pSiftKeypointsSrc (new pcl::PointCloud<PointT>);
    boost::shared_ptr< pcl::PointCloud<PointT> > pSiftKeypointsDst (new pcl::PointCloud<PointT>);
    pcl::copyPointCloud(src_cloud, src_sift_keypoint_indices, *pSiftKeypointsSrc );
    pcl::copyPointCloud(dst_cloud, dst_sift_keypoint_indices, *pSiftKeypointsDst);

    pcl::CorrespondencesPtr temp_correspondences ( new pcl::Correspondences );
    temp_correspondences->resize(pSiftKeypointsSrc->size ());

    for ( size_t keypointId = 0; keypointId < pSiftKeypointsSrc->points.size (); keypointId++ )
    {
        nearestKSearch ( dst_flann_index, src_sift_signatures[ keypointId ], K, indices, distances );

        pcl::Correspondence corr;
        corr.distance = distances[0][0];
        corr.index_query = keypointId;
        corr.index_match = indices[0][0];
        temp_correspondences->at(keypointId) = corr;
    }

    if(!use_gc)
    {
        pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::Ptr rej;
        rej.reset (new pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> ());
        pcl::CorrespondencesPtr after_rej_correspondences (new pcl::Correspondences ());

        rej->setMaximumIterations (50000);
        rej->setInlierThreshold (0.02);
        rej->setInputTarget (pSiftKeypointsDst);
        rej->setInputSource (pSiftKeypointsSrc);
        rej->setInputCorrespondences (temp_correspondences);
        rej->getCorrespondences (*after_rej_correspondences);

        Eigen::Matrix4f refined_pose;
        transformations.push_back( rej->getBestTransformation () );
        pcl::registration::TransformationEstimationSVD<PointT, PointT> t_est;
        t_est.estimateRigidTransformation (*pSiftKeypointsSrc, *pSiftKeypointsDst, *after_rej_correspondences, refined_pose);
        transformations.back() = refined_pose;
    }
    else
    {
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms;
        pcl::GeometricConsistencyGrouping<PointT, PointT> gcg_alg;

        gcg_alg.setGCThreshold (15);
        gcg_alg.setGCSize (0.01);
        gcg_alg.setInputCloud(pSiftKeypointsSrc);
        gcg_alg.setSceneCloud(pSiftKeypointsDst);
        gcg_alg.setModelSceneCorrespondences(temp_correspondences);

        std::vector<pcl::Correspondences> clustered_corrs;
        gcg_alg.recognize(new_transforms, clustered_corrs);
        transformations.insert(transformations.end(), new_transforms.begin(), new_transforms.end());
    }
}

std::vector<bool>
IOL::extractEuclideanClustersSmooth (
        const pcl::PointCloud<PointT>::ConstPtr &cloud,
        const pcl::PointCloud<pcl::Normal> &normals,
        const std::vector<bool> &initial_mask,
        const std::vector<bool> &bg_mask) const
{
    assert (cloud->points.size () == normals.points.size ());

    pcl::octree::OctreePointCloudSearch<PointT> octree(0.005f);
    octree.setInputCloud ( cloud );
    octree.addPointsFromInputCloud ();

    // Create a bool vector of processed point indices, and initialize it to false
    std::vector<bool> to_grow = initial_mask;
    std::vector<bool> in_cluster = initial_mask;

    bool stop = false;
    while(!stop)
    {
        stop = true;
        std::vector<bool> is_new_point (cloud->points.size (), false);  // do as long as there is no new point
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;

        for (size_t i = 0; i < cloud->points.size (); i++)
        {
            if (!to_grow[i])
                continue;

            if (octree.radiusSearch (cloud->points[i], param_.radius_, nn_indices, nn_distances))
            {
                for (size_t j = 0; j < nn_indices.size (); j++) // is nn_indices[0] the same point?
                {
                    if( !in_cluster[ nn_indices[j] ] && !bg_mask[ nn_indices[j] ])  // if nearest neighbor is not already an object and is not a point to be neglected (background)
                    {
                        //check smoothness constraint
                        Eigen::Vector3f n1 = normals.points[i].getNormalVector3fMap();
                        Eigen::Vector3f n2 = normals.points[nn_indices[j]].getNormalVector3fMap();
                        n1.normalize();
                        n2.normalize();
                        float dot_p = n1.dot(n2);

                        if (dot_p >= param_.eps_angle_)
                        {
                            stop = false;
                            is_new_point[ nn_indices[j] ] = true;
                            in_cluster[ nn_indices[j] ] = true;
                        }
                    }
                }
            }
        }
        to_grow = is_new_point;
    }
    return in_cluster;
}

void
IOL::updatePointNormalsFromSuperVoxels(const pcl::PointCloud<PointT>::Ptr & cloud,
                                            pcl::PointCloud<pcl::Normal>::Ptr & normals,
                                            const std::vector<bool> &obj_mask,
                                            std::vector<bool> &obj_mask_out,
                                            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &supervoxel_cloud,
                                            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &supervoxel_cloud_organized)
{
    assert( cloud->points.size() == normals->points.size() &&
            cloud->points.size() == obj_mask.size());

    pcl::SupervoxelClustering<PointT> super (param_.voxel_resolution_, param_.seed_resolution_, false);
    super.setInputCloud (cloud);
    super.setColorImportance (0.f);
    super.setSpatialImportance (0.5f);
    super.setNormalImportance (2.f);
    super.setNormalCloud(normals);
    std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
    super.extract (supervoxel_clusters);
    super.refineSupervoxels(2, supervoxel_clusters);
    supervoxel_cloud = super.getColoredVoxelCloud();
    supervoxel_cloud_organized = super.getColoredCloud();
    const pcl::PointCloud<pcl::PointXYZL>::Ptr supervoxels_labels_cloud = super.getLabeledCloud();

    std::cout << "Found " << supervoxel_clusters.size () << " supervoxels." << std::endl;

    const size_t max_label = super.getMaxLabel();
//    const pcl::PointCloud<pcl::PointNormal>::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);

//    //count for all labels how many pixels are in the initial indices
    std::vector<size_t> label_count (max_label+1, 0);

    for(size_t i = 0; i < supervoxels_labels_cloud->points.size(); i++)
    {
        const size_t label = static_cast<size_t>(supervoxels_labels_cloud->points[i].label);
        label_count[ label ]++;
    }

    obj_mask_out.resize(cloud->points.size());

    for(size_t i = 0; i < cloud->points.size(); i++)
    {
        const size_t label = supervoxels_labels_cloud->points[i].label;
        assert(label<label_count.size());

        if ( !obj_mask[i] || !pcl::isFinite(cloud->points[i]))
        {
            obj_mask_out[i] = false;
            continue;
        }

        if( obj_mask[i] && label==0)    // label 0 means the point could not be associated to any supervoxel (see supervoxelclustering doc) - we will pass this point therefore by definition
        {
            obj_mask_out[i] = true;
            continue;
        }

        std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr >::const_iterator it = supervoxel_clusters.find(label);
        if (it != supervoxel_clusters.end())
        {
//            // refine normals
            const Eigen::Vector3f sv_normal = it->second->normal_.getNormalVector3fMap();
            normals->points[i].getNormalVector3fMap() = sv_normal;

            const size_t tot_pts_in_supervoxel = it->second->voxels_->points.size();
            if( label_count[label]  > param_.ratio_supervoxel_ * tot_pts_in_supervoxel)
                obj_mask_out[i] = true;
            else
                obj_mask_out[i] = false;
        }
        else
        {
            std::cerr << "Cluster for label does not exist" << std::endl;
            obj_mask_out[i] = true;
        }
    }
}

void
IOL::nnSearch(const pcl::PointCloud<PointT> &object_points, const pcl::PointCloud<PointT>::ConstPtr &search_cloud,  std::vector<bool> &obj_mask)
{
    pcl::octree::OctreePointCloudSearch<PointT> octree(0.005f);
    octree.setInputCloud ( search_cloud );
    octree.addPointsFromInputCloud ();
    nnSearch(object_points, octree, obj_mask);
}

void
IOL::nnSearch(const pcl::PointCloud<PointT> &object_points, pcl::octree::OctreePointCloudSearch<PointT> &octree,  std::vector<bool> &obj_mask)
{
    //find neighbours from transferred object points
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    for(size_t i=0; i < object_points.points.size(); i++)
    {
        if ( ! pcl::isFinite(object_points.points[i]) )
        {
            PCL_WARN ("Warning: Point is NaN.\n");    // not sure if this causes somewhere else a problem. This condition should not be fulfilled.
            continue;
        }
        if ( octree.radiusSearch (object_points.points[i], param_.radius_, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
        {
            for( size_t nn_id = 0; nn_id < pointIdxRadiusSearch.size(); nn_id++)
                obj_mask[ pointIdxRadiusSearch[ nn_id ] ] = true;
        }
    }
}

std::vector<bool>
IOL::erodeIndices(const std::vector< bool > &obj_mask, const pcl::PointCloud<PointT> & cloud)
{
    assert (obj_mask.size() == cloud.height * cloud.width);

    cv::Mat mask = cv::Mat(cloud.height, cloud.width, CV_8UC1);
    std::vector<bool> mask_out(obj_mask.size());

    for(size_t i=0; i < obj_mask.size(); i++)
    {
        int r,c;
        r = i / mask.cols;
        c = i % mask.cols;

        if (obj_mask[i])
            mask.at<unsigned char>(r,c) = 255;
        else
            mask.at<unsigned char>(r,c) = 0;
    }

    cv::Mat const structure_elem = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
    cv::Mat close_result;
    cv::morphologyEx(mask, close_result, cv::MORPH_CLOSE, structure_elem);

    cv::Mat mask_dst;
    cv::erode(close_result, mask_dst, cv::Mat(), cv::Point(-1,-1), 3);

    //        cv::imshow("mask", mask);
    //        cv::imshow("close_result", close_result);
    //        cv::imshow("mask_dst", mask_dst);
    //        cv::waitKey(0);

    for(int r=0; r < mask_dst.rows; r++)
    {
        for(int c=0; c< mask_dst.cols; c++)
        {
            const int idx = r * mask_dst.cols + c;

            if (mask_dst.at<unsigned char>(r,c) > 0 && pcl::isFinite( cloud.points[idx] ) && cloud.points[idx].z < param_.chop_z_)
                mask_out[idx] = true;
            else
                mask_out[idx] = false;
        }
    }
    return mask_out;
}

bool
IOL::write_model_to_disk (const std::string &models_dir, const std::string &model_name, bool save_views)
{
    const std::string export_to = models_dir + "/" + model_name;
    io::createDirIfNotExist(export_to);
    pcl::io::savePCDFileBinary(export_to + "/3D_model.pcd", *cloud_normals_oriented_);

    if (save_views) //save recognition data with new poses
    {
        io::createDirIfNotExist(export_to + "/views");

        for(size_t i=0; i < keyframes_used_.size(); i++)
        {
            std::stringstream view_file;
            view_file << export_to << "/views/cloud_" << setfill('0') << setw(8) << i << ".pcd";
            pcl::io::savePCDFileBinary (view_file.str (), *keyframes_used_[i]);
            std::cout << view_file.str() << std::endl;

            std::string path_pose (view_file.str());
            boost::replace_last (path_pose, "cloud_", "pose_");
            boost::replace_last (path_pose, ".pcd", ".txt");
            io::writeMatrixToFile(path_pose, cameras_used_[i]);
            std::cout << path_pose << std::endl;

            std::string path_obj_indices (view_file.str());
            boost::replace_last (path_obj_indices, "cloud_", "object_indices_");
            boost::replace_last (path_obj_indices, ".pcd", ".txt");

            std::ofstream mask_f (path_obj_indices);
            for(const auto &idx : object_indices_clouds_used_[i])
                mask_f << idx << std::endl;
            mask_f.close();
        }
    }
    return true;
}

bool
IOL::save_model (const std::string &models_dir, const std::string &model_name, bool save_individual_views)
{
    size_t num_frames = grph_.size();

    std::vector< pcl::PointCloud<pcl::Normal>::Ptr > normals_used (num_frames);
    keyframes_used_.resize(num_frames);
    cameras_used_.resize(num_frames);
    object_indices_clouds_used_.resize(num_frames);

    // only used keyframes with have object points in them
    size_t kept_keyframes=0;
    for (size_t view_id = 0; view_id < grph_.size(); view_id++)
    {
        if ( createIndicesFromMask<size_t>(grph_[view_id].obj_mask_step_.back()).size() )
        {
            keyframes_used_[ kept_keyframes ] = grph_[view_id].cloud_;
            normals_used [ kept_keyframes ] = grph_[view_id].normal_;
            cameras_used_ [ kept_keyframes ] = grph_[view_id].camera_pose_;
            object_indices_clouds_used_[ kept_keyframes ] = createIndicesFromMask<size_t>( grph_[view_id].obj_mask_step_.back() );
            kept_keyframes++;
        }
    }

    keyframes_used_.resize(kept_keyframes);
    normals_used.resize(kept_keyframes);
    cameras_used_.resize(kept_keyframes);
    object_indices_clouds_used_.resize(kept_keyframes);
    std::vector<std::vector<std::vector<float> > > pt_properties (kept_keyframes);

    if ( kept_keyframes > 0)
    {
        //compute noise weights
        for(size_t i=0; i < kept_keyframes; i++)
        {
            NguyenNoiseModel<PointT>::Parameter nm_param;
            nm_param.use_depth_edges_ = true;
            NguyenNoiseModel<PointT> nm (nm_param);
            nm.setInputCloud(keyframes_used_[i]);
            nm.setInputNormals(normals_used[i]);
            nm.compute();
            pt_properties[i] = nm.getPointProperties();
        }

        pcl::PointCloud<PointT>::Ptr octree_cloud(new pcl::PointCloud<PointT>);
        NMBasedCloudIntegration<PointT> nmIntegration (nm_int_param_);
        nmIntegration.setInputClouds(keyframes_used_);
        nmIntegration.setTransformations(cameras_used_);
        nmIntegration.setInputNormals(normals_used);
        nmIntegration.setIndices( object_indices_clouds_used_ );
        nmIntegration.setPointProperties( pt_properties );
        nmIntegration.compute(octree_cloud);

        pcl::PointCloud<pcl::Normal>::Ptr octree_normals;
        nmIntegration.getOutputNormals(octree_normals);

        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filtered_with_normals_oriented (new pcl::PointCloud<pcl::PointXYZRGBNormal>());
        pcl::concatenateFields(*octree_normals, *octree_cloud, *filtered_with_normals_oriented);

        pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBNormal> sor;
        sor.setInputCloud (filtered_with_normals_oriented);
        sor.setMeanK (50);
        sor.setStddevMulThresh (3.0);
        sor.filter (*cloud_normals_oriented_);

        std::cout << "Saving " << kept_keyframes << " keyframes from " << num_frames << " to " << models_dir << std::endl;
        write_model_to_disk(models_dir, model_name, save_individual_views);
    }
    return true;
}

void
IOL::extractPlanePoints(const pcl::PointCloud<PointT>::ConstPtr &cloud,
                             const pcl::PointCloud<pcl::Normal>::ConstPtr &normals,
                             std::vector<ClusterNormalsToPlanes::Plane::Ptr> &planes)
{
    ClusterNormalsToPlanes pest(p_param_);
    DataMatrix2D<Eigen::Vector3f>::Ptr kp_cloud( new DataMatrix2D<Eigen::Vector3f>() );
    DataMatrix2D<Eigen::Vector3f>::Ptr kp_normals( new DataMatrix2D<Eigen::Vector3f>() );
    convertCloud(*cloud, *kp_cloud);
    convertNormals(*normals, *kp_normals);

    std::vector<ClusterNormalsToPlanes::Plane::Ptr> all_planes;
    pest.compute(*kp_cloud, *kp_normals, all_planes);
    planes.resize(all_planes.size());
    size_t kept=0;
    for (size_t cluster_id=0; cluster_id<all_planes.size(); cluster_id++)
    {
        float min_z = std::numeric_limits<float>::max();
        for(size_t cluster_pt_id=0; cluster_pt_id<all_planes[cluster_id]->indices.size(); cluster_pt_id++)
        {
            const int id = all_planes[cluster_id]->indices[cluster_pt_id];
            if ( cloud->points[id].z < min_z ) // do not consider points that are further away than a certain threshold
                min_z = cloud->points[id].z;
        }
        if(min_z < param_.chop_z_)
        {
            planes[kept] = all_planes[cluster_id];
            kept++;
        }
    }
    planes.resize(kept);
}

bool
IOL::merging_planes_reasonable(const modelView::SuperPlane &sp1, const modelView::SuperPlane &sp2) const
{
    float dist = std::abs(PlaneEstimationRANSAC::normalPointDist(sp1.pt, sp1.normal, sp2.pt));
    float dot  = sp1.normal.dot(sp2.normal);
//    std::cout << "dist: " << dist << ", dot: " << dot << std::endl;
    return (dist < 2 * p_param_.inlDist && dot > 0.95);
}

void
IOL::computePlaneProperties(const std::vector<ClusterNormalsToPlanes::Plane::Ptr> &planes,
                                       const std::vector< bool > &object_mask,
                                       const std::vector< bool > &occlusion_mask,
                                       const pcl::PointCloud<PointT>::ConstPtr &cloud,
                                       std::vector<modelView::SuperPlane> &super_planes) const
{
//    planes_not_on_object.resize(planes.size());
    super_planes.resize(planes.size());

//    size_t kept=0;
    for(size_t cluster_id=0; cluster_id<planes.size(); cluster_id++)
    {
        super_planes[cluster_id].pt = planes[cluster_id]->pt;
        super_planes[cluster_id].normal = planes[cluster_id]->normal;
        super_planes[cluster_id].is_plane = planes[cluster_id]->is_plane;
        super_planes[cluster_id].indices = planes[cluster_id]->indices;
        super_planes[cluster_id].visible_indices.resize( planes[cluster_id]->indices.size() );
        super_planes[cluster_id].object_indices.resize( planes[cluster_id]->indices.size() );
        super_planes[cluster_id].within_chop_z_indices.resize( planes[cluster_id]->indices.size() );

        size_t num_obj_pts = 0;
        size_t num_occluded_pts = 0;
        size_t num_plane_pts = 0;

        for (size_t cluster_pt_id=0; cluster_pt_id<planes[cluster_id]->indices.size(); cluster_pt_id++)
        {
            const int id = planes[cluster_id]->indices[cluster_pt_id];
            if ( cloud->points[id].z < param_.chop_z_ )
            {
                super_planes[cluster_id].within_chop_z_indices[ num_plane_pts++ ] = id;

                if ( object_mask[id] )
                     super_planes[cluster_id].object_indices[ num_obj_pts++ ] = id;

                if( !occlusion_mask[id] )
                    super_planes[cluster_id].visible_indices[ num_occluded_pts++ ] = id;

            }
        }
        super_planes[cluster_id].visible_indices.resize( num_occluded_pts );
        super_planes[cluster_id].object_indices.resize( num_obj_pts );
        super_planes[cluster_id].within_chop_z_indices.resize( num_plane_pts );

//        if ( num_plane_pts == 0 ||
//             ( (double)num_obj_pts/num_plane_pts < param_.ratio_cluster_obj_supported_ && (double)num_occluded_pts/num_plane_pts < param_.ratio_cluster_occluded_) )
//        {
//            planes_not_on_object[kept] = planes[cluster_id];
//            kept++;
//        }
    }
//    planes_not_on_object.resize(kept);
}

void
IOL::computeAbsolutePosesRecursive (const Graph & grph,
                              const Vertex start,
                              const Eigen::Matrix4f &accum,
                              std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & absolute_poses,
                              std::vector<bool> &hop_list)
{
    boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
    boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
    for (boost::tie (ei, ei_end) = boost::out_edges (start, grph); ei != ei_end; ++ei)
    {
        Vertex targ = boost::target (*ei, grph);
        size_t target_id = boost::target (*ei, grph);

        if(hop_list[target_id])
           continue;

        hop_list[target_id] = true;
        CamConnect my_e = weightmap[*ei];
        Eigen::Matrix4f intern_accum;
        Eigen::Matrix4f trans = my_e.transformation_;
        if( my_e.target_id_ != target_id)
        {
            Eigen::Matrix4f trans_inv;
            trans_inv = trans.inverse();
            trans = trans_inv;
        }
        intern_accum = accum * trans;
        absolute_poses[target_id] = intern_accum;
        computeAbsolutePosesRecursive (grph, targ, intern_accum, absolute_poses, hop_list);
    }
}

void
IOL::computeAbsolutePoses (const Graph & grph,
                     std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & absolute_poses)
{
  size_t num_frames = boost::num_vertices(grph);
  absolute_poses.resize( num_frames );
  std::vector<bool> hop_list (num_frames, false);
  Vertex source_view = 0;
  hop_list[0] = true;
  Eigen::Matrix4f accum = grph_[0].tracking_pose_;      // CAN IT ALSO BE grph[0] instead of class member?
  absolute_poses[0] = accum;
  computeAbsolutePosesRecursive (grph, source_view, accum, absolute_poses, hop_list);
}

bool
IOL::learn_object (const pcl::PointCloud<PointT> &cloud, const Eigen::Matrix4f &camera_pose, const std::vector<size_t> &initial_indices)
{
    size_t id = grph_.size();
    std::cout << "Computing indices for cloud " << id << std::endl
              << "===================================" << std::endl;
    grph_.resize(id + 1);
    modelView& view = grph_.back();
    pcl::copyPointCloud(cloud, *(view.cloud_));
    view.id_ = id;
    view.tracking_pose_ = camera_pose; //common::RotTrans2Mat4f(cloud.sensor_orientation_, cloud.sensor_origin_);
    view.tracking_pose_set_ = true;
    view.camera_pose_ = view.tracking_pose_;

    boost::add_vertex(view.id_, gs_);

    pcl::PointCloud<pcl::Normal>::Ptr normals_filtered (new pcl::PointCloud<pcl::Normal>());
    std::vector<ClusterNormalsToPlanes::Plane::Ptr> planes;

    computeNormals<PointT>(view.cloud_, view.normal_, param_.normal_method_);
    extractPlanePoints(view.cloud_, view.normal_, planes);

    octree_.setInputCloud ( view.cloud_ );
    octree_.addPointsFromInputCloud ();

    boost::shared_ptr<flann::Index<DistT> > flann_index;

    if ( param_.do_sift_based_camera_pose_estimation_ )
    {
        pcl::PointCloud<PointT> sift_keypoints;
        std::vector<float> sift_keypoint_scales;
        try
        {
            calcSiftFeatures( *view.cloud_, sift_keypoints, view.sift_keypoint_indices_, view.sift_signatures_, sift_keypoint_scales);
            convertToFLANN<DistT>(view.sift_signatures_, flann_index );
        }
        catch (int e)
        {
            param_.do_sift_based_camera_pose_estimation_ = false;
            std::cerr << "Something is wrong with the SIFT based camera pose estimation. Turning it off and using the given camera poses only." << std::endl;
        }
    }

    if (initial_indices.size())   // for first frame use given initial indices and erode them
    {
        std::vector<bool> initial_mask = createMaskFromIndices(initial_indices, view.cloud_->points.size());
        remove_nan_points(*view.cloud_, initial_mask);
        view.obj_mask_step_.push_back( initial_mask );
        view.is_pre_labelled_ = true;

        // remove nan values and points further away than chop_z_ parameter
        std::vector<size_t> initial_indices_wo_nan (initial_indices.size());
        size_t kept=0;
        for(size_t idx=0; idx<initial_indices.size(); idx++)
        {
            if ( pcl::isFinite( view.cloud_->points[initial_indices[idx]]) && view.cloud_->points[initial_indices[idx]].z < param_.chop_z_)
            {
                initial_indices_wo_nan[kept] = initial_indices[idx];
                kept++;
            }
        }
        initial_indices_wo_nan.resize(kept);

        //erode mask
        pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>());
        boost::shared_ptr <std::vector<int> > ObjectIndicesPtr (new std::vector<int>());
        boost::shared_ptr <const std::vector<int> > FilteredObjectIndicesPtr (new std::vector<int>());

        *ObjectIndicesPtr = convertVecSizet2VecInt(initial_indices_wo_nan);

        pcl::copyPointCloud(*view.cloud_, initial_indices_wo_nan, *cloud_filtered);
        pcl::StatisticalOutlierRemoval<PointT> sor(true);
        sor.setInputCloud (view.cloud_);
        sor.setIndices(ObjectIndicesPtr);
        sor.setMeanK (sor_params_.meanK_);
        sor.setStddevMulThresh (sor_params_.std_mul_);
        sor.filter (*cloud_filtered);
        FilteredObjectIndicesPtr = sor.getRemovedIndices();

        const std::vector<bool> obj_mask_initial = createMaskFromIndices(initial_indices_wo_nan, view.cloud_->points.size());
        const std::vector<bool> outlier_mask = createMaskFromIndices(*FilteredObjectIndicesPtr, view.cloud_->points.size());
        const std::vector<bool> obj_mask_wo_outlier = binary_operation(obj_mask_initial, outlier_mask, BINARY_OPERATOR::AND_N);

        view.obj_mask_step_.push_back( obj_mask_wo_outlier);

        std::vector<bool> obj_mask_eroded = erodeIndices(obj_mask_wo_outlier, *view.cloud_);
        view.obj_mask_step_.push_back( obj_mask_eroded );
        computePlaneProperties(planes, view.obj_mask_step_[0],
                               std::vector<bool>(view.cloud_->points.size(), false),
                               view.cloud_, view.planes_);
    }
    else
    {
        if ( param_.do_sift_based_camera_pose_estimation_ )
        {
            for (size_t view_id = 0; view_id < grph_.size(); view_id++)
            {
                if( view.id_ == grph_[view_id].id_)
                    continue;

                std::vector<CamConnect> transforms;
                CamConnect edge;
                edge.model_name_ = "camera_tracking";
                edge.source_id_ = view.id_;
                edge.target_id_ = grph_[view_id].id_;
                edge.transformation_ = view.tracking_pose_.inverse() * grph_[view_id].tracking_pose_ ;
                transforms.push_back( edge );

                try
                {
                    edge.model_name_ = "sift_background_matching";
                    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > sift_transforms;
                    estimateViewTransformationBySIFT( *grph_[view_id].cloud_, *view.cloud_,
                                                      grph_[view_id].sift_keypoint_indices_, view.sift_keypoint_indices_,
                                                      grph_[view_id].sift_signatures_, flann_index, sift_transforms);
                    for(size_t sift_tf_id = 0; sift_tf_id < sift_transforms.size(); sift_tf_id++)
                    {
                        edge.transformation_ = sift_transforms[sift_tf_id];
                        transforms.push_back(edge);
                    }
                }
                catch (int e)
                {
                    param_.do_sift_based_camera_pose_estimation_ = false;
                    std::cerr << "Something is wrong with the SIFT based camera pose estimation. Turning it off and using the given camera poses only." << std::endl;
                }

                size_t best_transform_id = 0;
                float lowest_edge_weight = std::numeric_limits<float>::max();
                for ( size_t trans_id = 0; trans_id < transforms.size(); trans_id++ )
                {
                    try
                    {
                        Eigen::Matrix4f icp_refined_trans;
                        v4r::calcEdgeWeightAndRefineTf<PointT>( grph_[view_id].cloud_, view.cloud_, transforms[ trans_id ].transformation_, transforms[ trans_id ].edge_weight, icp_refined_trans);
                        transforms[ trans_id ].transformation_ = icp_refined_trans,
                        std::cout << "Edge weight is " << transforms[ trans_id ].edge_weight << " for edge connecting vertex " <<
                                     transforms[ trans_id ].source_id_ << " and " << transforms[ trans_id ].target_id_ << " by " <<
                                     transforms[ trans_id ].model_name_ << std::endl;

                        if(transforms[ trans_id ].edge_weight < lowest_edge_weight)
                        {
                            lowest_edge_weight = transforms[ trans_id ].edge_weight;
                            best_transform_id = trans_id;
                        }
                    }
                    catch (int e)
                    {
                        transforms[ trans_id ].edge_weight = std::numeric_limits<float>::max();
                        param_.do_sift_based_camera_pose_estimation_ = false;
                        std::cerr << "Something is wrong with the SIFT based camera pose estimation. Turning it off and using the given camera poses only." << std::endl;
                        break;
                    }
                }
                boost::add_edge (transforms[best_transform_id].source_id_, transforms[best_transform_id].target_id_, transforms[best_transform_id], gs_);

                boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
                std::vector < Edge > spanning_tree;
                boost::kruskal_minimum_spanning_tree(gs_, std::back_inserter(spanning_tree));

                Graph grph_mst;
                std::cout << "Print the edges in the MST:" << std::endl;
                for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei)
                {
                    CamConnect my_e = weightmap[*ei];
                    std::cout << "[" << source(*ei, gs_) << "->" << target(*ei, gs_) << "] with weight " << my_e.edge_weight << " by " << my_e.model_name_ << std::endl;
                    boost::add_edge(source(*ei, gs_), target(*ei, gs_), weightmap[*ei], grph_mst);
                }

                std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > absolute_poses;
                computeAbsolutePoses(grph_mst, absolute_poses);

                for(size_t v_id=0; v_id<absolute_poses.size(); v_id++)
                {
                    grph_[ v_id ].camera_pose_ = absolute_poses [ v_id ];
                }
            }
        }

        std::vector<bool> is_occluded;
        for (size_t view_id = 0; view_id < grph_.size(); view_id++)
        {
            if( view.id_ != grph_[view_id].id_)
            {
                pcl::PointCloud<PointT>::Ptr new_search_pts, new_search_pts_aligned;
                new_search_pts.reset(new pcl::PointCloud<PointT>());
                new_search_pts_aligned.reset(new pcl::PointCloud<PointT>());
                pcl::copyPointCloud(*grph_[view_id].cloud_, grph_[view_id].obj_mask_step_.back(), *new_search_pts);
                const Eigen::Matrix4f tf = view.camera_pose_.inverse() * grph_[view_id].camera_pose_;
                pcl::transformPointCloud(*new_search_pts, *new_search_pts_aligned, tf);

                pcl::IterativeClosestPoint<PointT, PointT> icp;
                icp.setInputSource(new_search_pts_aligned);
                icp.setInputTarget(view.cloud_);
                icp.setMaxCorrespondenceDistance (0.02f);
                pcl::PointCloud<PointT>::Ptr icp_aligned_cloud (new pcl::PointCloud<PointT>());
                icp.align(*icp_aligned_cloud, Eigen::Matrix4f::Identity());
                *view.transferred_cluster_ += *icp_aligned_cloud;

                if (grph_[view_id].is_pre_labelled_)
                {
                    std::vector<bool> is_occluded_tmp = computeOccludedPoints(*grph_[view_id].cloud_,
                                                                                *view.cloud_,
                                                                                tf.inverse(),
                                                                                525.f, 0.01f, false);
                    if( is_occluded.size() == is_occluded_tmp.size())
                    {
                        is_occluded = binary_operation(is_occluded, is_occluded_tmp, BINARY_OPERATOR::AND); // is this correct?
                    }
                    else
                    {
                        is_occluded = is_occluded_tmp;
                    }
                }
            }
        }

        std::vector<bool> obj_mask_nn_search (view.cloud_->points.size(), false);
        nnSearch(*view.transferred_cluster_, octree_, obj_mask_nn_search);
        view.obj_mask_step_.push_back( obj_mask_nn_search);

        computePlaneProperties(planes, obj_mask_nn_search, is_occluded,
                               view.cloud_, view.planes_);
    }
    std::vector<bool> pixel_is_object = view.obj_mask_step_.back();

    // filter cloud based on planes not on object and not occluded in first frame
    std::vector<bool> pixel_is_neglected (view.cloud_->points.size(), false);
    for (size_t p_id=0; p_id<view.planes_.size(); p_id++)
    {
        for (size_t view_id = 0; view_id < grph_.size(); view_id++)
        {
            if( view.id_ != grph_[view_id].id_)
            {
                for (size_t p2_id=0; p2_id<grph_[view_id].planes_.size(); p2_id++)
                {
//                    std::cout << "Checking cluster new " << p_id << " and cluster old " <<
//                                 p2_id << " from view " << view_id << std::endl;
//                    std::cout << merging_planes_reasonable(view.planes_[p_id], grph_[view_id].planes_[p2_id]) << std::endl;

                    // if the planes can be merged (based on normals and distance), then filter new plane if old one has been filtered
                    if (grph_[view_id].planes_[p2_id].is_filtered && merging_planes_reasonable(view.planes_[p_id], grph_[view_id].planes_[p2_id]) && !plane_has_object(view.planes_[p_id]))
                        view.planes_[p_id].is_filtered = true;
                }
            }
        }
        if ( plane_is_filtered( view.planes_[p_id] ) )
            view.planes_[p_id].is_filtered = true;

        if ( view.planes_[p_id].is_filtered )
        {
            for (size_t c_pt_id=0; c_pt_id < view.planes_[p_id].indices.size(); c_pt_id++)
                pixel_is_neglected [ view.planes_[p_id].indices[ c_pt_id ] ] = true;
        }
    }
    for(size_t pt=0; pt<view.cloud_->points.size(); pt++)
    {
        if (view.cloud_->points[pt].z > param_.chop_z_)
            pixel_is_neglected[pt] = true;
    }

    view.scene_points_ = createIndicesFromMask<size_t>(pixel_is_neglected, true);
    view.obj_mask_step_.push_back( binary_operation(pixel_is_object, pixel_is_neglected, BINARY_OPERATOR::AND_N) );
    pcl::copyPointCloud(*view.normal_, view.scene_points_, *normals_filtered);

    std::vector<bool> obj_mask_enforced_by_supervoxel_consistency;
    updatePointNormalsFromSuperVoxels(view.cloud_,
                                      view.normal_,
                                      view.obj_mask_step_.back(),
                                      obj_mask_enforced_by_supervoxel_consistency,
                                      view.supervoxel_cloud_,
                                      view.supervoxel_cloud_organized_);
    view.obj_mask_step_.push_back( obj_mask_enforced_by_supervoxel_consistency );

    std::vector<bool> obj_mask_grown_by_smooth_surface = extractEuclideanClustersSmooth(view.cloud_,
                                                                                *view.normal_,
                                                                                obj_mask_enforced_by_supervoxel_consistency,
                                                                                pixel_is_neglected);
    view.obj_mask_step_.push_back(obj_mask_grown_by_smooth_surface);

    std::vector<bool> obj_mask_eroded = erodeIndices(obj_mask_grown_by_smooth_surface, *view.cloud_);
    remove_nan_points(*view.cloud_, obj_mask_eroded);
    view.obj_mask_step_.push_back( obj_mask_eroded );

    for( size_t step_id = 0; step_id<view.obj_mask_step_.size(); step_id++)
    {
        std::cout << "step " << step_id << ": " << createIndicesFromMask<size_t>(view.obj_mask_step_[step_id]).size() << " points." << std::endl;
    }

    if( view.is_pre_labelled_ && createIndicesFromMask<size_t>(view.obj_mask_step_.back()).size() < param_.min_points_for_transferring_)
    {
        view.obj_mask_step_.back() = view.obj_mask_step_[0];
        std::cout << "After postprocessing the initial frame not enough points are left. Therefore taking the original provided indices." << std::endl;
    }
//    visualize();
    return true;
}

void
IOL::initSIFT ()
{
    if (param_.do_sift_based_camera_pose_estimation_)
    {
#ifdef HAVE_SIFTGPU
        //-----Init-SIFT-GPU-Context--------
        static char kw[][16] = {"-m", "-fo", "-1", "-s", "-v", "1", "-pack"};
        char * argvv[] = {kw[0], kw[1], kw[2], kw[3],kw[4],kw[5],kw[6], NULL};

        int argcc = sizeof(argvv) / sizeof(char*);
        sift_.reset( new SiftGPU () );
        sift_->ParseParam (argcc, argvv);

        //create an OpenGL context for computation
        if (sift_->CreateContextGL () != SiftGPU::SIFTGPU_FULL_SUPPORTED)
            throw std::runtime_error ("PSiftGPU::PSiftGPU: No GL support!");
#endif
    }
}

void
IOL::printParams(std::ostream &ostr) const
{
    ostr << "Started incremental object learning with parameters: " << std::endl
         << "===================================================" << std::endl
         << "radius: " << param_.radius_ << std::endl
         << "eps_angle: " << param_.eps_angle_ << std::endl
         << "dist_threshold_growing_: " << param_.dist_threshold_growing_ << std::endl
         << "voxel resolution: " << param_.voxel_resolution_ << std::endl
         << "seed resolution: " << param_.seed_resolution_ << std::endl
         << "ratio_supervoxel: " << param_.ratio_supervoxel_ << std::endl
         << "max z distance: " << param_.chop_z_ << std::endl
         << "do_erosion: " << param_.do_erosion_ << std::endl
         << "do_sift_based_camera_pose_estimation_: " << param_.do_sift_based_camera_pose_estimation_ << std::endl
         << "transferring object indices from latest frame only: " << param_.transfer_indices_from_latest_frame_only_ << std::endl
         << "min_points_for_transferring_: " << param_.min_points_for_transferring_ << std::endl
         << "normal_method_: " << param_.normal_method_ << std::endl
         << "apply minimimum spanning tree: " << param_.do_mst_refinement_ << std::endl
         << "ratio_cluster_obj_supported_: " << param_.ratio_cluster_obj_supported_ << std::endl
         << "ratio_cluster_occluded_: " << param_.ratio_cluster_occluded_ << std::endl
         << "smooth_clustering_param_inlDist: " << p_param_.inlDist << std::endl
         << "smooth_clustering_param_inlDistSmooth: " << p_param_.inlDistSmooth << std::endl
         << "smooth_clustering_param_least_squares_refinement: " << p_param_.least_squares_refinement << std::endl
         << "smooth_clustering_param_minPoints: " << p_param_.minPoints << std::endl
         << "smooth_clustering_param_minPointsSmooth: " << p_param_.minPointsSmooth << std::endl
         << "smooth_clustering_param_smooth_clustering: " << p_param_.smooth_clustering << std::endl
         << "smooth_clustering_param_thrAngle: " << p_param_.thrAngle << std::endl
         << "smooth_clustering_param_thrAngleSmooth: " << p_param_.thrAngleSmooth << std::endl
         << "statistical_outlier_removal_meanK_: " << sor_params_.meanK_ << std::endl
         << "statistical_outlier_removal_std_mul_: " << sor_params_.std_mul_ << std::endl
         << "===================================================" << std::endl << std::endl;
}
}
}


#include "dynet/nodes.h"
#include "dynet/exec.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/fast-lstm.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <memory>
#include <type_traits>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options.hpp>

#include "s2s/dynet/dict.h"
#include "s2s/nn/attn.hpp"
#include "s2s/nn/base.hpp"
#include "s2s/nn/slahan.hpp"
#include "s2s/nn/lstm.hpp"
#include "s2s/nn/tagger.hpp"
#include "s2s/corpus/comp.hpp"
#include "s2s/corpus/dicts.hpp"
#include "s2s/decode/hypothesis.hpp"

#ifndef INCLUDE_GUARD_DECODER_GREEDY_HPP
#define INCLUDE_GUARD_DECODER_GREEDY_HPP

namespace s2s {

    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<encoder_decoder_attn>>& hyp_stack, std::vector<std::unique_ptr<encoder_decoder_attn> > &encdec, dicts &d, const s2s_options &opts){
        dynet::ComputationGraph cg;
        hyp_stack.resize(1);
        hyp_stack[0].init(batch_local, d, encdec, cg);
        for (unsigned int t = 1; t < encdec[0]->slen; ++t) {
            std::vector<std::vector<float> > output(batch_local.batch_size(), std::vector<float>(d.dict_set_trg.d_word.size(), 0.f));
            // for each model
            for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
                dynet::Expression i_att_t = encdec[model_id]->decoder_attention(cg, t, hyp_stack[0].prev());
                dynet::Expression i_out_t = encdec[model_id]->decoder_output(cg, t, i_att_t);
                dynet::Expression pred_att = softmax(i_att_t);
                std::vector<dynet::Tensor> results_att = cg.incremental_forward(pred_att).batch_elems();
                dynet::Expression pred_out = softmax(i_out_t);
                std::vector<dynet::Tensor> results_out = cg.incremental_forward(pred_out).batch_elems();
                // for each sentence
                for(unsigned int sent_id = 0; sent_id < results_out.size(); sent_id++){
                    hyp_stack[0].dist_att[model_id][sent_id].push_back(as_vector(results_att.at(sent_id)));
                    hyp_stack[0].dist_out[model_id][sent_id].push_back(as_vector(results_out.at(sent_id)));
                    for(unsigned int vocab_id = 0; vocab_id < hyp_stack[0].dist_out[model_id][sent_id][t-1].size(); vocab_id++){
                        output[sent_id][vocab_id] += log(hyp_stack[0].dist_out[model_id][sent_id][t-1][vocab_id]);
                    }
                }
            }
            // for each sentence
            for(unsigned int sent_id = 0; sent_id < output.size(); sent_id++){
                for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                    output[sent_id][vocab_id] /= (float)(encdec.size());
                }
            }
            // output vocab
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(t + 1 == batch_local.len_src[sent_id] || hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    w_id = d.dict_set_trg.end_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.end_id_word];
                }else{
                    for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                        if(vocab_id == d.dict_set_trg.end_id_word){
                            continue;
                        }
                        if(output[sent_id][vocab_id] > w_prob){
                            w_id = vocab_id;
                            w_prob = output[sent_id][vocab_id];
                        }
                    }
                }
                hyp_stack[0].prob[sent_id] += w_prob;
                hyp_stack[0].output[sent_id].push_back(w_id);
            }
            // end check
            unsigned int num_end = 0;
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                if(hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    num_end++;
                }
            }
            if(num_end == hyp_stack[0].output.size()){
                break;
            }
        }
    }

    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<encoder_decoder_base>>& hyp_stack, std::vector<std::unique_ptr<encoder_decoder_base> > &encdec, dicts &d, const s2s_options &opts){
        dynet::ComputationGraph cg;
        hyp_stack.resize(1);
        hyp_stack[0].init(batch_local, d, encdec, cg);
        for (unsigned int t = 1; t < encdec[0]->slen; ++t) {
            std::vector<std::vector<float> > output(batch_local.batch_size(), std::vector<float>(d.dict_set_trg.d_word.size(), 0.f));
            // for each model
            for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
                dynet::Expression i_out_t = encdec[model_id]->decoder_output(cg, hyp_stack[0].prev(), t);
                dynet::Expression pred_out = softmax(i_out_t);
                std::vector<dynet::Tensor> results_out = cg.incremental_forward(pred_out).batch_elems();
                // for each sentence
                for(unsigned int sent_id = 0; sent_id < results_out.size(); sent_id++){
                    hyp_stack[0].dist_out[model_id][sent_id].push_back(as_vector(results_out.at(sent_id)));
                    for(unsigned int vocab_id = 0; vocab_id < hyp_stack[0].dist_out[model_id][sent_id][t-1].size(); vocab_id++){
                        output[sent_id][vocab_id] += log(hyp_stack[0].dist_out[model_id][sent_id][t-1][vocab_id]);
                    }
                }
            }
            // for each sentence
            for(unsigned int sent_id = 0; sent_id < output.size(); sent_id++){
                for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                    output[sent_id][vocab_id] /= (float)(encdec.size());
                }
            }
            // output vocab
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(t + 1 == batch_local.len_src[sent_id] || hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    w_id = d.dict_set_trg.end_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.end_id_word];
                }else{
                    for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                        if(vocab_id == d.dict_set_trg.end_id_word){
                            continue;
                        }
                        if(output[sent_id][vocab_id] > w_prob){
                            w_id = vocab_id;
                            w_prob = output[sent_id][vocab_id];
                        }
                    }
                }
                hyp_stack[0].prob[sent_id] += w_prob;
                hyp_stack[0].output[sent_id].push_back(w_id);
            }
            // end check
            unsigned int num_end = 0;
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                if(hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    num_end++;
                }
            }
            if(num_end == hyp_stack[0].output.size()){
                break;
            }
        }
    }

    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<encoder_decoder_slahan>>& hyp_stack, std::vector<std::unique_ptr<encoder_decoder_slahan> > &encdec, dicts &d, const s2s_options &opts){
        dynet::ComputationGraph cg;
        hyp_stack.resize(1);
        hyp_stack[0].init(batch_local, d, encdec, cg);
        std::vector<std::vector<dynet::Expression> > h_att_self_all(encdec.size());
        for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
            h_att_self_all[model_id] = encdec[model_id]->attention(cg, batch_local);
            encdec[model_id]->recursive_attention(cg, h_att_self_all[model_id]);
        }
        for (unsigned int t = 1; t < encdec[0]->slen; ++t) {
            std::vector<std::vector<float> > output(batch_local.batch_size(), std::vector<float>(d.dict_set_trg.d_word.size(), 0.f));
            // for each model
            for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
                dynet::Expression i_out_t = encdec[model_id]->decoder_output(cg, t, hyp_stack[0].prev());
                dynet::Expression pred_out = softmax(i_out_t);
                std::vector<dynet::Tensor> results_out = cg.incremental_forward(pred_out).batch_elems();
                dynet::Expression pred_att = softmax(h_att_self_all[model_id][t]);
                std::vector<dynet::Tensor> results_att = cg.incremental_forward(pred_att).batch_elems();
                // for each sentence
                for(unsigned int sent_id = 0; sent_id < results_out.size(); sent_id++){
                    hyp_stack[0].dist_att[model_id][sent_id].push_back(as_vector(results_att.at(sent_id)));
                    hyp_stack[0].dist_out[model_id][sent_id].push_back(as_vector(results_out.at(sent_id)));
                    for(unsigned int vocab_id = 0; vocab_id < hyp_stack[0].dist_out[model_id][sent_id][t-1].size(); vocab_id++){
                        output[sent_id][vocab_id] += log(hyp_stack[0].dist_out[model_id][sent_id][t-1][vocab_id]);
                    }
                }
            }
            // for each sentence
            for(unsigned int sent_id = 0; sent_id < output.size(); sent_id++){
                for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                    output[sent_id][vocab_id] /= (float)(encdec.size());
                }
            }
            // output vocab
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(t + 1 == batch_local.len_src[sent_id] || hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    w_id = d.dict_set_trg.end_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.end_id_word];
                }else{
                    for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                        if(vocab_id == d.dict_set_trg.end_id_word){
                            continue;
                        }
                        if(output[sent_id][vocab_id] > w_prob){
                            w_id = vocab_id;
                            w_prob = output[sent_id][vocab_id];
                        }
                    }
                }
                hyp_stack[0].prob[sent_id] += w_prob;
                hyp_stack[0].output[sent_id].push_back(w_id);
            }
            // end check
            unsigned int num_end = 0;
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                if(hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    num_end++;
                }
            }
            if(num_end == hyp_stack[0].output.size()){
                break;
            }
        }
    }

    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<encoder_decoder_lstm>>& hyp_stack, std::vector<std::unique_ptr<encoder_decoder_lstm> > &encdec, dicts &d, const s2s_options &opts){
        dynet::ComputationGraph cg;
        hyp_stack.resize(1);
        hyp_stack[0].init(batch_local, d, encdec, cg);
        for (unsigned int t = 1; t < encdec[0]->slen; ++t) {
            std::vector<std::vector<float> > output(batch_local.batch_size(), std::vector<float>(d.dict_set_trg.d_word.size(), 0.f));
            // for each model
            for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
                std::vector<dynet::real> bit_features = encdec[model_id]->bit_features(t, hyp_stack[0].prevs, batch_local.align[t]);
                dynet::Expression i_out_t = encdec[model_id]->decoder_output(cg, t, batch_local.align[t], bit_features);
                dynet::Expression pred_out = softmax(i_out_t);
                std::vector<dynet::Tensor> results_out = cg.incremental_forward(pred_out).batch_elems();
                // for each sentence
                for(unsigned int sent_id = 0; sent_id < results_out.size(); sent_id++){
                    hyp_stack[0].dist_out[model_id][sent_id].push_back(as_vector(results_out.at(sent_id)));
                    for(unsigned int vocab_id = 0; vocab_id < hyp_stack[0].dist_out[model_id][sent_id][t-1].size(); vocab_id++){
                        output[sent_id][vocab_id] += log(hyp_stack[0].dist_out[model_id][sent_id][t-1][vocab_id]);
                    }
                }
            }
            // for each sentence
            for(unsigned int sent_id = 0; sent_id < output.size(); sent_id++){
                for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                    output[sent_id][vocab_id] /= (float)(encdec.size());
                }
            }
            // output vocab
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(t + 1 == batch_local.len_src[sent_id] || hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    w_id = d.dict_set_trg.end_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.end_id_word];
                }else{
                    for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                        if(vocab_id == d.dict_set_trg.end_id_word){
                            continue;
                        }
                        if(output[sent_id][vocab_id] > w_prob){
                            w_id = vocab_id;
                            w_prob = output[sent_id][vocab_id];
                        }
                    }
                }
                hyp_stack[0].prob[sent_id] += w_prob;
                hyp_stack[0].output[sent_id].push_back(w_id);
            }
            hyp_stack[0].update_prevs();
            // end check
            unsigned int num_end = 0;
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                if(hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    num_end++;
                }
            }
            if(num_end == hyp_stack[0].output.size()){
                break;
            }
        }
    }

    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<tagger>>& hyp_stack, std::vector<std::unique_ptr<tagger> > &encdec, dicts &d, const s2s_options &opts){
        dynet::ComputationGraph cg;
        hyp_stack.resize(1);
        hyp_stack[0].init(batch_local, d, encdec, cg);
        for (unsigned int t = 0; t < encdec[0]->slen; ++t) {
            std::vector<std::vector<float> > output(batch_local.batch_size(), std::vector<float>(d.dict_set_trg.d_word.size(), 0.f));
            // for each model
            for(unsigned int model_id = 0; model_id < encdec.size(); model_id++){
                dynet::Expression i_out_t = encdec[model_id]->output(cg, t);
                dynet::Expression pred_out = softmax(i_out_t);
                std::vector<dynet::Tensor> results_out = cg.incremental_forward(pred_out).batch_elems();
                // for each sentence
                for(unsigned int sent_id = 0; sent_id < results_out.size(); sent_id++){
                    hyp_stack[0].dist_out[model_id][sent_id].push_back(as_vector(results_out.at(sent_id)));
                    for(unsigned int vocab_id = 0; vocab_id < hyp_stack[0].dist_out[model_id][sent_id][t].size(); vocab_id++){
                        output[sent_id][vocab_id] += log(hyp_stack[0].dist_out[model_id][sent_id][t][vocab_id]);
                    }
                }
            }
            // for each sentence
            for(unsigned int sent_id = 0; sent_id < output.size(); sent_id++){
                for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                    output[sent_id][vocab_id] /= (float)(encdec.size());
                }
            }
            // output vocab
            for(unsigned int sent_id = 0; sent_id < hyp_stack[0].output.size(); sent_id++){
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(t == 0){
                    w_id = d.dict_set_trg.start_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.start_id_word];
                }else if(t == batch_local.len_src[sent_id] - 1 || hyp_stack[0].output[sent_id].back() == d.dict_set_trg.end_id_word){
                    w_id = d.dict_set_trg.end_id_word;
                    w_prob = output[sent_id][d.dict_set_trg.end_id_word];
                }else{
                    for(unsigned int vocab_id = 0; vocab_id < output[sent_id].size(); vocab_id++){
                        if(vocab_id == d.dict_set_trg.end_id_word){
                            continue;
                        }
                        if(output[sent_id][vocab_id] > w_prob){
                            w_id = vocab_id;
                            w_prob = output[sent_id][vocab_id];
                        }
                    }
                }
                hyp_stack[0].prob[sent_id] += w_prob;
                if(t == 0){
                    hyp_stack[0].output[sent_id][0] = w_id;
                }else{
                    hyp_stack[0].output[sent_id].push_back(w_id);
                }
            }
        }
    }

    template<class T_EncDec>
    void greedy_decode_ensemble(const batch& batch_local, std::vector<hypothesis<T_EncDec>>& hyp_stack, std::unique_ptr<T_EncDec>& encdec, dicts &d, const s2s_options &opts){
        std::vector<std::unique_ptr<T_EncDec> > encdec_vec;
        encdec_vec.push_back(std::move(encdec));
        greedy_decode_ensemble(batch_local, hyp_stack, encdec_vec, d, opts);
        encdec = std::move(encdec_vec[0]);
    }

};

#endif // INCLUDE_GUARD_DECODER_GREEDY_HPP