/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef AMG_TEMPLATES_HPP
#define AMG_TEMPLATES_HPP

#include <mfmg/common/amge.hpp>
#include <mfmg/common/exceptions.hpp>
#include <mfmg/common/utils.hpp>

#include <deal.II/distributed/tria.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/numerics/data_out.h>

#include <algorithm>
#include <fstream>
#include <map>
#include <unordered_map>

#ifdef DEAL_II_TRILINOS_WITH_ZOLTAN
// Zoltan random seed control is in an internal zz_rand.h file which is not
// installed with Trilinos. Thus, we duplicate the signature and the
// initialization value here.
#define ZOLTAN_RAND_INIT 123456789U
extern "C"
{
  extern void Zoltan_Srand(unsigned int, unsigned int *);
}
#endif

namespace mfmg
{
template <int dim, typename VectorType>
AMGe<dim, VectorType>::AMGe(MPI_Comm comm,
                            dealii::DoFHandler<dim> const &dof_handler)
    : _comm(comm), _dof_handler(dof_handler)
{
}

template <int dim, typename VectorType>
unsigned int AMGe<dim, VectorType>::build_agglomerates(
    boost::property_tree::ptree const &ptree) const
{
  std::string partitioner_type = ptree.get<std::string>("partitioner");
  std::transform(partitioner_type.begin(), partitioner_type.end(),
                 partitioner_type.begin(), ::tolower);
  if ((partitioner_type == "zoltan") || (partitioner_type == "metis"))
  {
#ifdef DEAL_II_TRILINOS_WITH_ZOLTAN
    // Always use the same seed for Zoltan
    Zoltan_Srand(ZOLTAN_RAND_INIT, NULL);
#endif
    unsigned int const n_desired_agglomerates =
        ptree.get<unsigned int>("n_agglomerates");

    return build_agglomerates_partitioner(partitioner_type,
                                          n_desired_agglomerates);
  }
  else if (partitioner_type == "block")
  {
    std::array<unsigned int, dim> agglomerate_dim;
    agglomerate_dim[0] = ptree.get<unsigned int>("nx");
    agglomerate_dim[1] = ptree.get<unsigned int>("ny");
    if (dim == 3)
      agglomerate_dim[2] = ptree.get<unsigned int>("nz");

    return build_agglomerates_block(agglomerate_dim);
  }
  else
    ASSERT_THROW(false, partitioner_type +
                            " is not a valid choice for the partitioner. The "
                            "acceptable values are zoltan, metis, and block.");
  return 0;
}

template <int dim, typename VectorType>
std::pair<std::vector<std::vector<unsigned int>>,
          std::vector<std::vector<unsigned int>>>
AMGe<dim, VectorType>::build_boundary_agglomerates() const
{
  auto filtered_iterators_range =
      filter_iterators(_dof_handler.active_cell_iterators(),
                       dealii::IteratorFilters::LocallyOwnedCell());
  std::vector<std::set<unsigned int>> agg_cell_set(_n_agglomerates);
  for (auto cell : filtered_iterators_range)
  {
    // The cell used_index 0 is reserved for artificial cell however to fill in
    // the vector we need to decrease the user_index by one to make sure that we
    // start filling the vector from the beginning.
    unsigned int const agg_index = cell->user_index() - 1;
    unsigned int const active_cell_index = cell->active_cell_index();
    agg_cell_set[agg_index].insert(active_cell_index);
  }

  dealii::DynamicSparsityPattern connectivity;
  dealii::GridTools::get_vertex_connectivity_of_cells(
      _dof_handler.get_triangulation(), connectivity);

  // TODO do this using multithreading. Maybe it's better to do everything in
  // the following for loop
  // Each agglomerate will create two new agglomerates: one composed of the
  // cells of the agglomerate which are on the boundary with another agglomerate
  // and another one composed of cells on other agglomerates that share a
  // boundary with the current agglomerate
  std::vector<std::vector<unsigned int>> interior_agglomerates;
  std::vector<std::vector<unsigned int>> halo_agglomerates;
  for (auto const &agg_cell : agg_cell_set)
  {
    std::vector<unsigned int> interior_boundary_cells;
    std::set<unsigned int> halo_cells_in_agg;
    for (auto const &agg_cell_it : agg_cell)
    {
      bool cell_in_agg = false;
      // Get the connectivity for the current cell
      auto connectivity_begin = connectivity.begin(agg_cell_it);
      auto connectivity_end = connectivity.end(agg_cell_it);
      for (auto connectivity_it = connectivity_begin;
           connectivity_it != connectivity_end; ++connectivity_it)
      {
        // Cells that are on the boundary of agglomerates and have in their
        // connectivity cells that are not part of the agglomerates
        if (agg_cell.count(connectivity_it->column()) == 0)
        {
          if (cell_in_agg == false)
          {
            interior_boundary_cells.push_back(agg_cell_it);
            cell_in_agg = true;
          }
          halo_cells_in_agg.insert(connectivity_it->column());
        }
      }
    }

    interior_agglomerates.emplace_back(interior_boundary_cells);
    halo_agglomerates.emplace_back(halo_cells_in_agg.begin(),
                                   halo_cells_in_agg.end());
  }

  return {interior_agglomerates, halo_agglomerates};
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::build_agglomerate_triangulation(
    unsigned int agglomerate_id,
    dealii::Triangulation<dim> &agglomerate_triangulation,
    std::map<typename dealii::Triangulation<dim>::active_cell_iterator,
             typename dealii::DoFHandler<dim>::active_cell_iterator>
        &agglomerate_to_global_tria_map) const
{
  std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator>
      agglomerate;
  for (auto cell : _dof_handler.active_cell_iterators())
    if (cell->user_index() == agglomerate_id)
      agglomerate.push_back(cell);

  build_agglomerate_triangulation(agglomerate, agglomerate_triangulation,
                                  agglomerate_to_global_tria_map);
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::build_agglomerate_triangulation(
    std::vector<unsigned int> const &cell_index,
    dealii::Triangulation<dim> &agglomerate_triangulation,
    std::map<typename dealii::Triangulation<dim>::active_cell_iterator,
             typename dealii::DoFHandler<dim>::active_cell_iterator>
        &agglomerate_to_global_tria_map) const
{
  std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator>
      agglomerate;
  agglomerate.reserve(cell_index.size());
  if (cell_index.size() > 0)
  {
    auto cell = _dof_handler.begin_active();
    std::advance(cell, cell_index[0]);
    agglomerate.push_back(cell);

    for (unsigned int i = 1; i < cell_index.size(); ++i)
    {
      std::advance(cell, cell_index[i] - cell_index[i - 1]);
      agglomerate.push_back(cell);
    }
  }

  build_agglomerate_triangulation(agglomerate, agglomerate_triangulation,
                                  agglomerate_to_global_tria_map);
}

template <int dim, typename VectorType>
std::vector<dealii::types::global_dof_index>
AMGe<dim, VectorType>::compute_dof_index_map(
    std::map<typename dealii::Triangulation<dim>::active_cell_iterator,
             typename dealii::DoFHandler<dim>::active_cell_iterator> const
        &patch_to_global_map,
    dealii::DoFHandler<dim> const &agglomerate_dof_handler) const
{
  std::vector<dealii::types::global_dof_index> dof_indices(
      agglomerate_dof_handler.n_dofs());

  unsigned int const dofs_per_cell = _dof_handler.get_fe().dofs_per_cell;
  std::vector<dealii::types::global_dof_index> agg_dof_indices(dofs_per_cell);
  std::vector<dealii::types::global_dof_index> global_dof_indices(
      dofs_per_cell);

  for (auto agg_cell : agglomerate_dof_handler.active_cell_iterators())
  {
    agg_cell->get_dof_indices(agg_dof_indices);
    auto global_cell = patch_to_global_map.at(agg_cell);
    global_cell->get_dof_indices(global_dof_indices);
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      dof_indices[agg_dof_indices[i]] = global_dof_indices[i];
  }

  return dof_indices;
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::output(std::string const &filename) const
{
  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler(_dof_handler);

  unsigned int const n_active_cells =
      _dof_handler.get_triangulation().n_active_cells();
  dealii::Vector<float> subdomain(n_active_cells);
  for (unsigned int i = 0; i < n_active_cells; ++i)
    subdomain(i) = _dof_handler.get_triangulation().locally_owned_subdomain();
  data_out.add_data_vector(subdomain, "subdomain");

  dealii::Vector<float> agglomerates(n_active_cells);
  unsigned int n = 0;
  for (auto cell : _dof_handler.active_cell_iterators())
  {
    if (cell->is_locally_owned())
      agglomerates(n) = cell->user_index();
    ++n;
  }
  data_out.add_data_vector(agglomerates, "agglomerates");

  data_out.build_patches();

  std::string full_filename =
      filename +
      std::to_string(
          _dof_handler.get_triangulation().locally_owned_subdomain());
  std::ofstream output((full_filename + ".vtu").c_str());
  data_out.write_vtu(output);

  if (dealii::Utilities::MPI::this_mpi_process(_comm) == 0)
  {
    unsigned int const comm_size =
        dealii::Utilities::MPI::n_mpi_processes(_comm);
    std::vector<std::string> full_filenames(comm_size);
    for (unsigned int i = 0; i < comm_size; ++i)
      full_filenames[0] = filename + std::to_string(i) + ".vtu";
    std::ofstream master_output(filename + ".pvtu");
    data_out.write_pvtu_record(master_output, full_filenames);
  }
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::compute_restriction_sparse_matrix(
    std::vector<dealii::Vector<typename VectorType::value_type>> const
        &eigenvectors,
    std::vector<std::vector<typename VectorType::value_type>> const
        &diag_elements,
    std::vector<std::vector<dealii::types::global_dof_index>> const
        &dof_indices_maps,
    std::vector<unsigned int> const &n_local_eigenvectors,
    dealii::LinearAlgebra::distributed::Vector<
        typename VectorType::value_type> const &locally_relevant_global_diag,
    dealii::TrilinosWrappers::SparseMatrix &restriction_sparse_matrix) const
{
  // Compute the sparsity pattern (Epetra_FECrsGraph)
  dealii::TrilinosWrappers::SparsityPattern restriction_sp =
      compute_restriction_sparsity_pattern(eigenvectors, dof_indices_maps,
                                           n_local_eigenvectors);

  // Build the restriction sparse matrix
  restriction_sparse_matrix.reinit(restriction_sp);
  std::pair<dealii::types::global_dof_index,
            dealii::types::global_dof_index> const local_range =
      restriction_sp.local_range();
  unsigned int const n_agglomerates = n_local_eigenvectors.size();
  unsigned int pos = 0;
  ASSERT(n_agglomerates == dof_indices_maps.size(),
         "dof_indices_maps has the wrong size: " +
             std::to_string(dof_indices_maps.size()) + " instead of " +
             std::to_string(n_agglomerates));
  for (unsigned int i = 0; i < n_agglomerates; ++i)
  {
    unsigned int const n_local_eig = n_local_eigenvectors[i];
    for (unsigned int k = 0; k < n_local_eig; ++k)
    {
      unsigned int const n_elem = eigenvectors[pos].size();
      ASSERT(n_elem == dof_indices_maps[i].size(),
             "dof_indices_maps[i] has the wrong size: " +
                 std::to_string(dof_indices_maps[i].size()) + " instead of " +
                 std::to_string(n_elem));
      for (unsigned int j = 0; j < n_elem; ++j)
      {
        dealii::types::global_dof_index const global_pos =
            dof_indices_maps[i][j];
        restriction_sparse_matrix.add(
            local_range.first + pos, global_pos,
            diag_elements[i][j] / locally_relevant_global_diag[global_pos] *
                eigenvectors[pos][j]);
      }
      ++pos;
    }
  }

  // Compress the matrix
  restriction_sparse_matrix.compress(dealii::VectorOperation::add);
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::compute_restriction_sparse_matrix(
    std::vector<dealii::Vector<typename VectorType::value_type>> const
        &eigenvectors,
    std::vector<std::vector<typename VectorType::value_type>> const
        &diag_elements,
    std::vector<std::vector<dealii::types::global_dof_index>> const
        &dof_indices_maps,
    std::vector<unsigned int> const &n_local_eigenvectors,
    dealii::LinearAlgebra::distributed::Vector<
        typename VectorType::value_type> const &locally_relevant_global_diag,
    std::shared_ptr<dealii::TrilinosWrappers::SparseMatrix>
        restriction_sparse_matrix,
    std::unique_ptr<dealii::TrilinosWrappers::SparseMatrix>
        &eigenvector_sparse_matrix,
    std::unique_ptr<dealii::TrilinosWrappers::SparseMatrix>
        &delta_eigenvector_matrix) const
{
  // Compute the sparsity pattern (Epetra_FECrsGraph)
  dealii::TrilinosWrappers::SparsityPattern restriction_sp =
      compute_restriction_sparsity_pattern(eigenvectors, dof_indices_maps,
                                           n_local_eigenvectors);

  // Build the sparse matrices
  restriction_sparse_matrix->reinit(restriction_sp);
  eigenvector_sparse_matrix.reset(
      new dealii::TrilinosWrappers::SparseMatrix(restriction_sp));
  // The sparsity pattern is different than for the other sparse matrices
  // because some of the entries that do not correspond to agglomerate boundary
  // are zeros. Because reinit requires the SparsityPattern which is harder to
  // compute, we instead use the constructor that computes the SparsityPattern
  // when compress() is called).
  delta_eigenvector_matrix.reset(new dealii::TrilinosWrappers::SparseMatrix(
      eigenvector_sparse_matrix->locally_owned_range_indices(),
      eigenvector_sparse_matrix->locally_owned_domain_indices(),
      eigenvector_sparse_matrix->get_mpi_communicator()));

  std::pair<dealii::types::global_dof_index,
            dealii::types::global_dof_index> const local_range =
      restriction_sp.local_range();
  unsigned int const n_agglomerates = n_local_eigenvectors.size();
  unsigned int pos = 0;
  ASSERT(n_agglomerates == dof_indices_maps.size(),
         "dof_indices_maps has the wrong size: " +
             std::to_string(dof_indices_maps.size()) + " instead of " +
             std::to_string(n_agglomerates));
  for (unsigned int i = 0; i < n_agglomerates; ++i)
  {
    unsigned int const n_local_eig = n_local_eigenvectors[i];
    for (unsigned int k = 0; k < n_local_eig; ++k)
    {
      unsigned int const n_elem = eigenvectors[pos].size();
      ASSERT(n_elem == dof_indices_maps[i].size(),
             "dof_indices_maps[i] has the wrong size: " +
                 std::to_string(dof_indices_maps[i].size()) + " instead of " +
                 std::to_string(n_elem));
      for (unsigned int j = 0; j < n_elem; ++j)
      {
        dealii::types::global_dof_index const global_pos =
            dof_indices_maps[i][j];
        // Fill restriction sparse matrix
        restriction_sparse_matrix->add(
            local_range.first + pos, global_pos,
            diag_elements[i][j] / locally_relevant_global_diag[global_pos] *
                eigenvectors[pos][j]);
        // Fill eigenvector sparse matrix
        eigenvector_sparse_matrix->add(local_range.first + pos, global_pos,
                                       eigenvectors[pos][j]);
        // Fill delta eigenvector sparse matrix
        delta_eigenvector_matrix->set(
            local_range.first + pos, global_pos,
            (diag_elements[i][j] / locally_relevant_global_diag[global_pos] -
             1.) *
                eigenvectors[pos][j]);
      }
      ++pos;
    }
  }

  // Compress the matrices
  restriction_sparse_matrix->compress(dealii::VectorOperation::add);
  eigenvector_sparse_matrix->compress(dealii::VectorOperation::add);
  delta_eigenvector_matrix->compress(dealii::VectorOperation::insert);
}

template <int dim, typename VectorType>
unsigned int AMGe<dim, VectorType>::build_agglomerates_block(
    std::array<unsigned int, dim> const &agglomerate_dim) const
{
  // Faces in deal.II are ordered as follows: left (x_m) = 0, right (x_p) = 1,
  // front (y_m) = 2, back (y_p) = 3, bottom (z_m) = 4, top (z_p) = 5
  unsigned int constexpr x_p = 1;
  unsigned int constexpr y_p = 3;
  unsigned int constexpr z_p = 5;

  // Flag the cells to create the agglomerates
  unsigned int agglomerate = 1;
  for (auto cell : _dof_handler.active_cell_iterators())
  {
    if ((cell->is_locally_owned()) && (cell->user_index() == 0))
    {
#if MFMG_DEBUG
      int const cell_level = cell->level();
#endif
      cell->set_user_index(agglomerate);
      auto current_z_cell = cell;
      unsigned int const d_3 = (dim < 3) ? 1 : agglomerate_dim.back();
      for (unsigned int k = 0; k < d_3; ++k)
      {
        auto current_y_cell = current_z_cell;
        for (unsigned int j = 0; j < agglomerate_dim[1]; ++j)
        {
          auto current_cell = current_y_cell;
          for (unsigned int i = 0; i < agglomerate_dim[0]; ++i)
          {
            current_cell->set_user_index(agglomerate);
            if (current_cell->at_boundary(x_p) == false)
            {
              // TODO For now, we assume that there is no adaptive refinement.
              // When we change this, we will need to switch to hp::DoFHandler
              auto neighbor_cell = current_cell->neighbor(x_p);
#if MFMG_DEBUG
              if ((!neighbor_cell->active()) ||
                  (neighbor_cell->level() != cell_level))
                throw std::runtime_error("Mesh locally refined");
#endif
              if (neighbor_cell->is_locally_owned())
                current_cell = neighbor_cell;
            }
            else
              break;
          }
          if (current_y_cell->at_boundary(y_p) == false)
          {
            auto neighbor_y_cell = current_y_cell->neighbor(y_p);
            if (neighbor_y_cell->is_locally_owned())
            {
#if MFMG_DEBUG
              if ((!neighbor_y_cell->active()) ||
                  (neighbor_y_cell->level() != cell_level))
                throw std::runtime_error("Mesh locally refined");
#endif
              current_y_cell = neighbor_y_cell;
            }
          }
          else
            break;
        }
        if ((dim == 3) && (current_z_cell->at_boundary(z_p) == false))
        {
          auto neighbor_z_cell = current_z_cell->neighbor(z_p);
          if (neighbor_z_cell->is_locally_owned())
          {
#if MFMG_DEBUG
            if ((!neighbor_z_cell->active()) ||
                (neighbor_z_cell->level() != cell_level))
              throw std::runtime_error("Mesh locally refined");
#endif
            current_z_cell = neighbor_z_cell;
          }
        }
        else
          break;
      }

      ++agglomerate;
    }
  }

  _n_agglomerates = agglomerate - 1;

  return _n_agglomerates;
}

template <int dim, typename VectorType>
unsigned int AMGe<dim, VectorType>::build_agglomerates_partitioner(
    std::string const &partitioner_type, unsigned int n_agglomerates) const
{
  // We cannot use deal.II wrappers to create the agglomerates because
  //   1) the wrappers only works on serial Triangulation
  //   2) they override the subdomain_id which is already used by p4est
  // Instead, we create the connectivity graph ourselves and then, we do the
  // partitioning.

  unsigned int const n_local_cells =
      _dof_handler.get_triangulation().n_active_cells();

  // Create the DynamicSparsityPattern
  dealii::DynamicSparsityPattern connectivity(n_local_cells);

  // Associate a local index to each cell
  unsigned int local_index = 0;
  std::map<std::pair<unsigned int, unsigned int>, unsigned int> index_map;
  for (auto cell : _dof_handler.active_cell_iterators())
  {
    if (cell->is_locally_owned())
    {
      index_map[std::make_pair(cell->level(), cell->index())] = local_index;
      ++local_index;
    }
  }

  // Fill the dynamic connectivity sparsity pattern
  for (auto cell : _dof_handler.active_cell_iterators())
  {
    if (cell->is_locally_owned())
    {
      unsigned int const index =
          index_map.at(std::make_pair(cell->level(), cell->index()));
      connectivity.add(index, index);
      for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell;
           ++f)
      {
        if ((cell->at_boundary(f) == false) &&
            (cell->neighbor(f)->is_locally_owned() == true) &&
            (cell->neighbor(f)->has_children() == false))
        {
          unsigned int const neighbor_index = index_map.at(std::make_pair(
              cell->neighbor(f)->level(), cell->neighbor(f)->index()));
          connectivity.add(index, neighbor_index);
          connectivity.add(neighbor_index, index);
        }
      }
    }
  }

  dealii::SparsityPattern cell_connectivity;
  cell_connectivity.copy_from(connectivity);

  // Partition the connection graph
  dealii::SparsityTools::Partitioner partitioner;
  if (partitioner_type == "metis")
    partitioner = dealii::SparsityTools::Partitioner::metis;
  else
    partitioner = dealii::SparsityTools::Partitioner::zoltan;
  std::vector<unsigned int> partition_indices(n_local_cells);
  dealii::SparsityTools::partition(cell_connectivity, n_agglomerates,
                                   partition_indices, partitioner);

  // Assign the agglomerate ID to all the locally owned cells. Zoltan does not
  // guarantee that the agglomerate IDs will consecutive so we need to
  // renumber them. The lowest agglomerate ID is one because zero is reserved
  // for ghost and artificial cells.
  unsigned int n_zoltan_agglomerates = 0;
  std::unordered_map<unsigned int, unsigned int> agglomerate_renumbering;
  for (auto cell : _dof_handler.active_cell_iterators())
  {
    if (cell->is_locally_owned())
    {
      unsigned int const index =
          index_map.at(std::make_pair(cell->level(), cell->index()));
      auto agg_id = agglomerate_renumbering.find(partition_indices[index]);
      if (agg_id == agglomerate_renumbering.end())
      {
        ++n_zoltan_agglomerates;
        agglomerate_renumbering[partition_indices[index]] =
            n_zoltan_agglomerates;
        cell->set_user_index(n_zoltan_agglomerates);
      }
      else
        cell->set_user_index(agg_id->second);
    }
  }

  _n_agglomerates = n_zoltan_agglomerates;

  return _n_agglomerates;
}

template <int dim, typename VectorType>
dealii::TrilinosWrappers::SparsityPattern
AMGe<dim, VectorType>::compute_restriction_sparsity_pattern(
    std::vector<dealii::Vector<double>> const &eigenvectors,
    std::vector<std::vector<dealii::types::global_dof_index>> const
        &dof_indices_maps,
    std::vector<unsigned int> const &n_local_eigenvectors) const
{
  // Compute the row IndexSet
  int const n_procs = dealii::Utilities::MPI::n_mpi_processes(this->_comm);
  int const rank = dealii::Utilities::MPI::this_mpi_process(this->_comm);
  unsigned int const n_local_rows(eigenvectors.size());
  std::vector<unsigned int> n_rows_per_proc(n_procs);
  n_rows_per_proc[rank] = n_local_rows;
  MPI_Allgather(MPI_IN_PLACE, 0, MPI_DATATYPE_NULL, &n_rows_per_proc[0], 1,
                MPI_UNSIGNED, this->_comm);

  dealii::types::global_dof_index n_total_rows =
      std::accumulate(n_rows_per_proc.begin(), n_rows_per_proc.end(),
                      static_cast<dealii::types::global_dof_index>(0));
  dealii::types::global_dof_index n_rows_before =
      std::accumulate(n_rows_per_proc.begin(), n_rows_per_proc.begin() + rank,
                      static_cast<dealii::types::global_dof_index>(0));
  dealii::IndexSet row_indexset(n_total_rows);
  row_indexset.add_range(n_rows_before, n_rows_before + n_local_rows);
  row_indexset.compress();

  // Build the sparsity pattern
  dealii::TrilinosWrappers::SparsityPattern sp(
      row_indexset, this->_dof_handler.locally_owned_dofs(), this->_comm);

  unsigned int const n_agglomerates = n_local_eigenvectors.size();
  unsigned int row = 0;
  for (unsigned int i = 0; i < n_agglomerates; ++i)
  {
    unsigned int const n_local_eig = n_local_eigenvectors[i];
    for (unsigned int j = 0; j < n_local_eig; ++j)
    {
      sp.add_entries(n_rows_before + row, dof_indices_maps[i].begin(),
                     dof_indices_maps[i].end());
      ++row;
    }
  }

  sp.compress();

  return sp;
}

template <int dim, typename VectorType>
void AMGe<dim, VectorType>::build_agglomerate_triangulation(
    std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator> const
        &agglomerate,
    dealii::Triangulation<dim> &agglomerate_triangulation,
    std::map<typename dealii::Triangulation<dim>::active_cell_iterator,
             typename dealii::DoFHandler<dim>::active_cell_iterator>
        &agglomerate_to_global_tria_map) const
{
  if (agglomerate.empty())
    return;
  // Map between the cells on the boundary and the faces on the boundary and the
  // associated boundary id.
  std::map<typename dealii::DoFHandler<dim>::active_cell_iterator,
           std::vector<std::pair<unsigned int, unsigned int>>>
      boundary_ids;
  for (auto cell : agglomerate)
    if (cell->at_boundary())
    {
      for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell;
           ++f)
      {
        if (cell->face(f)->at_boundary())
        {
          boundary_ids[cell].push_back(
              std::make_pair(f, cell->face(f)->boundary_id()));
        }
      }
    }

  // If the agglomerate has hanging nodes, the patch is bigger than
  // what we may expect because we cannot create a coarse triangulation with
  // hanging nodes. Thus, we need to use FE_Nothing to get ride of unwanted
  // cells.
  dealii::GridTools::build_triangulation_from_patch<dealii::DoFHandler<dim>>(
      agglomerate, agglomerate_triangulation, agglomerate_to_global_tria_map);

  // Copy the boundary IDs to the agglomerate triangulation
  for (auto const &boundary : boundary_ids)
  {
    auto const boundary_cell = boundary.first;

    auto agg_cell =
        std::find_if(agglomerate_to_global_tria_map.begin(),
                     agglomerate_to_global_tria_map.end(),
                     [&](auto const &agglomerate_cell) {
                       return (agglomerate_cell.second == boundary_cell);
                     });
    if (agg_cell != agglomerate_to_global_tria_map.end())
      for (auto &boundary_face : boundary.second)
        agg_cell->first->face(boundary_face.first)
            ->set_boundary_id(boundary_face.second);
  }
}
} // namespace mfmg

#endif


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_prismatic_hpp__
#define __pinocchio_joint_prismatic_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/spatial-axis.hpp"
#include "pinocchio/utils/axis-label.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, int _axis> struct MotionPrismaticTpl;

  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, int axis, typename MotionDerived>
  struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options, int _axis>
  struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // struct traits MotionPrismaticTpl

  template<typename _Scalar, int _Options, int _axis>
  struct MotionPrismaticTpl
  : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    MOTION_TYPEDEF_TPL(MotionPrismaticTpl);

    enum { axis = _axis };

    typedef SpatialAxis<_axis+LINEAR> Axis;
    typedef typename Axis::CartesianAxis3 CartesianAxis3;

    MotionPrismaticTpl() {}
    MotionPrismaticTpl(const Scalar & v) : m_v(v) {}

    inline PlainReturnType plain() const { return Axis() * m_v; }

    template<typename OtherScalar>
    MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
    {
      return MotionPrismaticTpl(alpha*m_v);
    }

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      typedef typename MotionDense<Derived>::Scalar OtherScalar;
      other.linear()[_axis] += (OtherScalar) m_v;
    }

    template<typename MotionDerived>
    void setTo(MotionDense<MotionDerived> & other) const
    {
      for(Eigen::DenseIndex k = 0; k < 3; ++k)
        other.linear()[k] = k == axis ? m_v : (Scalar)0;
      other.angular().setZero();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().setZero();
      v.linear().noalias() = m_v * (m.rotation().col(axis));
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));

      // Angular
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());

      // Angular
      mout.angular().setZero();
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    Scalar & linearRate() { return m_v; }
    const Scalar & linearRate() const { return m_v; }

    bool isEqual_impl(const MotionPrismaticTpl & other) const
    {
      return m_v == other.m_v;
    }

  protected:

    Scalar m_v;
  }; // struct MotionPrismaticTpl

  template<typename Scalar, int Options, int axis, typename MotionDerived>
  typename MotionDerived::MotionPlain
  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
            const MotionDense<MotionDerived> & m2)
  {
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
  }

  template<typename MotionDerived, typename S2, int O2, int axis>
  EIGEN_STRONG_INLINE
  typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2)
  {
    return m2.motionAction(m1);
  }

  template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl;

  template<typename _Scalar, int _Options, int _axis>
  struct traits< TransformPrismaticTpl<_Scalar,_Options,_axis> >
  {
    enum {
      axis = _axis,
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef const Vector3 LinearRef;
    typedef const Vector3 ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformPrismaticTpl

  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };

  template<typename _Scalar, int _Options, int axis>
  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
    typedef typename traits<TransformPrismaticTpl>::PlainType PlainType;

    typedef SpatialAxis<axis+LINEAR> Axis;
    typedef typename Axis::CartesianAxis3 CartesianAxis3;

    TransformPrismaticTpl() {}
    TransformPrismaticTpl(const Scalar & displacement)
    : m_displacement(displacement)
    {}

    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      res.rotation().setIdentity();
      res.translation()[axis] = m_displacement;

      return res;
    }

    operator PlainType() const { return plain(); }

    template<typename S2, int O2>
    typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
    se3action(const SE3Tpl<S2,O2> & m) const
    {
      typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
      ReturnType res(m);
      res.translation()[axis] += m_displacement;

      return res;
    }

    const Scalar & displacement() const { return m_displacement; }
    Scalar & displacement() { return m_displacement; }

    ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
    AngularType rotation() const { return AngularType(3,3); }

    bool isEqual(const TransformPrismaticTpl & other) const
    {
      return m_displacement == other.m_displacement;
    }

  protected:

    Scalar m_displacement;
  };

  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;

  template<typename _Scalar, int _Options, int axis>
  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionPrismaticTpl<Scalar,Options,axis> JointMotion;
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // traits ConstraintRevolute

  template<typename Scalar, int Options, int axis>
  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, int axis, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, int axis, typename ForceDerived>
  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };

  template<typename Scalar, int Options, int axis, typename ForceSet>
  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };

  template<typename _Scalar, int _Options, int axis>
  struct ConstraintPrismaticTpl
  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
    enum { NV = 1 };

    typedef SpatialAxis<LINEAR+axis> Axis;

    ConstraintPrismaticTpl() {};

    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      assert(v.size() == 1);
      return JointMotion(v[0]);
    }

    template<typename S2, int O2>
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
    se3Action(const SE3Tpl<S2,O2> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear() = m.rotation().col(axis);
      v.angular().setZero();
      return res;
    }

    template<typename S2, int O2>
    typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S2,O2> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear() = m.rotation().transpose().col(axis);
      v.angular().setZero();
      return res;
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      const ConstraintPrismaticTpl & ref;
      TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}

      template<typename ForceDerived>
      typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
      operator* (const ForceDense<ForceDerived> & f) const
      { return f.linear().template segment<1>(axis); }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename Derived>
      typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
      operator*(const Eigen::MatrixBase<Derived> & F )
      {
        assert(F.rows()==6);
        return F.row(LINEAR+axis);
      }

    }; // struct TransposeConst
    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
    DenseBase matrix_impl() const
    {
      DenseBase S;
      MotionRef<DenseBase> v(S);
      v << Axis();
      return S;
    }

    template<typename MotionDerived>
    typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
    motionAction(const MotionDense<MotionDerived> & m) const
    {
      typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v = m.cross(Axis());
      return res;
    }

    bool isEqual(const ConstraintPrismaticTpl &) const { return true; }

  }; // struct ConstraintPrismaticTpl

  template<typename S1, int O1,typename S2, int O2, int axis>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> >
  {
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,0> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;

        /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
        const S1
        &m = Y.mass(),
        &y = Y.lever()[1],
        &z = Y.lever()[2];
        res << m, S1(0), S1(0), S1(0), m*z, -m*y;

        return res;
      }
    };

    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,1> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;

        /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &z = Y.lever()[2];

        res << S1(0), m, S1(0), -m*z, S1(0), m*x;

        return res;
      }
    };

    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticTpl<S2,O2,2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & /*constraint*/)
      {
        ReturnType res;

        /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
        const S1
        &m = Y.mass(),
        &x = Y.lever()[0],
        &y = Y.lever()[1];

        res << S1(0), S1(0), m, m*y, -m*x, S1(0);

        return res;
      }
    };
  } // namespace impl

  template<typename M6Like,typename S2, int O2, int axis>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
  {
    typedef typename M6Like::ConstColXpr ReturnType;
  };

  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename M6Like, typename Scalar, int Options, int axis>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
    {
      typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                             const Constraint & /*constraint*/)
      {
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
        return Y.derived().col(Inertia::LINEAR + axis);
      }
    };
  } // namespace impl

  template<typename _Scalar, int _Options, int _axis>
  struct JointPrismaticTpl
  {
    typedef _Scalar Scalar;

    enum
    {
      Options = _Options,
      axis = _axis
    };
  };

  template<typename _Scalar, int _Options, int axis>
  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
  {
    enum {
      NQ = 1,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataPrismaticTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelPrismaticTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint_t;
    typedef TransformPrismaticTpl<Scalar,Options,axis> Transformation_t;
    typedef MotionPrismaticTpl<Scalar,Options,axis> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options, int axis>
  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };

  template<typename Scalar, int Options, int axis>
  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
  { typedef JointPrismaticTpl<Scalar,Options,axis> JointDerived; };

  template<typename _Scalar, int _Options, int axis>
  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataPrismaticTpl()
    : M((Scalar)0)
    , v((Scalar)0)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname()
    {
      return std::string("JointDataP") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

  }; // struct JointDataPrismaticTpl

  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
  {
    typedef JointModelPrismaticTpl<NewScalar,Options,axis> type;
  };

  template<typename _Scalar, int _Options, int axis>
  struct JointModelPrismaticTpl
  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelPrismaticTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];
      data.M.displacement() = q;
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
      data.v.linearRate() = v;
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U = I.col(Inertia::LINEAR + axis);
      data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
      data.UDinv.noalias() = data.U * data.Dinv[0];

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname()
    {
      return std::string("JointModelP") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelPrismaticTpl<NewScalar,Options,axis> cast() const
    {
      typedef JointModelPrismaticTpl<NewScalar,Options,axis> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelPrismaticTpl

  typedef JointPrismaticTpl<double,0,0> JointPX;
  typedef JointDataPrismaticTpl<double,0,0> JointDataPX;
  typedef JointModelPrismaticTpl<double,0,0> JointModelPX;

  typedef JointPrismaticTpl<double,0,1> JointPY;
  typedef JointDataPrismaticTpl<double,0,1> JointDataPY;
  typedef JointModelPrismaticTpl<double,0,1> JointModelPY;

  typedef JointPrismaticTpl<double,0,2> JointPZ;
  typedef JointDataPrismaticTpl<double,0,2> JointDataPZ;
  typedef JointModelPrismaticTpl<double,0,2> JointModelPZ;

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_prismatic_hpp__


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Implement uniform refinement of a domain.
 *
 * This module provides the `refine_uniformly` function for triangular and tetrahedral domains. This
 * function refines all edges of the given domain and segmentation.
 */

#ifndef ALGORITHMS_REFINE_UNIFORMLY_HPP
#define ALGORITHMS_REFINE_UNIFORMLY_HPP

#include "../domains/linear.hpp"
#include "../domains/triangular.hpp"
#include "../domains/quadrilateral.hpp"
#include "../domains/tetrahedral.hpp"

#include "../segmentations/linear.hpp"
#include "../segmentations/triangular.hpp"
#include "../segmentations/quadrilateral.hpp"
#include "../segmentations/tetrahedral.hpp"

#include <boost/python.hpp>
using namespace boost::python;

////////////////
// Triangular //
////////////////

/**
 * Refine all edges of the given triangular cartesian 2D domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TriangularCartesian2D_Domain_refine_uniformly(TriangularCartesian2D_Domain domain_in, TriangularCartesian2D_Segmentation segmentation_in);

/**
 * Refine all edges of the given triangular cartesian 3D domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TriangularCartesian3D_Domain_refine_uniformly(TriangularCartesian3D_Domain domain_in, TriangularCartesian3D_Segmentation segmentation_in);

/**
 * Refine all edges of the given triangular cylindrical domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TriangularCylindrical3D_Domain_refine_uniformly(TriangularCylindrical3D_Domain domain_in, TriangularCylindrical3D_Segmentation segmentation_in);

/**
 * Refine all edges of the given triangular polar domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TriangularPolar2D_Domain_refine_uniformly(TriangularPolar2D_Domain domain_in, TriangularPolar2D_Segmentation segmentation_in);

/**
 * Refine all edges of the given triangular spherical domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TriangularSpherical3D_Domain_refine_uniformly(TriangularSpherical3D_Domain domain_in, TriangularSpherical3D_Segmentation segmentation_in);

/////////////////
// Tetrahedral //
/////////////////

/**
 * Refine all edges of the given tetrahedral cartesian 3D domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TetrahedralCartesian3D_Domain_refine_uniformly(TetrahedralCartesian3D_Domain domain_in, TetrahedralCartesian3D_Segmentation segmentation_in);

/**
 * Refine all edges of the given tetrahedral cylindrical domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TetrahedralCylindrical3D_Domain_refine_uniformly(TetrahedralCylindrical3D_Domain domain_in, TetrahedralCylindrical3D_Segmentation segmentation_in);

/**
 * Refine all edges of the given tetrahedral spherical domain and segmentation.
 *
 * @param domain_in Domain to be refined.
 * @param segmentation_in Segmentation of the domain to be refined.
 * @return Python tuple with two elements:
 *         1. the refined domain, and
 *         2. the refined segmentation.
 *
 * @since 0.1.0
 */
tuple TetrahedralSpherical3D_Domain_refine_uniformly(TetrahedralSpherical3D_Domain domain_in, TetrahedralSpherical3D_Segmentation segmentation_in);

#endif /* end of include guard: ALGORITHMS_REFINE_UNIFORMLY_HPP */


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_
#define SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_

/**
 * @file
 * @brief Sparse matrix utilities.
 */

#include <Eigen/Sparse>

#include <numeric>
#include <optional>

namespace smooth::feedback {

/**
 * @brief Add block into a sparse matrix.
 *
 * After this function the output variable dest is s.t.
 *
 * dest[row0 + r, col0 + c] += scale * source[r, c]
 *
 * @param dest destination
 * @param row0 starting row for block
 * @param col0 starting column for block
 * @param source block values
 * @param scale scaling parameter
 * @param upper_only only add into upper triangular part
 *
 * @note Values are accessed with coeffRef().
 */
template<typename Source, int Options>
  requires(std::is_base_of_v<Eigen::EigenBase<Source>, Source>)
inline void block_add(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  const Source & source,
  double scale      = 1,
  double upper_only = false)
{
  for (auto c = 0; c < source.outerSize(); ++c) {
    for (Eigen::InnerIterator it(source, c); it; ++it) {
      if (!upper_only || row0 + it.row() <= col0 + it.col()) {
        dest.coeffRef(row0 + it.row(), col0 + it.col()) += scale * it.value();
      }
    }
  }
}

/**
 * @brief Write block into a sparse matrix.
 *
 * After this function the output variable dest is s.t.
 *
 * dest[row0 + r, col0 + c] = scale * source[r, c]
 *
 * @param dest destination
 * @param row0 starting row for block
 * @param col0 starting column for block
 * @param source block values
 * @param scale scaling parameter
 * @param upper_only only add into upper triangular part
 *
 * @note Values are accessed with coeffRef().
 */
template<typename Source, int Options>
  requires(std::is_base_of_v<Eigen::EigenBase<Source>, Source>)
inline void block_write(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  const Source & source,
  double scale      = 1,
  double upper_only = false)
{
  for (auto c = 0; c < source.outerSize(); ++c) {
    for (Eigen::InnerIterator it(source, c); it; ++it) {
      if (!upper_only || row0 + it.row() <= col0 + it.col()) {
        dest.coeffRef(row0 + it.row(), col0 + it.col()) = scale * it.value();
      }
    }
  }
}

/**
 * @brief Add identity matrix block into sparse matrix.
 *
 * After this function the output variable dest is s.t.
 *
 * dest[row0 + k, col0 + k] += scale, k = 0...n-1
 *
 * @param dest destination
 * @param row0 starting row for block
 * @param col0 starting column for block
 * @param n size of identity matrix
 * @param scale scaling parameter
 *
 * @note Values are accessed with coeffRef().
 */
template<int Options>
inline void block_add_identity(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  Eigen::Index n,
  double scale = 1)
{
  for (auto k = 0u; k < n; ++k) { dest.coeffRef(row0 + k, col0 + k) += scale; }
}

/**
 * @brief Write identity matrix block into sparse matrix.
 *
 * After this function the output variable dest is s.t.
 *
 * dest[row0 + k, col0 + k] += scale, k = 0...n-1
 *
 * @param dest destination
 * @param row0 starting row for block
 * @param col0 starting column for block
 * @param n size of identity matrix
 * @param scale scaling parameter
 *
 * @note Values are accessed with coeffRef().
 */
template<int Options>
inline void block_write_identity(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  Eigen::Index n,
  double scale = 1)
{
  for (auto k = 0u; k < n; ++k) { dest.coeffRef(row0 + k, col0 + k) = scale; }
}

/**
 * @brief Zero a sparse matrix expression without changing allocation.
 *
 * @note Expression must be write-able.
 *
 * @note More efficient for compressed expressions.
 */
template<typename SparseMat>
  requires(std::is_base_of_v<
           Eigen::SparseCompressedBase<std::decay_t<SparseMat>>,
           std::decay_t<SparseMat>>)
inline void set_zero(SparseMat && mat)
{
  if (mat.isCompressed()) {
    mat.coeffs().setZero();
  } else {
    for (auto i = 0; i < mat.outerSize(); ++i) {
      for (typename std::decay_t<decltype(mat)>::InnerIterator it(mat, i); it; ++it) {
        it.valueRef() = 0;
      }
    }
  }
}

/**
 * @brief Count number of explicit zeros in sparse matrix.
 *
 * @param mat sparse matrix
 */
template<typename Mat>
  requires(std::is_base_of_v<Eigen::EigenBase<Mat>, Mat>)
uint64_t count_explicit_zeros(const Mat & mat)
{
  uint64_t ret = 0;
  for (auto c = 0; c < mat.outerSize(); ++c) {
    for (Eigen::InnerIterator it(mat, c); it; ++it) {
      if (it.value() == 0) { ++ret; }
    }
  }
  return ret;
}

/**
 * @brief Mark explicit zeros in sparse matrix.
 *
 * @param mat sparse matrix
 *
 * Returns a matrix that has values as follows:
 *  - 0 for implicit zeros
 *  - 1 for non-zeros
 *  - 9 for explicit zeros
 */
template<typename Mat>
  requires(std::is_base_of_v<Eigen::EigenBase<Mat>, Mat>)
Eigen::MatrixX<typename Mat::Scalar> mark_explicit_zeros(const Mat & mat)
{
  Eigen::MatrixX<typename Mat::Scalar> ret;
  ret.setConstant(mat.rows(), mat.cols(), 0);
  for (auto c = 0; c < mat.outerSize(); ++c) {
    for (Eigen::InnerIterator it(mat, c); it; ++it) {
      if (it.value() == 0) {
        ret(it.row(), it.col()) = 9;
      } else {
        ret(it.row(), it.col()) = 1;
      }
    }
  }
  return ret;
}

/**
 * @brief (Right) Hessian of composed function \f$ (f \circ g)(x) \f$.
 *
 * @param[out] out result                           [No x No*Nx]
 * @param[in] Jf (Right) Jacobian of f at y = g(x)  [No x Ny   ]
 * @param[in] Hf (Right) Hessian of f at y = g(x)   [Ny x No*Ny]
 * @param[in] Jg (Right) Jacobian of g at x         [Ny x Nx   ]
 * @param[in] Hg (Right) Hessian of g at x          [Nx x Ny*Nx]
 * @param[in] r0 row to insert result
 * @param[in] r0 col to insert result
 *
 * @note out must have appropriate size
 */
template<typename S1, typename S2, typename S3, typename S4>
  requires(
    std::is_base_of_v<Eigen::EigenBase<S1>, S1> && std::is_base_of_v<Eigen::EigenBase<S2>, S2> &&
      std::is_base_of_v<Eigen::EigenBase<S3>, S3> && std::is_base_of_v<Eigen::EigenBase<S4>, S4>)
inline void d2r_fog(
  Eigen::SparseMatrix<double> & out,
  const S1 & Jf,
  const S2 & Hf,
  const S3 & Jg,
  const S4 & Hg,
  Eigen::Index r0 = 0,
  Eigen::Index c0 = 0)
{
  const auto No = Jf.rows();
  const auto Ny = Jf.cols();

  [[maybe_unused]] const auto Ni = Jg.rows();
  const auto Nx                  = Jg.cols();

  // check some dimensions
  assert(Ny == Ni);
  assert(Hf.rows() == Ny);
  assert(Hf.cols() == No * Ny);
  assert(Hg.rows() == Nx);
  assert(Hg.cols() == Ni * Nx);

  for (auto no = 0u; no < No; ++no) {
    // TODO sparse-sparse-sparse product is expensive and allocates temporary
    block_add(out, r0, c0 + no * Nx, Jg.transpose() * Hf.middleCols(no * Ny, Ny) * Jg);
  }

  for (auto i = 0u; i < Jf.outerSize(); ++i) {
    for (Eigen::InnerIterator it(Jf, i); it; ++it) {
      block_add(out, r0, c0 + it.row() * Nx, Hg.middleCols(it.col() * Nx, Nx), it.value());
    }
  }
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <gtkmm.h>
#include "gui.h"
#include <fstream>

using namespace std;
using Eigen::MatrixXd;

int main(int argc, char *argv[]) {
    ///
    /// The main function shall be responsible for calling various other functions and instantiating the classes for using their functions cop290
    ///

    // check3Dfile();
    auto app =
        Gtk::Application::create(argc, argv,
        "org.gtkmm.examples");
    MainWindow window;
    return app->run(window);
    // check2D();
    // return 0;
}

int check3D() {
    double thisarr [4] = {0,1,0,0};
    Object3D obj;
    Point v1,v2,v3,v4,v5;
    v1.setCoordinates(2,2,0);
    v2.setCoordinates(2,1,0);
    v3.setCoordinates(1,1,0);
    v4.setCoordinates(2,1,1);
    v5.setCoordinates(1,1,1);
    obj.vertices.push_back(v1);
    obj.vertices.push_back(v2);
    obj.vertices.push_back(v3);
    obj.vertices.push_back(v4);
    obj.vertices.push_back(v5);
    Edge e1,e2,e3,e4,e5,e6,e7,e8;
    e1.p1 = v1; e1.p2 = v2; e2.p1 = v1; e2.p2 = v3;
    e3.p1 = v1; e3.p2 = v4; e4.p1 = v1; e4.p2 = v5;
    e5.p1 = v2; e5.p2 = v4; e6.p1 = v2; e6.p2 = v3;
    e7.p1 = v4; e7.p2 = v5; e8.p1 = v3; e8.p2 = v5;
    obj.edges.push_back(e1);
    obj.edges.push_back(e2);
    obj.edges.push_back(e3);
    obj.edges.push_back(e4);
    obj.edges.push_back(e5);
    obj.edges.push_back(e6);
    obj.edges.push_back(e7);
    obj.edges.push_back(e8);
    Face f1,f2,f3,f4,f5;
    f1.vertices.push_back(0);
    f1.vertices.push_back(1);
    f1.vertices.push_back(2);
    f2.vertices.push_back(0);
    f2.vertices.push_back(1);
    f2.vertices.push_back(3);
    f3.vertices.push_back(0);
    f3.vertices.push_back(3);
    f3.vertices.push_back(4);
    f4.vertices.push_back(0);
    f4.vertices.push_back(2);
    f4.vertices.push_back(4);
    f5.vertices.push_back(1);
    f5.vertices.push_back(2);
    f5.vertices.push_back(3);
    f5.vertices.push_back(4);
    obj.faces.push_back(f1);
    obj.faces.push_back(f2);
    obj.faces.push_back(f3);
    obj.faces.push_back(f4);
    obj.faces.push_back(f5);
    PlaneProjection* p = obj.project3D(thisarr);
    cout << "Projection: " << *p << endl;
    p->rotatePlane();
    cout << "Rotated Projection: " << *p << endl;
    return 0;
}

PlaneProjection* input3Dfile(string filename){
    ///
    /// Function to get input of the 3D Object from file
    ///
    ifstream inFile (filename);
    string output;
    Object3D obj;
    double planearr [4];
    if(!(inFile.is_open())){
        cout<< "not started" <<endl;
    }
    map<string, int> labeltoindex;
    if(inFile.is_open()){
        cout << "start file"<<endl;
        if(!inFile.eof()){
            inFile >> output;
            cout<< "getting vertices"<<endl;
            if(output=="Vertices"){
                // inFile>> output;
                int tempindex = 0;
                while(output!=";"){
                    inFile >> output;
                    double n1 = atof(output.c_str());
                    inFile >> output;
                    double n2 = atof(output.c_str());
                    inFile >> output;
                    double n3 = atof(output.c_str());
                    inFile >> output;
                    string label = output;
                    Point tempoint;
                    tempoint.setCoordinatesAndLabel(n1,n2,n3, label);
                    // cout << "point is "<< tempoint<< endl;
                    obj.vertices.push_back(tempoint);
                    labeltoindex[label] = tempindex;
                    inFile >> output;
                    tempindex++;
                }
            }
            cout << "vertices finished" <<endl;
            inFile >> output;
            cout << "edges begin "<< endl;
            if(output=="Edges"){
                while(output!=";"){
                    inFile >> output;
                    string label1 = output;
                    inFile >> output;
                    string label2 = output;
                    auto point1ptr = find_if(obj.vertices.begin(),obj.vertices.end(),[label1](Point p)->bool{
                        return(p.label == label1);
                    });
                    auto point2ptr = find_if(obj.vertices.begin(),obj.vertices.end(),[label2](Point p)->bool{
                        return(p.label == label2);
                    });
                    Edge tempedge;
                    tempedge.p1 = * point1ptr;
                    tempedge.p2 = * point2ptr;
                    obj.edges.push_back(tempedge);
                    inFile >> output;
                }
            }
            cout << "edges finished"<< endl;
            inFile >> output;
            cout << "faces starting" << endl;
            if(output=="Faces"){
                while(output!=";"){
                    inFile >> output;
                    // vector<string> labelvec;
                    Face tempface;
                    while(output!="," && output!=";"){
                        string tempstr = output;
                        // labelvec.push_back(tempstr);
                        // cout << "tempstr is " <<tempstr << endl;
                        inFile >> output;
                        tempface.vertices.push_back(labeltoindex[tempstr]);

                    }
                    obj.faces.push_back(tempface);
                    // cout << "pushed face"<<endl;
                    // break;
                    // inFile >> output;
                }
            }
            cout << "faces finished" <<endl;
            inFile >> output;
            cout << "plane started "<< endl;
            if(output=="Plane"){
                inFile >> output;
                double n1 = atof(output.c_str());
                inFile >> output;
                double n2 = atof(output.c_str());
                inFile >> output;
                double n3 = atof(output.c_str());
                inFile >> output;
                double n4 = atof(output.c_str());
                // inFile >> output
                // planearr={n1,n2,n3,n4};
                planearr[0]=n1;
                planearr[1]=n2;
                planearr[2]=n3;
                planearr[3]=n4;
            }
            // cout << "plane finished" << endl;
        }
        cout << "file complete"<<endl;
        cout << "Object created from file: " << endl << obj << endl;
        PlaneProjection* p = new PlaneProjection;
        p = obj.project3D(planearr);
        cout << "Projection: " << *p << endl;
        p->rotatePlane();
        cout << "Rotated Projection: " << *p << endl;
        return p;
    }
    return 0;
}

int check2D() {
    Point etop, atop, btop, ctop, dtop, afront,dfront, bfront,cfront, efront, eside, dside, cside, aside, bside;
    etop.setCoordinatesAndLabel(1.5,1.5,0,"e");
    atop.setCoordinatesAndLabel(1,1,0,"a");
    btop.setCoordinatesAndLabel(2,1,0,"b");
    ctop.setCoordinatesAndLabel(2,2,0,"c");
    dtop.setCoordinatesAndLabel(1,2,0,"d");
    efront.setCoordinatesAndLabel(1.5,0,2,"e");
    afront.setCoordinatesAndLabel(1,0,1,"a");
    bfront.setCoordinatesAndLabel(2,0,1,"b");
    cfront.setCoordinatesAndLabel(2,0,1,"c");
    dfront.setCoordinatesAndLabel(1,0,1,"d");
    eside.setCoordinatesAndLabel(0,1.5,2,"e");
    aside.setCoordinatesAndLabel(0,1,1,"a");
    bside.setCoordinatesAndLabel(0,1,1,"b");
    cside.setCoordinatesAndLabel(0,2,1,"c");
    dside.setCoordinatesAndLabel(0,2,1,"d");
    ClusteredPoint atopcl,btopcl, etopcl, dtopcl, ctopcl, efrontcl, adfrontcl,bcfrontcl,esidecl, dcsidecl, absidecl;
    atopcl.points.push_back(atop);
    btopcl.points.push_back(btop);
    dtopcl.points.push_back(dtop);
    etopcl.points.push_back(etop);
    ctopcl.points.push_back(ctop);
    efrontcl.points.push_back(efront);
    adfrontcl.points.push_back(afront);
    adfrontcl.points.push_back(dfront);
    bcfrontcl.points.push_back(bfront);
    bcfrontcl.points.push_back(cfront);
    esidecl.points.push_back(eside);
    dcsidecl.points.push_back(dside);
    dcsidecl.points.push_back(cside);
    absidecl.points.push_back(aside);
    absidecl.points.push_back(bside);
    OrthoProjection topview, frontview,sideview;
    ClusteredPoint topspoints[] = {atopcl,btopcl,dtopcl,etopcl,ctopcl};
    for(int i=0;i<5;i++){
        topview.vertices.push_back(topspoints[i]);
    }
    //ClusteredPoint temp =topview.vertices[2];
    //cout << topview.vertices[2].points[0].label << endl;
    ClusteredPoint frontspoints[] ={efrontcl,adfrontcl,bcfrontcl};
    for(int i=0;i<3;i++){
        frontview.vertices.push_back(frontspoints[i]);
    }
    ClusteredPoint sidespoints[] = {esidecl,dcsidecl,absidecl};
    for(int i=0;i<3;i++){
        sideview.vertices.push_back(sidespoints[i]);
    }
    Edge2D abtope,bctope,cdtope,adtope,aetope,betope,cetope,detope,actope,bdtope;
    abtope.cp1 = atopcl; abtope.cp2 = btopcl;
    bctope.cp1 = btopcl; bctope.cp2 = ctopcl;
    cdtope.cp1 = ctopcl; cdtope.cp2 = dtopcl;
    adtope.cp1 = atopcl; adtope.cp2 = dtopcl;
    aetope.cp1 = atopcl; aetope.cp2 = etopcl;
    betope.cp1 = btopcl; betope.cp2 = etopcl;
    cetope.cp1 = ctopcl; cetope.cp2 = etopcl;
    detope.cp1 = dtopcl; detope.cp2 = etopcl;
    actope.cp1 = atopcl; actope.cp2 = ctopcl;
    bdtope.cp1 = btopcl; bdtope.cp2 = dtopcl;
    Edge2D topsedges[] ={abtope,bctope,cdtope,adtope,aetope,betope,cetope,detope,actope,bdtope};
    //cout << sizeof(topsedges)/sizeof(Edge2D);
    for(int i=0;i<10;i++){
        topview.edges.push_back(topsedges[i]);
    }
    Edge2D ebcfronte, adbcfronte,eadfronte;
    ebcfronte.cp1=efrontcl; ebcfronte.cp2=bcfrontcl;
    adbcfronte.cp1=adfrontcl; adbcfronte.cp2=bcfrontcl;
    eadfronte.cp1=efrontcl; eadfronte.cp2=adfrontcl;
    Edge2D frontsedges[] = {ebcfronte, adbcfronte,eadfronte};
    for(int i=0;i<3;i++){
        frontview.edges.push_back(frontsedges[i]);
    }
    Edge2D eabsidee, dcabsidee, edcsidee;
    eabsidee.cp1=esidecl; eabsidee.cp2=absidecl;
    dcabsidee.cp1=dcsidecl; dcabsidee.cp2=absidecl;
    edcsidee.cp1=esidecl; edcsidee.cp2=dcsidecl;
    Edge2D sidesedges[] ={eabsidee, dcabsidee, edcsidee};
    for(int i=0;i<3;i++){
        sideview.edges.push_back(sidesedges[i]);
    }
    Projection2D myproj;
    myproj.frontview=frontview;
    myproj.sideview=sideview;
    myproj.topview =topview;
    myproj.create3D();
    return 0;
}

Wireframe* input2Dfile(string filename){
    ///
    /// Function to get input of 2D Projections from a file
    ///
    // ofstream inFile;
    cout <<"start" <<endl;
	// inFile.open("example.txt");
    ifstream inFile (filename);
    if(!(inFile.is_open())){
        cout<< "not started" <<endl;
    }

	//char output[100];
	OrthoProjection top,front,side;
	Point temp2dpoint;
	string output;
    cout<< "file started"<<endl;
    if(inFile.is_open()){
        if(!inFile.eof())
		{
			inFile >> output;
			//transform(output.begin(), output.end(), output.begin(), ::tolower);
            cout<<"begin" <<endl;
			if(output == "Top"){
                cout << "entered Top "<<endl;
                inFile >> output;
                map<string, ClusteredPoint> sampmap;
                if(output=="Vertices"){
                    cout << "entered Vertices" << endl;
                    inFile >> output;
                    while(output!=";"){
                        double n1 = atof(output.c_str());
                        // cout << n1 << endl;
                        inFile >> output;
                        double n2 = atof(output.c_str());
                        inFile >> output;
                        vector<string> thisstr;
                        inFile >> output;
                        while(output!="]"){
                            // inFile >> output;
                            thisstr.push_back(output);
                            inFile >> output;
                        }
                        ClusteredPoint tempcluster;
                        for(int i=0;i<thisstr.size();i++){
                            Point tempoint;
                            tempoint.x = n1;
                            tempoint.y = n2;
                            tempoint.z = 0 ;
                            tempoint.label = thisstr[i];
                            cout << tempoint.label << endl;
                            cout << tempoint << endl;
                            tempcluster.points.push_back(tempoint);
                        }
                        sampmap[tempcluster.points[0].label] = tempcluster;
                        top.vertices.push_back(tempcluster);
                        inFile >> output;
                    }

                    while(output!=";;"){
                        cout << "ended vertices" << endl;
                        inFile >> output;
                        if(output=="Edges"){
                            cout << "entered edges " <<endl;

                            while(output!=";;"){
                                inFile >> output;
                                string a1 = output;
                                inFile >> output;
                                string a2 = output;
                                // cout << "a1 and a2 are " <<a1<<" "<<a2<<endl;
                                ClusteredPoint c1 = sampmap[a1];
                                ClusteredPoint c2 = sampmap[a2];
                                Edge2D thisedge2d;
                                thisedge2d.cp1 = c1;
                                thisedge2d.cp2 = c2;
                                top.edges.push_back(thisedge2d);
                                inFile >> output;
                            }
                            cout << "edge size is " << top.edges.size() << endl;
                            // cout << "last edge is " << top.edges[top.edges.size()-1].cp1.points[0] << " : "<< top.edges[top.edges.size()-1].cp2.points[0] <<endl;
                        }
                    }

                }

            }
            //yha se
            inFile >> output;
            if(output=="Front"){
                cout << "entered Front" <<endl;
                inFile >> output;

                map<string, ClusteredPoint> sampmap;
                if(output=="Vertices"){
                    cout << "entered Vertices" << endl;
                    inFile >> output;
                    while(output!=";"){
                        double n1 = atof(output.c_str());
                        // cout << n1 << endl;
                        inFile >> output;
                        double n2 = atof(output.c_str());
                        inFile >> output;
                        vector<string> thisstr;
                        inFile >> output;
                        while(output!="]"){
                            // inFile >> output;
                            thisstr.push_back(output);
                            inFile >> output;
                        }
                        ClusteredPoint tempcluster;
                        for(int i=0;i<thisstr.size();i++){
                            Point tempoint;
                            tempoint.x = n1;
                            tempoint.y = 0;
                            tempoint.z = n2 ;
                            tempoint.label = thisstr[i];
                            cout << tempoint.label << endl;
                            cout << tempoint << endl;
                            tempcluster.points.push_back(tempoint);
                        }
                        sampmap[tempcluster.points[0].label] = tempcluster;
                        front.vertices.push_back(tempcluster);
                        inFile >> output;
                    }

                    while(output!=";;"){
                        cout << "ended vertices" << endl;
                        inFile >> output;
                        if(output=="Edges"){
                            cout << "entered edges " <<endl;

                            while(output!=";;"){
                                inFile >> output;
                                string a1 = output;
                                inFile >> output;
                                string a2 = output;
                                ClusteredPoint c1 = sampmap[a1];
                                ClusteredPoint c2 = sampmap[a2];
                                Edge2D thisedge2d;
                                thisedge2d.cp1 = c1;
                                thisedge2d.cp2 = c2;
                                front.edges.push_back(thisedge2d);
                                inFile >> output;
                            }
                            cout << "edge size is " << front.edges.size() << endl;
                        }
                    }
                }
                //end doc
            }
            inFile >> output;
            if(output=="Side"){

                cout << "entered Side" <<endl;
                inFile >> output;

                map<string, ClusteredPoint> sampmap;
                if(output=="Vertices"){
                    cout << "entered Vertices" << endl;
                    inFile >> output;
                    while(output!=";"){
                        double n1 = atof(output.c_str());
                        // cout << n1 << endl;
                        inFile >> output;
                        double n2 = atof(output.c_str());
                        inFile >> output;
                        vector<string> thisstr;
                        inFile >> output;
                        while(output!="]"){
                            // inFile >> output;
                            thisstr.push_back(output);
                            inFile >> output;
                        }
                        ClusteredPoint tempcluster;
                        for(int i=0;i<thisstr.size();i++){
                            Point tempoint;
                            tempoint.x = 0;
                            tempoint.y = n1 ;
                            tempoint.z = n2 ;
                            tempoint.label = thisstr[i];
                            cout << tempoint.label << endl;
                            cout << tempoint << endl;
                            tempcluster.points.push_back(tempoint);
                        }
                        sampmap[tempcluster.points[0].label] = tempcluster;
                        side.vertices.push_back(tempcluster);
                        inFile >> output;
                    }

                    while(output!=";;"){
                        cout << "ended vertices" << endl;
                        inFile >> output;
                        if(output=="Edges"){
                            cout << "entered edges " <<endl;

                            while(output!=";;"){
                                inFile >> output;
                                string a1 = output;
                                inFile >> output;
                                string a2 = output;
                                ClusteredPoint c1 = sampmap[a1];
                                ClusteredPoint c2 = sampmap[a2];
                                Edge2D thisedge2d;
                                thisedge2d.cp1 = c1;
                                thisedge2d.cp2 = c2;
                                side.edges.push_back(thisedge2d);
                                inFile >> output;
                            }
                            cout << "edge size is " << side.edges.size() << endl;
                            inFile >> output;
                        }
                    }
                }
            //end doc
            }
		}
        cout << "lets start the computation now "<< endl;
        // we have top front side
        Wireframe frame;
        Projection2D myproj;
        myproj.frontview = front;
        myproj.sideview = side;
        myproj.topview = top;
        frame = myproj.create3D();
        Wireframe* retFrame;
        retFrame = new Wireframe;
        for(int i = 0; i < frame.vertices.size(); i++) {
            retFrame->vertices.push_back(frame.vertices[i]);
        }
        for(int i = 0; i < frame.edges.size(); i++) {
            Edge edge;
            edge.p1.setCoordinates(frame.edges[i].p1.x,frame.edges[i].p1.y,frame.edges[i].p1.z);
            edge.p2.setCoordinates(frame.edges[i].p2.x,frame.edges[i].p2.y,frame.edges[i].p2.z);
            retFrame->edges.push_back(edge);
        }
        // cout << "retFrame: " << *retFrame << endl;
        retFrame->normalise();
        // cout << "retFrame after normalise: " << endl << *retFrame << endl;
        return retFrame;
    }
    return 0;
}

PlaneProjection* createObject(Object3D* object, double plane[4]) {
    ///
    /// This function shall make use of the gtk library, to interactively take input from the user and returns the 3D object created from the user input
    ///
    cout << "In createObject" << endl; // ----------Remove
    Object3D temp_obj;
    for(int i = 0; i < object->vertices.size(); i++)
        temp_obj.vertices.push_back(object->vertices[i]);
    for(int i = 0; i < object->edges.size(); i++)
        temp_obj.edges.push_back(object->edges[i]);
    for(int i = 0; i < object->faces.size(); i++)
        temp_obj.faces.push_back(object->faces[i]);
    cout << "Input Object:" << endl << temp_obj;
    for(int i = 0; i < 4; i++)
        cout << plane[i];
    PlaneProjection* res = new PlaneProjection;
    res = temp_obj.project3D(plane);
    cout << "Object Returned: " << endl; // --------Remove
    cout << *res << endl; // --------Remove
    res->rotatePlane();
    cout << "Plane Rotated:" << endl; // ---------Remove
    cout << *res << endl; // ---------Remove
    return res;
}

Wireframe* createProjection(Projection2D* projection) {
    ///
    /// This function shall make use of the gtk library, to interactively take input from the user, to form a 2D projection and returns the Projection created from the user input
    ///
    cout << "In createProjection" << endl; // ----------Remove
    Wireframe newFrame;
    newFrame = projection->create3D();
    Wireframe* retFrame;
    retFrame = new Wireframe;
    for(int i = 0; i < newFrame.vertices.size(); i++) {
        retFrame->vertices.push_back(newFrame.vertices[i]);
    }
    for(int i = 0; i < newFrame.edges.size(); i++) {
        Edge edge;
        edge.p1.setCoordinates(newFrame.edges[i].p1.x,newFrame.edges[i].p1.y,newFrame.edges[i].p1.z);
        edge.p2.setCoordinates(newFrame.edges[i].p2.x,newFrame.edges[i].p2.y,newFrame.edges[i].p2.z);
        retFrame->edges.push_back(edge);
    }
    // cout << "retFrame: " << *retFrame << endl;
    retFrame->normalise();
    // cout << "retFrame after normalise: " << endl << *retFrame << endl;
    return retFrame;
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:40:16

#ifndef HSSUSY_INPUT_PARAMETERS_H
#define HSSUSY_INPUT_PARAMETERS_H

#include <complex>
#include <Eigen/Core>

namespace flexiblesusy {

struct HSSUSY_input_parameters {
   double MSUSY;
   double M1Input;
   double M2Input;
   double M3Input;
   double MuInput;
   double mAInput;
   double MEWSB;
   double AtInput;
   double TanBeta;
   double LambdaLoopOrder;
   Eigen::Matrix<double,3,3> msq2;
   Eigen::Matrix<double,3,3> msu2;
   Eigen::Matrix<double,3,3> msd2;
   Eigen::Matrix<double,3,3> msl2;
   Eigen::Matrix<double,3,3> mse2;

   HSSUSY_input_parameters()
      : MSUSY(0), M1Input(0), M2Input(0), M3Input(0), MuInput(0), mAInput(0),
   MEWSB(0), AtInput(0), TanBeta(0), LambdaLoopOrder(0), msq2(Eigen::Matrix<
   double,3,3>::Zero()), msu2(Eigen::Matrix<double,3,3>::Zero()), msd2(
   Eigen::Matrix<double,3,3>::Zero()), msl2(Eigen::Matrix<double,3,3>::Zero()),
   mse2(Eigen::Matrix<double,3,3>::Zero())

   {}

   Eigen::ArrayXd get() const;
   void set(const Eigen::ArrayXd&);
};

std::ostream& operator<<(std::ostream&, const HSSUSY_input_parameters&);

} // namespace flexiblesusy

#endif