#include <deal.II/base/function.h>
#include <deal.II/base/function_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/symmetric_tensor.h>
#include <deal.II/base/tensor_function.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/solver_bicgstab.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_raviart_thomas.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

#include <deal.II/lac/sparse_direct.h>
#include <deal.II/base/timer.h>
#include <deal.II/grid/grid_in.h>

#include <cmath>
#include <fstream>
#include <iostream>

#include "json.hpp"

using namespace dealii;
using Json = nlohmann::json;

template <int dim>
class THMConsolidation
{
public:
  THMConsolidation(const unsigned degree, const nlohmann::json json);
  void run();

private:
  void make_grid();
  void setup_system();
  void assemble_system();
  void solve(unsigned step);
  void output_results(unsigned int step, unsigned int current_time) const;

  Triangulation<dim>        triangulation_;
  FESystem<dim>             fe_;
  DoFHandler<dim>           dof_handler_;
  //AffineConstraints<double> constraints_;

/*
  BlockSparsityPattern      sparsity_pattern_;
  BlockSparseMatrix<double> system_matrix_;
  BlockVector<double> solution_;
  BlockVector<double> system_rhs_;
*/
  SparsityPattern      sparsity_pattern_;
  SparseMatrix<double> system_matrix_;
  SparseMatrix<double> system_matrix_k_;
  SparseMatrix<double> system_matrix_c_;
  Vector<double> solution_;
  Vector<double> old_solution_;
  Vector<double> system_rhs_;
  Vector<double> solution_var_;

  unsigned         degree_;
  // double           model_length_;
  // double           height_over_width_;
  double            inner_radius_;
  double            outer_radius_;
  double            subdivision_;
  double            max_steps_;
  double           time_step_;
  double           current_time_;
  double           k_in_;
  double           k_T_in_;
  double           alpha_in;
  double           lambda_in_;
  double           mu_in_;
  double           pressure_in_;
  double           porosity_in_;
  double           density_in_;
  double           alpha_sw_in_;
  double           h_cap_sw_in_;
  double           step_torr_;
  double           first_step_;
  double           Q_in_;
  const double     gamma_w=1;
};

// For Dirichlet boundary conditions
// Deformation Dirichlet boundary conditions
template <int dim>
class DeformationDirichletBoundary : public Function<dim>
{
public:
  DeformationDirichletBoundary() : Function<dim>(dim + 2) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_geometry(const double inner, const double outer) {inner_radius_ = inner; outer_radius_ = outer;}
private:
  double inner_radius_;
  double outer_radius_;
};

template <int dim>
void DeformationDirichletBoundary<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  // if (p[2] == 0) {
  //   for (unsigned int c = 0; c < 3; ++c) {
  //     if (c==2){
  //       values(c) = 0;
  //     }
  //     else {
  //       values(c) = 0;
  //     }

  //   }
  //   //values(0) = 11; values(1) = 21; values(2) = 51;
  // } else if (p[0] == 0 || p[1] == 0 || std::abs(p[0]-model_length_) < 1e-12 || std::abs(p[1]-model_length_) < 1e-12) {
  //   for (unsigned int c = 0; c < 2; ++c) values(c) = 0;
  //   //values(0) = 31; values(1) = 41;
  // if (std::abs(std::pow(p[0],2)+std::pow(p[1],2)+std::pow(p[2],2)-std::pow(inner_radius_,2))<=1e-5)
  // {
  //   values(0)=0; values(1)=0;values(2)=0;
  // }
   values(0)=0; values(1)=0;values(2)=0;
  // if (std::abs(std::pow(p[0],2)+std::pow(p[1],2)+std::pow(p[2],2)-std::pow(outer_radius_,2))==0)
  // {
  //   values(0)=0; values(1)=0;values(2)=0;
  // }

}

// Pressure dirichlet boundary
template <int dim>
class PressureDirichletBoundary : public Function<dim>
{
public:
  PressureDirichletBoundary() : Function<dim>(dim +2) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_geometry(const double inner, const double outer) {inner_radius_ = inner; outer_radius_ = outer;}
private:
  double inner_radius_;
  double outer_radius_;
};

template <int dim>
void PressureDirichletBoundary<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  // if (std::abs(p[2] - model_length_*height_over_width_) < 1e-12) values(3) = 0;
  // // if (std::abs(p[2] - 0.0) < 1e-12) values(3) = 0.1;
  // if (std::abs(std::pow(p[0],2)+std::pow(p[1],2)+std::pow(p[2],2)-std::pow(outer_radius_,2))<=1e-5)
  // {
  //   values(3)=0;
  // }
  values(3)=0;
}

// Temperature dirichlet boundary
template <int dim>
class TemperatureDirichletBoundary : public Function<dim>
{
public:
  TemperatureDirichletBoundary() : Function<dim>(dim + 1) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_geometry(const double inner, const double outer) {inner_radius_ = inner; outer_radius_ = outer;}
private:
  double inner_radius_;
  double outer_radius_;
};

template <int dim>
void TemperatureDirichletBoundary<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{

  // if (std::abs(std::pow(p[0],2)+std::pow(p[1],2)+std::pow(p[2],2)-std::pow(outer_radius_,2))<=1e-5)
  // {
  //   values(4)=0;
  // }
   values(4)=0;
  // if (std::abs(std::pow(p[0],2)+std::pow(p[1],2)+std::pow(p[2],2)-std::pow(inner_radius_,2))<=1e-3)
  // {
  //   values(4)=0;
  // }
}

template <int dim>
class DeformationNeumannBoundary : public TensorFunction<1,dim>
{
public:
  DeformationNeumannBoundary() : TensorFunction<1,dim>() {}
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<Tensor<1,dim>> & values) const override;
  void get_pressure(const double pressure) {pressure_ = pressure; }
private:
  double pressure_;
};

template <int dim>
void DeformationNeumannBoundary<dim>::value_list(const std::vector<Point<dim>> &points,
                                                 std::vector<Tensor<1,dim>> & values) const
{
  (void)points;
  AssertDimension(points.size(), values.size());
  // for(auto &value : values) {
  //   value[0] = 0; value[1] = 0; value[2] = -pressure_;
  // }
}
// Pressure Neumann boundary
template <int dim>
class PressureNeumannBoundary : public TensorFunction<1,dim>
{
public:
  PressureNeumannBoundary() : TensorFunction<1,dim>() {}
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<Tensor<1,dim>> & values) const override;
  void get_pressure_grad(const Tensor<1,dim> pressure_grad) {pressure_grad_ = pressure_grad; }
private:
  Tensor<1,dim> pressure_grad_;
};

template <int dim>
void PressureNeumannBoundary<dim>::value_list(const std::vector<Point<dim>> &points,
                                                 std::vector<Tensor<1,dim>> & values) const
{
  (void)points;
  AssertDimension(points.size(), values.size());
  for(auto &value : values) {
    value[0] = 0; value[1] = 0; value[2] = 0;
  }
}
// Temperature Neumann boundary
// template <int dim>
// class TemperatureNeumannBoundary : public TensorFunction<1,dim>
// {
// public:
//   TemperatureNeumannBoundary() : TensorFunction<1,dim>() {}
//   virtual void value_list(const std::vector<Point<dim>> &points,
//                           std::vector<Tensor<1,dim>> & values) const override;
//   void get_Q(const double Q) {Q_ = Q; }
// private:
//   double Q_;
// };
// template <int dim>
// void TemperatureNeumannBoundary<dim>::value_list(const std::vector<Point<dim>> &points,
//                                                  std::vector<Tensor<1,dim>> & values) const
// {
//   (void) points;
//   for(auto &value : values) {
//     ;
//   }
// }
template <int dim>
class TemperatureNeumannBoundary : public Function<dim>
{
public:
  TemperatureNeumannBoundary() : Function<dim>(dim + 2) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_Q(const double Q) {Q_ = Q; }
private:
  double Q_;
};

template <int dim>
void TemperatureNeumannBoundary<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  (void)p;
  for(auto &value : values) {
     value=Q_;
  }
}

// For initial conditions
template <int dim>
class DeformationInitial : public Function<dim>
{
public:
  DeformationInitial() : Function<dim>(dim + 2) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
};

template <int dim>
void DeformationInitial<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  (void)p;
  for (unsigned int c = 0; c < this->n_components; ++c)
    values(c) = 0.0;
}

template <int dim>
class PressureInitial : public Function<dim>
{
public:
  PressureInitial() : Function<dim>(dim + 2) {}
  virtual void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_pressure(const double pressure) {pressure_ = pressure; }
private:
  double pressure_;
};

template <int dim>
void PressureInitial<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  (void)p;
  values(3) = 0;
}

// Initial temperature
template <int dim>
class TemperatureInitial : public Function<dim>
{
public:
  TemperatureInitial() : Function<dim>(dim + 2) {}
  void vector_value(const Point<dim> &p, Vector<double> &  value) const override;
  void get_temperature(const double temperature) {temperature_ = temperature; }
private:
  double temperature_;
};

template <int dim>
void TemperatureInitial<dim>::vector_value(const Point<dim> &p, Vector<double> &  values) const
{
  (void)p;
  values(4) = 0;
}

// Initial porosity
template <int dim>
class PorosityInitial : public Function<dim>
{
public:
  PorosityInitial() : Function<dim>(1) {}
  double value(const Point<dim> &p, const unsigned int component=0) const override;
  void get_porosity(const double porosity) {porosity_ = porosity; }
private:
  double porosity_;
};

template <int dim>
double PorosityInitial<dim>::value(const Point<dim> &p, const unsigned int component) const
{
  return porosity_;
}

template <int dim>
THMConsolidation<dim>::THMConsolidation(const unsigned degree, const nlohmann::json json)
  : degree_(degree)
  , fe_(FE_Q<dim>(degree+1), dim, FE_Q<dim>(degree), 1, FE_Q<dim>(degree), 1)
  , dof_handler_(triangulation_)
{
  // model_length_ = json["parameters"]["model_length"].template get<double>();
  // height_over_width_ = json["parameters"]["height_over_width"].template get<double>();
  inner_radius_ = json["parameters"]["inner_radius"].template get<unsigned>();
  outer_radius_ = json["parameters"]["outer_radius"].template get<unsigned>();
  subdivision_ = json["parameters"]["subdivision"].template get<unsigned>();
  max_steps_ = json["parameters"]["max_steps"].template get<unsigned>();
  time_step_ = json["parameters"]["time_step"].template get<double>();
  // pressure_in_ = json["parameters"]["loading_on_top"].template get<double>();
  k_in_ = json["parameters"]["k"].template get<double>();
  k_T_in_ = json["parameters"]["k_T"].template get<double>();
  alpha_in = json["parameters"]["alpha"].template get<double>();
  lambda_in_ = json["parameters"]["lambda"].template get<double>();
  mu_in_ = json["parameters"]["mu"].template get<double>();
  porosity_in_ = json["parameters"]["porosity"].template get<double>();
  density_in_ = json["parameters"]["density"].template get<double>();
  Q_in_=json["parameters"]["Q"].template get<double>();
  alpha_sw_in_=json["parameters"]["alpha_sw"].template get<double>();
  h_cap_sw_in_=json["parameters"]["h_cap_sw"].template get<double>();
  std::cout<<"inner_radius: "<<inner_radius_<<", outer_radius: "<<outer_radius_<<std::endl
           <<"Subdivision: "<<subdivision_<<std::endl
           <<"Total steps: "<<max_steps_<<std::endl<<"Time step: "<<time_step_<<std::endl
           <<"k: "<<k_in_<<std::endl
           <<"lambda: "<<lambda_in_<<", mu: "<<mu_in_<<std::endl
           <<"k_T: "<<k_T_in_<<", Q_in: "<<Q_in_<<std::endl;
}

template <int dim>
void THMConsolidation<dim>::make_grid()
{
  //Tensor<1,dim> TP1;
  //TP1(0)=0;TP1(1)=0;TP1(2)=0;
  //Tensor<1,dim> TP2;
  //TP2(0)=model_length_;TP2(1)=model_length_;TP2(2)=model_length_;
  const Point<dim,double> grid_p1(0.0, 0.0, 0.0);
  // const Point<dim,double> grid_p2(model_length_, model_length_, model_length_ * height_over_width_);
  // const std::vector<unsigned int> repetitions{subdivision_, subdivision_, subdivision_ * height_over_width_};
   GridGenerator::hyper_shell(triangulation_,grid_p1, inner_radius_, outer_radius_,0);
  // GridIn<dim> gridin;
  // gridin.attach_triangulation(triangulation_);
  // std::ifstream f("void_sphere3.msh");
  // gridin.read_msh(f);

  //triangulation_.refine_global(2);
  for (unsigned int i=0; i<=subdivision_; i++)
  {
    std::cout<<(float(i)/float(subdivision_))*inner_radius_+(1-float(i)/float(subdivision_))*outer_radius_<<std::endl;
    int j=0;
    int j0=0;
    for (const auto &cell : triangulation_.active_cell_iterators())
  {
    cell->clear_refine_flag();
       j0=j0+1;
    if (grid_p1.distance(cell->center()) <= (float(i)/float(subdivision_))*inner_radius_+(1-float(i)/float(subdivision_))*outer_radius_)
    {
      // std::cout<<grid_p1.distance(cell->center())<<std::endl;
      j=j+1;
      cell->set_refine_flag();

    }
  }
    triangulation_.execute_coarsening_and_refinement();
    std::cout<<i<<" refine loop "<<j0<<" all "<<j<<"refined"<<std::endl;
  }

  // triangulation_.refine_global(subdivision_);

  // Set boundary indicator for applying boundary conditions
  // -x:1, +x:2, -y:3, +y:4, -z:5, +z:6
  // if (dim == 3) {
  //   for (const auto &cell : triangulation_.active_cell_iterators()) {
  //     for (const auto &face : cell->face_iterators()) {
  //       if (face->center()[0] == 0) face->set_all_boundary_ids(1);
  //       else if (std::abs(face->center()[0] - model_length_) < 1e-12) face->set_all_boundary_ids(2);
  //     }
  //   }
  //   for (const auto &cell : triangulation_.active_cell_iterators()) {
  //     for (const auto &face : cell->face_iterators()) {
  //       if (face->center()[1] == 0) face->set_all_boundary_ids(3);
  //       else if (std::abs(face->center()[1] - model_length_) < 1e-12) face->set_all_boundary_ids(4);
  //     }
  //   }
  //   for (const auto &cell : triangulation_.active_cell_iterators()) {
  //     for (const auto &face : cell->face_iterators()) {
  //       if (face->center()[2] == 0) face->set_all_boundary_ids(5);
  //       else if (std::abs(face->center()[2] - model_length_*height_over_width_) < 1e-12) face->set_all_boundary_ids(6);
  //     }
  //   }
  // }
  // Set boundary indicator for applying boundary conditions
  // inner:1 outer 2
  if (dim==3)
  {
    double k1=0;
    double k2=0;
    int i=0;
    int j=0;
    for (const auto &cell : triangulation_.active_cell_iterators())
    {
      i++;
      j=0;
      for (const auto &face : cell->face_iterators())
      {
        j++;
        if (std::abs(
          std::pow(face->center()[0],2)+
          std::pow(face->center()[1],2)+
          std::pow(face->center()[2],2)
          - std::pow(inner_radius_,2)) < 0.1)
          {
            face->set_all_boundary_ids(1);
            k1++;
            //std::cout<<i<<" id1 "<<j<<std::endl;
          }
        else if (std::abs(
          std::pow(face->center()[0],2)+
          std::pow(face->center()[1],2)+
          std::pow(face->center()[2],2)
          - std::pow(outer_radius_,2)) <10)
          {
            face->set_all_boundary_ids(2);
            k2++;
            // std::cout<<"bc2: "<<face->center()[0]<<","<<face->center()[1]<<","<<face->center()[2]<<std::endl;
            //std::cout<<i<<" id2 "<<j<<std::endl;
          }
          else
          {
            face->set_all_boundary_ids(3);
          }

      }
    }
    std::cout<<"k1 "<<k1<<",k2 "<<k2<<std::endl;
  }

  dof_handler_.distribute_dofs(fe_);

  // Re-numner dof, make velocities and pressures are not intermingled
  DoFRenumbering::component_wise(dof_handler_);

/*
  // Set constraints to apply Dirichlet boundary conditions
  {
    constraints_.clear();

    DeformationDirichletBoundary<dim> deformation_dirichlet;
    deformation_dirichlet.get_model_length_and_ratio(model_length_, height_over_width_);
    PressureDirichletBoundary<dim> pressure_dirichlet;
    pressure_dirichlet.get_model_length_and_ratio(model_length_, height_over_width_);

    FEValuesExtractors::Vector deformations_bottom(0);
    FEValuesExtractors::Scalar pressure(dim);
    std::vector<bool> deformations_side{true, true, false, false};
    DoFTools::make_hanging_node_constraints(dof_handler_, constraints_);
    for (int i = 1; i <= 4; ++i) {
      VectorTools::interpolate_boundary_values(dof_handler_,
                                               i,
                                               deformation_dirichlet,
                                               constraints_,
                                               fe_.component_mask(deformations_side));
    }
    VectorTools::interpolate_boundary_values(dof_handler_,
                                             5,
                                             deformation_dirichlet,
                                             constraints_,
                                             fe_.component_mask(deformations_bottom));
     VectorTools::interpolate_boundary_values(dof_handler_,
                                              6,
                                              pressure_dirichlet,
                                              constraints_,
                                              fe_.component_mask(pressure));
//
          // VectorTools::interpolate_boundary_values(dof_handler_,
          //                                      5,
          //                                      pressure_dirichlet,
          //                                      constraints_,
          //                                      fe_.component_mask(pressure));

  }
  constraints_.close();
*/

  // Count the number of velocity and pressure dofs
  const std::vector<types::global_dof_index> dofs_per_component =
    DoFTools::count_dofs_per_fe_component(dof_handler_);
  unsigned int n_u = 0;
  for(int i = 0; i < dim; ++i) n_u += dofs_per_component[i];
  const unsigned int n_p = dofs_per_component[dim];
  const unsigned int n_T = dofs_per_component[dim+1];

  std::cout << "Number of active cells: " << triangulation_.n_active_cells()<< std::endl
            << "Total number of cells: " << triangulation_.n_cells()<< std::endl
            << "Number of degrees of freedom: " << dof_handler_.n_dofs()
            << " (" << n_u << '+' << n_p << "+"<<n_T<<')' << std::endl;

  // Allocate sparsity patterns
/*
  BlockDynamicSparsityPattern dsp(2, 2);
  dsp.block(0, 0).reinit(n_u, n_u);
  dsp.block(1, 0).reinit(n_p, n_u);
  dsp.block(0, 1).reinit(n_u, n_p);
             +
  DoFTools::make_sparsity_pattern(dof_handler_, dsp, constraints_, false);

  sparsity_pattern_.copy_from(dsp);
  system_matrix_.reinit(sparsity_pattern_);

  // Resize the solution and right hand side vectors
  solution_.reinit(2);
  solution_.block(0).reinit(n_u);
  solution_.block(1).reinit(n_p);
  solution_.collect_sizes();
  system_rhs_.reinit(2);
  system_rhs_.block(0).reinit(n_u);
  system_rhs_.block(1).reinit(n_p);
  system_rhs_.collect_sizes();
*/
  DynamicSparsityPattern dsp(n_u+n_p+n_T, n_u+n_p+n_T);
  DoFTools::make_sparsity_pattern(dof_handler_, dsp/*, constraints_, false*/);
  sparsity_pattern_.copy_from(dsp);
  system_matrix_.reinit(sparsity_pattern_);
  system_matrix_=0;
  system_matrix_k_.reinit(sparsity_pattern_);
  system_matrix_k_=0;
  system_matrix_c_.reinit(sparsity_pattern_);
  system_matrix_c_=0;
  solution_.reinit(n_u+n_p+n_T);
  solution_ = 0;
  system_rhs_.reinit(n_u+n_p+n_T);
  system_rhs_=0;
}

// Assemble matrix system
template <int dim>
void THMConsolidation<dim>::assemble_system()
{
  const std::vector<size_t> deformation_idx{0, 1, 2};
  const std::vector<size_t> pressure_idx{3};
  const std::vector<size_t> temperature_idx{4};

  system_matrix_ = 0;
  system_matrix_k_ = 0;
  system_matrix_c_ = 0;
  system_rhs_    = 0;

  QGauss<dim>     quadrature_formula(degree_+1 );
  QGauss<dim - 1> face_quadrature_formula(degree_+1 );

  FEValues<dim>     fe_values(fe_,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);
  FEFaceValues<dim> fe_face_values(fe_,
                                   face_quadrature_formula,
                                   update_values | update_normal_vectors |
                                     update_quadrature_points |
                                     update_JxW_values);

  const unsigned int dofs_per_cell = fe_.dofs_per_cell;

  const unsigned int n_q_points      = quadrature_formula.size();
  const unsigned int n_face_q_points = face_quadrature_formula.size();

  // Initialize local matrices
  FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
  FullMatrix<double> local_matrix_k(dofs_per_cell, dofs_per_cell);
  FullMatrix<double> local_matrix_c(dofs_per_cell, dofs_per_cell);
  Vector<double>     local_rhs(dofs_per_cell);

  // Initialize local old solution containers
  std::vector<Vector<double>> old_solution_values(n_q_points,
                                                  Vector<double>(dim+2));
  //std::vector<Vector<double>> old_solution_values_face(n_face_q_points,
                                                       //Vector<double>(dim + 1));

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

  // Initialize defined functions for boundary values
  DeformationNeumannBoundary<dim> deformation_neumann_boundary;
  TemperatureNeumannBoundary<dim> temperature_neumann_boundary;
  temperature_neumann_boundary.get_Q(Q_in_);
  // deformation_neumann_boundary.get_pressure(pressure_in_);
  //PressureNeumannBoundary<dim> pressure_neumann_boundary;
  //GravityValues<dim> gravity_boundary_values;
  // Constant parameters
  std::vector<double> lambda_values(n_q_points);
  std::vector<double> mu_values(n_q_points);
  std::vector<double> k_values(n_q_points);
  std::vector<double> k_T_values(n_q_points);
  std::vector<double> alpha_values(n_q_points);
  std::vector<double> alpha_sw_values(n_q_points);
  std::vector<double> porosity_values(n_q_points);
  std::vector<double> h_cap_w_values(n_q_points);
  std::vector<double> h_cap_sw_values(n_q_points);
  std::vector<double> density_values(n_q_points);
  std::vector<double> pressure_values(n_q_points);

  Functions::ConstantFunction<dim> lambda(lambda_in_), mu(mu_in_), k(k_in_), alpha(alpha_in), k_T(k_T_in_),
   porosity(porosity_in_),density(density_in_),alpha_sw(alpha_sw_in_),h_cap_sw(h_cap_sw_in_);

  // std::vector<Tensor<1, dim>>         deformation_neumann_boundary_values(n_face_q_points);
  std::vector<double>         temperature_neumann_boundary_values(n_face_q_points);

  //std::vector<Tensor<1, dim>> boundary_g_values(n_q_points);
  //std::vector<Tensor<1, dim>> pressure_neumann_boundary_values(n_q_points);
  std::vector<double>         boundary_k_T_values(n_face_q_points);
  const FEValuesExtractors::Vector deformations(0);
  const FEValuesExtractors::Scalar pressure(dim);
  const FEValuesExtractors::Scalar temperature(dim+1);
  // std::cout<<"cell_iterators started"<<std::endl;

  for (const auto &cell : dof_handler_.active_cell_iterators()) {
    fe_values.reinit(cell);
    local_matrix = 0;
    local_matrix_k = 0;
    local_matrix_c = 0;
    local_rhs    = 0;

    lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
    mu.value_list(fe_values.get_quadrature_points(), mu_values);
    k.value_list(fe_values.get_quadrature_points(), k_values);
    k_T.value_list(fe_values.get_quadrature_points(), k_T_values);
    alpha.value_list(fe_values.get_quadrature_points(), alpha_values);
    porosity.value_list(fe_values.get_quadrature_points(), porosity_values);
    density.value_list(fe_values.get_quadrature_points(), density_values);
    alpha_sw.value_list(fe_values.get_quadrature_points(), alpha_sw_values);
    h_cap_sw.value_list(fe_values.get_quadrature_points(), h_cap_sw_values);

    // Get old solution value at Gauss points
    fe_values.get_function_values(solution_, old_solution_values);
    // std::cout<<"intergation"<<std::endl;

    // std::cout<<"intergation"<<std::endl;

    for (unsigned int q = 0; q < n_q_points; ++q) {
      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
        // std::cout<<"i index"<<std::endl;
        const unsigned int component_i = fe_.system_to_component_index(i).first;

        const Tensor<1, dim> phi_i_u = fe_values[deformations].value(i, q);
        const Tensor<2, dim> grad_phi_i_u = fe_values[deformations].gradient(i, q);
        const SymmetricTensor<2,dim> symmgrad_phi_i_u = fe_values[deformations].symmetric_gradient(i, q);
        const double phi_i_p     = fe_values[pressure].value(i, q);
        const double phi_i_T     = fe_values[temperature].value(i, q);
                // std::cout<<"break"<<std::endl;
        const Tensor<1, dim> grad_phi_i_p = fe_values[pressure].gradient(i, q);
        const Tensor<1, dim> grad_phi_i_T = fe_values[temperature].gradient(i, q);
        const double div_phi_i_u=fe_values[deformations].divergence(i,q);

        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
          // std::cout<<"j index"<<std::endl;
          const unsigned int component_j =  fe_.system_to_component_index(j).first;
          const Tensor<1, dim> phi_j_u = fe_values[deformations].value(j, q);
          //const Tensor<2, dim> grad_phi_j_u = fe_values[deformations].gradient(j, q);
          const SymmetricTensor<2,dim> symmgrad_phi_j_u = fe_values[deformations].symmetric_gradient(j, q);
          const double phi_j_p     = fe_values[pressure].value(j, q);
          const double phi_j_T     = fe_values[temperature].value(j, q);
          const Tensor<1, dim> grad_phi_j_p = fe_values[pressure].gradient(j, q);
          const Tensor<1, dim> grad_phi_j_T = fe_values[temperature].gradient(j, q);
          const double div_phi_j_u=fe_values[deformations].divergence(j,q);

          Tensor<1, dim> sub_solution_deformation;
          double sub_solution_pressure;
          for(auto id : deformation_idx) sub_solution_deformation[id] = old_solution_values[q](id);
          sub_solution_pressure = old_solution_values[q](dim);
/*
          local_matrix(i, j) +=
             (
             (lambda_values[q] * div_phi_i_u * div_phi_j_u)
             +
             (2 * mu_values[q] *symmgrad_phi_i_u * symmgrad_phi_j_u)
             +
             (-div_phi_i_u*phi_j_p)
             +
             (grad_phi_i_p * k_values[q] * grad_phi_j_p)/gamma_w
             +
             ( phi_i_p*div_phi_j_u/(time_step_) )
             )*fe_values.JxW(q);
*/

          local_matrix_k(i, j) +=
             (
             (lambda_values[q] * div_phi_i_u * div_phi_j_u)
             +
             (2 * mu_values[q] *symmgrad_phi_i_u * symmgrad_phi_j_u) //K_uu
             +
             (+div_phi_i_u*phi_j_p) //Q_up
             +
             (-grad_phi_i_p * k_values[q] * grad_phi_j_p)/gamma_w //K_pp
             +
             (alpha_values[q]*div_phi_i_u*phi_j_T) //Q_uT
             +
             (k_T_values[q]*grad_phi_i_T*grad_phi_j_T) //K_tt0
             //+
             //(k_T_values[q]*h_cap_w_values[q]/gamma_w*phi_i_T*pressure_values[q]*grad_phi_j_p*grad_phi_j_T) // K_tt1
             )*fe_values.JxW(q);

          local_matrix_c(i, j) +=
             (
             ( phi_i_p*div_phi_j_u ) //R_pu
             +
             //(porosity_values[q]*alpha_values[q]*phi_i_p*phi_j_T)//R_pT Rui's formulation
             (alpha_sw_values[q]*phi_i_p*phi_j_T) //R_pT booker's formulation
             +
             (density_values[q]*h_cap_sw_values[q]*phi_i_T*phi_j_T) //C_TT
             )*fe_values.JxW(q)/(time_step_/* std::pow(10,step-1)*/);
            //  if (local_matrix_c(i, j)!=0)
            //  {
            //    std::cout<<local_matrix_c(i, j)<<" C"<<local_matrix_k(i,j)<<" K  "<<std::endl;

            //  }
/*
          local_rhs(i) +=
           (
             phi_i_p * div_phi_j_u / (time_step_ )
           )
           *
            (
              phi_j_u * sub_solution_deformation +k.value_list(fe_values.get_quadrature_points(), k_values);
              phi_j_p * sub_solution_pressure
            )
            * fe_values.JxW(q);        system_matrix_.add(local_dof_indices[i],
                           local_dof_indices[j],
                           local_matrix_c(i, j));
*/
        } // for j

      } // for i

        //std::cout<<"local rhs "<<local_rhs<<std::endl;

    } // for q

    // Apply Neumann boundary conditions
    for (const auto &face : cell->face_iterators()) {
      if (face->boundary_id() == 1) {

        fe_face_values.reinit(cell, face);

        // deformation_neumann_boundary.value_list(fe_face_values.get_quadrature_points(), deformation_neumann_boundary_values);

        temperature_neumann_boundary.value_list(fe_face_values.get_quadrature_points(), temperature_neumann_boundary_values);

        k_T.value_list(fe_face_values.get_quadrature_points(), boundary_k_T_values);
        //gravity_boundary_values.value_list(fe_face_values.get_quadrature_points(), boundary_g_values);

        for (unsigned int q = 0; q < n_face_q_points; ++q) {
          const Tensor<1, dim> nf = fe_face_values.normal_vector(q);

          for (unsigned int i = 0; i < dofs_per_cell; ++i) {
            const unsigned int component_i = fe_.system_to_component_index(i).first;
            const Tensor<1, dim> phi_i_u = fe_face_values[deformations].value(i, q);
            const double phi_i_p = fe_face_values[pressure].value(i, q);
            const double phi_i_T = fe_face_values[temperature].value(i, q);
            const Tensor<1, dim> grad_phi_i_T = fe_values[temperature].gradient(i, q);

            local_rhs(i) +=
             (
              // (phi_i_u * deformation_neumann_boundary_values[q]) //A_u
              +
              // ((grad_phi_i_T *nf)*temperature_neumann_boundary_values[q]/boundary_k_T_values[q])
              (phi_i_T*Q_in_/boundary_k_T_values[q]) //A_T
             ) * fe_face_values.JxW(q);
            //  std::cout<<temperature_neumann_boundary_values[q]<<" BCvalue"<<std::endl;
            //  if (local_rhs(i)!=0){
            //    std::cout<<local_rhs(i)<<" nonzero RHS"<<std::endl;
            //  }
            //  std::cout<<local_rhs(i)<<" RHS"<<std::endl;

          }
        }
      }
    } // for face

    // Assemble local matrix into global
    cell->get_dof_indices(local_dof_indices);
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      for (unsigned int j = 0; j < dofs_per_cell; ++j) {
        system_matrix_.add(local_dof_indices[i],
                           local_dof_indices[j],
                           local_matrix_k(i, j));
        system_matrix_.add(local_dof_indices[i],
                           local_dof_indices[j],
                           local_matrix_c(i, j));
        system_matrix_c_.add(local_dof_indices[i],
                           local_dof_indices[j],
                           local_matrix_c(i, j));
      }
    }
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      system_rhs_(local_dof_indices[i]) += local_rhs(i);
    //constraints_.distribute_local_to_global(local_matrix, local_s, local_dof_indices, system_matrix_, system_rhs_);

  } // for cell
// for (int i=0; i<system_matrix_.n(); ++i)
// {
//   if (system_matrix_(i,i)==0)
//   {
//     std::cout<<system_matrix_(i,i)<<" K "<<std::endl;
//   }
// }

  // rhs += C/t * solution
  system_matrix_c_.vmult_add(system_rhs_, solution_);

  // Apply Dirichlet boundary conditions
  {
    DeformationDirichletBoundary<dim> deformation_dirichlet;
    deformation_dirichlet.get_geometry(inner_radius_, outer_radius_);
    PressureDirichletBoundary<dim> pressure_dirichlet;
    pressure_dirichlet.get_geometry(inner_radius_, outer_radius_);
    TemperatureDirichletBoundary<dim> temperature_dirichlet;
    temperature_dirichlet.get_geometry(inner_radius_, outer_radius_);

//     FEValuesExtractors::Vector deformations_bottom(0);
//     FEValuesExtractors::Scalar pressure(dim);
//     FEValuesExtractors::Scalar temperature(dim);
//     std::vector<bool> deformations_side{true, true, false, false};

//     for (int i = 1; i <= 4; ++i) {
//       std::map<types::global_dof_index, double> deformation_side_values;
//       VectorTools::interpolate_boundary_values(dof_handler_,
//                                                i,
//                                                deformation_dirichlet,
//                                                deformation_side_values,
//                                                fe_.component_mask(deformations_side));
//       MatrixTools::apply_boundary_values(deformation_side_values, system_matrix_, solution_, system_rhs_);
//     }

//     std::map<types::global_dof_index, double> deformation_bottom_values;
//     VectorTools::interpolate_boundary_values(dof_handler_,
//                                              5,
//                                              deformation_dirichlet,
//                                              deformation_bottom_values,
//                                              fe_.component_mask(deformations_bottom));
//     MatrixTools::apply_boundary_values(deformation_bottom_values, system_matrix_, solution_, system_rhs_);

//     std::map<types::global_dof_index, double> pressure_values;
//     VectorTools::interpolate_boundary_values(dof_handler_,
//                                              6,
//                                              pressure_dirichlet,
//                                              pressure_values,
//                                              fe_.component_mask(pressure));
//     MatrixTools::apply_boundary_values(pressure_values, system_matrix_, solution_, system_rhs_);
//   }
     FEValuesExtractors::Vector deformation(0);
     FEValuesExtractors::Scalar pressure(dim);
     FEValuesExtractors::Scalar temperature(dim+1);
     std::map<types::global_dof_index, double> deformation_values;
     VectorTools::interpolate_boundary_values(dof_handler_,
                                             1,
                                             deformation_dirichlet,
                                             deformation_values,
                                             fe_.component_mask(deformation));
        //  VectorTools::interpolate_boundary_values(dof_handler_,
        //                                      2,
        //                                      deformation_dirichlet,
        //                                      deformation_values,
        //                                      fe_.component_mask(deformation));
        //  VectorTools::interpolate_boundary_values(dof_handler_,
        //                                      3,
        //                                      deformation_dirichlet,
        //                                      deformation_values,
        //                                      fe_.component_mask(deformation));
     MatrixTools::apply_boundary_values(deformation_values, system_matrix_, solution_, system_rhs_);
     std::map<types::global_dof_index, double> pressure_values;
     VectorTools::interpolate_boundary_values(dof_handler_,
                                             2,
                                             pressure_dirichlet,
                                             pressure_values,
                                             fe_.component_mask(pressure));
     MatrixTools::apply_boundary_values(pressure_values, system_matrix_, solution_, system_rhs_);
    std::map<types::global_dof_index, double> temperature_values;
     VectorTools::interpolate_boundary_values(dof_handler_,
                                             2,
                                             temperature_dirichlet,
                                             temperature_values,
                                             fe_.component_mask(temperature));
     MatrixTools::apply_boundary_values(temperature_values, system_matrix_, solution_, system_rhs_);
 }
}

// GMRES solver
template <int dim>
void THMConsolidation<dim>::solve(unsigned step)
{
  std::cout<<"solver started"<<std::endl;
  //std::ofstream out("sparsity_pattern.svg");
  //sparsity_pattern_.print_svg(out);
  //  double mxmax=system_matrix_.el(0,0);
  // for(int i=0;  i<system_matrix_.m(); i++){
  //   for(int j=0;  j<system_matrix_.n(); j++){
  //     // std::cout<<i<<" "<<j<<" i j "<<std::endl;
  //     if(mxmax<system_matrix_.el(i,j)){
  //       mxmax=system_matrix_.el(i,j);

  //     }
  //   }
  // }
  // // std::cout<<mxmax<<" mxmax "<<std::endl;
  // // std::cout<<system_matrix_.m()<<" m "<<system_matrix_.n()<<" n "<<std::endl;

  //  double mxcmax=system_matrix_c_.el(0,0);
  // for(int i=0;  i<system_matrix_c_.m(); i++){
  //   for(int j=0;  j<system_matrix_c_.n(); j++){
  //     // std::cout<<i<<" "<<j<<" i j "<<std::endl;
  //     if(mxcmax<system_matrix_c_.el(i,j)){
  //       mxcmax=system_matrix_c_.el(i,j);

  //     }
  //   }
  // }
  // std::cout<<mxcmax<<" mxmax "<<std::endl;

  //  std::ofstream out1("matrix.txt");
  //  system_matrix_.print_as_numpy_arrays(out1);
  //  std::ofstream out2("matrixc.txt");
  //  system_matrix_c_.print_as_numpy_arrays(out2);

if (step > 0)
{ std::cout<<system_rhs_.l2_norm()<<" l2fnorm "<<std::endl   ;
  SolverControl               solver_control(std::max<std::size_t>(10000,
                                                      system_rhs_.size()/100),
                                1e-3* system_rhs_.l2_norm(),true,true);

   SolverGMRES<Vector<double>> solver(solver_control);
  //SolverBicgstab<Vector<double>> solver(solver_control);
   //PreconditionSOR<SparseMatrix<double>> preconditioner;
  // PreconditionIdentity preconditioner;
   //preconditioner.initialize(system_matrix_,1.3);
   PreconditionSSOR<SparseMatrix<double> > preconditioner;
   preconditioner.initialize(system_matrix_, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(.6));
   solver.solve(system_matrix_, solution_, system_rhs_, preconditioner);

   Vector<double> residual(dof_handler_.n_dofs());

   system_matrix_.vmult(residual, solution_);
   residual -= system_rhs_;
   std::cout << "   Iterations required for convergence: "
             << solver_control.last_step() << '\n'
             << "   Max norm of residual:                "
             << residual.linfty_norm() << '\n';
             }

else
{
    SparseDirectUMFPACK A_direct;
  A_direct.initialize(system_matrix_);
  //A_direct.vmult(solution_, system_rhs_);
  A_direct.factorize(system_matrix_);
  A_direct.solve(system_rhs_);
  solution_ = system_rhs_;
}
/*
  // Direct solver

*/
/*
    std::cout
             << "   Max norm of rhs: "
             << system_rhs_.linfty_norm() << '\n' << std::endl;
    std::cout
             << "   Max norm of matrix: "
             << system_matrix_.linfty_norm() << '\n' << std::endl;
    std::cout << "Solving linear system... ";
    Timer timer;
    SparseDirectUMFPACK A_direct;
    A_direct.initialize(system_matrix_);
    A_direct.vmult(solution_, system_rhs_);
    timer.stop();
    std::cout << "done (" << timer.cpu_time() << "s)" << std::endl;
    std::cout
             << "   Max norm of solution: "
             << solution_.linfty_norm() << '\n' << std::endl;
    Vector<double> residual(dof_handler_.n_dofs());
    system_matrix_.vmult(residual, solution_);
    residual -= system_rhs_;
    std::cout
             << "   Max norm of residual: "
             << residual.linfty_norm() << '\n' << std::endl;
*/
  //constraints_.distribute(solution_);
}

// Output results
template <int dim>
void THMConsolidation<dim>::output_results(unsigned int time_step, unsigned current_time) const
{
  std::vector<std::string> solution_names={ "ux", "uy", "uz", "p", "T"};

  // std::vector<DataComponentInterpretation::DataComponentInterpretation>
  //   data_component_interpretation(
  //     dim, DataComponentInterpretation::component_is_part_of_vector);
  // data_component_interpretation.push_back(
  //   DataComponentInterpretation::component_is_scalar);

  DataOut<dim> data_out;
  data_out.attach_dof_handler(dof_handler_);
  data_out.add_data_vector(solution_,
                           solution_names);
  data_out.build_patches();
  std::ofstream time_file ("time.txt",std::ios::app);
  double value       = current_time_;
    //myfile.write (*conversion, strlen (conversion));
  time_file <<value<<"\n";
  time_file.close();

  std::ofstream output(
    "outputfiles/solution-" + Utilities::int_to_string(time_step, 2) + ".vtk");
  data_out.write_vtk(output);
}

template <int dim>
void THMConsolidation<dim>::run()
{
  Timer timer_grid;
  make_grid();

  // Apply initial values
  {
    std::vector<bool> deformations{true, true, true, false, false};
    std::vector<bool> pressure{false, false, false, true, false};
    std::vector<bool> temperature{false, false, false, false, true};

    // PressureInitial<dim> pressure_initial;
    // pressure_initial.get_pressure(pressure_in_);

    VectorTools::interpolate(dof_handler_, DeformationInitial<dim>(),
                             solution_, deformations);
    VectorTools::interpolate(dof_handler_, PressureInitial<dim>(),
                             solution_, pressure);
    VectorTools::interpolate(dof_handler_, TemperatureInitial<dim>(),
                             solution_, temperature);
  }
  std::cout<<"output step 0"<<std::endl;
  output_results(0,0);
  std::cout<<"output step 0 end"<<std::endl;
  timer_grid.stop();
  std::cout << "Initialize mesh and initial conditions: " << timer_grid.cpu_time() << "s" << std::endl;

// // Normal time step
//   first_step_=time_step_;
//   // std::cout<<step_torr_;
//   current_time_=0;
//   double err=Q_in_*std::sqrt(system_rhs_.size())+1;
//   old_solution_=0;
//   for(unsigned step = 1; step<max_steps_ ; ++step) {
//     if (step==1) {
//       time_step_=first_step_;
//     }

//     Timer timer_assembler;
//     std::cout<<"assembler started"<<std::endl;
//     assemble_system();
//     timer_assembler.stop();
//     std::cout << "Step: "<<step<<", assembler: " << timer_assembler.cpu_time() << "s" << std::endl;
//     std::cout << "l2norm of rhs: " << system_rhs_.l2_norm()<<std::endl;

//     Timer timer_solver;
//     solve(step);
//     timer_solver.stop();
//     std::cout << "Step: "<<step<<", solver: " << timer_solver.cpu_time() << "s" << std::endl;
//     solution_var_=old_solution_;
//     solution_var_-=solution_;
//     std::cout<<solution_var_.l2_norm()<<" norm"<<std::endl;
//     std::cout<<time_step_<<" adaptive_time_step"<<std::endl;
//     current_time_+=time_step_;
//     std::cout<<current_time_<<" current_time"<<std::endl;
//     old_solution_=solution_;
//     if (step>1){
//       err=solution_var_.l2_norm();
//     }
//     // std::cout<<err<<" err"<<std::endl;
//     // std::cout<<1e-2*pressure_in_*std::sqrt(system_rhs_.size())<<" err1"<<std::endl;

//     Timer timer_out;
//     output_results(step,current_time_);
//     timer_out.stop();
//     std::cout << "Step: "<<step<<", output vtk files: " << timer_out.cpu_time() << "s" << std::endl;

//   }
// //Normal time step ends

  //Adaptive time step
  first_step_=0.01*time_step_*gamma_w*(inner_radius_/subdivision_)*(inner_radius_/subdivision_)/(6*2*mu_in_*k_in_);
  step_torr_=0.01*Q_in_*std::sqrt(system_rhs_.size());
  // std::cout<<inner_radius_<<" inner"<<std::endl;
  std::cout<<step_torr_<<" step_torr"<<std::endl;
  std::cout<<first_step_<<" frist_step"<<std::endl;
  current_time_=0;
  double err=Q_in_*std::sqrt(system_rhs_.size())+1;
  old_solution_=0;
  //for(unsigned step = 1; err>1e-12*Q_in_*std::sqrt(system_rhs_.size()) ; ++step) {
  for(unsigned step = 1; current_time_< 20000; ++step) {

    if (step==1) {
      time_step_=first_step_;
    }

    Timer timer_assembler;
    std::cout<<"assembler started"<<std::endl;
    assemble_system();
    timer_assembler.stop();
    std::cout << "Step: "<<step<<", assembler: " << timer_assembler.cpu_time() << "s" << std::endl;
    std::cout << "l2norm of rhs: " << system_rhs_.l2_norm()<<std::endl;

    Timer timer_solver;
    solve(step);
    timer_solver.stop();
    std::cout << "Step: "<<step<<", solver: " << timer_solver.cpu_time() << "s" << std::endl;
    solution_var_=old_solution_;
    solution_var_-=solution_;
    std::cout<<solution_var_.l2_norm()<<" norm"<<std::endl;
    std::cout<<time_step_<<" adaptive_time_step"<<std::endl;
    current_time_+=time_step_;
    std::cout<<current_time_<<" current_time"<<std::endl;
    old_solution_=solution_;
    if (step>1){
      err=solution_var_.l2_norm();
    }
    // std::cout<<err<<" err"<<std::endl;
    // std::cout<<1e-2*pressure_in_*std::sqrt(system_rhs_.size())<<" err1"<<std::endl;

    Timer timer_out;
    output_results(step,current_time_);
    timer_out.stop();
    std::cout << "Step: "<<step<<", output vtk files: " << timer_out.cpu_time() << "s" << std::endl;
    std::cout<<"err: "<<err<<std::endl;
    if (err<step_torr_/10){
      time_step_*=1.5;
      std::cout<<"larger step"<<std::endl;
    }
    else if (err>step_torr_*10){
      time_step_/=2;
      std::cout<<"smaller step"<<std::endl;
      if (time_step_<first_step_){
        time_step_=first_step_;
      }
    }
  }

// adaptive time step ends
}

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

  try {

    if (argc != 2) {
      std::cout << "Usage: ./thm /path/to/input.json\n";
      throw std::runtime_error("Incorrect number of input arguments");
    }

    const std::string filename = argv[1];

    // Input file
    std::ifstream in(filename);
    const nlohmann::json input_json = Json::parse(in);

    std::string title = input_json["title"].template get<std::string>();
    unsigned degree = input_json["degree"].template get<unsigned>();
    std::cout<<title<<", with degree: "<<degree<<std::endl;

    THMConsolidation<3> thm_consolidation_solver(degree, input_json);
    thm_consolidation_solver.run();

  } catch (std::exception& exc) {
    std::cerr << std::endl
              << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Exception on processing: " << std::endl
              << exc.what() << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  } catch (...) {
    std::cerr << std::endl
              << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Unknown exception!" << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  }
  return 0;

}


#include <alglib/optimization.h>
#include <functional>
#include <limits>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "perceive/foundation.hpp"

#include "levenberg-marquardt.hpp"

using namespace alglib;

namespace perceive
{
static void opt_fun(const real_1d_array& x, real_1d_array& fi, void* ptr)
{
   using cost_fun_t = std::function<double(const double*)>;
   fi[0] = (static_cast<cost_fun_t*>(ptr))->operator()(x.getcontent());
}

// ---------------------------------------- Levenberg-Marquardt (double version)

void levenberg_marquardt(std::function<double(const double*)> fn,
                         unsigned n,
                         double start[],
                         double xmin[],
                         double reqmin,
                         double diffstep, // 0.0001
                         int kcount,
                         int& icount,
                         int& ifault)
{
   levenberg_marquardt(
       fn, n, start, xmin, reqmin, diffstep, 1, kcount, icount, ifault);
   // real_1d_array x;
   // minlmstate state;
   // minlmreport rep;

   // icount = ifault = 0;

   // // Copy in start point
   // x.setlength(n);
   // for(unsigned i = 0; i < n; ++i)
   //     x[i] = start[i];

   // minlmcreatev(n, 1, x, diffstep, state);           // Initialize state
   // minlmsetcond(state, reqmin, kcount);              // constraints
   // alglib::minlmoptimize(state, opt_fun, NULL, &fn); // Optimize
   // minlmresults(state, x, rep);                      // Get results

   // // Unpack
   // for(unsigned i = 0; i < n; ++i)
   //     xmin[i] = x[i];

   // icount = rep.iterationscount;
   // ifault = rep.terminationtype;
}

// ----------------------------------------- Levenberg-Marquardt (float version)

void levenberg_marquardt(std::function<float(const float*)> fn,
                         unsigned n,
                         float start[],
                         float xmin[],
                         float reqmin,
                         float diffstep, // 0.0001
                         int kcount,
                         int& icount,
                         int& ifault)
{
   std::vector<float> buffer(n);
   std::vector<double> start_d(n);
   std::vector<double> xmin_d(n);

   for(unsigned i = 0; i < n; ++i) start_d[i] = double(start[i]);
   for(unsigned i = 0; i < n; ++i) xmin_d[i] = double(xmin[i]);

   auto cost_fn = [&](const double* X) -> double {
      for(auto& val : buffer) val = float(*X++);
      return double(fn(&buffer[0]));
   };

   levenberg_marquardt(cost_fn,
                       n,
                       &start_d[0],
                       &xmin_d[0],
                       double(reqmin),
                       double(diffstep),
                       kcount,
                       icount,
                       ifault);

   for(unsigned i = 0; i < n; ++i) xmin[i] = float(xmin_d[i]);
}

// --------------------------------------------------------------- With restarts

void levenberg_marquardt(std::function<double(const double*)> fn,
                         unsigned n,
                         double start[],
                         double xmin[],
                         double reqmin,
                         double diffstep,
                         int numres,
                         int kcount,
                         int& icount,
                         int& ifault)
{
   real_1d_array x;
   minlmstate state;
   minlmreport rep;
   unsigned final_count = 0;

   icount = ifault = 0;

   double best_val = std::numeric_limits<double>::max();
   std::function<double(const double*)> fn2z = [&](const double* X) {
      auto val = fn(X);
      if(val < best_val) {
         best_val = val;
         memcpy(&xmin[0], X, sizeof(double) * n);
      }
      return val;
   };

   // Copy in start point
   x.setlength(n);
   for(unsigned i = 0; i < n; ++i) x[i] = start[i];

   try {
      minlmcreatev(n, 1, x, diffstep, state);             // Initialize state
      minlmsetcond(state, reqmin, kcount);                // constraints
      alglib::minlmoptimize(state, opt_fun, NULL, &fn2z); // Optimize
      minlmresults(state, x, rep);                        // Get results

      // Unpack
      icount += rep.iterationscount;
      for(unsigned i = 0; i < n; ++i) xmin[i] = x[i];

      for(int itr = 1; itr < numres && rep.terminationtype == 2; ++itr) {
         // cout << "------------------------------------------------------"
         //      << endl;
         for(unsigned i = 0; i < n; ++i) x[i] = xmin[i];
         diffstep *= 0.1;
         minlmcreatev(n, 1, x, diffstep, state);             // Initialize state
         minlmsetcond(state, reqmin, kcount);                // constraints
         alglib::minlmoptimize(state, opt_fun, NULL, &fn2z); // Optimize
         minlmresults(state, x, rep);
         icount += rep.iterationscount;
         for(unsigned i = 0; i < n; ++i) xmin[i] = x[i];
      }

      ifault = int(rep.terminationtype);
   } catch(alglib::ap_error& e) {
      ifault = -9;
      LOG_ERR(format("Exception in levenberg-marquardt: {}", e.msg));
   } catch(std::exception& e) {
      ifault = -9;
      LOG_ERR(format("Exception in levenberg-marquardt: {}", e.what()));
   } catch(...) {
      ifault = -9;
   }
}

void levenberg_marquardt(std::function<float(const float*)> fn,
                         unsigned n,
                         float start[],
                         float xmin[],
                         float reqmin,
                         float diffstep,
                         int numres, // diffstep *= 0.1 between each restart
                         int kcount,
                         int& icount,
                         int& ifault)
{
   std::vector<float> buffer(n);
   std::vector<double> start_d(n);
   std::vector<double> xmin_d(n);

   for(unsigned i = 0; i < n; ++i) start_d[i] = double(start[i]);
   for(unsigned i = 0; i < n; ++i) xmin_d[i] = double(xmin[i]);

   auto cost_fn = [&](const double* X) -> double {
      for(auto& val : buffer) val = float(*X++);
      return double(fn(&buffer[0]));
   };

   levenberg_marquardt(cost_fn,
                       n,
                       &start_d[0],
                       &xmin_d[0],
                       double(reqmin),
                       double(diffstep),
                       numres,
                       kcount,
                       icount,
                       ifault);

   for(unsigned i = 0; i < n; ++i) xmin[i] = float(xmin_d[i]);
}

// ----------------------------------------------- Levenberg-Marquardt fault-str

const char* levenberg_marquardt_fault_str(int ifault)
{
   switch(ifault) {
   case -9: return "exception";
   case -8: return "NAN/INF value detected either in function or Jacobian";
   case -7: return "derivative correctness check failed";
   case -5: return "inappropriate solver was used";
   case -3: return "constraints are inconsistent";
   case 2: return "relative step is no more than EpsX.";
   case 5: return "MaxIts steps was taken";
   case 7: return "stopping conditions forbid further improvement";
   case 8: return "terminated by  user via MinLMRequestTermination()";
   default: return "Unknown ifault condition";
   }
}

} // namespace perceive


//
// Created by phili on 26.12.2019.
//

#define _USE_MATH_DEFINES
#include <iostream>
#include <Eigen/Dense>
#include <vector>

#include <cmath>

#ifndef M_PI
  #define M_PI 3.14159265358979323846
#endif

using namespace Eigen;

std::vector<double> f(MatrixXd &M, MatrixXd &V, const MatrixXd &D, double rr){
  MatrixXd tt;
  Matrix<bool, Eigen::Dynamic, 1> is_covered;
  std::vector<double> d;

  for (size_t k=0; k<10; k++){
    tt = M.colwise()-V.col(k);
    tt = D*tt;
    is_covered = tt.colwise().squaredNorm().array() <= rr;
    std::cout << is_covered << std::endl;
    for(size_t i=0; i<M.cols(); i++){
      if(is_covered(i)){
        d.push_back(M(0,i));
      }
    }
  }
  return d;
}

int main(){

  MatrixXd M;
  MatrixXd D;
  MatrixXd V;
  std::vector<double> blub;

  M = MatrixXd::Random(10,1000);
  V = MatrixXd::Random(10,10);
  D = MatrixXd::Random(10,10);
  for (double k=0; k<1000; k+=10.){
    blub = f(M,V,D, k);
    for (auto bbb : blub){
      std::cout << bbb << std::endl;
    }
  }
  return 0;
}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <cmath>
#include <typeinfo>
#include <typeindex>
#include <cassert>
#include <unordered_map>
#include <functional>

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

#include <logging.hpp>
#include <matrix_serialization.hpp>
#include <lapack.hpp>

namespace po = boost::program_options;

auto init_options() -> po::options_description
{
	po::options_description desc;

	desc.add_options()
		( "help", "Produce the help message." )
		( "type", po::value<std::string>()->required()
		, "Type of an element of the hamiltonian matrix. It may be one of: "
		  "float, double, cfloat, cdouble." )
		( "energies", po::value<std::string>()->required()
		, "Name of the file where to save the eigenenergies of the system." )
		( "states", po::value<std::string>()->required()
		, "Name of the file where to save the eigenestates of the system." );

	return desc;
}

auto element_type(std::string input) -> std::type_index
{
	using namespace std::string_literals;
	static std::unordered_map<std::string, std::type_index> const types =
		{ { "float"s,   std::type_index(typeid(float))                }
		, { "double"s,  std::type_index(typeid(double))               }
		, { "cfloat"s,  std::type_index(typeid(std::complex<float>))  }
		, { "cdouble"s, std::type_index(typeid(std::complex<double>)) }
		};

	boost::to_lower(input);
	try {
		return types.at(input);
	} catch(std::out_of_range & e) {
		std::cerr << "Invalid element type `" + input + "`!\n";
		throw;
	}
}

template<class _Help, class _Run>
auto process_command_line( int argc, char** argv
                         , _Help&& help
						 , _Run&& run ) -> void
{
	auto const description = init_options();
	po::variables_map vm;

	po::store(po::command_line_parser(argc, argv).options(description).run()
	         , vm );

	if(vm.count("help")) {
		help(description);
		return;
	}

	po::notify(vm);

	run( element_type(vm["type"].as<std::string>())
	   , vm["energies"].as<std::string>()
	   , vm["states"].as<std::string>()
	   );
}

template<class _T, class _IStream, class _OStream1, class _OStream2>
auto solve( _IStream & input
          , _OStream1 & energies_output
		  , _OStream2 & states_output ) -> void
{
	boost::log::sources::severity_logger<tcm::severity_level> lg;

	LOG(lg, info) << "Reading Hamiltonian...";
	tcm::Matrix<_T> H;
	boost::archive::binary_iarchive input_archive{input};
	input_archive >> H;

	tcm::Matrix<tcm::utils::Base<_T>>   E{H.height(), 1};
	tcm::Matrix<_T>                   Psi{H.height(), H.height()};

	LOG(lg, info) << "Diagonalizing...";
	tcm::lapack::heevr(H, E, Psi);

	LOG(lg, info) << "Saving results...";
	boost::archive::binary_oarchive energies_archive{energies_output};
	energies_archive << E;
	boost::archive::binary_oarchive states_archive{states_output};
	states_archive << Psi;

	LOG(lg, info) << "Done!";
}

auto run( std::type_index type
        , std::string const& energies_filename
		, std::string const& states_filename ) -> void
{
	std::ofstream energies_file{energies_filename};
	if(not energies_file) {
		throw std::runtime_error{ "Could not open `" + energies_filename
		                        + "` for writing." };
	}
	std::ofstream states_file{states_filename};
	if(not states_file) {
		throw std::runtime_error{ "Could not open `" + states_filename
		                        + "` for writing." };
	}

	using solve_function_t = std::function<void()>;
	static std::unordered_map<std::type_index, solve_function_t> const
	dispatch = {
		{ std::type_index(typeid(float))
		, [&energies_file, &states_file]()
		  {solve<float>(std::cin, energies_file, states_file); } },
		{ std::type_index(typeid(double))
		, [&energies_file, &states_file]()
		  {solve<double>(std::cin, energies_file, states_file); } },
		{ std::type_index(typeid(std::complex<float>))
		, [&energies_file, &states_file]()
		  {solve<std::complex<float>>(std::cin, energies_file, states_file); } },
		{ std::type_index(typeid(std::complex<double>))
		, [&energies_file, &states_file]()
		  {solve<std::complex<double>>(std::cin, energies_file, states_file);} }
	};

	tcm::setup_console_logging();
	dispatch.at(type)();
}

int main(int argc, char** argv)
{
	process_command_line
		( argc, argv
		, [](auto desc) { std::cout << desc << '\n'; }
		, &run
		);
	return 0;
}


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

  @copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/invlog_10.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/constant/log_2olog_10.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <tuple>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;

    template < class A0 >
    struct logarithm< A0, tag::not_simd_type, double>
    {
      static BOOST_FORCEINLINE void kernel_log(A0 a0,
                                    A0& dk,
                                    A0& hfsq,
                                    A0& s,
                                    A0& r,
                                    A0& f) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0, signed>;
        A0 x;
        i_t k;
        std::tie(x, k) = fast_(frexp)(a0);
        const i_t x_lt_sqrthf = (Sqrt_2o_2<A0>() > x) ? Mone<i_t>() : Zero<i_t>();
        k += x_lt_sqrthf;
        f = dec(x+bitwise_and(x, genmask(x_lt_sqrthf)));
        dk = tofloat(k);
        s = f/(Two<A0>()+f);
        A0 z = sqr(s);
        A0 w = sqr(z);
        A0 t1= w*horn<A0,
                      0x3fd999999997fa04ll,
                      0x3fcc71c51d8e78afll,
                      0x3fc39a09d078c69fll
                      > (w);
        A0 t2= z*horn<A0,
                      0x3fe5555555555593ll,
                      0x3fd2492494229359ll,
                      0x3fc7466496cb03dell,
                      0x3fc2f112df3e5244ll
                      > (w);
        r = t2+t1;
        hfsq = Half<A0>()*sqr(f);
      }

      static BOOST_FORCEINLINE A0 log( A0 a0) BOOST_NOEXCEPT
      {
        // ln(2)hi  =  6.93147180369123816490e-01  or  0x3fe62e42fee00000
        // ln(2)lo  =  1.90821492927058770002e-10  or  0x3dea39ef35793c76
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return  dk*Constant<A0, 0x3fe62e42fee00000ll>()-
          ((hfsq-(s*(hfsq+R)+dk*Constant<A0, 0x3dea39ef35793c76ll>()))-f);
      }

      static BOOST_FORCEINLINE A0 log2(A0 a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
      }

      static BOOST_FORCEINLINE A0 log10(A0 a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return -(hfsq-(s*(hfsq+R))-f)*Invlog_10<A0>()+dk*Log_2olog_10<A0>();
      }
    };

  }
} }
#endif


#pragma once

#include <algorithm>
#include <cassert>
#include <iterator>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/optional.hpp>

#include "vi_algo.hpp"

namespace vi
{
    namespace ea
    {
        namespace nsga2
        {
            bool
            dominates(const std::vector<double>& objective_values_a, const std::vector<double>& objective_values_b)
            {
                assert(objective_values_a.size() == objective_values_b.size());

                auto objective_value_it           = objective_values_a.begin();
                auto other_objective_value_it     = objective_values_b.begin();
                const auto objective_value_end_it = objective_values_a.end();

                while (objective_value_it != objective_value_end_it)
                {
                    if (*other_objective_value_it <= *objective_value_it)
                    {
                        return false;
                    }

                    ++objective_value_it;
                    ++other_objective_value_it;
                }

                return true;
            }

            template <typename genotype_type>
            struct individual
            {
                genotype_type            genotype{};
                std::vector<double>      objective_values{};
                std::vector<individual*> S{};
                double                   crowding_distance{};
                unsigned                 n{};
                unsigned                 rank{};

                individual(genotype_type       genotype,
                           std::vector<double> objective_values)
                    : genotype(std::move(genotype)),
                      objective_values(std::move(objective_values))
                {
                }

                bool
                operator<(const individual& other) const
                {
                    return rank < other.rank || (rank == other.rank && crowding_distance > other.crowding_distance);
                }
            };

            struct options
            {
                double   crossover_rate{};
                double   mutation_rate{};
                unsigned objective_count{};
                unsigned population_size{};
                unsigned tournament_group_size{};
                double   tournament_randomness{};
            };

            template <typename genotype_type,
                      typename genotype_creator_type,
                      typename objective_evaluator_type,
                      typename crossover_operator_type,
                      typename mutation_operator_type>
            struct system
            {
                using individual_type = individual<genotype_type>;

                options                                       system_options;
                genotype_creator_type                         genotype_creator;
                objective_evaluator_type                      objective_evaluator;
                crossover_operator_type                       crossover_operator;
                mutation_operator_type                        mutation_operator;

                unsigned                                      generation;
                std::vector<double>                           range_min;
                std::vector<double>                           range_max;
                std::vector<boost::optional<individual_type>> extreme_min;
                std::vector<boost::optional<individual_type>> extreme_max;

                std::vector<individual_type>                  current_population{};
                std::vector<individual_type>                  next_population{};
                std::vector<individual_type>                  offspring{};
                std::vector<std::vector<individual_type*>>    fronts;

                std::bernoulli_distribution                   crossover_distribution;
                std::bernoulli_distribution                   mutation_distribution;
                std::set<unsigned>                            tournament_group{};
                std::bernoulli_distribution                   tournament_select_best_distribution;

                template <typename random_generator_type>
                system(random_generator_type&   random_generator,
                       const options&           system_options,
                       genotype_creator_type    genotype_creator,
                       objective_evaluator_type objective_evaluator,
                       crossover_operator_type  crossover_operator,
                       mutation_operator_type   mutation_operator)
                    : system_options{system_options},
                      genotype_creator{std::move(genotype_creator)},
                      objective_evaluator{std::move(objective_evaluator)},
                      crossover_operator{std::move(crossover_operator)},
                      mutation_operator{std::move(mutation_operator)},
                      generation{0},
                      range_min(system_options.objective_count,   +std::numeric_limits<double>::infinity()),
                      range_max(system_options.objective_count,   -std::numeric_limits<double>::infinity()),
                      extreme_min(system_options.objective_count),
                      extreme_max(system_options.objective_count),
                      fronts(system_options.population_size),
                      crossover_distribution{system_options.crossover_rate},
                      mutation_distribution{system_options.mutation_rate},
                      tournament_select_best_distribution{1.0 - system_options.tournament_randomness}
                {
                    current_population.reserve(system_options.population_size);
                    next_population.reserve(system_options.population_size);
                    offspring.reserve(system_options.population_size);
                    create_initial_population(random_generator);
                }

                template <typename random_generator_type>
                void
                create_initial_population(random_generator_type& random_generator)
                {
                    current_population.clear();

                    for (auto& front : fronts)
                    {
                        front.clear();
                    }

                    for (auto i = system_options.population_size; i != 0; --i)
                    {
                        auto genotype         = genotype_creator(random_generator);
                        auto objective_values = objective_evaluator(genotype);
                        current_population.emplace_back(std::move(genotype), std::move(objective_values));
                    }
                }

                void
                assign_crowding_distances(
                    std::vector<individual_type*>& front, const bool is_non_dominated_front)
                {
                    for (auto individual : front)
                    {
                        individual->crowding_distance = 0.0;
                    }

                    for (unsigned objective = 0; objective != system_options.objective_count; ++objective)
                    {
                        std::sort(front.begin(), front.end(),
                            [objective](individual_type* a, individual_type* b)
                            {
                                return a->objective_values[objective] < b->objective_values[objective];
                            });

                        const auto min_individual = *front.begin();
                        const auto max_individual = *front.rbegin();

                        min_individual->crowding_distance = std::numeric_limits<double>::infinity();
                        max_individual->crowding_distance = std::numeric_limits<double>::infinity();

                        range_min[objective] = std::min(range_min[objective], min_individual->objective_values[objective]);
                        range_max[objective] = std::max(range_max[objective], max_individual->objective_values[objective]);

                        if (is_non_dominated_front)
                        {
                            extreme_min[objective] = *min_individual;
                        }

                        if ((!extreme_max[objective]) or
                            (max_individual->objective_values[objective] > extreme_max[objective]->objective_values[objective]))
                        {
                            extreme_max[objective] = *max_individual;
                        }

                        const auto range_delta   = range_max[objective] - range_min[objective];
                        const auto range_scaling = range_delta != 0.0 ? 1.0 / range_delta : 1.0;

                        for (typename std::vector<individual_type*>::size_type i = 1; i < front.size() - 1; ++i)
                        {
                            front[i]->crowding_distance += range_scaling *
                                (front[i + 1]->objective_values[objective] -
                                 front[i - 1]->objective_values[objective]);
                        }
                    }
                }

                template <typename random_generator_type>
                void
                evolve(random_generator_type& random_generator)
                {
                    for (auto& entry : extreme_max)
                    {
                        entry = boost::none;
                    }

                    offspring.clear();
                    generate_individuals(random_generator);

                    std::move(offspring.begin(), offspring.end(), std::back_inserter(current_population));
                    fast_non_dominated_sort();

                    next_population.clear();
                    auto remaining = system_options.population_size;
                    auto first = true;

                    for (auto& front : fronts)
                    {
                        if (remaining > 0)
                        {
                            assign_crowding_distances(front, first);
                            first = false;

                            const auto front_size = static_cast<unsigned>(front.size());

                            if (front_size <= remaining)
                            {
                                for (auto individual : front)
                                {
                                    next_population.emplace_back(std::move(*individual));
                                }

                                remaining -= front_size;
                            }
                            else
                            {
                                // Shuffle to avoid order bias from crowding_distance_assignment
                                std::shuffle(front.begin(), front.end(), random_generator);
                                std::sort(front.begin(), front.end(),
                                    [](const auto a, const auto b)
                                    {
                                        return (*a) < (*b);
                                    });

                                for (auto individual = front.begin(); individual != front.begin() + remaining; ++individual)
                                {
                                    next_population.emplace_back(std::move(**individual));
                                }

                                remaining = 0;
                            }
                        }
                        else
                        {
                            if (!front.empty())
                            {
                                front.clear();
                            }
                            else
                            {
                                break;
                            }
                        }
                    }

                    current_population.clear();
                    std::swap(current_population, next_population);
                    ++generation;
                }

                void
                fast_non_dominated_sort()
                {
                    for (auto& front : fronts)
                    {
                        front.clear();
                    }

                    for (auto& individual : current_population)
                    {
                        individual.S.clear();
                        individual.n = 0U;
                    }

                    const auto end = current_population.end();

                    for (auto p = current_population.begin(); p != end; ++p)
                    {
                        for (auto q = p + 1; q != end; ++q)
                        {
                            if (dominates(p->objective_values, q->objective_values))
                            {
                                p->S.push_back(&*q);
                                ++q->n;
                            }
                            else if (dominates(q->objective_values, p->objective_values))
                            {
                                q->S.push_back(&*p);
                                ++p->n;
                            }
                        }

                        if (p->n == 0)
                        {
                            p->rank = 0;
                            fronts[0].push_back(&*p);
                        }
                    }

                    for (auto front_index = 0U; !fronts[front_index].empty(); ++front_index)
                    {
                        for (auto p : fronts[front_index])
                        {
                            for (auto q : p->S)
                            {
                                if (--q->n == 0)
                                {
                                    q->rank = front_index + 1U;
                                    fronts[front_index + 1].push_back(&*q);
                                }
                            }
                        }
                    }
                }

                template <typename random_generator_type>
                void
                generate_individuals(random_generator_type& random_generator)
                {
                    while (offspring.size() < system_options.population_size)
                    {
                        const auto parent_a = tournament_selector(random_generator, current_population);
                        const auto parent_b = tournament_selector(random_generator, current_population);

                        genotype_type child_a_genotype{};
                        genotype_type child_b_genotype{};

                        if (crossover_distribution(random_generator))
                        {
                            std::tie(child_a_genotype, child_b_genotype) = crossover_operator(
                                random_generator, parent_a->genotype, parent_b->genotype);
                        }
                        else
                        {
                            child_a_genotype = parent_a->genotype;
                            child_b_genotype = parent_b->genotype;
                        }

                        if (mutation_distribution(random_generator))
                        {
                            mutation_operator(random_generator, child_a_genotype);
                        }

                        if (mutation_distribution(random_generator))
                        {
                            mutation_operator(random_generator, child_b_genotype);
                        }

                        auto child_a_objective_values = objective_evaluator(child_a_genotype);
                        auto child_b_objective_values = objective_evaluator(child_b_genotype);

                        offspring.emplace_back(std::move(child_a_genotype), std::move(child_a_objective_values));
                        offspring.emplace_back(std::move(child_b_genotype), std::move(child_b_objective_values));
                    }
                }

                template <typename random_generator_type>
                const individual_type*
                tournament_selector(random_generator_type&                       random_generator,
                                    const typename std::vector<individual_type>& population)
                {
                    const bool select_best_individual = tournament_select_best_distribution(random_generator);

                    if (select_best_individual)
                    {
                        ::vi::algo::generate_unique_in_range(
                            random_generator,
                            tournament_group,
                            0U,
                            system_options.population_size - 1U,
                            system_options.tournament_group_size);

                        const individual_type* selected_individual = nullptr;

                        for (const auto index : tournament_group)
                        {
                            const auto& individual = population[index];
                            if (!selected_individual or (individual < (*selected_individual)))
                            {
                                selected_individual = &individual;
                            }
                        }

                        return selected_individual;
                    }
                    else
                    {
                        const auto random_individual_index = std::uniform_int_distribution<unsigned>{
                            0U, static_cast<unsigned>(population.size()) - 1U}(random_generator);
                        return &(population[random_individual_index]);
                    }
                }
            };

            template <typename genotype_type,
                      typename random_generator_type,
                      typename genotype_creator_type,
                      typename objective_evaluator_type,
                      typename crossover_operator_type,
                      typename mutation_operator_type>
            auto
            build_system(random_generator_type&   random_generator,
                         const options&           system_options,
                         genotype_creator_type    genotype_creator,
                         objective_evaluator_type objective_evaluator,
                         crossover_operator_type  crossover_operator,
                         mutation_operator_type   mutation_operator)
            {
                using system_type = system<genotype_type,
                                           genotype_creator_type,
                                           objective_evaluator_type,
                                           crossover_operator_type,
                                           mutation_operator_type>;

                return system_type{random_generator,
                                   system_options,
                                   std::move(genotype_creator),
                                   std::move(objective_evaluator),
                                   std::move(crossover_operator),
                                   std::move(mutation_operator)};
            }
        }
    }
}


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

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <exception>
#include <vector>

using namespace std;

typedef boost::adjacency_list< boost::vecS, // OutEdgeList
    boost::vecS, // VertexList
    boost::undirectedS, // Directed
    boost::no_property, // VertexProperties
    boost::no_property, // EdgeProperties
    boost::no_property, // GraphProperties
    boost::listS // EdgeList
    >
    Graph;

typedef boost::graph_traits< Graph >::vertex_descriptor vertex_descriptor;

typedef boost::graph_traits< Graph >::edge_descriptor edge_descriptor;

typedef boost::graph_traits< Graph >::vertex_iterator vertex_iterator;

typedef boost::graph_traits< Graph >::edge_iterator edge_iterator;

int main(int argc, char** argv)
{
    Graph iG, vG;
    vector< edge_descriptor > iG_o;
    vector< edge_descriptor > vG_o;

    iG_o.push_back(boost::add_edge(0, 1, iG).first);
    iG_o.push_back(boost::add_edge(0, 2, iG).first);
    iG_o.push_back(boost::add_edge(0, 3, iG).first);
    iG_o.push_back(boost::add_edge(0, 4, iG).first);
    iG_o.push_back(boost::add_edge(1, 2, iG).first);
    iG_o.push_back(boost::add_edge(3, 4, iG).first);

    vG_o.push_back(boost::add_edge(1, 2, vG).first);
    vG_o.push_back(boost::add_edge(2, 0, vG).first);
    vG_o.push_back(boost::add_edge(2, 3, vG).first);
    vG_o.push_back(boost::add_edge(4, 3, vG).first);
    vG_o.push_back(boost::add_edge(0, 3, vG).first);
    vG_o.push_back(boost::add_edge(0, 4, vG).first);

    vector< bool > inL(iG_o.size(), false);

    std::vector< std::vector< bool > > coll;
    boost::tree_collector< std::vector< std::vector< bool > >,
        std::vector< bool > >
        tree_collector(coll);
    boost::two_graphs_common_spanning_trees(
        iG, iG_o, vG, vG_o, tree_collector, inL);

    std::vector< std::vector< bool > >::iterator it;
    for (it = coll.begin(); it != coll.end(); ++it)
    {
        // Here you can play with the trees that the algorithm has found.
    }

    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>

using namespace dealii;

//! Generate a hypercube, and output it as an svg file.
void first_grid(Triangulation<2> &triangulation)
{
  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

  std::ofstream out("grid-1.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.svg" << std::endl;
}

//! Generate a locally refined hyper_shell, and output it as an svg file.
void second_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  // triangulation.reset_manifold(0);

  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.svg" << std::endl;
}

//! Create an L-shaped domain with one global refinement, and write it on
// `third_grid.vtk`.  Refine the L-shaped mesh adaptively around the re-entrant
// corner three times (after the global refinement you already did), but with a
// twist: refine all cells with the distance between the center of the cell and
// re-entrant corner is smaller than 1/3.
void third_grid(Triangulation<2> &tria)
{
  // Insert code here
}

//! Returns a tuple with number of levels, number of cells, number of active
// cells. Test this with all of  your meshes.
std::tuple<unsigned int, unsigned int, unsigned int>
get_info(const Triangulation<2> &)
{
  // Insert code here
  return std::make_tuple(0, 0, 0);
}

int
main()
{
  {
    Triangulation<2> triangulation;
    first_grid(triangulation);
  }
  {
    Triangulation<2> triangulation;
    second_grid(triangulation);
  }
}


// STL includes.
#include <string>
#include <vector>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <iterator>

// Boost includes.
#include <boost/iterator/function_output_iterator.hpp>

// CGAL includes.
#include <CGAL/Timer.h>
#include <CGAL/Random.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>

#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>

// Type declarations.
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;

using FT       = typename Kernel::FT;
using Point_3  = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;

using Input_range = CGAL::Point_set_3<Point_3>;
using Point_map   = typename Input_range::Point_map;
using Normal_map  = typename Input_range::Vector_map;

using Neighbor_query = CGAL::Shape_detection::Point_set::K_neighbor_query<Kernel, Input_range, Point_map>;
using Region_type    = CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<Kernel, Input_range, Point_map, Normal_map>;
using Region_growing = CGAL::Shape_detection::Region_growing<Input_range, Neighbor_query, Region_type>;

using Indices      = std::vector<std::size_t>;
using Output_range = CGAL::Point_set_3<Point_3>;
using Points_3     = std::vector<Point_3>;

// Define an insert iterator.
struct Insert_point_colored_by_region_index {

  using argument_type = Indices;
  using result_type   = void;

  using Color_map =
  typename Output_range:: template Property_map<unsigned char>;

  const Input_range& m_input_range;
  const   Point_map  m_point_map;
       Output_range& m_output_range;
        std::size_t& m_number_of_regions;

  Color_map m_red, m_green, m_blue;

  Insert_point_colored_by_region_index(
    const Input_range& input_range,
    const   Point_map  point_map,
         Output_range& output_range,
          std::size_t& number_of_regions) :
  m_input_range(input_range),
  m_point_map(point_map),
  m_output_range(output_range),
  m_number_of_regions(number_of_regions) {

    m_red =
    m_output_range.template add_property_map<unsigned char>("red", 0).first;
    m_green =
    m_output_range.template add_property_map<unsigned char>("green", 0).first;
    m_blue =
    m_output_range.template add_property_map<unsigned char>("blue", 0).first;
  }

  result_type operator()(const argument_type& region) {

    CGAL::Random rand(static_cast<unsigned int>(m_number_of_regions));
    const unsigned char r =
    static_cast<unsigned char>(64 + rand.get_int(0, 192));
    const unsigned char g =
    static_cast<unsigned char>(64 + rand.get_int(0, 192));
    const unsigned char b =
    static_cast<unsigned char>(64 + rand.get_int(0, 192));

    for (const std::size_t index : region) {
      const auto& key = *(m_input_range.begin() + index);

      const Point_3& point = get(m_point_map, key);
      const auto it = m_output_range.insert(point);

      m_red[*it]   = r;
      m_green[*it] = g;
      m_blue[*it]  = b;
    }
    ++m_number_of_regions;
  }
}; // Insert_point_colored_by_region_index

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

  std::cout << std::endl <<
    "region_growing_on_point_set_3 example started"
  << std::endl << std::endl;

  std::cout <<
    "Note: if 0 points are loaded, please specify the path to the file data/point_set_3.xyz by hand!"
  << std::endl << std::endl;

  // Load xyz data either from a local folder or a user-provided file.
  std::ifstream in(argc > 1 ? argv[1] : CGAL::data_file_path("points_3/point_set_3.xyz"));
  CGAL::IO::set_ascii_mode(in);

  if (!in) {
    std::cout <<
    "Error: cannot read the file point_set_3.xyz!" << std::endl;
    std::cout <<
    "You can either create a symlink to the data folder or provide this file by hand."
    << std::endl << std::endl;
    return EXIT_FAILURE;
  }

  const bool with_normal_map = true;
  Input_range input_range(with_normal_map);

  in >> input_range;
  in.close();

  std::cout <<
    "* loaded "
  << input_range.size() <<
    " points with normals"
  << std::endl;

  // Default parameter values for the data file point_set_3.xyz.
  const std::size_t k                     = 12;
  const FT          max_distance_to_plane = FT(2);
  const FT          max_accepted_angle    = FT(20);
  const std::size_t min_region_size       = 50;

  // Create instances of the classes Neighbor_query and Region_type.
  Neighbor_query neighbor_query(
    input_range,
    k,
    input_range.point_map());

  Region_type region_type(
    input_range,
    max_distance_to_plane, max_accepted_angle, min_region_size,
    input_range.point_map(), input_range.normal_map());

  // Create an instance of the region growing class.
  Region_growing region_growing(
    input_range, neighbor_query, region_type);

  // Run the algorithm.
  Output_range output_range;
  std::size_t number_of_regions = 0;

  Insert_point_colored_by_region_index inserter(
     input_range, input_range.point_map(),
    output_range, number_of_regions);

  CGAL::Timer timer;

  timer.start();
  region_growing.detect(
    boost::make_function_output_iterator(inserter));
  timer.stop();

  // Print the number of found regions.
  std::cout << "* " << number_of_regions <<
    " regions have been found in " << timer.time() << " seconds"
  << std::endl;

  // Save the result to a file in the user-provided path if any.
  if (argc > 2) {

    const std::string path     = argv[2];
    const std::string fullpath = path + "regions_point_set_3.ply";

    std::ofstream out(fullpath);
    out << output_range;

    std::cout << "* found regions are saved in " << fullpath << std::endl;
    out.close();
  }

  // Get all unassigned items.
  Indices unassigned_items;
  region_growing.unassigned_items(std::back_inserter(unassigned_items));

  // Print the number of unassigned items.
  std::cout << "* " << unassigned_items.size() <<
    " points do not belong to any region"
  << std::endl;

  // Store all unassigned points.
  Points_3 unassigned_points;
  unassigned_points.reserve(unassigned_items.size());

  for (const auto index : unassigned_items) {
    const auto& key = *(input_range.begin() + index);

    const Point_3& point = get(input_range.point_map(), key);
    unassigned_points.push_back(point);
  }

  std::cout << "* " << unassigned_points.size() <<
    " unassigned points are stored"
  << std::endl;

  std::cout << std::endl <<
    "region_growing_on_point_set_3 example finished"
  << std::endl << std::endl;

  return EXIT_SUCCESS;
}