//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@imes.mavt.ethz.ch)
// ETH Zurich, Center of Structure Technologies (www.imes.ethz.ch/st)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

#include <boost/config.hpp>
#include <vector>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/wavefront.hpp>

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

/*
  Sample Output
  #####################################
  ### First light of sloan-ordering ###
  #####################################

  original bandwidth: 8
  original profile: 42
  original max_wavefront: 7
  original aver_wavefront: 4.2
  original rms_wavefront: 4.58258

  Starting vertex: 0
  Pseudoperipheral vertex: 9
  Pseudoperipheral radius: 4

  Sloan ordering starting at: 0
    0 8 3 7 5 2 4 6 1 9
    bandwidth: 4
    profile: 28
    max_wavefront: 4
    aver_wavefront: 2.8
    rms_wavefront: 2.93258

  Sloan ordering without a start-vertex:
    8 0 3 7 5 2 4 6 1 9
    bandwidth: 4
    profile: 27
    max_wavefront: 4
    aver_wavefront: 2.7
    rms_wavefront: 2.84605

  ###############################
  ### sloan-ordering finished ###
  ###############################
*/

int main(int , char* [])
{
  cout << endl;
  cout << "#####################################" << endl;
  cout << "### First light of sloan-ordering ###" << endl;
  cout << "#####################################" << endl << endl;

  using namespace boost;
  using namespace std;

  //Defining the graph type
  typedef adjacency_list<
    setS,
    vecS,
    undirectedS,
    property<
    vertex_color_t,
    default_color_type,
    property<
    vertex_degree_t,
    int,
    property<
    vertex_priority_t,
    double > > > > Graph;

  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef graph_traits<Graph>::vertices_size_type size_type;

  typedef std::pair<std::size_t, std::size_t> Pair;

  Pair edges[14] = { Pair(0,3), //a-d
                     Pair(0,5),  //a-f
                     Pair(1,2),  //b-c
                     Pair(1,4),  //b-e
                     Pair(1,6),  //b-g
                     Pair(1,9),  //b-j
                     Pair(2,3),  //c-d
                     Pair(2,4),  //c-e
                     Pair(3,5),  //d-f
                     Pair(3,8),  //d-i
                     Pair(4,6),  //e-g
                     Pair(5,6),  //f-g
                     Pair(5,7),  //f-h
                     Pair(6,7) }; //g-h

  //Creating a graph and adding the edges from above into it
  Graph G(10);
  for (int i = 0; i < 14; ++i)
    add_edge(edges[i].first, edges[i].second, G);

  //Creating two iterators over the vertices
  graph_traits<Graph>::vertex_iterator ui, ui_end;

  //Creating a property_map with the degrees of the degrees of each vertex
  property_map<Graph,vertex_degree_t>::type deg = get(vertex_degree, G);
  for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
    deg[*ui] = degree(*ui, G);

  //Creating a property_map for the indices of a vertex
  property_map<Graph, vertex_index_t>::type index_map = get(vertex_index, G);

  std::cout << "original bandwidth: " << bandwidth(G) << std::endl;
  std::cout << "original profile: " << profile(G) << std::endl;
  std::cout << "original max_wavefront: " << max_wavefront(G) << std::endl;
  std::cout << "original aver_wavefront: " << aver_wavefront(G) << std::endl;
  std::cout << "original rms_wavefront: " << rms_wavefront(G) << std::endl;

  //Creating a vector of vertices
  std::vector<Vertex> sloan_order(num_vertices(G));
  //Creating a vector of size_type
  std::vector<size_type> perm(num_vertices(G));

  {

    //Setting the start node
    Vertex s = vertex(0, G);
    int ecc;   //defining a variable for the pseudoperipheral radius

    //Calculating the pseudoeperipheral node and radius
    Vertex e = pseudo_peripheral_pair(G, s, ecc, get(vertex_color, G), get(vertex_degree, G) );

    cout << endl;
    cout << "Starting vertex: " << s << endl;
    cout << "Pseudoperipheral vertex: " << e << endl;
    cout << "Pseudoperipheral radius: " << ecc << endl << endl;

    //Sloan ordering
    sloan_ordering(G, s, e, sloan_order.begin(), get(vertex_color, G),
                           get(vertex_degree, G), get(vertex_priority, G));

    cout << "Sloan ordering starting at: " << s << endl;
    cout << "  ";

    for (std::vector<Vertex>::const_iterator i = sloan_order.begin();
         i != sloan_order.end(); ++i)
      cout << index_map[*i] << " ";
    cout << endl;

    for (size_type c = 0; c != sloan_order.size(); ++c)
      perm[index_map[sloan_order[c]]] = c;
    std::cout << "  bandwidth: "
              << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  profile: "
              << profile(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  max_wavefront: "
              << max_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  aver_wavefront: "
              << aver_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  rms_wavefront: "
              << rms_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
  }

    /////////////////////////////////////////////////
    //Version including finding a good starting point
    /////////////////////////////////////////////////

    {
      //sloan_ordering
      sloan_ordering(G, sloan_order.begin(),
                        get(vertex_color, G),
                        make_degree_map(G),
                        get(vertex_priority, G) );

      cout << endl << "Sloan ordering without a start-vertex:" << endl;
      cout << "  ";
      for (std::vector<Vertex>::const_iterator i=sloan_order.begin();
           i != sloan_order.end(); ++i)
        cout << index_map[*i] << " ";
      cout << endl;

      for (size_type c = 0; c != sloan_order.size(); ++c)
        perm[index_map[sloan_order[c]]] = c;
      std::cout << "  bandwidth: "
                << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  profile: "
                << profile(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  max_wavefront: "
                << max_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  aver_wavefront: "
                << aver_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  rms_wavefront: "
                << rms_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
    }

  cout << endl;
  cout << "###############################" << endl;
  cout << "### sloan-ordering finished ###" << endl;
  cout << "###############################" << endl << endl;
  return 0;

}


/**
 * @file orientation.cc
 */
#include <usml/sensors/orientation.h>
#include <boost/numeric/ublas/io.hpp>

using namespace usml::sensors ;

/**
 * Default constructor
 */
orientation::orientation()
    : _heading(0.0), _pitch(0.0), _roll(0.0),
      _axis(3,0)
{

}

/**
 * Pitch, heading, roll constructor
 */
orientation::orientation(
    double heading, double pitch, double roll,
    vector<double> ref_axis )
    : _heading(-pitch*M_PI/180.0),
      _pitch(-heading*M_PI/180.0),
      _roll(roll*M_PI/180.0),
      _axis(ref_axis)
{
    apply_rotation() ;
}

/**
 * Tilt angle/direction constructor
 */
orientation::orientation( double angle, double direction )
    : _heading(0.0), _pitch(0.0), _roll(0.0),
      _axis(3,0)
{
    compute_orientation(angle,direction) ;
}

/**
 * Destructor
 */
orientation::~orientation()
{

}

/**
 * Computes pitch, heading, and roll as a function angle/direction from
 * the vertical reference axis
 */
void orientation::compute_orientation(double angle, double direction)
{
    if( angle <= M_PI_2 ) {
        double sin_theta2 = sin(angle)*sin(angle) ;
        double sin_phi2 = sin(direction)*sin(direction) ;
        double sqrt_theta_phi = std::sqrt(1 - sin_theta2*sin_phi2) ;
        _roll = std::atan2( (cos(angle) / sqrt_theta_phi) , (cos(direction)*sin(angle) / sqrt_theta_phi) ) ;
        _pitch = std::atan2( sqrt_theta_phi, (-sin(angle)*sin(direction)) ) ;
        _heading = 0.0 ;
    } else {
        _pitch = angle ;
        _heading = direction ;
        _roll = 0.0 ;
    }
}

/**
 * Applies a rotation from one coordinate system to the
 * current rotated coordinates for asymmetric systems.
 */
void orientation::apply_rotation(
    double& de, double& az,
    double* de_prime, double* az_prime )
{
    _theta = M_PI_2 - de ;
    _phi = az ;
    _axis(0) = sin(_theta) * cos(_phi) ;
    _axis(1) = sin(_theta) * sin(_phi) ;
    _axis(2) = cos(_theta) ;
    apply_rotation() ;
    *de_prime = M_PI_2 - _theta ;
    *az_prime = _phi ;
}

/**
 * Updates the heading, pitch, and roll
 */
void orientation::update_orientation( double h, double p, double r )
{
   _heading = -h*M_PI/180.0 ;
   _pitch = -p*M_PI/180.0 ;
   _roll = r*M_PI/180.0 ;
   apply_rotation() ;
}

/**
* Updates the tilt angle and direction.
*/
void orientation::update_orientation(double angle, double direction)
{
    compute_orientation(angle,direction) ;
}

/**
 * Applies a rotation from one coordinate system to the
 * current rotated coordinates for symmetric systems.
 */
void orientation::apply_rotation()
{
    _x = _axis(0)*cos(_heading)*cos(_roll) +
         _axis(2)*( sin(_heading)*sin(_pitch) + cos(_heading)*cos(_pitch)*sin(_roll) ) +
         _axis(1)*( -cos(_pitch)*sin(_heading) + cos(_heading)*sin(_pitch)*sin(_roll) ) ;
    _y = _axis(0)*cos(_roll)*sin(_heading) +
         _axis(2)*( -cos(_heading)*sin(_pitch) + cos(_pitch)*sin(_heading)*sin(_roll) ) +
         _axis(1)*( cos(_heading)*cos(_pitch) + sin(_heading)*sin(_pitch)*sin(_roll) ) ;
    _z = _axis(2)*cos(_pitch)*cos(_roll) +
         _axis(1)*cos(_roll)*sin(_pitch) -
         _axis(0)*sin(_roll) ;
    convert_to_spherical() ;
}

/**
 * Converts the store vector in spherical coordinates to
 * cartesian coordinates.
 */
void orientation::convert_to_cartesian()
{
    _x = sin(_theta) * cos(_phi) ;
    _y = sin(_theta) * sin(_phi) ;
    _z = cos(_theta) ;
}

/**
 * Converts the stored vector in cartesian coordinates to
 * spherical coordinates.
 */
void orientation::convert_to_spherical()
{
//    double rho = std::sqrt( _x*_x + _y*_y + _z*_z ) ;
    _theta = std::acos( _z - 1e-10 ) ;
    _theta = std::fmod( _theta, M_PI ) ;
    _phi = std::atan2( _y, _x ) ;
    _phi = std::fmod( _phi, 2.0*M_PI ) ;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2010 - 2020 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 directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Guido Kanschat, Texas A&M University, 2009
 */

// 线性代数的包含文件。一个普通的SparseMatrix，它又将包括SparsityPattern和Vector类的必要文件。

#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/precondition_block.h>
#include <deal.II/lac/block_vector.h>

// 包括用于设置网格的文件

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>

// FiniteElement类和DoFHandler的包含文件。

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_dgp.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/dofs/dof_tools.h>

// 使用MeshWorker框架的包含文件

#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/assembler.h>
#include <deal.II/meshworker/loop.h>

// 与拉普拉斯相关的局部积分器的包含文件

#include <deal.II/integrators/laplace.h>

// 支持多网格方法

#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_matrix.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>

// 最后，我们从库中取出我们的精确解，以及正交和附加工具。

#include <deal.II/base/function_lib.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>

#include <iostream>
#include <fstream>

// deal.II库的所有类都在dealii命名空间中。为了节省打字，我们告诉编译器也要在其中搜索名字。

namespace Step39
{
  using namespace dealii;

// 这是我们用来设置边界值的函数，也是我们比较的精确解。

  Functions::SlitSingularityFunction<2> exact_solution;
// @sect3{The local integrators}

// MeshWorker将局部积分与单元格和面的循环分离开来。因此，我们必须编写局部积分类来生成矩阵、右手边和误差估计器。

// 所有这些类都有相同的三个函数，分别用于对单元、边界面和内部面的积分。局部积分所需的所有信息都由 MeshWorker::IntegrationInfo<dim>. 提供。请注意，函数的签名不能改变，因为它是由 MeshWorker::integration_loop(). 所期望的。

// 第一个定义局部积分器的类负责计算单元和面矩阵。它被用来组装全局矩阵以及水平矩阵。

  template <int dim>
  class MatrixIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &                 dinfo,
              typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void
         boundary(MeshWorker::DoFInfo<dim> &                 dinfo,
                  typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void face(MeshWorker::DoFInfo<dim> &                 dinfo1,
              MeshWorker::DoFInfo<dim> &                 dinfo2,
              typename MeshWorker::IntegrationInfo<dim> &info1,
              typename MeshWorker::IntegrationInfo<dim> &info2) const override;
  };

// 在每个单元上，我们对Dirichlet形式进行积分。我们使用LocalIntegrators中的现成积分库来避免自己编写这些循环。同样地，我们实现了Nitsche边界条件和单元间的内部惩罚通量。

// 边界和通量项需要一个惩罚参数，这个参数应该根据单元的大小和多项式的度数来调整。在 LocalIntegrators::Laplace::compute_penalty() 中可以找到关于这个参数的安全选择，我们在下面使用这个参数。

  template <int dim>
  void MatrixIntegrator<dim>::cell(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    LocalIntegrators::Laplace::cell_matrix(dinfo.matrix(0, false).matrix,
                                           info.fe_values());
  }

  template <int dim>
  void MatrixIntegrator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const unsigned int degree = info.fe_values(0).get_fe().tensor_degree();
    LocalIntegrators::Laplace::nitsche_matrix(
      dinfo.matrix(0, false).matrix,
      info.fe_values(0),
      LocalIntegrators::Laplace::compute_penalty(dinfo, dinfo, degree, degree));
  }

// 内部面使用内部惩罚方法

  template <int dim>
  void MatrixIntegrator<dim>::face(
    MeshWorker::DoFInfo<dim> &                 dinfo1,
    MeshWorker::DoFInfo<dim> &                 dinfo2,
    typename MeshWorker::IntegrationInfo<dim> &info1,
    typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const unsigned int degree = info1.fe_values(0).get_fe().tensor_degree();
    LocalIntegrators::Laplace::ip_matrix(
      dinfo1.matrix(0, false).matrix,
      dinfo1.matrix(0, true).matrix,
      dinfo2.matrix(0, true).matrix,
      dinfo2.matrix(0, false).matrix,
      info1.fe_values(0),
      info2.fe_values(0),
      LocalIntegrators::Laplace::compute_penalty(
        dinfo1, dinfo2, degree, degree));
  }

// 第二个局部积分器建立了右手边。在我们的例子中，右手边的函数为零，这样，这里只设置了弱形式的边界条件。

  template <int dim>
  class RHSIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &                 dinfo,
              typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void
         boundary(MeshWorker::DoFInfo<dim> &                 dinfo,
                  typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void face(MeshWorker::DoFInfo<dim> &                 dinfo1,
              MeshWorker::DoFInfo<dim> &                 dinfo2,
              typename MeshWorker::IntegrationInfo<dim> &info1,
              typename MeshWorker::IntegrationInfo<dim> &info2) const override;
  };

  template <int dim>
  void
  RHSIntegrator<dim>::cell(MeshWorker::DoFInfo<dim> &,
                           typename MeshWorker::IntegrationInfo<dim> &) const
  {}

  template <int dim>
  void RHSIntegrator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe           = info.fe_values();
    Vector<double> &         local_vector = dinfo.vector(0).block(0);

    std::vector<double> boundary_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), boundary_values);

    const unsigned int degree = fe.get_fe().tensor_degree();
    const double penalty = 2. * degree * (degree + 1) * dinfo.face->measure() /
                           dinfo.cell->measure();

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      for (unsigned int i = 0; i < fe.dofs_per_cell; ++i)
        local_vector(i) +=
          (-penalty * fe.shape_value(i, k)              // (-sigma * v_i(x_k)
           + fe.normal_vector(k) * fe.shape_grad(i, k)) // + n * grad v_i(x_k))
          * boundary_values[k] * fe.JxW(k);             // u^D(x_k) * dx
  }

  template <int dim>
  void
  RHSIntegrator<dim>::face(MeshWorker::DoFInfo<dim> &,
                           MeshWorker::DoFInfo<dim> &,
                           typename MeshWorker::IntegrationInfo<dim> &,
                           typename MeshWorker::IntegrationInfo<dim> &) const
  {}

//第三个局部积分器负责对误差估计的贡献。这是由Karakashian和Pascal（2003）提出的标准能量估计器。

  template <int dim>
  class Estimator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &                 dinfo,
              typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void
         boundary(MeshWorker::DoFInfo<dim> &                 dinfo,
                  typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void face(MeshWorker::DoFInfo<dim> &                 dinfo1,
              MeshWorker::DoFInfo<dim> &                 dinfo2,
              typename MeshWorker::IntegrationInfo<dim> &info1,
              typename MeshWorker::IntegrationInfo<dim> &info2) const override;
  };

// 单元的贡献是离散解的拉普拉斯，因为右手边是零。

  template <int dim>
  void
  Estimator<dim>::cell(MeshWorker::DoFInfo<dim> &                 dinfo,
                       typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    const std::vector<Tensor<2, dim>> &DDuh = info.hessians[0][0];
    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        const double t = dinfo.cell->diameter() * trace(DDuh[k]);
        dinfo.value(0) += t * t * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

// 在边界，我们简单地使用边界残差的加权形式，即有限元解和正确边界条件之间的差值的规范。

  template <int dim>
  void Estimator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    std::vector<double> boundary_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), boundary_values);

    const std::vector<double> &uh = info.values[0][0];

    const unsigned int degree = fe.get_fe().tensor_degree();
    const double penalty = 2. * degree * (degree + 1) * dinfo.face->measure() /
                           dinfo.cell->measure();

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        const double diff = boundary_values[k] - uh[k];
        dinfo.value(0) += penalty * diff * diff * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

// 最后，在内部面，估计器由解的跳跃和它的法向导数组成，并进行适当的加权。

  template <int dim>
  void
  Estimator<dim>::face(MeshWorker::DoFInfo<dim> &                 dinfo1,
                       MeshWorker::DoFInfo<dim> &                 dinfo2,
                       typename MeshWorker::IntegrationInfo<dim> &info1,
                       typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const FEValuesBase<dim> &          fe   = info1.fe_values();
    const std::vector<double> &        uh1  = info1.values[0][0];
    const std::vector<double> &        uh2  = info2.values[0][0];
    const std::vector<Tensor<1, dim>> &Duh1 = info1.gradients[0][0];
    const std::vector<Tensor<1, dim>> &Duh2 = info2.gradients[0][0];

    const unsigned int degree = fe.get_fe().tensor_degree();
    const double       penalty1 =
      degree * (degree + 1) * dinfo1.face->measure() / dinfo1.cell->measure();
    const double penalty2 =
      degree * (degree + 1) * dinfo2.face->measure() / dinfo2.cell->measure();
    const double penalty = penalty1 + penalty2;
    const double h       = dinfo1.face->measure();

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        const double diff1 = uh1[k] - uh2[k];
        const double diff2 =
          fe.normal_vector(k) * Duh1[k] - fe.normal_vector(k) * Duh2[k];
        dinfo1.value(0) +=
          (penalty * diff1 * diff1 + h * diff2 * diff2) * fe.JxW(k);
      }
    dinfo1.value(0) = std::sqrt(dinfo1.value(0));
    dinfo2.value(0) = dinfo1.value(0);
  }

// 最后我们有一个误差的积分器。由于不连续Galerkin问题的能量准则不仅涉及到单元内部的梯度差，还涉及到跨面和边界的跳跃项，所以我们不能仅仅使用  VectorTools::integrate_difference().  而是使用MeshWorker接口来自己计算误差。

//有几种不同的方法来定义这个能量准则，但是所有的方法都是随着网格大小的变化而等价的（有些不是随着多项式程度的变化而等价）。这里，我们选择
// @f[ \|u\|_{1,h} =
//  \sum_{K\in \mathbb T_h} \|\nabla u\|_K^2 + \sum_{F \in F_h^i}
//  4\sigma_F\|\average{ u \mathbf n}\|^2_F + \sum_{F \in F_h^b}
//  2\sigma_F\|u\|^2_F
//  @f]

  template <int dim>
  class ErrorIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &                 dinfo,
              typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void
         boundary(MeshWorker::DoFInfo<dim> &                 dinfo,
                  typename MeshWorker::IntegrationInfo<dim> &info) const override;
    void face(MeshWorker::DoFInfo<dim> &                 dinfo1,
              MeshWorker::DoFInfo<dim> &                 dinfo2,
              typename MeshWorker::IntegrationInfo<dim> &info1,
              typename MeshWorker::IntegrationInfo<dim> &info2) const override;
  };

// 这里我们有关于单元格的集成。目前MeshWorker中还没有很好的接口可以让我们访问正交点中的正则函数值。因此，我们必须在单元格积分器中创建精确函数值和梯度的向量。之后，一切照旧，我们只需将差值的平方加起来。

// 除了计算能量准则的误差，我们还利用网格工作者的能力同时计算两个函数并在同一个循环中计算L2的误差。很明显，这个函数没有任何跳跃项，只出现在单元格的积分中。

  template <int dim>
  void ErrorIntegrator<dim>::cell(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &   fe = info.fe_values();
    std::vector<Tensor<1, dim>> exact_gradients(fe.n_quadrature_points);
    std::vector<double>         exact_values(fe.n_quadrature_points);

    exact_solution.gradient_list(fe.get_quadrature_points(), exact_gradients);
    exact_solution.value_list(fe.get_quadrature_points(), exact_values);

    const std::vector<Tensor<1, dim>> &Duh = info.gradients[0][0];
    const std::vector<double> &        uh  = info.values[0][0];

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        double sum = 0;
        for (unsigned int d = 0; d < dim; ++d)
          {
            const double diff = exact_gradients[k][d] - Duh[k][d];
            sum += diff * diff;
          }
        const double diff = exact_values[k] - uh[k];
        dinfo.value(0) += sum * fe.JxW(k);
        dinfo.value(1) += diff * diff * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
    dinfo.value(1) = std::sqrt(dinfo.value(1));
  }

  template <int dim>
  void ErrorIntegrator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &                 dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    std::vector<double> exact_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), exact_values);

    const std::vector<double> &uh = info.values[0][0];

    const unsigned int degree = fe.get_fe().tensor_degree();
    const double penalty = 2. * degree * (degree + 1) * dinfo.face->measure() /
                           dinfo.cell->measure();

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        const double diff = exact_values[k] - uh[k];
        dinfo.value(0) += penalty * diff * diff * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

  template <int dim>
  void ErrorIntegrator<dim>::face(
    MeshWorker::DoFInfo<dim> &                 dinfo1,
    MeshWorker::DoFInfo<dim> &                 dinfo2,
    typename MeshWorker::IntegrationInfo<dim> &info1,
    typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const FEValuesBase<dim> &  fe  = info1.fe_values();
    const std::vector<double> &uh1 = info1.values[0][0];
    const std::vector<double> &uh2 = info2.values[0][0];

    const unsigned int degree = fe.get_fe().tensor_degree();
    const double       penalty1 =
      degree * (degree + 1) * dinfo1.face->measure() / dinfo1.cell->measure();
    const double penalty2 =
      degree * (degree + 1) * dinfo2.face->measure() / dinfo2.cell->measure();
    const double penalty = penalty1 + penalty2;

    for (unsigned k = 0; k < fe.n_quadrature_points; ++k)
      {
        const double diff = uh1[k] - uh2[k];
        dinfo1.value(0) += (penalty * diff * diff) * fe.JxW(k);
      }
    dinfo1.value(0) = std::sqrt(dinfo1.value(0));
    dinfo2.value(0) = dinfo1.value(0);
  }

//  @sect3{The main class}

// 这个类做主要的工作，就像前面的例子一样。关于这里声明的函数的描述，请参考下面的实现。

  template <int dim>
  class InteriorPenaltyProblem
  {
  public:
    using CellInfo = MeshWorker::IntegrationInfo<dim>;

    InteriorPenaltyProblem(const FiniteElement<dim> &fe);

    void run(unsigned int n_steps);

  private:
    void   setup_system();
    void   assemble_matrix();
    void   assemble_mg_matrix();
    void   assemble_right_hand_side();
    void   error();
    double estimate();
    void   solve();
    void   output_results(const unsigned int cycle) const;

// 与离散化有关的成员对象在这里。

    Triangulation<dim>        triangulation;
    const MappingQ1<dim>      mapping;
    const FiniteElement<dim> &fe;
    DoFHandler<dim>           dof_handler;

// 然后，我们有与全局离散系统相关的矩阵和向量。

    SparsityPattern      sparsity;
    SparseMatrix<double> matrix;
    Vector<double>       solution;
    Vector<double>       right_hand_side;
    BlockVector<double>  estimates;

// 最后，我们有一组与多级预处理程序相关的稀疏模式和稀疏矩阵。 首先，我们有一个水平矩阵和它的稀疏性模式。

    MGLevelObject<SparsityPattern>      mg_sparsity;
    MGLevelObject<SparseMatrix<double>> mg_matrix;

// 当我们在局部细化的网格上进行局部平滑的多重网格时，需要额外的矩阵；见Kanschat（2004）。这里是这些边缘矩阵的稀疏性模式。我们只需要一个，因为上矩阵的模式是下矩阵的转置。实际上，我们并不太关心这些细节，因为MeshWorker正在填充这些矩阵。

    MGLevelObject<SparsityPattern> mg_sparsity_dg_interface;

// 精细化边缘的通量矩阵，将精细级自由度与粗略级自由度相耦合。

    MGLevelObject<SparseMatrix<double>> mg_matrix_dg_down;

// 精细化边缘的通量矩阵的转置，将粗级自由度耦合到精细级。

    MGLevelObject<SparseMatrix<double>> mg_matrix_dg_up;
  };

// 构造函数简单地设置了粗略的网格和DoFHandler。FiniteElement作为一个参数被提供，以实现灵活性。

  template <int dim>
  InteriorPenaltyProblem<dim>::InteriorPenaltyProblem(
    const FiniteElement<dim> &fe)
    : triangulation(Triangulation<dim>::limit_level_difference_at_vertices)
    , mapping()
    , fe(fe)
    , dof_handler(triangulation)
    , estimates(1)
  {
    GridGenerator::hyper_cube_slit(triangulation, -1, 1);
  }

// 在这个函数中，我们设置了线性系统的维度和全局矩阵以及水平矩阵的稀疏性模式。

  template <int dim>
  void InteriorPenaltyProblem<dim>::setup_system()
  {

// 首先，我们用有限元将自由度分布在网格上并对其进行编号。

    dof_handler.distribute_dofs(fe);
    dof_handler.distribute_mg_dofs();
    unsigned int n_dofs = dof_handler.n_dofs();

// 然后，我们已经知道代表有限元函数的向量的大小。

    solution.reinit(n_dofs);
    right_hand_side.reinit(n_dofs);

// 接下来，我们为全局矩阵设置稀疏性模式。由于我们事先不知道行的大小，所以我们首先填充一个临时的DynamicSparsityPattern对象，一旦完成，就将其复制到常规的SparsityPattern中。

    DynamicSparsityPattern dsp(n_dofs);
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity.copy_from(dsp);
    matrix.reinit(sparsity);

    const unsigned int n_levels = triangulation.n_levels();

// 全局系统已经设置好了，现在我们来关注一下级别矩阵。我们调整所有矩阵对象的大小，以便每一级都有一个矩阵。

    mg_matrix.resize(0, n_levels - 1);
    mg_matrix.clear_elements();
    mg_matrix_dg_up.resize(0, n_levels - 1);
    mg_matrix_dg_up.clear_elements();
    mg_matrix_dg_down.resize(0, n_levels - 1);
    mg_matrix_dg_down.clear_elements();

// 在为水平矩阵调用<tt>clear()</tt>之后更新稀疏模式很重要，因为矩阵通过SmartPointer和Subscriptor机制锁定了稀疏模式。

    mg_sparsity.resize(0, n_levels - 1);
    mg_sparsity_dg_interface.resize(0, n_levels - 1);

// 现在，所有的对象都准备好了，可以在每一层容纳一个稀疏模式或矩阵。剩下的就是在每一层设置稀疏模式了。

    for (unsigned int level = mg_sparsity.min_level();
         level <= mg_sparsity.max_level();
         ++level)
      {

// 这些与上面的全局矩阵的行数大致相同，现在是每个级别的。

        DynamicSparsityPattern dsp(dof_handler.n_dofs(level));
        MGTools::make_flux_sparsity_pattern(dof_handler, dsp, level);
        mg_sparsity[level].copy_from(dsp);
        mg_matrix[level].reinit(mg_sparsity[level]);

// 另外，我们需要初始化各层之间细化边缘的转移矩阵。它们被存储在两个索引中较细的索引处，因此在0层没有这样的对象。

        if (level > 0)
          {
            DynamicSparsityPattern dsp;
            dsp.reinit(dof_handler.n_dofs(level - 1),
                       dof_handler.n_dofs(level));
            MGTools::make_flux_sparsity_pattern_edge(dof_handler, dsp, level);
            mg_sparsity_dg_interface[level].copy_from(dsp);
            mg_matrix_dg_up[level].reinit(mg_sparsity_dg_interface[level]);
            mg_matrix_dg_down[level].reinit(mg_sparsity_dg_interface[level]);
          }
      }
  }

// 在这个函数中，我们组装全局系统矩阵，这里的全局是指我们解决的离散系统的矩阵，它覆盖了整个网格。

  template <int dim>
  void InteriorPenaltyProblem<dim>::assemble_matrix()
  {

// 首先，我们需要设置提供我们集成值的对象。这个对象包含了所有需要的FEValues和FEFaceValues对象，并且自动维护它们，使它们总是指向当前单元。为此，我们首先需要告诉它，在哪里计算，计算什么。由于我们没有做任何花哨的事情，我们可以依靠他们对正交规则的标准选择。

// 由于他们的默认更新标志是最小的，我们另外添加我们需要的东西，即所有对象（单元格、边界和内部面）上的形状函数的值和梯度。之后，我们准备初始化容器，它将创建所有必要的FEValuesBase对象进行整合。

    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags update_flags = update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

// 这就是我们整合本地数据的对象。它由MatrixIntegrator中的局部整合例程填充，然后由汇编器用来将信息分配到全局矩阵中。

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 此外，我们还需要一个将局部矩阵装配到全局矩阵的对象。这些装配器对象拥有目标对象结构的所有知识，在这里是一个稀疏矩阵，可能的约束和网格结构。

    MeshWorker::Assembler::MatrixSimple<SparseMatrix<double>> assembler;
    assembler.initialize(matrix);

// 现在是我们自己编码的部分，局部积分器。这是唯一与问题有关的部分。

    MatrixIntegrator<dim> integrator;

// 现在，我们把所有的东西都扔到 MeshWorker::loop(), 中，在这里遍历网格的所有活动单元，计算单元和面的矩阵，并把它们集合到全局矩阵中。我们在这里使用变量<tt>dof_handler</tt>，以便使用全局自由度的编号。

    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_active(),
                                           dof_handler.end(),
                                           dof_info,
                                           info_box,
                                           integrator,
                                           assembler);
  }

// 现在，我们对水平矩阵做同样的处理。不太令人惊讶的是，这个函数看起来像前一个函数的孪生兄弟。事实上，只有两个小的区别。

  template <int dim>
  void InteriorPenaltyProblem<dim>::assemble_mg_matrix()
  {
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags update_flags = update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 很明显，需要用一个填充水平矩阵的汇编器来代替。请注意，它也会自动填充边缘矩阵。

    MeshWorker::Assembler::MGMatrixSimple<SparseMatrix<double>> assembler;
    assembler.initialize(mg_matrix);
    assembler.initialize_fluxes(mg_matrix_dg_up, mg_matrix_dg_down);

    MatrixIntegrator<dim> integrator;

// 这里是与前一个函数的另一个不同之处：我们在所有单元上运行，而不仅仅是活动单元。而且我们使用以 _mg 结尾的函数，因为我们需要每一层的自由度，而不是全局的编号。

    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_mg(),
                                           dof_handler.end_mg(),
                                           dof_info,
                                           info_box,
                                           integrator,
                                           assembler);
  }

// 这里我们有另一个assemble函数的克隆。与组装系统矩阵的区别在于，我们在这里组装了一个向量。

  template <int dim>
  void InteriorPenaltyProblem<dim>::assemble_right_hand_side()
  {
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags                         update_flags =
      update_quadrature_points | update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 因为这个汇编器允许我们填充多个向量，所以接口要比上面复杂一些。向量的指针必须存储在一个AnyData对象中。虽然这在这里似乎造成了两行额外的代码，但实际上在更复杂的应用中它是很方便的。

    MeshWorker::Assembler::ResidualSimple<Vector<double>> assembler;
    AnyData                                               data;
    data.add<Vector<double> *>(&right_hand_side, "RHS");
    assembler.initialize(data);

    RHSIntegrator<dim> integrator;
    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_active(),
                                           dof_handler.end(),
                                           dof_info,
                                           info_box,
                                           integrator,
                                           assembler);

    right_hand_side *= -1.;
  }

// 现在，我们已经对构建离散线性系统的所有函数进行了编码，现在是我们实际解决它的时候了。

  template <int dim>
  void InteriorPenaltyProblem<dim>::solve()
  {

// 选择的求解器是共轭梯度。

    SolverControl            control(1000, 1.e-12);
    SolverCG<Vector<double>> solver(control);

// 现在我们正在设置多级预处理程序的组件。首先，我们需要在网格层之间进行转移。我们在这里使用的对象为这些转移生成了稀疏矩阵。

    MGTransferPrebuilt<Vector<double>> mg_transfer;
    mg_transfer.build(dof_handler);

// 然后，我们需要一个精确的解算器来解算最粗层次上的矩阵。

    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from(mg_matrix[0]);
    MGCoarseGridHouseholder<double, Vector<double>> mg_coarse;
    mg_coarse.initialize(coarse_matrix);

// 虽然转移和粗略网格求解器几乎是通用的，但为平滑器提供了更多的灵活性。首先，我们选择Gauss-Seidel作为我们的平滑方法。

    GrowingVectorMemory<Vector<double>> mem;
    using RELAXATION = PreconditionSOR<SparseMatrix<double>>;
    mg::SmootherRelaxation<RELAXATION, Vector<double>> mg_smoother;
    RELAXATION::AdditionalData                         smoother_data(1.);
    mg_smoother.initialize(mg_matrix, smoother_data);

// 在每个级别上做两个平滑步骤。

    mg_smoother.set_steps(2);

// 由于SOR方法不是对称的，但我们在下面使用共轭梯度迭代，这里有一个技巧，使多级预处理器成为对称算子，即使是对非对称平滑器。

    mg_smoother.set_symmetric(true);

// 平滑器类可以选择实现变量V型循环，我们在这里不需要。

    mg_smoother.set_variable(false);

// 最后，我们必须将我们的矩阵包裹在一个具有所需乘法函数的对象中。

    mg::Matrix<Vector<double>> mgmatrix(mg_matrix);
    mg::Matrix<Vector<double>> mgdown(mg_matrix_dg_down);
    mg::Matrix<Vector<double>> mgup(mg_matrix_dg_up);

// 现在，我们准备设置V型循环算子和多级预处理程序。

    Multigrid<Vector<double>> mg(
      mgmatrix, mg_coarse, mg_transfer, mg_smoother, mg_smoother);

// 让我们不要忘记因为自适应细化而需要的边缘矩阵。

    mg.set_edge_flux_matrices(mgdown, mgup);

// 在所有的准备工作完成后，将Multigrid对象包装成另一个对象，它可以作为一个普通的预处理程序使用。

    PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double>>>
      preconditioner(dof_handler, mg, mg_transfer);

// 并用它来解决这个系统。

    solver.solve(matrix, solution, right_hand_side, preconditioner);
  }

// 另一个克隆的集合函数。与之前的最大区别是，这里我们也有一个输入向量。

  template <int dim>
  double InteriorPenaltyProblem<dim>::estimate()
  {

// 估算器的结果存储在一个每个单元格有一个条目的向量中。由于deal.II中的单元格没有编号，我们必须建立自己的编号，以便使用这个向量。对于下面使用的汇编器来说，结果存储在向量的哪个分量中的信息是由每个单元的user_index变量传送的。我们需要在这里设置这个编号。

// 另一方面，有人可能已经使用了用户指数。所以，让我们做个好公民，在篡改它们之前保存它们。

    std::vector<unsigned int> old_user_indices;
    triangulation.save_user_indices(old_user_indices);

    estimates.block(0).reinit(triangulation.n_active_cells());
    unsigned int i = 0;
    for (const auto &cell : triangulation.active_cell_iterators())
      cell->set_user_index(i++);

// 这就像以前一样开始。

    MeshWorker::IntegrationInfoBox<dim> info_box;
    const unsigned int                  n_gauss_points =
      dof_handler.get_fe().tensor_degree() + 1;
    info_box.initialize_gauss_quadrature(n_gauss_points,
                                         n_gauss_points + 1,
                                         n_gauss_points);

// 但现在我们需要通知信息框我们要在正交点上评估的有限元函数。首先，我们用这个向量创建一个AnyData对象，这个向量就是我们刚刚计算的解。

    AnyData solution_data;
    solution_data.add<const Vector<double> *>(&solution, "solution");

// 然后，我们告诉单元格的 Meshworker::VectorSelector ，我们需要这个解决方案的二次导数（用来计算拉普拉斯）。因此，选择函数值和第一导数的布尔参数是假的，只有选择第二导数的最后一个参数是真的。

    info_box.cell_selector.add("solution", false, false, true);

// 在内部和边界面，我们需要函数值和第一导数，但不需要第二导数。

    info_box.boundary_selector.add("solution", true, true, false);
    info_box.face_selector.add("solution", true, true, false);

// 我们继续像以前一样，除了默认的更新标志已经被调整为我们上面要求的值和导数之外。

    info_box.add_update_flags_boundary(update_quadrature_points);
    info_box.initialize(fe, mapping, solution_data, solution);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 汇编器在每个单元格中存储一个数字，否则这与右侧的计算是一样的。

    MeshWorker::Assembler::CellsAndFaces<double> assembler;
    AnyData                                      out_data;
    out_data.add<BlockVector<double> *>(&estimates, "cells");
    assembler.initialize(out_data, false);

    Estimator<dim> integrator;
    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_active(),
                                           dof_handler.end(),
                                           dof_info,
                                           info_box,
                                           integrator,
                                           assembler);

// 就在我们返回错误估计的结果之前，我们恢复旧的用户索引。

    triangulation.load_user_indices(old_user_indices);
    return estimates.block(0).l2_norm();
  }

// 这里我们把我们的有限元解和（已知的）精确解进行比较，计算梯度和函数本身的平均二次误差。这个函数是上面那个估计函数的克隆。

// 由于我们分别计算能量和L2-norm的误差，我们的块向量在这里需要两个块。

  template <int dim>
  void InteriorPenaltyProblem<dim>::error()
  {
    BlockVector<double> errors(2);
    errors.block(0).reinit(triangulation.n_active_cells());
    errors.block(1).reinit(triangulation.n_active_cells());

    std::vector<unsigned int> old_user_indices;
    triangulation.save_user_indices(old_user_indices);
    unsigned int i = 0;
    for (const auto &cell : triangulation.active_cell_iterators())
      cell->set_user_index(i++);

    MeshWorker::IntegrationInfoBox<dim> info_box;
    const unsigned int                  n_gauss_points =
      dof_handler.get_fe().tensor_degree() + 1;
    info_box.initialize_gauss_quadrature(n_gauss_points,
                                         n_gauss_points + 1,
                                         n_gauss_points);

    AnyData solution_data;
    solution_data.add<Vector<double> *>(&solution, "solution");

    info_box.cell_selector.add("solution", true, true, false);
    info_box.boundary_selector.add("solution", true, false, false);
    info_box.face_selector.add("solution", true, false, false);

    info_box.add_update_flags_cell(update_quadrature_points);
    info_box.add_update_flags_boundary(update_quadrature_points);
    info_box.initialize(fe, mapping, solution_data, solution);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    MeshWorker::Assembler::CellsAndFaces<double> assembler;
    AnyData                                      out_data;
    out_data.add<BlockVector<double> *>(&errors, "cells");
    assembler.initialize(out_data, false);

    ErrorIntegrator<dim> integrator;
    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_active(),
                                           dof_handler.end(),
                                           dof_info,
                                           info_box,
                                           integrator,
                                           assembler);
    triangulation.load_user_indices(old_user_indices);

    deallog << "energy-error: " << errors.block(0).l2_norm() << std::endl;
    deallog << "L2-error:     " << errors.block(1).l2_norm() << std::endl;
  }

// 创建图形输出。我们通过整理其各个组成部分的名称来产生文件名，包括我们用两个数字输出的细化周期。

  template <int dim>
  void
  InteriorPenaltyProblem<dim>::output_results(const unsigned int cycle) const
  {
    const std::string filename =
      "sol-" + Utilities::int_to_string(cycle, 2) + ".gnuplot";

    deallog << "Writing solution to <" << filename << ">..." << std::endl
            << std::endl;
    std::ofstream gnuplot_output(filename);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u");
    data_out.add_data_vector(estimates.block(0), "est");

    data_out.build_patches();

    data_out.write_gnuplot(gnuplot_output);
  }

// 最后是自适应循环，或多或少和前面的例子一样。

  template <int dim>
  void InteriorPenaltyProblem<dim>::run(unsigned int n_steps)
  {
    deallog << "Element: " << fe.get_name() << std::endl;
    for (unsigned int s = 0; s < n_steps; ++s)
      {
        deallog << "Step " << s << std::endl;
        if (estimates.block(0).size() == 0)
          triangulation.refine_global(1);
        else
          {
            GridRefinement::refine_and_coarsen_fixed_fraction(
              triangulation, estimates.block(0), 0.5, 0.0);
            triangulation.execute_coarsening_and_refinement();
          }

        deallog << "Triangulation " << triangulation.n_active_cells()
                << " cells, " << triangulation.n_levels() << " levels"
                << std::endl;

        setup_system();
        deallog << "DoFHandler " << dof_handler.n_dofs() << " dofs, level dofs";
        for (unsigned int l = 0; l < triangulation.n_levels(); ++l)
          deallog << ' ' << dof_handler.n_dofs(l);
        deallog << std::endl;

        deallog << "Assemble matrix" << std::endl;
        assemble_matrix();
        deallog << "Assemble multilevel matrix" << std::endl;
        assemble_mg_matrix();
        deallog << "Assemble right hand side" << std::endl;
        assemble_right_hand_side();
        deallog << "Solve" << std::endl;
        solve();
        error();
        deallog << "Estimate " << estimate() << std::endl;
        output_results(s);
      }
  }
} // namespace Step39

int main()
{
  try
    {
      using namespace dealii;
      using namespace Step39;

      deallog.depth_console(2);
      std::ofstream logfile("deallog");
      deallog.attach(logfile);
      FE_DGQ<2>                 fe1(3);
      InteriorPenaltyProblem<2> test1(fe1);
      test1.run(12);
    }
  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;
}


#ifndef LINEARSOLVERS_H_
#define LINEARSOLVERS_H_

#include <Eigen/Sparse>
//#include <SuiteSparse_config.h>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/OrderingMethods>
//#include <Eigen/SPQRSupport>
#include <unsupported/Eigen/IterativeSolvers>
//#include <Eigen/PaStiXSupport>

using namespace Eigen;

namespace echo
{

namespace solvers
{

class ConfigurationParameter{

public:
    ConfigurationParameter(){
        max_iterations=100;
        tol=1E-06;
        verbose=true;
    }
    ~ConfigurationParameter(){}
    unsigned int max_iterations;
    double tol;
    bool verbose;
};

/**
* Solve a linear system using conjugate gradient
* The 'config' argument is an object of the class ConfigurationParameter
* which has the following members:
*   unsigned int max_iterations [100]
*   double tol [1E-06]
*   bool [true]
*/
template <class SparseMatrixType, class DenseVectorType>
void solveWithConjugateGradient(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config);

/**
* Solve a linear system using sparseQR
* The 'config' argument is an object of the class ConfigurationParameter
* which has the following members:
*   unsigned int max_iterations [100]
*   double tol [1E-06]
*   bool [true]
*/
//template <class SparseMatrixType, class DenseVectorType>
//void solveWithSparseQR(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config);

/**
* Solve a linear system using SPQR from SuiteSparse. Requires SuiteSparse to run
* The 'config' argument is an object of the class ConfigurationParameter
* which has the following members:
*   unsigned int max_iterations [100]
*   double tol [1E-06]
*   bool [true]
*/
//template <class SparseMatrixType, class DenseVectorType>
//void solveWithSPQR(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config);

/**
* Solve a linear system using biconjugate gradient stabilized
* The 'config' argument is an object of the class ConfigurationParameter
* which has the following members:
*   unsigned int max_iterations [100]
*   double tol [1E-06]
*   bool [true]
*/
template <class SparseMatrixType, class DenseVectorType>
void solveWithBiCGSTAB(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config);

/**
 * This implementation of GMRES is provided *as is* by Eigen, with no support planned.
 * Documentation here http://eigen.tuxfamily.org/dox/unsupported/classEigen_1_1GMRES.html
 */
template <class SparseMatrixType, class DenseVectorType>
void solveWithGMRES(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config);

}

}

/// --------------------------------------------------------------------///
/// 				         IMPLEMENTATION          				    ///
/// --------------------------------------------------------------------///

template <class SparseMatrixType, class DenseVectorType>
void echo::solvers::solveWithConjugateGradient(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config)
{

    ConjugateGradient<SparseMatrixType > solver;
    solver.setMaxIterations(config.max_iterations);
    solver.setTolerance(config.tol);
    solver.compute(A);
    if(solver.info()!=Success)
    {
        /// decomposition failed
        if (config.verbose)          std::cerr << "\t\tERROR: CG decomposition failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    x = solver.solve(b);
    if(solver.info()!=Success)
    {
        /// solving failed
        if (config.verbose) std::cerr << "\t\tERROR: CG solver failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    if (config.verbose) std::cout << "\t\t#iterations:     " << solver.iterations() << std::endl;
    if (config.verbose) std::cout << "\t\testimated error: " << solver.error()      << std::endl;

    /// For using solve with guess refer to documentation here: http://eigen.tuxfamily.org/dox-devel/classEigen_1_1ConjugateGradient.html
}

/*template <class SparseMatrixType, class DenseVectorType>
void echo::solvers::solveWithSparseQR(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config)
{

    SparseQR<SparseMatrixType, COLAMDOrdering<int> > solver;
    //solver.setMaxIterations(config.max_iterations);
    //solver.setTolerance(config.tol);
    solver.compute(A);
    if(solver.info()!=Success)
    {
        /// decomposition failed
        if (config.verbose)          std::cerr << "\t\tERROR: SparseQR decomposition failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    x = solver.solve(b);
    if(solver.info()!=Success)
    {
        /// solving failed
        if (config.verbose) std::cerr << "\t\tERROR: SparseQR solver failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    //if (config.verbose) std::cout << "\t\t#iterations:     " << solver.iterations() << std::endl;
    //if (config.verbose) std::cout << "\t\testimated error: " << solver.error()      << std::endl;

    /// For using solve with guess refer to documentation here: http://eigen.tuxfamily.org/dox-devel/classEigen_1_1ConjugateGradient.html
}
*/
/*template <class SparseMatrixType, class DenseVectorType>
void echo::solvers::solveWithSPQR(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config)
{

    SPQR<SparseMatrixType > solver;
    //solver.setMaxIterations(config.max_iterations);
    //solver.setTolerance(config.tol);

    solver.compute(A);

    if(solver.info()!=Success)
    {
        /// decomposition failed
        if (config.verbose)          std::cerr << "\t\tERROR: SPQR decomposition failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    x = solver.solve(b);
    if(solver.info()!=Success)
    {
        /// solving failed
        if (config.verbose) std::cerr << "\t\tERROR: SPQR solver failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    //if (config.verbose) std::cout << "\t\t#iterations:     " << solver.iterations() << std::endl;
    //if (config.verbose) std::cout << "\t\testimated error: " << solver.error()      << std::endl;

    /// For using solve with guess refer to documentation here: http://eigen.tuxfamily.org/dox-devel/classEigen_1_1ConjugateGradient.html
}
*/
template <class SparseMatrixType, class DenseVectorType>
void echo::solvers::solveWithBiCGSTAB(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config){

    BiCGSTAB<SparseMatrixType, IncompleteLUT<double> > solver;
    solver.setMaxIterations(config.max_iterations);
    solver.setTolerance(config.tol);
    solver.compute(A);
    if(solver.info()!=Success)
    {
        /// decomposition failed
        if (config.verbose)          std::cerr << "\t\tERROR: BiCGSTAB decomposition failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    x = solver.solve(b);
    if(solver.info()!=Success)
    {
        /// solving failed
        if (config.verbose) std::cerr << "\t\tERROR: BiCGSTAB solver failed with a matrix of size "<< A.rows() <<"x"<<A.cols() <<std::endl;
    }
    if (config.verbose) std::cout << "\t\t#iterations:     " << solver.iterations() << std::endl;
    if (config.verbose) std::cout << "\t\testimated error: " << solver.error()      << std::endl;

}

template <class SparseMatrixType, class DenseVectorType>
void echo::solvers::solveWithGMRES(const SparseMatrixType &A, const DenseVectorType &b, DenseVectorType &x, ConfigurationParameter &config){

    GMRES<SparseMatrixType, IncompleteLUT<double> > solver;
    solver.setMaxIterations(config.max_iterations);
    solver.setTolerance(config.tol);
    solver.compute(A);
    if(solver.info()!=Success)
    {
        /// decomposition failed
        if (config.verbose)          std::cerr << "\t\tERROR: GMRES decomposition failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    x = solver.solve(b);
    if(solver.info()!=Success)
    {
        /// solving failed
        if (config.verbose) std::cerr << "\t\tERROR: GMRES solver failed with a matrix of size "<< A.rows() <<"x"<<A.cols()<<std::endl;
    }
    if (config.verbose) std::cout << "\t\t#iterations:     " << solver.iterations() << std::endl;
    if (config.verbose) std::cout << "\t\testimated error: " << solver.error()      << std::endl;

}
#endif /* LINEARSOLVERS_H_*/


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

// Third party includes
#include <boost/format.hpp>

// Local VOTCA includes
#include "votca/xtp/anderson_mixing.h"

namespace votca {
namespace xtp {

void Anderson::Configure(const Index order, const double alpha) {
  order_ = order + 1;
  alpha_ = alpha;
}

void Anderson::UpdateOutput(const Eigen::VectorXd &newOutput) {

  // Check if max mixing history is reached and adding new step to history
  Index size = output_.size();
  if (size > order_ - 1) {
    output_.erase(output_.begin());
  }
  output_.push_back(newOutput);
}

void Anderson::UpdateInput(const Eigen::VectorXd &newInput) {
  Index size = output_.size();
  if (size > order_ - 1) {
    input_.erase(input_.begin());
  }
  input_.push_back(newInput);
}

const Eigen::VectorXd Anderson::MixHistory() {

  const Index iteration = output_.size();
  const Index used_history = iteration - 1;
  Eigen::VectorXd OutMixed = output_.back();
  Eigen::VectorXd InMixed = input_.back();

  if (iteration > 1 && order_ > 1) {

    Eigen::VectorXd DeltaN = OutMixed - InMixed;

    // Building Linear System for Coefficients
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(used_history, used_history);
    Eigen::VectorXd c = Eigen::VectorXd::Zero(used_history);

    for (Index m = 1; m < iteration; m++) {

      c(m - 1) = (DeltaN - output_[used_history - m] + input_[used_history - m])
                     .dot(DeltaN);

      for (Index j = 1; j < iteration; j++) {
        A(m - 1, j - 1) =
            (DeltaN - output_[used_history - m] + input_[used_history - m])
                .dot((DeltaN - output_[used_history - j] +
                      input_[used_history - j]));
      }
    }
    // Solving the System to obtain coefficients
    Eigen::VectorXd coefficients = A.fullPivHouseholderQr().solve(c);

    // Mixing the Potentials
    for (Index n = 1; n < iteration; n++) {

      OutMixed += coefficients(n - 1) *
                  (output_[used_history - n] - output_[used_history]);
      InMixed += coefficients(n - 1) *
                 (input_[used_history - n] - input_[used_history]);
    }
  }

  // Returning the linear Mix of Input and Output
  return alpha_ * OutMixed + (1 - alpha_) * InMixed;
}
}  // namespace xtp
}  // namespace votca


#include "Network.hpp"
#include <stdexcept>
#include <math.h>
#include <ctime>
#include <cstdlib>
#include <algorithm>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

Network::Network(const std::vector<int> &layers, double lR):
	learningRate(lR),
	nbLayers(layers.size()),
	errors(),
	inputs(),
	outputs(),
	weights(),
	biases(),
	functions(),
	derivates()
{
	if (layers.size() < 3)
		throw std::invalid_argument("Network must contain at least one hidden layer");

	std::srand(std::time(0));
	this->initializeNetwork(layers);

	/* Setting activation functions & derivates */
	this->functions.push_back(0);
	this->derivates.push_back(0);
	for (int i=0; i < this->nbLayers - 2; ++i)
	{
		this->functions.push_back(new Function(&sigmoid));
		this->derivates.push_back(new Function(&sigmoidPrime));
	}
	this->functions.push_back(new Function(&identity));
	this->derivates.push_back(new Function(&identityPrime));
}

Network::~Network()
{
	for (int i=0; i < this->nbLayers; ++i)
	{
		delete this->errors[i];
		delete this->outputs[i];
		delete this->inputs[i];

		if (i != this->nbLayers - 1)
		{
			delete this->biases[i];
			delete this->weights[i];
		}

		if (this->functions[i])
			delete this->functions[i];
		if (this->derivates[i])
			delete this->derivates[i];
	}
}

/*
	Initializes all the components of the network
 	(weights, biases, inputs, outputs, errors)
*/
void 						Network::initializeNetwork(const std::vector<int> &layers)
{
	/* Initializing input & hidden layers */
	for (unsigned int layer=0; layer < layers.size() - 1; ++layer)
	{
		int rows = layers[layer + 1];
		int columns = layers[layer];

		matrix<double> 	*currentWeights = new matrix<double>(rows, columns);
		vector<double> 	*currentErrors = new vector<double>(columns);
		vector<double> 	*currentInputs = new vector<double>(columns);
		vector<double> 	*currentOutputs = new vector<double>(columns);
		vector<double>	*currentBiases = new vector<double>(rows);

		for (unsigned int i=0; i < currentWeights->size1(); ++i)
		{
			for (unsigned j=0; j < currentWeights->size2(); ++j)
			{
				(*currentWeights)(i, j) = static_cast<double>(std::rand()) / RAND_MAX;
			}
		}

		for (int i=0; i < rows; ++i)
		{
			(*currentBiases)[i] = static_cast<double>(std::rand()) / RAND_MAX;
		}

		std::fill(currentErrors->begin(), currentErrors->end(), 0.0);
		std::fill(currentInputs->begin(), currentInputs->end(), 0.0);
		std::fill(currentOutputs->begin(), currentOutputs->end(), 0.0);

		this->weights.push_back(currentWeights);
		this->errors.push_back(currentErrors);
		this->inputs.push_back(currentInputs);
		this->outputs.push_back(currentOutputs);
		this->biases.push_back(currentBiases);
	}

	/* Output layer */
	int outputSize = layers[layers.size() - 1];

	vector<double> *outputErrors = new vector<double>(outputSize);
	vector<double> *outputInputs = new vector<double>(outputSize);
	vector<double> *outputOutputs = new vector<double>(outputSize);

	std::fill(outputErrors->begin(), outputErrors->end(), 0.0);
	std::fill(outputInputs->begin(), outputInputs->end(), 0.0);
	std::fill(outputOutputs->begin(), outputOutputs->end(), 0.0);

	this->errors.push_back(outputErrors);
	this->inputs.push_back(outputInputs);
	this->outputs.push_back(outputOutputs);
}

vector<double> *Network::predict(const vector<double> &X)
{
	if (X.size() != this->inputs[0]->size())
		throw std::length_error("Invalid number of features");

	return this->feedForward(X);
}

/* Propagates the input through the network and returns the output */
vector<double> 	*Network::feedForward(const vector<double> &X)
{
	delete this->inputs[0];
	delete this->outputs[0];

	this->inputs[0] = new vector<double>(X);
	this->outputs[0] = new vector<double>(X);

	for (int i=1; i < this->nbLayers; ++i)
	{
		delete this->inputs[i];
		delete this->outputs[i];

		this->inputs[i] = new vector<double>(prod(*(this->weights[i - 1]), *(this->outputs[i - 1])) + *(this->biases[i - 1]));
		this->outputs[i] = (*(this->functions[i]))(*(this->inputs[i]));
	}

	return this->outputs[this->outputs.size() - 1];
}

/* Fits the network using the data given in parameter */
void			Network::fit(matrix<double> &X, matrix<double> &Y, const int iterations)
{
	if (X.size1() != Y.size1())
		throw std::length_error("Features and targets must have the same length");

	for (int iter=0; iter < iterations; ++iter)
	{
		for (unsigned int idx=0; idx < X.size1(); ++idx)
		{
			matrix_row<matrix<double> > rowX(X, idx);
			matrix_row<matrix<double> > rowY(Y, idx);

			if (rowX.size() != this->inputs[0]->size())
				throw std::length_error("Invalid number of features");

			this->updateWeights(this->row2vec(rowX), this->row2vec(rowY));
		}
	}
}

/* Updates the weights in the network by using the backpropagation algorithm */
void			Network::updateWeights(vector<double> *features, vector<double> *target)
{
	Function				f(*(this->derivates[this->derivates.size() - 1]));
	vector<double> 	*output = this->feedForward(*features);
	vector<double> 	diff = *output - *target;
	vector<double>	*deriv = f(*(this->outputs[this->outputs.size() - 1]));
	vector<double>	*err = new vector<double>(element_prod(*deriv, diff));

	delete deriv;
	delete this->errors[this->errors.size() - 1];
	this->errors[this->errors.size() - 1] = err;

	for (int i=this->nbLayers - 2; i > 0; --i)
	{
		vector<double>	*deriv = (*(this->derivates[i]))(*(this->inputs[i]));
		vector<double>	mul(prod(trans(*(this->weights[i])), *(this->errors[i + 1])));
		matrix<double>	*oldW = this->weights[i];
		vector<double>	*oldB = this->biases[i];

		delete this->errors[i];
		this->errors[i] = new vector<double>(element_prod((*deriv), mul));
		this->weights[i] = new matrix<double>(*(this->weights[i]) - (outer_prod(*(this->errors[i + 1]), *(this->outputs[i])) * this->learningRate));
		this->biases[i] = new vector<double>(*(this->biases[i]) - (*(this->errors[i + 1]) * this->learningRate));

		delete oldW;
		delete oldB;
		delete deriv;
	}

	matrix<double>	*oldW = this->weights[0];
	vector<double>	*oldB = this->biases[0];
	this->weights[0] = new matrix<double>(*(this->weights[0]) - (outer_prod(*(this->errors[1]), *(this->outputs[0])) * this->learningRate));
	this->biases[0] = new vector<double>(*(this->biases[0]) - (*(this->errors[1])  * this->learningRate));

	delete features;
	delete target,
	delete oldW;
	delete oldB;
}

vector<double> 	*Network::sigmoid(const vector<double> &input)
{
	vector<double> *result = new vector<double>(input.size());

	for (unsigned int i=0; i < input.size(); ++i)
	{
		(*result)[i] = (1.0 / (1.0 + exp(-input[i])));
	}

	return result;
}

vector<double> 	*Network::sigmoidPrime(const vector<double> &input)
{
	vector<double>	*result = new vector<double>(input.size());

	for (unsigned int i=0; i < input.size(); ++i)
	{
		double sigmoid = (1.0 / (1.0 + exp(-input[i])));
		(*result)[i] = sigmoid * (1 - sigmoid);
	}

	return result;
}

vector<double>	*Network::identity(const vector<double> &input)
{
	return new vector<double>(input);
}

vector<double>	*Network::identityPrime(const vector<double> &input)
{
	vector<double>	*result = new vector<double>(input.size());

	for (unsigned int i=0; i < input.size(); ++i)
	{
		(*result)[i] = 1;
	}

	return result;
}

vector<double>	*Network::row2vec(const matrix_row<matrix<double> > &row) const
{
	vector<double>	vec(row.size());

	std::copy(row.begin(), row.end(), vec.begin());

	return new vector<double>(vec);
}

std::ostream&	operator<<(std::ostream& os, const Network& net)
{
	for (unsigned int layer=0; layer < net.outputs.size(); ++layer)
	{
		os << "-> Layer " << layer << " : " << net.outputs[layer]->size() << " neuron(s)" << std::endl;
	}

    return os;
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <complex>

using Eigen::MatrixXcd;
using Eigen::MatrixXd;
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;

// we firstly make a quite naive example, take n, s and return polynomial a
Eigen::MatrixXcd solve_poly_a(int n, int s, Eigen::MatrixXcd R)
{
	int hat_s = 2 * s + 1;
	int p = 3;

	MatrixXcd C = MatrixXcd::Zero(n, n);
	MatrixXcd C1 = MatrixXcd::Zero(n, n-hat_s+1);
	MatrixXcd C2 = MatrixXcd::Zero(n, hat_s-1);
	MatrixXcd W_fake = MatrixXcd::Zero(n, p);
	MatrixXcd WPerp = MatrixXcd::Zero(hat_s-1, n);

	MatrixXcd A = MatrixXcd::Zero(s, s);
	MatrixXcd b = MatrixXcd::Zero(s, 1);

	MatrixXd G = MatrixXd::Random(p, 1);
	MatrixXd Q1 = MatrixXd::Random(n-2*s-1,p);
	MatrixXd Q = MatrixXd::Ones(n-2*s,p);
	MatrixXd eps = MatrixXd::Zero(n, 1);

	Q.topRows(n-2*s-1) = Q1;
	eps.topRows(s) = -100.0*MatrixXd::Ones(s, 1);

	double factor1 = 1/std::sqrt(n);
	// generate C matrix:
	for (int i = 0; i < n; ++i)
	{
		for (int j = 0; j < n; ++j)
		{
			if (j >= i)
			{
				if ((i == 0) || (j == 0))
				{
					C(i, j) = std::complex<double>(1, 0);
				}
				else{
					std::complex<double> temptElementValue = std::complex<double>(0, (-2*i*j*M_PI)/n);
					C(i, j) = std::exp(temptElementValue);
				}
			}
			else{
				C(i, j) = C(j, i);
			}

		}
	}
	C *= factor1;
	// fetch C1 and C2:
	C1 = C.leftCols(n-hat_s+1);
	C2 = C.rightCols(hat_s-1);

	WPerp = C2.adjoint();
	W_fake = C1 * Q;

	//MatrixXcd R = W_fake * G + eps;
	// we assume here R is passed by Python side
	// and it is a complex vector, shape of R should
	// be n * 1
	MatrixXcd E2 = WPerp * R;

	// form A * x = b:
	for (int i = 0; i < s; ++i)
	{
		A.row(i) = E2.col(0).segment(2*s-i-s-1,s).transpose();
		b.row(i) = E2.row(2*s-i-1);
	}
	//MatrixXcd alpha = A.colPivHouseholderQr().solve(b);
	MatrixXcd alpha = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
	return alpha;
}

namespace py = pybind11;

PYBIND11_MODULE(coding, m)
{
  m.doc() = "pybind11 coding plugin";

  m.def("solve_poly_a", &solve_poly_a, py::arg("n"), py::arg("s"), py::arg("R"));
  //m.def("solve_poly_a", &solve_poly_a, py::arg("n"), py::arg("s"));
}


/**
 * \file
 *
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <boost/math/special_functions/pow.hpp>
#include <cmath>

#include "MaterialLib/MPL/Properties/ExponentialProperty.h"

namespace MaterialPropertyLib
{
ExponentialProperty::ExponentialProperty(
    PropertyDataType const& property_reference_value, ExponentData const& v)
    : _exponent_data(v)
{
    _value = property_reference_value;
}

PropertyDataType ExponentialProperty::value(
    VariableArray const& variable_array,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    return std::get<double>(_value) *
           std::exp(
               -std::get<double>(_exponent_data.factor) *
               (std::get<double>(
                    variable_array[static_cast<int>(_exponent_data.type)]) -
                std::get<double>(_exponent_data.reference_condition)));
}

PropertyDataType ExponentialProperty::dValue(
    VariableArray const& variable_array, Variable const primary_variable,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    return _exponent_data.type == primary_variable
               ? -std::get<double>(_value) *
                     std::get<double>(_exponent_data.factor) *
                     std::exp(
                         -std::get<double>(_exponent_data.factor) *
                         (std::get<double>(variable_array[static_cast<int>(
                              _exponent_data.type)]) -
                          std::get<double>(_exponent_data.reference_condition)))
               : decltype(_value){};
}

PropertyDataType ExponentialProperty::d2Value(
    VariableArray const& variable_array, Variable const pv1, Variable const pv2,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    return _exponent_data.type == pv1 && _exponent_data.type == pv2
               ? std::get<double>(_value) *
                     boost::math::pow<2>(
                         std::get<double>(_exponent_data.factor)) *
                     std::exp(
                         -std::get<double>(_exponent_data.factor) *
                         (std::get<double>(variable_array[static_cast<int>(
                              _exponent_data.type)]) -
                          std::get<double>(_exponent_data.reference_condition)))
               : decltype(_value){};
}

}  // namespace MaterialPropertyLib


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

#include <boost/math/special_functions/bernoulli.hpp>
#include <boost/chrono.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

private:
   typename Clock::time_point m_start;
};

void time()
{
   stopwatch<boost::chrono::high_resolution_clock> w;
   //
   // Sum the first 1000 Bernoulli numbers:
   //
   boost::multiprecision::cpp_dec_float_50 sum = 0;
   for(unsigned i = 0; i < 1000; ++i)
   {
      sum += boost::math::bernoulli_b2n<boost::multiprecision::cpp_dec_float_50>(i);
   }
   double t = boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   std::cout << "Total execution time = " << std::setprecision(3) << t << "s" << std::endl;
   std::cout << "Sum was: " << sum << std::endl;
}

int main()
{
   // Call time() twice: first call results in the cache being populated, second used already cached values:
   time();
   time();
   return 0;
}

/*

No atomic int:

Total execution time = 0.646s
Sum was: 2.86e+4134
Total execution time = 0.000341s
Sum was: 2.86e+4134

With atomic int:

Total execution time = 0.643s
Sum was: 2.86e+4134
Total execution time = 0.000309s
Sum was: 2.86e+4134

No threads:

Total execution time = 0.652s
Sum was: 2.86e+4134
Total execution time = 0.000281s
Sum was: 2.86e+4134

*/


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include "class.hpp"
#include <unistd.h>
#include <limits.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
using namespace std;

PARAMETERS::PARAMETERS(string parameter_file, string gmsh_file)
{
    cout << "\n";
    check_mpi();
    meshfile = gmsh_file;
    cout << "PARAMETERS::PARAMETERS gmsh_file: " << meshfile << endl;
    does_file_exist(meshfile.c_str(), "Called from PARAMETERS::read_calc_parameters");
    read_calc_parameters(parameter_file);
    verify_parameters();
    get_problem_dimension_msh();
    calc_cloud_num_points();
    calc_polynomial_term_exponents();
    cout << "\n";
}

void PARAMETERS::read_calc_parameters(string parameter_file)
{
    does_file_exist(parameter_file.c_str(), "Called from PARAMETERS::read_calc_parameters");
    char ctemp[5000], ctemp_2[100];
    string stemp;
    int itemp;
    double dtemp;
    FILE *file;
    file = fopen(parameter_file.c_str(), "r");
    cout << endl;
    fscanf(file, "%[^,],%i\n", ctemp, &poly_deg);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, poly_deg);
    fscanf(file, "%[^,],%i\n", ctemp, &phs_deg);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, phs_deg);
    fscanf(file, "%[^,],%lf\n", ctemp, &cloud_size_multiplier);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, cloud_size_multiplier);

    fscanf(file, "%[^,],%i\n", ctemp, &nt);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, nt);
    fscanf(file, "%[^,],%lf\n", ctemp, &Courant);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, Courant);
    fscanf(file, "%[^,],%s\n", ctemp, ctemp_2);
    solver_type = ctemp_2;
    printf("PARAMETERS::read_calc_parameters Read %s = %s\n", ctemp, solver_type.c_str());
    fscanf(file, "%[^,],%lf\n", ctemp, &steady_tolerance);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, steady_tolerance);
    fscanf(file, "%[^,],%lf\n", ctemp, &solver_tolerance);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, solver_tolerance);
    fscanf(file, "%[^,],%i\n", ctemp, &euclid_precond_level_hypre);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, euclid_precond_level_hypre);
    fscanf(file, "%[^,],%i\n", ctemp, &gmres_kdim);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, gmres_kdim);
    fscanf(file, "%[^,],%lf\n", ctemp, &precond_droptol);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, precond_droptol);
    fscanf(file, "%[^,],%i\n", ctemp, &n_iter);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, n_iter);

    cout << endl;
}

void PARAMETERS::verify_parameters()
{
    if (poly_deg < 2 || poly_deg > 15)
    {
        printf("\n\nERROR from PARAMETERS::verify_parameters poly_deg should be in range [2, 15]; current value: %i\n\n", poly_deg);
        throw bad_exception();
    }
    if (phs_deg != 3 && phs_deg != 5 && phs_deg != 7 && phs_deg != 9 && phs_deg != 11)
    {
        printf("\n\nERROR from PARAMETERS::verify_parameters phs_deg should be 3, 5, 7, 9, or 11; current value: %i\n\n", phs_deg);
        throw bad_exception();
    }
    if (strcmp(solver_type.c_str(), "hypre_ilu_gmres") != 0 && strcmp(solver_type.c_str(), "eigen_direct") != 0 && strcmp(solver_type.c_str(), "eigen_ilu_bicgstab") != 0)
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters solver_type should be either hypre_ilu_gmres, eigen_ilu_bicgstab or eigen_direct; current value: " << solver_type << "\n\n";
        throw bad_exception();
    }

    if (meshfile.find_last_of(".") + 1 == meshfile.size())
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters extension of meshfile should be msh; \nUnable to find extension in the meshfile: " << meshfile << "\n\n";
        throw bad_exception();
    }
    string extension = meshfile.substr(meshfile.find_last_of(".") + 1, meshfile.size());
    if (extension != "msh")
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters extension of meshfile should be msh; \nCurrent extension: " << extension << " of the meshfile: " << meshfile << "\n\n";
        throw bad_exception();
    }

    output_file_prefix = meshfile;
    if (output_file_prefix.find_last_of("/") < output_file_prefix.size()) //remove location details from mesh file name if required
        output_file_prefix = output_file_prefix.substr(output_file_prefix.find_last_of("/") + 1, output_file_prefix.size());
    if (output_file_prefix.find_last_of(".") < output_file_prefix.size()) //remove extension from mesh file name if required
        output_file_prefix.replace(output_file_prefix.begin() + output_file_prefix.find_last_of("."), output_file_prefix.end(), "");
    output_file_prefix += "_polydeg_" + to_string(poly_deg);
    cout << "PARAMETERS::verify_parameters output_file_prefix: " << output_file_prefix << endl;
}

void PARAMETERS::calc_cloud_num_points()
{
    if (dimension == 2)
        num_poly_terms = ((int)(0.5 * (poly_deg + 1) * (poly_deg + 2)));
    else
        num_poly_terms = ((int)((poly_deg + 1) * (poly_deg + 2) * (poly_deg + 3) / 6)); //sum(0.5*(1:poly_deg+1).*(2:poly_deg+2)) = (poly_deg + 1) * (poly_deg + 2) * (poly_deg + 3) / 6
    cloud_size = (int)(ceil(cloud_size_multiplier * num_poly_terms));
    cout << "PARAMETERS::calc_cloud_num_points num_poly_terms: " << num_poly_terms << ", cloud_size: " << cloud_size << endl;
}

void PARAMETERS::get_problem_dimension_msh()
{ //identify whether its a 2D or 3D gmsh grid

    clock_t start = clock();
    int dim = 2;
    FILE *file;
    int itemp, ncv, cv_type, eof_flag = 0;
    double dtemp;
    char temp[50];
    file = fopen(meshfile.c_str(), "r");
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$MeshFormat") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $MeshFormat not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    // fscanf(file, "%s", temp);
    fgets(temp, 50, file);
    if (strcmp(temp, "2.2 0 8\n") != 0)
    {
        printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh MeshFormat should be 2.2 0 8; but %s file has %s format instead\n\n", meshfile.c_str(), temp);
        throw bad_exception();
    }
    eof_flag = 0;
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$Nodes") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $Nodes not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    eof_flag = 0;
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$Elements") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $Elements not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    fscanf(file, "%i ", &ncv);
    // cout << "get_problem_dimension_msh ncv = " << ncv << endl;
    //reference: http://www.manpagez.com/info/gmsh/gmsh-2.2.6/gmsh_63.php
    for (int icv = 0; icv < ncv; icv++)
    {
        fscanf(file, "%i ", &itemp);   //cv number
        fscanf(file, "%i ", &cv_type); //cv type
        if (cv_type == 4 || cv_type == 5 || cv_type == 6 || cv_type == 7)
        { //4-node tetrahedron || 8-node hexahedron || 6-node prism || 5-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 11 || cv_type == 12 || cv_type == 13 || cv_type == 14)
        { //10-node tetrahedron || 27-node hexahedron || 18-node prism || 14-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 17 || cv_type == 18 || cv_type == 19)
        { //20-node hexahedron || 15-node prism || 13-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 29 || cv_type == 30 || cv_type == 31)
        { //20-node tetrahedron || 35-node tetrahedron || 56-node tetrahedron
            dim = 3;
            break; //3D element found
        }
        fscanf(file, "%*[^\n]\n"); //skip reading remaining row
    }
    dimension = dim;
    cout << "PARAMETERS::get_problem_dimension_msh problem dimension: " << dimension << endl;
}

void PARAMETERS::calc_polynomial_term_exponents()
{
    polynomial_term_exponents.resize(num_poly_terms, dimension);
    Eigen::MatrixXi deg_temp;
    if (dimension == 2)
    {
        polynomial_term_exponents.row(0) = Eigen::MatrixXi::Zero(1, dimension);                                        //constant term
        polynomial_term_exponents.block(1, 0, dimension, dimension) = Eigen::MatrixXi::Identity(dimension, dimension); //two linear terms
        int previous_index = 1;
        for (int deg = 2; deg <= poly_deg; deg++)
        { //quadratic onwards terms
            deg_temp.resize(deg + 1, dimension);
            for (int i = previous_index; i < previous_index + deg; i++)
            {
                deg_temp(i - previous_index, 0) = 1 + polynomial_term_exponents(i, 0); //exponents of 'x'
                deg_temp(i - previous_index, 1) = polynomial_term_exponents(i, 1);     //exponents of 'y'
            }
            deg_temp(deg, 0) = 0;
            deg_temp(deg, 1) = deg;
            previous_index = previous_index + deg;
            polynomial_term_exponents.block(previous_index, 0, deg + 1, dimension) = deg_temp;
        }
    }
    else
    {
        polynomial_term_exponents.row(0) = Eigen::MatrixXi::Zero(1, dimension);                                        //constant term
        polynomial_term_exponents.block(1, 0, dimension, dimension) = Eigen::MatrixXi::Identity(dimension, dimension); //three linear terms
        int previous_index = 1;
        for (int deg = 2; deg <= poly_deg; deg++)
        { //quadratic onwards terms
            deg_temp.resize(0.5 * (deg + 1) * (deg + 2), dimension);
            for (int i = previous_index; i < previous_index + (deg * (deg + 1) / 2); i++)
            {
                deg_temp(i - previous_index, 0) = 1 + polynomial_term_exponents(i, 0); //exponents of 'x'
                deg_temp(i - previous_index, 1) = polynomial_term_exponents(i, 1);     //exponents of 'y'
                deg_temp(i - previous_index, 2) = polynomial_term_exponents(i, 2);     //exponents of 'z'
            }
            for (int i = previous_index + (0.5 * deg * (deg - 1)); i < previous_index + (0.5 * deg * (deg + 1)); i++)
            {
                deg_temp(i - previous_index + deg, 0) = polynomial_term_exponents(i, 0);     //exponents of 'x'
                deg_temp(i - previous_index + deg, 1) = 1 + polynomial_term_exponents(i, 1); //exponents of 'y'
                deg_temp(i - previous_index + deg, 2) = polynomial_term_exponents(i, 2);     //exponents of 'z'
            }
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 0) = 0;
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 1) = 0;
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 2) = deg;
            previous_index = previous_index + (0.5 * deg * (deg + 1));
            polynomial_term_exponents.block(previous_index, 0, 0.5 * (deg + 1) * (deg + 2), dimension) = deg_temp;
        }
    }
}

void PARAMETERS::calc_dt(Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_x, Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_y, Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_z, Eigen::SparseMatrix<double, Eigen::RowMajor> &laplacian, double u0, double v0, double w0, double alpha)
{
    Eigen::VectorXcd eigval_grad_x, eigval_grad_y, eigval_grad_z, eigval_laplacian;
    eigval_grad_x = calc_largest_magnitude_eigenvalue(grad_x);
    eigval_grad_y = calc_largest_magnitude_eigenvalue(grad_y);
    if (dimension == 3)
        eigval_grad_z = calc_largest_magnitude_eigenvalue(grad_z);
    eigval_laplacian = calc_largest_magnitude_eigenvalue(laplacian);
    grad_x_eigval_real = eigval_grad_x[0].real(), grad_x_eigval_imag = eigval_grad_x[0].imag();
    grad_y_eigval_real = eigval_grad_y[0].real(), grad_y_eigval_imag = eigval_grad_y[0].imag();
    if (dimension == 3)
        grad_z_eigval_real = eigval_grad_z[0].real(), grad_z_eigval_imag = eigval_grad_z[0].imag();
    laplace_eigval_real = eigval_laplacian[0].real(), laplace_eigval_imag = eigval_laplacian[0].imag();
    printf("\nPARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_x: (%g, %g)\n", eigval_grad_x[0].real(), eigval_grad_x[0].imag());
    printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_y: (%g, %g)\n", eigval_grad_y[0].real(), eigval_grad_y[0].imag());
    if (dimension == 3)
        printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_z: (%g, %g)\n", eigval_grad_z[0].real(), eigval_grad_z[0].imag());
    printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: laplacian: (%g, %g)\n", eigval_laplacian[0].real(), eigval_laplacian[0].imag());

    double evalues = u0 * sqrt((eigval_grad_x[0].real() * eigval_grad_x[0].real()) + (eigval_grad_x[0].imag() * eigval_grad_x[0].imag()));
    evalues += v0 * sqrt((eigval_grad_y[0].real() * eigval_grad_y[0].real()) + (eigval_grad_y[0].imag() * eigval_grad_y[0].imag()));
    if (dimension == 3)
        evalues += w0 * sqrt((eigval_grad_z[0].real() * eigval_grad_z[0].real()) + (eigval_grad_z[0].imag() * eigval_grad_z[0].imag()));
    evalues += alpha * sqrt((eigval_laplacian[0].real() * eigval_laplacian[0].real()) + (eigval_laplacian[0].imag() * eigval_laplacian[0].imag()));
    dt = 2.0 / evalues; //forward Euler
    dt = dt * Courant;
    printf("PARAMETERS::calc_dt Courant: %g, dt: %g seconds\n\n", Courant, dt);
}


//
// Created by lejonmcgowan on 4/16/16.
//

#include "Transform.h"
#include <Eigen/Geometry>
#include <util/MathUtils.h>
Transform::Transform()
{

}

void Transform::addTranslate(const Eigen::Vector3f& translate)
{
    Eigen::Affine3f affineTranslate(Eigen::Translation3f(translate[0], translate[1], translate[2]));
    transformations.push_back(affineTranslate);
    setDirty();
}

void Transform::addRotate(const Eigen::Vector3f& rotate)
{
    Eigen::Affine3f rotx(Eigen::AngleAxisf(MathHelper::toRadians(rotate[0]), Eigen::Vector3f(1, 0, 0)));
    Eigen::Affine3f roty(Eigen::AngleAxisf(MathHelper::toRadians(rotate[1]), Eigen::Vector3f(0, 1, 0)));
    Eigen::Affine3f rotz(Eigen::AngleAxisf(MathHelper::toRadians(rotate[2]), Eigen::Vector3f(0, 0, 1)));
    transformations.push_back(rotz * roty * rotx);
    setDirty();
}

void Transform::addScale(const Eigen::Vector3f& scale)
{
    Eigen::Affine3f affineScale(Eigen::Scaling(scale[0], scale[1], scale[2]));
    transformations.push_back(affineScale);
    setDirty();
}

Eigen::Matrix4f Transform::getTransformMatrix()
{
    if (dirty)
    {
        Eigen::Matrix4f finalTransform;
        finalTransform.setIdentity();
        for (auto currentTransform: transformations)
            finalTransform = currentTransform.matrix() * finalTransform;
        transform = finalTransform;
    }
    dirty = false;
    return transform;
}

void Transform::setDirty()
{
    dirty = true;
    identity = false;
}

Ray Transform::transformRay(const Ray& ray)
{
    Ray transformRay;
    Eigen::Vector4f vectorO, vectorD;
    vectorO << ray.origin[0], ray.origin[1], ray.origin[2], 1;
    vectorD << ray.direction[0], ray.direction[1], ray.direction[2], 0;

    vectorO = getTransformMatrix() * vectorO;
    vectorD = getTransformMatrix() * vectorD;

    transformRay.origin << vectorO[0], vectorO[1], vectorO[2];
    transformRay.direction << vectorD[0], vectorD[1], vectorD[2];
    transformRay.direction.normalize();

    return transformRay;
}

Eigen::Vector3f Transform::transformPoint(const Eigen::Vector3f& point)
{
    Eigen::Vector3f transformPoint;
    Eigen::Vector4f vector;
    vector << point[0], point[1], point[2], 1;

    vector = getTransformMatrix() * vector;

    transformPoint << vector[0], vector[1], vector[2];

    return transformPoint;
}


//
// Copyright 2021 Sayan Chaudhuri <sayanchaudhuri758@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_FAST_FEATURE_DETECTOR_HPP
#define BOOST_GIL_IMAGE_PROCESSING_FAST_FEATURE_DETECTOR_HPP

#include <boost/gil/image.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/locator.hpp>
#include <boost/gil/point.hpp>
#include <algorithm>
#include <cmath>
#include <vector>

namespace boost { namespace gil {
namespace detail {

/// \brief Implements the FAST corner detection algorithm by Edward Rosten.
///       Algorithm :-
///       1.Consider a circle of 16 pixels around a given pixel(Bresenham circle)
///       2.Decide a threshold t
///       3.Let I_p be the intensity of the central pixel.
///       4.Find if there are n consecutive pixels in those 16 pixels where the intensities of all
///         pixels are <I_p-t or >I_p+t.Here n is taken as 9.
///       5.For step 4, to detect that a pixel is not a corner,check first whether the intensities
///         at pixels from 0 to 6 (with a step of 2)or the 8th pixel from each of them satisfies
///         condition of step 4. If not,check the same condition on odd numbered pixels or their
///         corresponding 8th pixel.
///       6.If the checks in step 5 is not affirmative, the given pixel cannot
///         be a corner.
///       7.Otherwise, proceed to find whether there are n consecutive pixels to satisfy the
///         criterion. If yes, p is a corner,else no.

///       @param buffer -type of source image.Must be grayscale
///       @param r      -row index of the candidate pixel
///       @param c      -column index of candidate pixel
///       @param points -pixel locations for bresenham circle around the given
///                       pixel
///       @param t      -threshold
template <typename SrcView>
bool fast_feature_detector(
    SrcView const& buffer,
    std::size_t r,
    std::size_t c,
    std::vector<point_t>& points,
    int t)
{
    auto src_loc = buffer.xy_at(c, r);
    std::vector<int> threshold_indicator;
    std::vector<int> intensity_array(16);
    std::vector<decltype(src_loc.cache_location(0, -1))> pointers(16);
    // storing intensities of pixels on circumference beforehand to decrease runtime
    for (std::ptrdiff_t i = 0; i < 16; i++)
    {
        pointers[i]        = src_loc.cache_location(points[i][0], points[i][1]);
        intensity_array[i] = src_loc[pointers[i]];
    }
    // calculating the flags to be used during segment test
    auto const I_p = buffer(point_t(c, r));

    std::transform(
        intensity_array.begin(),
        intensity_array.end(),
        back_inserter(threshold_indicator),
        [low = I_p - t, hi = I_p + t](auto const& intensity) {
            if (intensity < low)
                return -1;
            else if (intensity > hi)
                return 1;
            else
                return 0;
        });

    std::transform(
        intensity_array.begin(),
        intensity_array.end(),
        back_inserter(threshold_indicator),
        [low = I_p - t, hi = I_p + t](auto const& intensity) {
            if (intensity < low)
                return -1;
            else if (intensity > hi)
                return 1;
            else
                return 0;
        });

    // high speed test for eliminating non-corners
    for (std::ptrdiff_t i = 0; i <= 6; i += 2)
    {
        if (threshold_indicator[i] == 0 && threshold_indicator[i + 8] == 0)
            return false;
    }
    for (std::ptrdiff_t i = 1; i <= 7; i += 2)
    {
        if (threshold_indicator[i] == 0 && threshold_indicator[i + 8] == 0)
            return false;
    }

    // final segment test
    bool is_feature_point =
        threshold_indicator.end() !=
            std::search_n(threshold_indicator.begin(), threshold_indicator.end(), 9, -1) ||
        threshold_indicator.end() !=
            std::search_n(threshold_indicator.begin(), threshold_indicator.end(), 9, 1);
    return is_feature_point;
}

///\brief assigns a score to each detected corner to measure their degree of
/// cornerness.
///           Algorithm
///           Perform a binary search on threshold t to find out the maximum
///           threshold for which a corner remains a corner
///
///           @param src         -type of input image
///           @param i           -row index of the detected corner
///           @param j           -column index of the detected corner
///           @param points      -pixel locations for bresenham circle around
///                               the given pixel
///           @param threshold   -initial threshold given as input
template <typename SrcView>
std::size_t calculate_score(
    SrcView const& src,
    std::size_t i,
    std::size_t j,
    std::vector<point_t>& points,
    std::size_t threshold)
{
    std::size_t low  = threshold;
    std::size_t high = 255;
    // score measure used= highest threshold for which a corner remains a corner.
    // The cornerness of a corner decreases with increasing threshold
    while (high - low > 1)
    {
        std::size_t mid = (low + high) / 2;
        if (fast_feature_detector(src, i, j, points, mid))
        {
            low = mid;
        }
        else
        {
            high = mid - 1;
        }
    }
    return low - 1;
}
}  // namespace detail

/// \brief public function for using fast feature detector
///        @param src         -type of input image
///        @param keypoints   -vector for storing the locations of
///                            keypoints(corners)
///        @param scores      -vector for scores of each detected keypoint
///        @param nonmax      -indicates whether to perform nonmaximum
///                            suppression or not
///        @param threshold   -initial threshold given as input
template <typename SrcView>
void fast(
    SrcView const& src,
    std::vector<point_t>& keypoints,
    std::vector<std::size_t>& scores,
    bool nonmax           = true,
    std::size_t threshold = 10)
{
    // coordinates of a bresenham circle of radius 3
    std::vector<point_t> final_points_clockwise{
        point_t(3, 0),
        point_t(3, 1),
        point_t(2, 2),
        point_t(1, 3),
        point_t(0, 3),
        point_t(-1, 3),
        point_t(-2, 2),
        point_t(-3, 1),
        point_t(-3, 0),
        point_t(-3, -1),
        point_t(-2, -2),
        point_t(-1, -3),
        point_t(0, -3),
        point_t(1, -3),
        point_t(2, -2),
        point_t(3, -1)};
    // FAST features only calculated on grayscale images
    auto input_image_view = color_converted_view<gray8_pixel_t>(src);
    gray8_image_t fast_image(src.dimensions());
    // scores to be used during nonmaximum suppression
    gray8_view_t fast_score_matrix = view(fast_image);
    fill_pixels(fast_score_matrix, gray8_pixel_t(0));
    std::vector<point_t> kp;

    for (std::size_t i = 3; i < src.height() - 3; i++)
    {
        for (std::size_t j = 3; j < src.width() - 3; j++)
        {
            if (detail::fast_feature_detector(
                    input_image_view, i, j, final_points_clockwise, threshold))
            {
                kp.push_back(point_t(j, i));
            }
        }
    }

    for (auto u : kp)
    {
        int score = 0;
        score     = detail::calculate_score(
            input_image_view, u[1], u[0], final_points_clockwise, threshold);
        fast_score_matrix(u[0], u[1])[0] = gray8_pixel_t(score);
    }

    for (auto u : kp)
    {
        std::size_t i     = u[1];
        std::size_t j     = u[0];
        std::size_t score = 0;
        score             = int(fast_score_matrix(j, i)[0]);
        // performing nonmaximum suppression
        if (!nonmax || score > fast_score_matrix(j - 1, i)[0] &&
                           score > fast_score_matrix(j + 1, i)[0] &&
                           score > fast_score_matrix(j - 1, i - 1)[0] &&
                           score > fast_score_matrix(j, i - 1)[0] &&
                           score > fast_score_matrix(j + 1, i - 1)[0] &&
                           score > fast_score_matrix(j - 1, i + 1)[0] &&
                           score > fast_score_matrix(j, i + 1)[0] &&
                           score > fast_score_matrix(j + 1, i + 1)[0])
        {
            keypoints.push_back(u);
            scores.push_back(score);
        }
    }
}

}}  // namespace boost::gil
#endif