#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here

  // number of edges
  int N = mesh.NumEntities(1);
  // number of vertices
  int M = mesh.NumEntities(2);

  Eigen::SparseMatrix<int, Eigen::RowMajor> G(N,M);

  G.reserve(Eigen::VectorXi::Constant(N, 2));

  for(int i = 0; i < N; i++) {
    auto vertices = mesh.Entities(1)[i]->SubEntities(1);
    G.coeffRef(i, mesh.Index(*vertices[0])) = 1;
    G.coeffRef(i, mesh.Index(*vertices[1])) = -1;
  }

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {

  // number of cells
  int N = mesh.NumEntities(0);
  // number of edges
  int M = mesh.NumEntities(1);

  Eigen::SparseMatrix<int, Eigen::RowMajor> D(N,M);

  D.reserve(Eigen::VectorXi::Constant(N, 4));

  for(int i = 0; i < N; i++) {

    auto edges = mesh.Entities(0)[i]->SubEntities(1);

    for(int k = 0; k < edges.size(); k++) {
      D.coeffRef(i, mesh.Index(*edges[k])) = lf::mesh::to_sign(mesh.Entities(0)[i]->RelativeOrientations()[k]);
    }

  }

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  return (computeCellEdgeIncidenceMatrix(mesh) * computeEdgeVertexIncidenceMatrix(mesh)).norm() == 0;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Points
Compile: g++ -I $(BOOST_DIR) -Wall -std=c++11  -o 02_simplefeatures 02_simplefeatures.cpp
*/

#include<iostream>
#include<fstream>
#include <boost/geometry.hpp>

namespace bg = boost::geometry;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef bg::model::linestring<point> linestring;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon

// Note: the higher order objects usually have all features of a random_access container (or actually are some)

int main(int argc, char **argv)
{
    // let us make a nice triangle
    point a(0,0),b(1,1),c(2,0);
    polygon triangle;
    bg::append(bg::exterior_ring(triangle),a);
    bg::append(bg::exterior_ring(triangle),b);
    bg::append(bg::exterior_ring(triangle),c);
    std::cout << bg::wkt(a) << std::endl;
    std::cout << bg::wkt(b) << std::endl;
    std::cout << bg::wkt(c) << std::endl;
    std::cout << bg::wkt(triangle) << std::endl;

    // point in polygon (within relation)
    std::cout << std::boolalpha; // we want all bools to be written as true/false

    bg::correct(triangle); // make it CCW!
    std::cout << "Corrected Geometry: " << bg::wkt(triangle) << std::endl;
    std::cout << "Within: " << bg::within (bg::make<point>(1.0,0.5),triangle) << std::endl;

    // Let us now create our first file for QGIS:
    //
    // Algorithm in short:
    // 1) take two random (integer) triangles in [0,0]-[10,10].
    // 2) compute most important relations
    // 3) write a CSV file using WKT containing the two polygons for inspection

    std::srand(std::time(0)); //use current time as seed for random generator

    auto random_point = []()->point {return bg::make<point>(static_cast<int> (std::rand()%10),static_cast<int> (std::rand()%10));};

    //std::cout << bg::wkt(random_point()) << std::endl;
    polygon A,B;
    for (size_t i=0; i<3; i++) bg::append(bg::exterior_ring(A),random_point());
    for (size_t i=0; i<3; i++) bg::append(bg::exterior_ring(B),random_point());

    // remove these lines to see catastrophic results including negative area!
    bg::correct(A);
    bg::correct(B);

    std::cout << bg::wkt(A) << std::endl;
    std::cout << bg::wkt(B) << std::endl;

    // DE-9IM Matrix
    bg::de9im::matrix matrix = boost::geometry::relation(A,B);
    std::string code = matrix.str();
    std::cout << "relation: " << code << std::endl;
    // generic relate operation:

    bg::de9im::mask mask("T*F**F***"); // within
    auto p = random_point();
    bool check = bg::relate(p, A, mask);
    std::cout << "A random point " << bg::wkt(p) << " related: " << check << std::endl;

    // some algorithms relations:

    std::cout << "area: " << bg::area(A) << std::endl;
    std::cout << "covered_by: " <<  bg::covered_by (A,B) << std::endl;
    std::cout << "disjoint: " <<  bg::disjoint(A,B) << std::endl;
    std::cout << "equals: " <<  bg::equals(A,B) << std::endl;
    std::cout << "intersects: " <<  bg::intersects   (A,B) << std::endl;
    std::cout << "overlaps: " <<  bg::overlaps (A,B) << std::endl;
    std::cout << "touches: " <<  bg::touches (A,B) << std::endl;
    std::cout << "within: " <<  bg::within (A,B) << std::endl;

    // write CSV
    {
    std::ofstream ofs("geometry.csv");
    ofs << "wkt" << std::endl;
    ofs << bg::wkt(A)<< std::endl;
    ofs << bg::wkt(B) << std::endl;
    ofs.close();
    }

    // and now let us test within with random floating points
    {
    std::ofstream ofs("points.csv");
    ofs << "wkt; within" << std::endl;
    for (size_t i=0; i < 500; i++)
    {
	auto p = bg::make<point>( static_cast<double>(std::rand())/RAND_MAX*10.0,static_cast<double>(std::rand())/RAND_MAX*10.0);
	auto rel1 = bg::within(p,A);
	auto rel2 = bg::within(p,B);
	int score = (rel1 << 1) + rel2;
	ofs << bg::wkt(p) << ";" << score << std::endl;
    }
    }

    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 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.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Toby D. Young, Polish Academy of Sciences,
 *          Wolfgang Bangerth, Texas A&M University
 */

// @sect3{Include files}

// 正如介绍中提到的，本程序基本上只是  step-4  的一个小修改版本。因此，以下大部分的include文件都是在那里使用的，或者至少是在以前的教程程序中已经使用的。

#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/utilities.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/full_matrix.h>

// IndexSet用于设置每个  PETScWrappers::MPI::Vector:  的大小。
#include <deal.II/base/index_set.h>

// PETSc出现在这里是因为SLEPc依赖于这个库。

#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>

// 然后我们需要实际导入SLEPc提供的求解器接口。

#include <deal.II/lac/slepc_solver.h>

// 我们还需要一些标准的C++。

#include <fstream>
#include <iostream>

// 最后，和以前的程序一样，我们将所有的deal.II类和函数名导入到本程序中所有的名字空间中。

namespace Step36
{
  using namespace dealii;
// @sect3{The EigenvalueProblem class template}

// 下面是主类模板的类声明。它看起来和在  step-4  中已经展示过的差不多了。

  template <int dim>
  class EigenvalueProblem
  {
  public:
    EigenvalueProblem(const std::string &prm_file);
    void run();

  private:
    void         make_grid_and_dofs();
    void         assemble_system();
    unsigned int solve();
    void         output_results() const;

    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;

// 有了这些例外情况。对于我们的特征值问题，我们既需要左手边的刚度矩阵，也需要右手边的质量矩阵。我们还需要的不仅仅是一个解函数，而是一整套我们想要计算的特征函数，以及相应的特征值。

    PETScWrappers::SparseMatrix             stiffness_matrix, mass_matrix;
    std::vector<PETScWrappers::MPI::Vector> eigenfunctions;
    std::vector<double>                     eigenvalues;

// 然后，我们需要一个对象来存储几个运行时参数，我们将在输入文件中指定。

    ParameterHandler parameters;

// 最后，我们将有一个对象，包含对我们自由度的 "约束"。如果我们有自适应细化的网格（目前的程序中没有），这可能包括悬挂节点约束。这里，我们将存储边界节点的约束  $U_i=0$  。

    AffineConstraints<double> constraints;
  };
// @sect3{Implementation of the EigenvalueProblem class}
// @sect4{EigenvalueProblem::EigenvalueProblem}

// 首先是构造函数。主要的新部分是处理运行时的输入参数。我们需要首先声明它们的存在，然后从输入文件中读取它们的值，该文件的名称被指定为该函数的参数。

  template <int dim>
  EigenvalueProblem<dim>::EigenvalueProblem(const std::string &prm_file)
    : fe(1)
    , dof_handler(triangulation)
  {

// TODO研究为什么获得正确的特征值退化所需的最小细化步骤数为6

    parameters.declare_entry(
      "Global mesh refinement steps",
      "5",
      Patterns::Integer(0, 20),
      "The number of times the 1-cell coarse mesh should "
      "be refined globally for our computations.");
    parameters.declare_entry("Number of eigenvalues/eigenfunctions",
                             "5",
                             Patterns::Integer(0, 100),
                             "The number of eigenvalues/eigenfunctions "
                             "to be computed.");
    parameters.declare_entry("Potential",
                             "0",
                             Patterns::Anything(),
                             "A functional description of the potential.");

    parameters.parse_input(prm_file);
  }
// @sect4{EigenvalueProblem::make_grid_and_dofs}

// 下一个函数在域 $[-1,1]^d$ 上创建一个网格，根据输入文件的要求对其进行多次细化，然后给它附加一个DoFHandler，将矩阵和向量初始化为正确的大小。我们还建立了对应于边界值的约束  $u|_{\partial\Omega}=0$  。

// 对于矩阵，我们使用PETSc包装器。这些包装器能够在非零条目被添加时分配必要的内存。这看起来效率很低：我们可以先计算稀疏模式，用它来初始化矩阵，然后在我们插入条目时，我们可以确定我们不需要重新分配内存和释放之前使用的内存。一种方法是使用这样的代码。用
// @code
//    DynamicSparsityPattern
//       dsp (dof_handler.n_dofs(),
//            dof_handler.n_dofs());
//    DoFTools::make_sparsity_pattern (dof_handler, dsp);
//    dsp.compress ();
//    stiffness_matrix.reinit (dsp);
//    mass_matrix.reinit (dsp);
//  @endcode
//  代替下面两个 reinit() 的刚度和质量矩阵的调用。

// 不幸的是，这并不完全可行。上面的代码可能会导致在非零模式下的一些条目，我们只写零条目；最值得注意的是，对于那些属于边界节点的行和列的非对角线条目，这一点是成立的。这不应该是一个问题，但是不管什么原因，PETSc的ILU预处理程序（我们用来解决特征值求解器中的线性系统）不喜欢这些额外的条目，并以错误信息中止。

// 在没有任何明显的方法来避免这种情况的情况下，我们干脆选择第二种最好的方法，即让PETSc在必要时分配内存。也就是说，由于这不是一个时间上的关键部分，这整个事件就不再重要了。

  template <int dim>
  void EigenvalueProblem<dim>::make_grid_and_dofs()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(
      parameters.get_integer("Global mesh refinement steps"));
    dof_handler.distribute_dofs(fe);

    DoFTools::make_zero_boundary_constraints(dof_handler, constraints);
    constraints.close();

    stiffness_matrix.reinit(dof_handler.n_dofs(),
                            dof_handler.n_dofs(),
                            dof_handler.max_couplings_between_dofs());
    mass_matrix.reinit(dof_handler.n_dofs(),
                       dof_handler.n_dofs(),
                       dof_handler.max_couplings_between_dofs());

// 下一步是处理特征谱的问题。在这种情况下，输出是特征值和特征函数，所以我们将特征函数和特征值列表的大小设置为与我们在输入文件中要求的一样大。当使用 PETScWrappers::MPI::Vector, 时，Vector是使用IndexSet初始化的。IndexSet不仅用于调整 PETScWrappers::MPI::Vector 的大小，而且还将 PETScWrappers::MPI::Vector 中的一个索引与一个自由度联系起来（更详细的解释见 step-40 ）。函数complete_index_set()创建了一个IndexSet，每个有效的索引都是这个集合的一部分。请注意，这个程序只能按顺序运行，如果并行使用，将抛出一个异常。

    IndexSet eigenfunction_index_set = dof_handler.locally_owned_dofs();
    eigenfunctions.resize(
      parameters.get_integer("Number of eigenvalues/eigenfunctions"));
    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      eigenfunctions[i].reinit(eigenfunction_index_set, MPI_COMM_WORLD);

    eigenvalues.resize(eigenfunctions.size());
  }
// @sect4{EigenvalueProblem::assemble_system}

// 在这里，我们从局部贡献 $A^K_{ij} = \int_K \nabla\varphi_i(\mathbf x) \cdot \nabla\varphi_j(\mathbf x) + V(\mathbf x)\varphi_i(\mathbf x)\varphi_j(\mathbf x)$ 和 $M^K_{ij} = \int_K \varphi_i(\mathbf x)\varphi_j(\mathbf x)$ 中分别组合出全局刚度和质量矩阵。如果你看过以前的教程程序，这个函数应该会很熟悉。唯一新的东西是使用我们从输入文件中得到的表达式，设置一个描述势 $V(\mathbf x)$ 的对象。然后我们需要在每个单元的正交点上评估这个对象。如果你见过如何评估函数对象（例如，见 step-5 中的系数），这里的代码也会显得相当熟悉。

  template <int dim>
  void EigenvalueProblem<dim>::assemble_system()
  {
    QGauss<dim> quadrature_formula(fe.degree + 1);

    FEValues<dim> fe_values(fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_stiffness_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);

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

    FunctionParser<dim> potential;
    potential.initialize(FunctionParser<dim>::default_variable_names(),
                         parameters.get("Potential"),
                         typename FunctionParser<dim>::ConstMap());

    std::vector<double> potential_values(n_q_points);
    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_values.reinit(cell);
        cell_stiffness_matrix = 0;
        cell_mass_matrix      = 0;

        potential.value_list(fe_values.get_quadrature_points(),
                             potential_values);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                cell_stiffness_matrix(i, j) +=           //
                  (fe_values.shape_grad(i, q_point) *    //
                     fe_values.shape_grad(j, q_point)    //
                   +                                     //
                   potential_values[q_point] *           //
                     fe_values.shape_value(i, q_point) * //
                     fe_values.shape_value(j, q_point)   //
                   ) *                                   //
                  fe_values.JxW(q_point);                //

                cell_mass_matrix(i, j) +=              //
                  (fe_values.shape_value(i, q_point) * //
                   fe_values.shape_value(j, q_point)   //
                   ) *                                 //
                  fe_values.JxW(q_point);              //
              }

// 现在我们有了本地矩阵的贡献，我们把它们转移到全局对象中，并处理好零边界约束。

        cell->get_dof_indices(local_dof_indices);

        constraints.distribute_local_to_global(cell_stiffness_matrix,
                                               local_dof_indices,
                                               stiffness_matrix);
        constraints.distribute_local_to_global(cell_mass_matrix,
                                               local_dof_indices,
                                               mass_matrix);
      }

// 在函数的最后，我们告诉PETSc，矩阵现在已经完全组装好了，稀疏矩阵表示法现在可以被压缩了，因为不会再添加任何条目。

    stiffness_matrix.compress(VectorOperation::add);
    mass_matrix.compress(VectorOperation::add);

// 在离开函数之前，我们计算虚假的特征值，这些特征值是由零Dirichlet约束引入到系统中的。正如介绍中所讨论的，使用Dirichlet边界条件，加上位于域的边界的自由度仍然是我们所求解的线性系统的一部分，引入了一些虚假的特征值。下面，我们输出它们所处的区间，以确保我们在计算中出现时可以忽略它们。

    double min_spurious_eigenvalue = std::numeric_limits<double>::max(),
           max_spurious_eigenvalue = -std::numeric_limits<double>::max();

    for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
      if (constraints.is_constrained(i))
        {
          const double ev         = stiffness_matrix(i, i) / mass_matrix(i, i);
          min_spurious_eigenvalue = std::min(min_spurious_eigenvalue, ev);
          max_spurious_eigenvalue = std::max(max_spurious_eigenvalue, ev);
        }

    std::cout << "   Spurious eigenvalues are all in the interval "
              << "[" << min_spurious_eigenvalue << ","
              << max_spurious_eigenvalue << "]" << std::endl;
  }
// @sect4{EigenvalueProblem::solve}

// 这是该程序的关键新功能。现在系统已经设置好了，现在是实际解决问题的好时机：和其他例子一样，这是使用 "解决 "程序来完成的。从本质上讲，它的工作原理与其他程序一样：你设置一个SolverControl对象，描述我们要解决的线性系统的精度，然后我们选择我们想要的解算器类型。这里我们选择了SLEPc的Krylov-Schur求解器，对于这类问题来说，这是一个相当快速和强大的选择。

  template <int dim>
  unsigned int EigenvalueProblem<dim>::solve()
  {

// 我们从这里开始，就像我们通常做的那样，指定我们想要的收敛控制。

    SolverControl                    solver_control(dof_handler.n_dofs(), 1e-9);
    SLEPcWrappers::SolverKrylovSchur eigensolver(solver_control);

// 在我们实际求解特征函数和-值之前，我们还必须选择哪一组特征值来求解。让我们选择那些实部最小的特征值和相应的特征函数（事实上，我们在这里解决的问题是对称的，所以特征值是纯实部的）。之后，我们就可以真正让SLEPc做它的工作了。

    eigensolver.set_which_eigenpairs(EPS_SMALLEST_REAL);

    eigensolver.set_problem_type(EPS_GHEP);

    eigensolver.solve(stiffness_matrix,
                      mass_matrix,
                      eigenvalues,
                      eigenfunctions,
                      eigenfunctions.size());

// 上述调用的输出是一组向量和数值。在特征值问题中，特征函数只确定到一个常数，这个常数可以很随意地固定。由于对特征值问题的原点一无所知，SLEPc除了将特征向量归一到 $l_2$ （向量）准则外，没有其他选择。不幸的是，这个规范与我们从特征函数角度可能感兴趣的任何规范没有什么关系： $L_2(\Omega)$ 规范，或者也许是 $L_\infty(\Omega)$ 规范。

//让我们选择后者，重新划分特征函数的尺度，使其具有 $\|\phi_i(\mathbf x)\|_{L^\infty(\Omega)}=1$ 而不是 $\|\Phi\|_{l_2}=1$ （其中 $\phi_i$ 是 $i$ 第三个特征function， $\Phi_i$ 是相应的结点值矢量）。对于这里选择的 $Q_1$ 元素，我们知道函数 $\phi_i(\mathbf x)$ 的最大值是在其中一个节点达到的，所以 $\max_{\mathbf x}\phi_i(\mathbf x)=\max_j (\Phi_i)_j$ ，使得在 $L_\infty$ 准则下的归一化是微不足道的。请注意，如果我们选择 $Q_k$ 元素与 $k>1$ ，这就不容易了：在那里，一个函数的最大值不一定要在一个节点上达到，所以 $\max_{\mathbf x}\phi_i(\mathbf x)\ge\max_j (\Phi_i)_j$ （尽管平等通常几乎是真的）。

    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      eigenfunctions[i] /= eigenfunctions[i].linfty_norm();

// 最后返回收敛所需的迭代次数。

    return solver_control.last_step();
  }
// @sect4{EigenvalueProblem::output_results}

// 这是本程序的最后一个重要功能。它使用DataOut类来生成特征函数的图形输出，以便以后进行可视化。它的工作原理与其他许多教程中的程序一样。

// 整个函数的集合被输出为一个单一的VTK文件。

  template <int dim>
  void EigenvalueProblem<dim>::output_results() const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);

    for (unsigned int i = 0; i < eigenfunctions.size(); ++i)
      data_out.add_data_vector(eigenfunctions[i],
                               std::string("eigenfunction_") +
                                 Utilities::int_to_string(i));

// 唯一值得讨论的可能是，由于势在输入文件中被指定为函数表达式，因此最好能将其与特征函数一起以图形形式表示。实现这一目的的过程相对简单：我们建立一个代表 $V(\mathbf x)$ 的对象，然后将这个连续函数插值到有限元空间。我们还将结果附加到DataOut对象上，以便进行可视化。

    Vector<double> projected_potential(dof_handler.n_dofs());
    {
      FunctionParser<dim> potential;
      potential.initialize(FunctionParser<dim>::default_variable_names(),
                           parameters.get("Potential"),
                           typename FunctionParser<dim>::ConstMap());
      VectorTools::interpolate(dof_handler, potential, projected_potential);
    }
    data_out.add_data_vector(projected_potential, "interpolated_potential");

    data_out.build_patches();

    std::ofstream output("eigenvectors.vtk");
    data_out.write_vtk(output);
  }
// @sect4{EigenvalueProblem::run}

// 这是一个对一切都有顶层控制的函数。它几乎与  step-4  中的内容完全相同。

  template <int dim>
  void EigenvalueProblem<dim>::run()
  {
    make_grid_and_dofs();

    std::cout << "   Number of active cells:       "
              << triangulation.n_active_cells() << std::endl
              << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;

    assemble_system();

    const unsigned int n_iterations = solve();
    std::cout << "   Solver converged in " << n_iterations << " iterations."
              << std::endl;

    output_results();

    std::cout << std::endl;
    for (unsigned int i = 0; i < eigenvalues.size(); ++i)
      std::cout << "      Eigenvalue " << i << " : " << eigenvalues[i]
                << std::endl;
  }
} // namespace Step36
// @sect3{The main function}
int main(int argc, char **argv)
{
  try
    {
      using namespace dealii;
      using namespace Step36;

      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

// 这个程序只能在串行中运行。否则，将抛出一个异常。

      AssertThrow(Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) == 1,
                  ExcMessage(
                    "This program can only be run in serial, use ./step-36"));

      EigenvalueProblem<2> problem("step-36.prm");
      problem.run();
    }

// 在这期间，我们一直在注意是否有任何异常应该被生成。如果是这样的话，我们就会惊慌失措...

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

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

// 如果没有抛出异常，我们就告诉程序不要再胡闹了，乖乖地退出。

  std::cout << std::endl << "   Job done." << std::endl;

  return 0;
}


#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <Eigen/StdVector>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string estimated_file = "./estimated.txt";
string groundtruth_file = "./groundtruth.txt";

void DrawTrajectory(vector<Sophus::SE3> estimated_poses,vector<Sophus::SE3> groundtruth_poses) {
    if (estimated_poses.empty() || groundtruth_poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);

        for (size_t i = 0; i < estimated_poses.size() - 1; i++) {
            glColor3f(1 - (float) i / estimated_poses.size(), 0.0f, (float) i / estimated_poses.size());
            glBegin(GL_LINES);
            auto p1 = estimated_poses[i], p2 = estimated_poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for (size_t i = 0; i < groundtruth_poses.size() - 1; i++) {
            glColor3f(1 - (float) i / groundtruth_poses.size(), 0.0f, (float) i / groundtruth_poses.size());
            glBegin(GL_LINES);
            auto p1 = groundtruth_poses[i], p2 = groundtruth_poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

vector<Sophus::SE3> readPose(string filename)
{
    vector<Sophus::SE3> poses;
    /// implement pose reading code
    // start your code here (5~10 lines)

    ifstream fin(filename);
    for ( int i=0; i<612; i++ )
    {
        double data[8] = {0};
        for ( auto& d:data )
            fin>>d;
        Eigen::Quaterniond q( data[7], data[4], data[5], data[6] );
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3_qt(q,t);
        poses.push_back(SE3_qt);
    }

    return poses;
    // end your code here

}

double RMSE(vector<Sophus::SE3> estimated_poses,vector<Sophus::SE3> groundtruth_poses)
{
    double sum = 0;
    for(int i=0;i<estimated_poses.size();i++)
    {
        double error = 0;
        double e = sqrt(  (groundtruth_poses.at(i).inverse() * estimated_poses.at(i)).log().transpose() *   (groundtruth_poses.at(i).inverse() * estimated_poses.at(i)).log()   );

        error = pow(abs(e),2);
        sum+= error;
    }
    double rmse = pow(sum/estimated_poses.size(),0.5);
    return rmse;
}

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

    vector<Sophus::SE3> estimated_poses;
    vector<Sophus::SE3> groundtruth_poses;

    estimated_poses = readPose(estimated_file);
    groundtruth_poses = readPose(groundtruth_file);

    std::cout<<RMSE(estimated_poses,groundtruth_poses)<<endl;;
    DrawTrajectory(estimated_poses,groundtruth_poses);
    return 0;
}


#include <Eigen/Dense>
#include <iostream>

typedef Eigen::Matrix<float, 3, 3> MyMatrix33f;
typedef Eigen::Matrix<float, 3, 1> MyVector3f;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MyMatrix;

int main() {
  {
    // declaration
    MyMatrix33f a;
    MyVector3f v;
    MyMatrix m(10, 15);

    // initialization
    a = MyMatrix33f::Zero();
    std::cout << "Zero matrix:\n" << a << std::endl;

    a = MyMatrix33f::Identity();
    std::cout << "Identity matrix:\n" << a << std::endl;

    v = MyVector3f::Random();
    std::cout << "Random vector:\n" << v << std::endl;

    a << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    std::cout << "Comma initilized matrix:\n" << a << std::endl;

    a(0, 0) = 3;
    std::cout << "Matrix with changed element[0][0]:\n" << a << std::endl;

    int data[] = {1, 2, 3, 4};
    Eigen::Map<Eigen::RowVectorXi> v_map(data, 4);
    std::cout << "Row vector mapped to array:\n" << v_map << std::endl;

    std::vector<float> vdata = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    Eigen::Map<MyMatrix33f> a_map(vdata.data());
    std::cout << "Matrix mapped to array:\n" << a_map << std::endl;
  }
  // arithmetic
  {
    Eigen::Matrix2d a;
    a << 1, 2, 3, 4;
    Eigen::Matrix2d b;
    b << 1, 2, 3, 4;

    // element wise operations
    Eigen::Matrix2d result = a.array() * b.array();
    std::cout << "element wise a * b :\n" << result << std::endl;

    result = a.array() / b.array();
    std::cout << "element wise a / b :\n" << result << std::endl;

    a = b.array() * 4;
    std::cout << "element wise a = b * 4 :\n" << a << std::endl;

    // matrix operations
    result = a + b;
    std::cout << "matrices a + b :\n" << result << std::endl;

    a += b;
    std::cout << "matrices a += b :\n" << result << std::endl;

    result = a * b;
    std::cout << "matrices a * b :\n" << result << std::endl;
  }

  // patial access
  {
    Eigen::MatrixXf m = Eigen::MatrixXf::Random(4, 4);
    std::cout << "Random 4x4 matrix :\n" << m << std::endl;

    Eigen::Matrix2f b =
        m.block(1, 1, 2, 2);  // coping the middle part of matrix
    std::cout << "Middle of 4x4 matrix :\n" << b << std::endl;

    m.block(1, 1, 2, 2) *= 0;  // change values in original matrix
    std::cout << "Modified middle of 4x4 matrix :\n" << m << std::endl;

    m.row(1).array() += 3;
    std::cout << "Modified row of 4x4 matrix :\n" << m << std::endl;

    m.col(2).array() /= 4;
    std::cout << "Modified col of 4x4 matrix :\n" << m << std::endl;
  }

  // broadcasting
  {
    Eigen::MatrixXf mat = Eigen::MatrixXf::Random(2, 4);
    std::cout << "Random 2x4 matrix :\n" << mat << std::endl;

    Eigen::VectorXf v(2);  // column vector
    v << 100, 100;
    mat.colwise() += v;
    std::cout << "Sum broadcasted over columns :\n" << mat << std::endl;
  }
  return 0;
};


/*
 * Instruction.cpp
 *
 *  Created on: 2015/01/18
 *      Author: kryozahiro
 */

#include "Instruction.h"

#include <cassert>
#include <cmath>
#include <boost/lexical_cast.hpp>
using namespace std;

void Instruction::set(Opcode op, int ret, int mem1, int mem2) {
	this->op = op;
	this->ret = ret;
	this->arg1 = mem1;
	this->arg2 = mem2;
}

void Instruction::operator()(unsigned int& pc, unsigned int end, bool& condition, std::vector<double>& memory) const {
	//条件が満たされていないときに分岐以外の命令に到達したらスキップする
	if (!condition and op != Opcode::IF and op != Opcode::IF_GT and op != Opcode::IF_LE) {
		condition = true;
		return;
	}

	switch (op) {
	case Opcode::AND:
		memory[ret] = static_cast<int>(memory[arg1]) & static_cast<int>(memory[arg2]);
		break;
	case Opcode::OR:
		memory[ret] = static_cast<int>(memory[arg1]) | static_cast<int>(memory[arg2]);
		break;
	case Opcode::NOT:
		memory[ret] = ~static_cast<int>(memory[arg1]);
		break;
	case Opcode::ADD:
		memory[ret] = memory[arg1] + memory[arg2];
		break;
	case Opcode::SUB:
		memory[ret] = memory[arg1] - memory[arg2];
		break;
	case Opcode::MUL:
		memory[ret] = memory[arg1] * memory[arg2];
		break;
	case Opcode::DIV:
		memory[ret] = (memory[arg2] != 0) ? memory[arg1] / memory[arg2] : 0;
		break;
	case Opcode::IF:
		condition &= static_cast<int>(memory[arg1]);
		break;
	case Opcode::IF_GT:
		condition &= (memory[arg1] > memory[arg2]);
		break;
	case Opcode::IF_LE:
		condition &= (memory[arg1] <= memory[arg2]);
		break;
	case Opcode::JMP:
		pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		break;
	case Opcode::JG:
		if (memory[arg1] > 0) {
			pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		}
		break;
	case Opcode::JLE:
		if (memory[arg1] <= 0) {
			pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		}
		break;
	case Opcode::SIN:
		memory[ret] = sin(memory[arg1]);
		break;
	case Opcode::COS:
		memory[ret] = cos(memory[arg1]);
		break;
	case Opcode::SQRT:
		memory[ret] = sqrt(abs(memory[arg1]));
		break;
	case Opcode::EXP:
		memory[ret] = exp(memory[arg1]);
		break;
	case Opcode::LOG:
		memory[ret] = (memory[arg1] != 0) ? log(abs(memory[arg1])) : 0;
		break;
	case Opcode::IMM:
		memory[ret] = arg1 * memory.size() + arg2;
		break;
	case Opcode::NOP:
		//do nothing
		break;
	default:
		assert(false);
	}
}

string Instruction::toString() const {
	return "[" + to_string(ret) + "] " + boost::lexical_cast<string>(op) + " [" + to_string(arg1) + "] [" + to_string(arg2) + "] ";
}

Instruction::Opcode Instruction::getOpcode() const {
	return op;
}

int Instruction::getRet() const {
	return ret;
}

int Instruction::getArg1() const {
	return arg1;
}

int Instruction::getArg2() const {
	return arg2;
}


#ifndef SSMPACK_MODEL_LINEAR_GAUSSIAN_HPP
#define SSMPACK_MODEL_LINEAR_GAUSSIAN_HPP

#include <armadillo>

namespace ssmkit {
namespace map {

struct LinearGaussian {
  using TParameter = std::tuple<arma::vec, arma::mat>;
  using TConditionVAR = arma::vec;

  LinearGaussian(arma::mat trans, arma::mat cov) : transfer{trans},
  covariance{cov} {}
// should not be overloaded, should not be template
  TParameter operator()(const TConditionVAR &x) const {
    return std::make_tuple(transfer * x, covariance);
  }

  arma::mat transfer;
  arma::mat covariance;
};

} // namespace map
} // namespace ssmkit

#endif //SSMPACK_MODEL_LINEAR_GAUSSIAN_HPP


/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp

 [begin_description]
 Coefficients for the Adams Moulton method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< class Value , size_t Steps >
class adams_moulton_coefficients ;

template< class Value >
class adams_moulton_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 1 >()
      {
        (*this)[0] = static_cast< Value >( 1 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 2 >()
      {
        (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 3 >()
      {
        (*this)[0] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
        (*this)[1] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
        (*this)[2] = -static_cast< Value >( 1 ) / static_cast< Value >( 12 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 4 >()
      {
        (*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 8 );
        (*this)[1] = static_cast< Value >( 19 ) / static_cast< Value >( 24 );
        (*this)[2] = -static_cast< Value >( 5 ) / static_cast< Value >( 24 );
        (*this)[3] = static_cast< Value >( 1 ) / static_cast< Value >( 24 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 5 >()
      {
        (*this)[0] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
        (*this)[1] = static_cast< Value >( 323 ) / static_cast< Value >( 360 );
        (*this)[2] = -static_cast< Value >( 11 ) / static_cast< Value >( 30 );
        (*this)[3] = static_cast< Value >( 53 ) / static_cast< Value >( 360 );
        (*this)[4] = -static_cast< Value >( 19 ) / static_cast< Value >( 720 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 6 >()
      {
        (*this)[0] = static_cast< Value >( 95 ) / static_cast< Value >( 288 );
        (*this)[1] = static_cast< Value >( 1427 ) / static_cast< Value >( 1440 );
        (*this)[2] = -static_cast< Value >( 133 ) / static_cast< Value >( 240 );
        (*this)[3] = static_cast< Value >( 241 ) / static_cast< Value >( 720 );
        (*this)[4] = -static_cast< Value >( 173 ) / static_cast< Value >( 1440 );
        (*this)[5] = static_cast< Value >( 3 ) / static_cast< Value >( 160 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 7 >()
      {
        (*this)[0] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
        (*this)[1] = static_cast< Value >( 2713 ) / static_cast< Value >( 2520 );
        (*this)[2] = -static_cast< Value >( 15487 ) / static_cast< Value >( 20160 );
        (*this)[3] = static_cast< Value >( 586 ) / static_cast< Value >( 945 );
        (*this)[4] = -static_cast< Value >( 6737 ) / static_cast< Value >( 20160 );
        (*this)[5] = static_cast< Value >( 263 ) / static_cast< Value >( 2520 );
        (*this)[6] = -static_cast< Value >( 863 ) / static_cast< Value >( 60480 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 8 >()
      {
        (*this)[0] = static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
        (*this)[1] = static_cast< Value >( 139849 ) / static_cast< Value >( 120960 );
        (*this)[2] = -static_cast< Value >( 4511 ) / static_cast< Value >( 4480 );
        (*this)[3] = static_cast< Value >( 123133 ) / static_cast< Value >( 120960 );
        (*this)[4] = -static_cast< Value >( 88547 ) / static_cast< Value >( 120960 );
        (*this)[5] = static_cast< Value >( 1537 ) / static_cast< Value >( 4480 );
        (*this)[6] = -static_cast< Value >( 11351 ) / static_cast< Value >( 120960 );
        (*this)[7] = static_cast< Value >( 275 ) / static_cast< Value >( 24192 );
      }
};

} // detail
} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED


//
// Copyright © 2019 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "LogSoftmax.hpp"

#include <TensorUtils.hpp>

#include <cmath>

#include <boost/assert.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace
{

inline bool ValidateAxis(int axis, unsigned int numDimensions)
{
    const int sNumDimensions = boost::numeric_cast<int>(numDimensions);
    return axis < sNumDimensions && axis >= -sNumDimensions;
}

} // anonymous namespace

namespace armnn
{

void LogSoftmax(Decoder<float>& input,
                Encoder<float>& output,
                const TensorInfo& inputInfo,
                const LogSoftmaxDescriptor& descriptor)
{
    const unsigned int numDimensions = inputInfo.GetNumDimensions();

    bool axisIsValid = ValidateAxis(descriptor.m_Axis, numDimensions);
    BOOST_ASSERT_MSG(axisIsValid,
        "Axis index is not in range [-numDimensions, numDimensions).");
    boost::ignore_unused(axisIsValid);

    unsigned int uAxis = descriptor.m_Axis < 0  ?
        numDimensions - boost::numeric_cast<unsigned int>(std::abs(descriptor.m_Axis)) :
        boost::numeric_cast<unsigned int>(descriptor.m_Axis);

    const TensorShape& inputShape = inputInfo.GetShape();
    const unsigned int outerSize  = armnnUtils::GetNumElementsBetween(inputShape, 0, uAxis);
    const unsigned int axisSize   = inputShape[uAxis];
    const unsigned int innerSize  = armnnUtils::GetNumElementsBetween(inputShape,
                                                                      uAxis + 1,
                                                                      inputShape.GetNumDimensions());

    for (unsigned int outer = 0; outer < outerSize; ++outer)
    {
        for (unsigned int inner = 0; inner < innerSize; ++inner)
        {
            // Find max
            input[outer * axisSize * innerSize + inner];
            float maxValue = input.Get();
            for (unsigned int i = 1u; i < axisSize; ++i)
            {
                input[(outer * axisSize + i) * innerSize + inner];
                maxValue = std::max(maxValue, input.Get());
            }

            // Compute sum
            float sum = 0.0f;
            for (unsigned int i = 0u; i < axisSize; ++i)
            {
                input[(outer * axisSize + i) * innerSize + inner];
                sum += std::exp((input.Get() - maxValue) * descriptor.m_Beta);
            }

            // Compute log sum
            const float logSum = std::log(sum);

            // Compute result
            for (unsigned int i = 0u; i < axisSize; ++i)
            {
                const unsigned int index = (outer * axisSize + i) * innerSize + inner;

                input [index];
                output[index];

                output.Set((input.Get() - maxValue) * descriptor.m_Beta - logSum);
            }
        }
    }
}

} // namespace armnn


#include "CirclePatterns.h"
#include <Eigen/SparseQR>

CirclePatterns::CirclePatterns(shared_ptr<ManifoldSurfaceMesh> mesh0, Vertex infVertex,
    EdgeData<bool> eMask, EdgeData<bool> eBdry,FaceData<bool> fMask, int optScheme0,
    vector<double>& solve, Eigen::VectorXd thetas):
mesh(mesh0),
infVertex(infVertex),
eMask(eMask),
eBdry(eBdry),
fMask(fMask),
angles(mesh->nHalfedges()),
thetas(thetas),
radii(mesh->nFaces()),
eIntIndices(mesh->nEdges()),
imaginaryHe(0),
OptScheme(optScheme0)
{
    // I added a plus 1 here and at radii; should figure why I need to
    solver.n = mesh->nFaces();
    sol = solve;
    uv = VertexData<Eigen::Vector2d> (*mesh);
    eInd = mesh->getEdgeIndices();
    vInd = mesh->getVertexIndices();
    fInd = mesh->getFaceIndices();
}

inline double Cl2(double x) {
    if (x == 0.0) return 0.0;
    x = std::remainder(x, 2*M_PI);
    if (x == 0.0) return 0.0;

    if (fabs(x) <= 2.0944) {
        double xx = x * x;
        return ((((((((((((2.3257441143020875e-22 * xx
                           + 1.0887357368300848e-20) * xx
                           + 5.178258806090624e-19) * xx
                           + 2.5105444608999545e-17) * xx
                           + 1.2462059912950672e-15) * xx
                           + 6.372636443183181e-14) * xx
                           + 3.387301370953521e-12) * xx
                           + 1.8978869988971e-10) * xx
                           + 1.1482216343327455e-8) * xx
                           + 7.873519778281683e-7) * xx
                           + 0.00006944444444444444) * xx
                           + 0.013888888888888888) * xx
                           - log(fabs(x)) + 1.0) * x;
    }

    x += ((x > 0.0) ? - M_PI : M_PI);
    double xx = x * x;
    return ((((((((((((3.901950904063069e-15 * xx
                       + 4.566487567193635e-14) * xx
                       + 5.429792727596476e-13) * xx
                       + 6.5812165661369675e-12) * xx
                       + 8.167010963952222e-11) * xx
                       + 1.0440290284867003e-9) * xx
                       + 1.3870999114054669e-8) * xx
                       + 1.941538399871733e-7) * xx
                       + 2.927965167548501e-6) * xx
                       + 0.0000496031746031746) * xx
                       + 0.0010416666666666667) * xx
                       + 0.041666666666666664) * xx
                       + log(0.5)) * x;
}

double ImLi2Sum(double dp, double theta) {
    double tStar = M_PI - theta;
    double x = 2*atan(tanh(0.5*dp) * tan(0.5*tStar));

    return x*dp + Cl2(x + tStar) + Cl2(-x + tStar) - Cl2(2.0*tStar);
}

double fe(double dp, double theta) {
    return atan2(sin(theta), exp(dp) - cos(theta));
}

void CirclePatterns::computeEnergy(double& energy, const Eigen::VectorXd& rho)
{
    energy = 0.0;

    // sum over edges
    for (Edge e : mesh->edges()) {
        if (eMask[e]) {
            int fk = e.halfedge().face().getIndex();

            if (eBdry[e]) {
                energy -= 2 * (M_PI - thetas[e.getIndex()]) * rho[fk];

            } else {
                int fl = e.halfedge().twin().face().getIndex();
                energy += ImLi2Sum(rho[fk] - rho[fl], thetas[e.getIndex()]) -
                          (M_PI - thetas[e.getIndex()]) * (rho[fk] + rho[fl]);
            }
        }
    }

    // sum over faces
    for (Face f: mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) energy += 2*M_PI*rho[fInd[f]];
    }
}

void CirclePatterns::computeGradient(Eigen::VectorXd& gradient, const Eigen::VectorXd& rho)
{
    // loop over faces
    for (Face f : mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) {
            int fk = fInd[f];
            gradient[fk] = 2*M_PI;

            // sum of adjacent edges
            Halfedge he = f.halfedge();
            do {
                Edge e = he.edge();
                if (eBdry[e]) {
                    gradient[fk] -= 2*(M_PI - thetas[eInd[e]]);

                } else {
                    Halfedge h = e.halfedge();
                    int fl = fk == (int)fInd[h.face()] ? fInd[h.twin().face()] : fInd[h.face()];
                    gradient[fk] -= 2*fe(rho[fk] - rho[fl], thetas[e.getIndex()]);
                }

                he = he.next();
            } while (he != f.halfedge());
        }
    }
}

void CirclePatterns::computeHessian(Eigen::SparseMatrix<double>& hessian, const Eigen::VectorXd& rho)
{
    std::vector<Eigen::Triplet<double>> HTriplets;

    for (Edge e : mesh->edges()) {
        if (!eBdry[e] && eMask[e]) {
            int fk = fInd[e.halfedge().face()];
            int fl = fInd[e.halfedge().twin().face()];

            double hessval = sin(thetas[eInd[e]]) / (cosh(rho(fk) - rho(fl)) - cos(thetas[eInd[e]]));
            HTriplets.push_back(Eigen::Triplet<double>(fk, fk, hessval + 1e-8));
            HTriplets.push_back(Eigen::Triplet<double>(fl, fl, hessval + 1e-8));
            HTriplets.push_back(Eigen::Triplet<double>(fk, fl, -hessval));
            HTriplets.push_back(Eigen::Triplet<double>(fl, fk, -hessval));
        }
    }

    hessian.setFromTriplets(HTriplets.begin(), HTriplets.end());
}

void CirclePatterns::setRadii()
{
    for (Face f : mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) radii[fInd[f]] = exp(solver.x[fInd[f]]);
    }
}

bool CirclePatterns::computeRadii()
{
    MeshHandle handle;
    handle.computeEnergy = std::bind(&CirclePatterns::computeEnergy, this, _1, _2);
    handle.computeGradient = std::bind(&CirclePatterns::computeGradient, this, _1, _2);
    handle.computeHessian = std::bind(&CirclePatterns::computeHessian, this, _1, _2);

    solver.handle = &handle;
    if (OptScheme == GRAD_DESCENT) solver.gradientDescent();
    else if (OptScheme == NEWTON) solver.newton();
    else if (OptScheme == TRUST_REGION) solver.trustRegion();
    else solver.lbfgs();

    // set radii
    setRadii();

    return true;
}

void CirclePatterns::computeAnglesAndEdgeLengths(Eigen::VectorXd& lengths)
{
    for (Edge e : mesh->edges()) {
        if(eMask[e]) {
            Halfedge h1 = e.halfedge();

            if (eBdry[e]) {
                angles[h1.getIndex()] = M_PI - thetas[eInd[e]];

            } else {
                Halfedge h2 = h1.twin();
                double dp = log(radii[h1.face().getIndex()]) - log(radii[h2.face().getIndex()]);
                angles[h1.getIndex()] = fe(dp, thetas[e.getIndex()]);
                angles[h2.getIndex()] = fe(-dp, thetas[e.getIndex()]);
            }

            lengths[eInd[e]] = 2.0*radii[h1.face().getIndex()]*sin(angles[h1.getIndex()]);
        }
    }
}

void CirclePatterns::performFaceLayout(Halfedge he, const Eigen::Vector2d& dir,
                                       Eigen::VectorXd& lengths, std::unordered_map<int, bool>& visited,
                                       std::stack<Edge>& stack)
{
    if (he.isInterior() && fMask[he.face()]) {
        int fIdx = he.face().getIndex();
        if (visited.find(fIdx) == visited.end()) {
            Halfedge next = he.next();
            Halfedge prev = he.next().next();

            // compute new uv position
            double angle = angles[next.getIndex()];
            Eigen::Vector2d newDir = {cos(angle)*dir[0] - sin(angle)*dir[1],
                                      sin(angle)*dir[0] + cos(angle)*dir[1]};

            uv[prev.vertex()] = uv[he.vertex()] + newDir*lengths[eInd[prev.edge()]];

            // mark face as visited
            visited[fIdx] = true;

            // push edges onto stack
            if(eMask[next.edge()]) stack.push(next.edge());
            if(eMask[prev.edge()]) stack.push(prev.edge());
        }
    }
}

void CirclePatterns::setUVs()
{
    // compute edge lengths
    Eigen::VectorXd lengths(mesh->nEdges());
    computeAnglesAndEdgeLengths(lengths);

    // push any edge
    std::stack<Edge> stack;
    Edge e0;
    for (Edge e :mesh->edges()) {
        if(eMask[e]) {
            e0 = e;
            break;
        }
    };
    stack.push(e0);

    uv[e0.halfedge().vertex()] = Eigen::Vector2d::Zero();
    uv[e0.halfedge().next().vertex()] = Eigen::Vector2d(lengths[eInd[e0]], 0);

    // perform layout
    std::unordered_map<int, bool> visited;
    while (!stack.empty()) {
        Edge e = stack.top();
        stack.pop();

        Halfedge h1 = e.halfedge();
        Halfedge h2 = h1.twin();

        // compute edge vector

        Eigen::Vector2d dir = uv[h2.vertex()] - uv[h1.vertex()];

        dir.normalize();
        // boundary edges
        performFaceLayout(h1, dir, lengths, visited, stack);
        performFaceLayout(h2, -dir, lengths, visited, stack);
    }

    normalize();
}

VertexData<Eigen::Vector2d> CirclePatterns::parameterize() {
    // set interior edge indices
    int eIdx = 0;
    for (Edge e : mesh->edges()) {
        if (eMask[e]) {
            if (!eBdry[e])
                eIntIndices[eInd[e]] = eIdx++;
            else {
                eIntIndices[eInd[e]] = -1;
                imaginaryHe++;
            }
        }
    }

    // compute radii
    if (!computeRadii()) {
        std::cout << "Unable to compute radii" << std::endl;
        return VertexData<Eigen::Vector2d>();
    }

    // set uvs
    setUVs();
    return uv;
}
double CirclePatterns::uvArea(Face f) {
    if (f.isBoundaryLoop() || !fMask[f]) {
        return 0;
    }

    const Eigen::Vector2d& a(uv[f.halfedge().vertex()]);
    const Eigen::Vector2d& b(uv[f.halfedge().next().vertex()]);
    const Eigen::Vector2d& c(uv[f.halfedge().next().next().vertex()]);

    const Eigen::Vector2d u = b - a;
    const Eigen::Vector2d v = c - a;

    return 0.5 * (u.x()*v.y() - v.x()*u.y());
}

Eigen::Vector2d CirclePatterns::uvBarycenter(Face f) {
    if (f.isBoundaryLoop() || !fMask[f]) {
        return Eigen::Vector2d::Zero();
    }

    const Eigen::Vector2d& a(uv[f.halfedge().vertex()]);
    const Eigen::Vector2d& b(uv[f.halfedge().next().vertex()]);
    const Eigen::Vector2d& c(uv[f.halfedge().next().next().vertex()]);

    return (a + b + c) / 3.0;
}
void CirclePatterns::normalize() {
    // compute center
    double totalArea = 0;
    Eigen::Vector2d center = Eigen::Vector2d::Zero();
    uv[infVertex.getIndex()] = Eigen::Vector2d::Zero();
    /*
    uv[infVertex.getIndex()].x() = -8;
    uv[infVertex.getIndex()].y() = 5;
    */
    for (Face f : mesh->faces()) {
        if (fMask[f]){
            double area = uvArea(f);
            center += area * uvBarycenter(f);
            totalArea += area;
        }
    }
    center /= totalArea;

    // shift
    double r = 0.0;
    for (Vertex v : mesh->vertices()) {
        if (v != infVertex) {
            uv[v] -= center;
            r = std::max(r, uv[v].squaredNorm());
        }
    }

    // scale
    r = sqrt(r);
    for (Vertex v : mesh->vertices()) {
        uv[v] /= r;
    }
}

inline double shift(double c) {
    return (c + 2.) * 500;
}


#define BOOST_TEST_MODULE test_lemon

#include <iostream>
#include <boost/test/unit_test.hpp>

#include <lemon/adaptors.h>
#include <lemon/bellman_ford.h>

#define private public
#include "graph.h"
#include "flowgraph.h"
#include "residualgraph.h"

using namespace dpct;

BOOST_AUTO_TEST_CASE(pure_lemon)
{
    typedef lemon::ListDigraph LGraph;
    typedef LGraph::Node Node;
    typedef LGraph::Arc Arc;
    typedef LGraph::ArcMap<double> DistMap;
    LGraph g;
    DistMap dist(g);

    Node s = g.addNode();
    Node t = g.addNode();

    Node n_1_1 = g.addNode();
    Node n_1_2 = g.addNode();
    Node n_2_1 = g.addNode();
    Node n_2_2 = g.addNode();
    Node n_2_3 = g.addNode();

    Node d_1_1 = g.addNode();
    Node d_1_2 = g.addNode();

    Arc app1 = g.addArc(s, n_1_1);
    dist[app1] = 0.0;
    Arc app2 = g.addArc(s, n_1_2);
    dist[app2] = 0.0;

    Arc div1 = g.addArc(s, d_1_1);
    dist[div1] = -4.0;
    Arc div2 = g.addArc(s, d_1_2);
    dist[div2] = -4.0;

    Arc move1 = g.addArc(n_1_1, n_2_1);
    dist[move1] = -4.0;
    Arc move2 = g.addArc(n_1_1, n_2_2);
    dist[move2] = -3.0;
    Arc move3 = g.addArc(n_1_2, n_2_2);
    dist[move3] = -1.0;
    Arc move4 = g.addArc(n_1_2, n_2_3);
    dist[move4] = -4.0;

    Arc child1 = g.addArc(d_1_1, n_2_1);
    dist[child1] = -4.0;
    Arc child2 = g.addArc(d_1_1, n_2_2);
    dist[child2] = -3.0;
    Arc child3 = g.addArc(d_1_2, n_2_2);
    dist[child3] = -1.0;
    Arc child4 = g.addArc(d_1_2, n_2_3);
    dist[child4] = -4.0;

    Arc dis1 = g.addArc(n_2_1, t);
    dist[dis1] = -2.0;
    Arc dis2 = g.addArc(n_2_2, t);
    dist[dis2] = -2.0;
    Arc dis3 = g.addArc(n_2_3, t);
    dist[dis3] = -4.0;

    // graph adapter to hide the two division arcs
    LGraph::ArcMap<bool> divisionArcEnabledMap(g);
    for(LGraph::ArcIt a(g); a != lemon::INVALID; ++a)
        divisionArcEnabledMap[a] = true;
    divisionArcEnabledMap[div1] = false;
    divisionArcEnabledMap[div2] = false;

    typedef lemon::FilterArcs<LGraph> FilteredLGraph;
    FilteredLGraph filteredG(g, divisionArcEnabledMap);

    // ------------------------------------------------
    // find shortest path
    typedef lemon::BellmanFord<FilteredLGraph, DistMap> BellmanFord;
    BellmanFord bf(filteredG, dist);
    bf.init();
    bf.addSource(s);

    if(bf.checkedStart())
    {
        std::cout << "\n******************************\n[BellmanFord]: found shortest path at distance " << bf.dist(t) << std::endl;

        std::cout << "Found path is: ";
        for(Node v = t; v != s; v=bf.predNode(v))
        {
            std::cout << filteredG.id(v) << "=" << g.id(v) << " <- ";
        }
        std::cout << filteredG.id(s) << "=" << g.id(s) << std::endl;
    }

    // ------------------------------------------------
    // build up capacities and residual graph
    LGraph::ArcMap<int> capacities(g);
    LGraph::ArcMap<int> flowMap(g);
    for(LGraph::ArcIt a(g); a != lemon::INVALID; ++a)
    {
        capacities[a] = 1;
        flowMap[a] = 0;
    }

    for(Arc a = bf.predArc(t); a != lemon::INVALID; a=bf.predArc(g.source(a)))
    {
        std::cout << "setting arc " << "(" << g.id(g.source(a)) << ", " << g.id(g.target(a)) << ") to contain 1 flow" << std::endl;
        flowMap[a] = 1;
    }

    // update filtered graph to contain the appropriate division now!
    divisionArcEnabledMap[div2] = true;
    divisionArcEnabledMap[child4] = false; // cannot use the same child as parent path
    filteredG = FilteredLGraph(g, divisionArcEnabledMap);

    typedef lemon::ResidualDigraph< FilteredLGraph, LGraph::ArcMap<int>, LGraph::ArcMap<int> > ResidualGraph;
    typedef ResidualGraph::ArcMap<double> ResidualDistMap;

    // ------------------------------------------------
    // find shortest path in residual graph and augment flow
    {
        ResidualGraph residualG(filteredG, capacities, flowMap);
        ResidualDistMap residualDist(residualG);
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            if(residualG.forward(a))
            {
                residualDist[a] = dist[lemon::findArc(g, residualG.source(a), residualG.target(a))];
            }
            else
            {
                residualDist[a] = -1.0 * dist[lemon::findArc(g, residualG.target(a), residualG.source(a))];
            }
        }

        std::cout << "Residual Graph has edges: " << std::endl;
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            std::cout << "(" << residualG.id(residualG.source(a)) << ", " << residualG.id(residualG.target(a)) << ") "
            << (residualG.forward(a)?"forward":"backward")
            << " cost: " << residualDist[a]
            << std::endl;
        }

        typedef lemon::BellmanFord<ResidualGraph, ResidualDistMap> ResidualBellmanFord;
        ResidualBellmanFord rbf(residualG, residualDist);
        rbf.init();
        rbf.addSource(s);

        if(rbf.checkedStart())
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found shortest path at distance " << rbf.dist(t) << std::endl;

            std::cout << "Found path is: ";
            for(Node v = t; v != s; v=rbf.predNode(v))
            {
                std::cout << residualG.id(v) << "=" << g.id(v) << " <- ";
            }
            std::cout << residualG.id(s) << "=" << g.id(s) << std::endl;

            for(ResidualGraph::Arc a = rbf.predArc(t); a != lemon::INVALID; a=rbf.predArc(residualG.source(a)))
            {
                int delta = (residualG.forward(a) ? 1 : -1);
                flowMap[a] += delta;
                std::cout << "setting arc " << "(" << g.id(g.source(a)) << ", " << g.id(g.target(a))
                        << "), delta: " << delta
                        << " new flow: " << flowMap[a]
                        << (residualG.forward(a) ? " forward" : " backward")
                        << std::endl;
            }
        }
        else
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found negative weight directed cycle!" << std::endl;
            lemon::Path<ResidualGraph> path = rbf.negativeCycle();
            for(lemon::Path<ResidualGraph>::ArcIt it(path); it != lemon::INVALID; ++it)
            {
                std::cout << "(" << residualG.id(residualG.source(it)) << "=" << g.id(residualG.source(it)) << ", " << residualG.id(residualG.target(it)) << "=" << g.id(residualG.target(it)) << ") ";
            }
            std::cout << std::endl;
        }

        // found path that used division 2 of node 3, so disallow "unusing" the parent of the division before the division
        divisionArcEnabledMap[div2] = true;
        divisionArcEnabledMap[app2] = false;
        filteredG = FilteredLGraph(g, divisionArcEnabledMap);
    }
    // ------------------------------------------------
    // again, update flow, should find path s,n_1_1,n_2_1,t (= 0,2,4,1)
    {
        ResidualGraph residualG(filteredG, capacities, flowMap);
        ResidualDistMap residualDist(residualG);
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            if(residualG.forward(a))
            {
                residualDist[a] = dist[lemon::findArc(g, residualG.source(a), residualG.target(a))];
            }
            else
            {
                residualDist[a] = -1.0 * dist[lemon::findArc(g, residualG.target(a), residualG.source(a))];
            }
        }

        std::cout << "Residual Graph has edges: " << std::endl;
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            std::cout << "(" << residualG.id(residualG.source(a)) << ", " << residualG.id(residualG.target(a)) << ") "
            << (residualG.forward(a)?"forward":"backward")
            << " cost: " << residualDist[a]
            << std::endl;
        }

        typedef lemon::BellmanFord<ResidualGraph, ResidualDistMap> ResidualBellmanFord;
        ResidualBellmanFord rbf(residualG, residualDist);
        rbf.init();
        rbf.addSource(s);

        if(rbf.checkedStart())
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found shortest path at distance " << rbf.dist(t) << std::endl;

            std::cout << "Found path is: ";
            for(Node v = t; v != s; v=rbf.predNode(v))
            {
                std::cout << residualG.id(v) << "=" << g.id(v) << " <- ";
            }
            std::cout << residualG.id(s) << "=" << g.id(s) << std::endl;

            for(ResidualGraph::Arc a = rbf.predArc(t); a != lemon::INVALID; a=rbf.predArc(residualG.source(a)))
            {
                int delta = (residualG.forward(a) ? 1 : -1);
                flowMap[a] += delta;
                std::cout << "setting arc " << "(" << g.id(g.source(a)) << ", " << g.id(g.target(a))
                        << "), delta: " << delta
                        << " new flow: " << flowMap[a]
                        << (residualG.forward(a) ? " forward" : " backward")
                        << std::endl;
            }
        }
        else
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found negative weight directed cycle!" << std::endl;
            lemon::Path<ResidualGraph> path = rbf.negativeCycle();
            for(lemon::Path<ResidualGraph>::ArcIt it(path); it != lemon::INVALID; ++it)
            {
                std::cout << "(" << residualG.id(residualG.source(it)) << "=" << g.id(residualG.source(it)) << ", " << residualG.id(residualG.target(it)) << "=" << g.id(residualG.target(it)) << ") ";
            }
            std::cout << std::endl;
        }

        // found path that now enables division 1
        divisionArcEnabledMap[div1] = true;
        divisionArcEnabledMap[child1] = false;
        filteredG = FilteredLGraph(g, divisionArcEnabledMap);
    }

    // ------------------------------------------------
    // again, update flow, should find cycle s, d_1_1, n_2_2, d_1_2, s
    {
        ResidualGraph residualG(filteredG, capacities, flowMap);
        ResidualDistMap residualDist(residualG);
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            if(residualG.forward(a))
            {
                residualDist[a] = dist[lemon::findArc(g, residualG.source(a), residualG.target(a))];
            }
            else
            {
                residualDist[a] = -1.0 * dist[lemon::findArc(g, residualG.target(a), residualG.source(a))];
            }
        }

        std::cout << "Residual Graph has edges: " << std::endl;
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            std::cout << "(" << residualG.id(residualG.source(a)) << ", " << residualG.id(residualG.target(a)) << ") "
            << (residualG.forward(a)?"forward":"backward")
            << " cost: " << residualDist[a]
            << std::endl;
        }

        typedef lemon::BellmanFord<ResidualGraph, ResidualDistMap> ResidualBellmanFord;
        ResidualBellmanFord rbf(residualG, residualDist);
        rbf.init();
        rbf.addSource(s);

        if(rbf.checkedStart())
        {
            if(!rbf.reached(t))
            {
                std::cout << ">>>>>> Finished!" << std::endl;
                return;
            }

            std::cout << "\n******************************\n[ResidualBellmanFord]: found shortest path at distance " << rbf.dist(t) << std::endl;

            std::cout << "Found path is: ";
            for(Node v = t; v != s; v=rbf.predNode(v))
            {
                std::cout << residualG.id(v) << "=" << g.id(v) << " <- ";
            }
            std::cout << residualG.id(s) << "=" << g.id(s) << std::endl;

            for(ResidualGraph::Arc a = rbf.predArc(t); a != lemon::INVALID; a=rbf.predArc(residualG.source(a)))
            {
                int delta = (residualG.forward(a) ? 1 : -1);
                flowMap[a] += delta;
                std::cout << "setting arc " << "(" << g.id(g.source(a)) << ", " << g.id(g.target(a))
                        << "), delta: " << delta
                        << " new flow: " << flowMap[a]
                        << (residualG.forward(a) ? " forward" : " backward")
                        << std::endl;
            }
        }
        else
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found negative weight directed cycle!" << std::endl;
            lemon::Path<ResidualGraph> path = rbf.negativeCycle();
            for(lemon::Path<ResidualGraph>::ArcIt it(path); it != lemon::INVALID; ++it)
            {
                int delta = (residualG.forward(it) ? 1 : -1);
                flowMap[lemon::findArc(g, residualG.target(it), residualG.source(it))] += delta;
                std::cout << "(" << residualG.id(residualG.source(it)) << "=" << g.id(residualG.source(it)) << ", " << residualG.id(residualG.target(it)) << "=" << g.id(residualG.target(it))
                        << "), delta: " << delta
                        << " new flow: " << flowMap[lemon::findArc(g, residualG.target(it), residualG.source(it))]
                        << (residualG.forward(it) ? " forward" : " backward")
                        << std::endl;
            }
        }

        // flow has been redirected to use cheaper path
        // but we still have to re-enable the move from
        divisionArcEnabledMap[app2] = true;
        filteredG = FilteredLGraph(g, divisionArcEnabledMap);
    }

    // ------------------------------------------------
    // again, but there shouldn't be any paths left
    {
        ResidualGraph residualG(filteredG, capacities, flowMap);
        ResidualDistMap residualDist(residualG);
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            if(residualG.forward(a))
            {
                residualDist[a] = dist[lemon::findArc(g, residualG.source(a), residualG.target(a))];
            }
            else
            {
                residualDist[a] = -1.0 * dist[lemon::findArc(g, residualG.target(a), residualG.source(a))];
            }
        }

        std::cout << "Residual Graph has edges: " << std::endl;
        for(ResidualGraph::ArcIt a(residualG); a != lemon::INVALID; ++a)
        {
            std::cout << "(" << residualG.id(residualG.source(a)) << ", " << residualG.id(residualG.target(a)) << ") "
            << (residualG.forward(a)?"forward":"backward")
            << " cost: " << residualDist[a]
            << std::endl;
        }

        typedef lemon::BellmanFord<ResidualGraph, ResidualDistMap> ResidualBellmanFord;
        ResidualBellmanFord rbf(residualG, residualDist);
        rbf.init();
        rbf.addSource(s);

        if(rbf.checkedStart())
        {
            if(!rbf.reached(t))
            {
                std::cout << ">>>>>> Finished!" << std::endl;
                return;
            }

            std::cout << "\n******************************\n[ResidualBellmanFord]: found shortest path at distance " << rbf.dist(t) << std::endl;

            std::cout << "Found path is: ";
            for(Node v = t; v != s; v=rbf.predNode(v))
            {
                std::cout << residualG.id(v) << "=" << g.id(v) << " <- ";
            }
            std::cout << residualG.id(s) << "=" << g.id(s) << std::endl;

            for(ResidualGraph::Arc a = rbf.predArc(t); a != lemon::INVALID; a=rbf.predArc(residualG.source(a)))
            {
                int delta = (residualG.forward(a) ? 1 : -1);
                flowMap[a] += delta;
                std::cout << "setting arc " << "(" << g.id(g.source(a)) << ", " << g.id(g.target(a))
                        << "), delta: " << delta
                        << " new flow: " << flowMap[a]
                        << (residualG.forward(a) ? " forward" : " backward")
                        << std::endl;
            }
        }
        else
        {
            std::cout << "\n******************************\n[ResidualBellmanFord]: found negative weight directed cycle!" << std::endl;
            lemon::Path<ResidualGraph> path = rbf.negativeCycle();
            for(lemon::Path<ResidualGraph>::ArcIt it(path); it != lemon::INVALID; ++it)
            {
                int delta = (residualG.forward(it) ? 1 : -1);
                std::cout << "(" << residualG.id(residualG.source(it)) << "=" << g.id(residualG.source(it)) << ", " << residualG.id(residualG.target(it)) << "=" << g.id(residualG.target(it)) << ") ";
                flowMap[lemon::findArc(g, residualG.source(it), residualG.target(it))] += delta;
            }
            std::cout << std::endl;
        }
    }
}

BOOST_AUTO_TEST_CASE( flowgraph_simple )
{
    FlowGraph g;
    typedef FlowGraph::FullNode Node;
    typedef FlowGraph::Arc Arc;

    Node n_1_1 = g.addNode({0.0});
    Node n_1_2 = g.addNode({0.0});
    Node n_1_3 = g.addNode({0.0});
    Node n_2_1 = g.addNode({0.0});
    Node n_2_2 = g.addNode({0.0});
    Node n_2_3 = g.addNode({0.0});

    FlowGraph::Node s = g.getSource();
    FlowGraph::Node t = g.getTarget();

    Arc app1 = g.addArc(s, n_1_1.u, {0.0});
    Arc app2 = g.addArc(s, n_1_2.u, {0.0});
    Arc app3 = g.addArc(s, n_2_1.u, {10.0});
    Arc app4 = g.addArc(s, n_2_2.u, {10.0});
    Arc app5 = g.addArc(s, n_2_3.u, {10.0});
    Arc app6 = g.addArc(s, n_1_3.u, {0.0});

    Arc move1 = g.addArc(n_1_1, n_2_1, {-4.0});
    Arc move2 = g.addArc(n_1_1, n_2_2, {-3.0});
    Arc move3 = g.addArc(n_1_2, n_2_2, {-1.0});
    Arc move4 = g.addArc(n_1_2, n_2_3, {-4.0});
    Arc move5 = g.addArc(n_1_3, n_2_3, {2.0});

    Arc dis1 = g.addArc(n_2_1.v, t, {-2.0});
    Arc dis2 = g.addArc(n_2_2.v, t, {-2.0});
    Arc dis3 = g.addArc(n_2_3.v, t, {-4.0});
    Arc dis4 = g.addArc(n_1_1.v, t, {10.0});
    Arc dis5 = g.addArc(n_1_2.v, t, {10.0});
    Arc dis6 = g.addArc(n_1_3.v, t, {-1.0});

    Arc div1 = g.allowMitosis(n_1_1, {-4.0});
    Arc div2 = g.allowMitosis(n_1_2, {-4.0});

    g.maxFlowMinCostTracking();

    BOOST_CHECK_EQUAL(g.getFlowMap()[app1], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[app2], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[app3], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[app4], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[app5], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[app6], 1);

    BOOST_CHECK_EQUAL(g.getFlowMap()[dis1], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[dis2], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[dis3], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[dis4], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[dis5], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[dis6], 1);

    BOOST_CHECK_EQUAL(g.getFlowMap()[div1], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[div2], 0);

    BOOST_CHECK_EQUAL(g.getFlowMap()[move1], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[move2], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[move3], 0);
    BOOST_CHECK_EQUAL(g.getFlowMap()[move4], 1);
    BOOST_CHECK_EQUAL(g.getFlowMap()[move5], 0);
}

/*
The following test cannot work as long as we use the alternative way of checking for tokens on a path

BOOST_AUTO_TEST_CASE( tokenizedbellmanford_have_tokens )
{
    FlowGraph g;
    typedef FlowGraph::FullNode Node;
    typedef FlowGraph::Arc Arc;

    Node n_1_1 = g.addNode({0.0});
    Node n_2_1 = g.addNode({0.0});
    Node n_2_2 = g.addNode({0.0});

    FlowGraph::Node s = g.getSource();
    FlowGraph::Node t = g.getTarget();

    Arc app1 = g.addArc(s, n_1_1.u, {0.0});

    Arc move1 = g.addArc(n_1_1, n_2_1, {-4.0});
    Arc move2 = g.addArc(n_1_1, n_2_2, {-3.0});

    Arc dis1 = g.addArc(n_2_1.v, t, {-2.0});
    Arc dis2 = g.addArc(n_2_2.v, t, {-2.0});

    const FlowGraph::Graph& baseGraph = g.getGraph();
    ResidualGraph rg(baseGraph);

    // only enable forward arcs, we just want a simple scenario to check whether token collection works
    for(FlowGraph::Graph::ArcIt a(baseGraph); a != lemon::INVALID; ++a)
    {
        rg.updateArc(a, ResidualGraph::Forward, g.getArcCost(a, 0), 1);
    }

    size_t tokenId = 12;
    rg.addProvidedToken(app1, ResidualGraph::Forward, tokenId);
    rg.addForbiddenToken(move1, ResidualGraph::Forward, tokenId);

    ResidualGraph::ShortestPathResult sp = rg.findShortestPath(s, t);
    BOOST_CHECK(sp.second < 0); // check that we found an augmenting path
    g.printPath(sp.first);

    // make sure we did not go along forbidden move1 arc
    for(auto arcFlowPair : sp.first)
    {
        BOOST_CHECK(arcFlowPair.first != move1);
    }

    // try again, this time without forbidding the provided token
    rg.removeForbiddenToken(move1, ResidualGraph::Forward, tokenId);
    sp = rg.findShortestPath(s, t);
    BOOST_CHECK(sp.second < 0); // check that we found an augmenting path
    g.printPath(sp.first);

    // make sure we did not go along move2 arc, because move1 is cheaper now
    for(auto arcFlowPair : sp.first)
    {
        BOOST_CHECK(arcFlowPair.first != move2);
    }
}
*/


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <Eigen/LU>

#include "tsef.h"
#include "../parameters.h"

CHARGEFW2_METHOD(TSEF)

double K(int i);

double K(int i) {
    double vals[] = {0.556, 0.778, 1.000, 1.053, 1.087, 1.091};
    if (i > 6)
        return vals[5];
    else
        return vals[i - 1];
}

std::vector<double> TSEF::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    const double alpha = 14.4;

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        A(i, i) = parameters_->atom()->parameter(atom::hardness)(atom_i);
        b(i) = - parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = molecule.atoms()[j];
            int bd = molecule.bond_distance(atom_i, atom_j);
            auto x = alpha * K(bd) / (0.84 * bd + 0.46);
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A(n, n) = 0;
    b(n) = molecule.total_charge();

    Eigen::VectorXd q = A.partialPivLu().solve(b).head(n);
    return std::vector<double>(q.data(), q.data() + q.size());
}


#include <Eigen/Dense>
#include <iostream>
#include "string_aligner.h"

int main(int argc, char* argv[]) {
    if (argc != 3) {
        std::cerr << "Error: two strings expected as arguments" << std::endl;
        std::exit(1);
    }
    std::string str1(argv[1]);
    std::string str2(argv[2]);
    Matrix_t scoring_matrix(4, 4);
    const std::string alphabet("ACGT");
    scoring_matrix <<  2, -1,  1, -1,
                      -1,  2, -1,  1,
                       1, -1,  2, -1,
                      -1,  1, -1,  2;
    const int gap_socre {-2};
    StringAligner aligner(alphabet, scoring_matrix, gap_socre);
    auto [aligned1, aligned2] = aligner.align(str1, str2);
    std::cout << aligned1 << "\n"
              << aligned2 << std::endl;
    return 0;
}


#include <Eigen/Sparse>
#include <iostream>

int main()
{
	Eigen::SparseVector<double> vec(10);
	vec.insert(2) = 23;
	vec.insert(5) = 13.5;
	std::cout << vec << std::endl;

	Eigen::SparseMatrix<double> mat(10, 10);
	mat.insert(2, 4) = 2.4;
	mat.insert(5, 1) = 3.53;
	std::cout << mat << std::endl;

}


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
#endif
#include <csim/update_ops_cpp.hpp>

TEST(UpdateTest, SingleQubitPauliTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	UINT target, pauli;

	Eigen::MatrixXcd Identity(2, 2);
	Identity << 1, 0, 0, 1;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		/* single qubit Pauli gate */
		target = rand_int(n);
		pauli = rand_int(4);
		single_qubit_Pauli_gate(target, pauli, state, dim);
		test_state = get_expanded_eigen_matrix_with_identity(target, get_eigen_matrix_single_Pauli(pauli), n) * test_state;
		state_equal(state, test_state, dim, "single Pauli gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitPauliRotationTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	UINT target, pauli;
	double angle;

	Eigen::MatrixXcd Identity(2, 2);
	Identity << 1, 0, 0, 1;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		target = rand_int(n);
		pauli = rand_int(3) + 1;
		angle = rand_real();
		single_qubit_Pauli_rotation_gate(target, pauli, angle, state, dim);
		test_state = get_expanded_eigen_matrix_with_identity(target, cos(angle / 2)*Identity + 1.i * sin(angle / 2) * get_eigen_matrix_single_Pauli(pauli), n) * test_state;
		state_equal(state, test_state, dim, "single rotation Pauli gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, MultiQubitPauliTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	UINT pauli;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		// multi pauli whole
		std::vector<UINT> pauli_whole, pauli_partial, pauli_partial_index;
		std::vector<std::pair<UINT, UINT>> pauli_partial_pair;

		pauli_whole.resize(n);
		for (UINT i = 0; i < n; ++i) {
			pauli_whole[i] = rand_int(4);
		}
		multi_qubit_Pauli_gate_whole_list(pauli_whole.data(), n, state, dim);
		test_state = get_eigen_matrix_full_qubit_pauli(pauli_whole) * test_state;
		state_equal(state, test_state, dim, "multi Pauli whole gate");

		// multi pauli partial
		pauli_partial.clear();
		pauli_partial_index.clear();
		pauli_partial_pair.clear();
		for (UINT i = 0; i < n; ++i) {
			pauli = rand_int(4);
			pauli_whole[i] = pauli;
			if (pauli != 0) {
				pauli_partial_pair.push_back(std::make_pair(i, pauli));
			}
		}
		std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
		for (auto val : pauli_partial_pair) {
			pauli_partial_index.push_back(val.first);
			pauli_partial.push_back(val.second);
		}
		multi_qubit_Pauli_gate_partial_list(pauli_partial_index.data(), pauli_partial.data(), (UINT)pauli_partial.size(), state, dim);
		test_state = get_eigen_matrix_full_qubit_pauli(pauli_whole) * test_state;
		state_equal(state, test_state, dim, "multi Pauli partial gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, MultiQubitPauliRotationTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	UINT pauli;
	double angle;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

	for (UINT rep = 0; rep < max_repeat; ++rep) {

		std::vector<UINT> pauli_whole, pauli_partial, pauli_partial_index;
		std::vector<std::pair<UINT, UINT>> pauli_partial_pair;

		// multi pauli rotation whole
		pauli_whole.resize(n);
		for (UINT i = 0; i < n; ++i) {
			pauli_whole[i] = rand_int(4);
		}
		angle = rand_real();
		multi_qubit_Pauli_rotation_gate_whole_list(pauli_whole.data(), n, angle, state, dim);
		test_state = (cos(angle / 2)*whole_I + 1.i * sin(angle / 2)* get_eigen_matrix_full_qubit_pauli(pauli_whole)) * test_state;
		state_equal(state, test_state, dim, "multi Pauli rotation whole gate");

		// multi pauli rotation partial
		pauli_partial.clear();
		pauli_partial_index.clear();
		pauli_partial_pair.clear();
		for (UINT i = 0; i < n; ++i) {
			pauli = rand_int(4);
			pauli_whole[i] = pauli;
			if (pauli != 0) {
				pauli_partial_pair.push_back(std::make_pair(i, pauli));
			}
		}
		std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
		for (auto val : pauli_partial_pair) {
			pauli_partial_index.push_back(val.first);
			pauli_partial.push_back(val.second);
		}
		angle = rand_real();
		multi_qubit_Pauli_rotation_gate_partial_list(pauli_partial_index.data(), pauli_partial.data(), (UINT)pauli_partial.size(), angle, state, dim);
		test_state = (cos(angle / 2)*whole_I + 1.i * sin(angle / 2)* get_eigen_matrix_full_qubit_pauli(pauli_whole)) * test_state;
		state_equal(state, test_state, dim, "multi Pauli rotation partial gate");
	}
	release_quantum_state(state);
}