/**
 * @file expfittedupwind_main.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>

#include "expfittedupwind.h"

int main() {
  // Define Mesh-independent Data:
#if SOLUTION
  auto f = [](Eigen::Vector2d x) { return 0.0; };

  Eigen::Vector2d q = Eigen::Vector2d::Ones(2);
  auto Psi = [&q](Eigen::Vector2d x) { return q.dot(x); };

  auto g = [&Psi](Eigen::Vector2d x) { return std::exp(Psi(x)); };

  auto ref_sol = [&Psi](Eigen::Vector2d x) { return std::exp(Psi(x)); };
#else
  //====================
  // Your code goes here
  //====================
#endif

  // Output file
  std::ofstream L2output;
  L2output.open("L2error.txt");
  L2output << "No. of dofs, L2 error" << std::endl;

  // generate a mesh hierarchy:
  unsigned int reflevels = 6;
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  auto top_mesh = builder.Build();

  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(top_mesh,
                                                              reflevels);
  lf::refinement::MeshHierarchy& multi_mesh{*multi_mesh_p};
  multi_mesh.PrintInfo(std::cout);

  // get number of levels:
  auto L = multi_mesh.NumLevels();

  // perform computations on all levels:
  for (int l = 0; l < L; ++l) {
    // Compute finite element solution and compute L2 error on current level:
    double L2_err = 1.0;

    // get current mesh and fe space
    auto mesh_p = multi_mesh.getMesh(l);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};
    const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

#if SOLUTION
    // wrap Psi and the reference solution into a mesh function on the current
    // level
    auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(Psi);
    Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);
    auto mf_ref_sol = lf::mesh::utils::MeshFunctionGlobal(ref_sol);

    // compute the finite element solution and wrap it into a mesh function
    Eigen::VectorXd sol_vec =
        ExpFittedUpwind::SolveDriftDiffusionDirBVP(fe_space, mu, f, g);
    auto mf_sol = lf::fe::MeshFunctionFE(fe_space, sol_vec);

    // evaluate L2 error:
    L2_err = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::uscalfe::squaredNorm(mf_sol - mf_ref_sol), 3));
#else
    //====================
    // Your code goes here
    //====================
#endif

    L2output << N_dofs << ", " << L2_err << std::endl;
    std::cout << N_dofs << "," << L2_err << std::endl;
  }

  L2output.close();

  // Plot the computed L2 error
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/L2error.txt " CURRENT_BINARY_DIR "/results.eps");

  return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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/graph/dag_shortest_paths.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <iostream>

// Example from Introduction to Algorithms by Cormen, et all p.537.

// Sample output:
//  r: inifinity
//  s: 0
//  t: 2
//  u: 6
//  v: 5
//  x: 3

int main()
{
  using namespace boost;
  typedef adjacency_list<vecS, vecS, directedS,
    property<vertex_distance_t, int>, property<edge_weight_t, int> > graph_t;
  graph_t g(6);
  enum verts { r, s, t, u, v, x };
  char name[] = "rstuvx";
  add_edge(r, s, 5, g);
  add_edge(r, t, 3, g);
  add_edge(s, t, 2, g);
  add_edge(s, u, 6, g);
  add_edge(t, u, 7, g);
  add_edge(t, v, 4, g);
  add_edge(t, x, 2, g);
  add_edge(u, v, -1, g);
  add_edge(u, x, 1, g);
  add_edge(v, x, -2, g);

  property_map<graph_t, vertex_distance_t>::type
    d_map = get(vertex_distance, g);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ has trouble with the named-parameter mechanism, so
  // we make a direct call to the underlying implementation function.
  std::vector<default_color_type> color(num_vertices(g));
  std::vector<std::size_t> pred(num_vertices(g));
  default_dijkstra_visitor vis;
  std::less<int> compare;
  closed_plus<int> combine;
  property_map<graph_t, edge_weight_t>::type w_map = get(edge_weight, g);
  dag_shortest_paths(g, s, d_map, w_map, &color[0], &pred[0],
     vis, compare, combine, (std::numeric_limits<int>::max)(), 0);
#else
  dag_shortest_paths(g, s, distance_map(d_map));
#endif

  graph_traits<graph_t>::vertex_iterator vi , vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    if (d_map[*vi] == (std::numeric_limits<int>::max)())
      std::cout << name[*vi] << ": inifinity\n";
    else
      std::cout << name[*vi] << ": " << d_map[*vi] << '\n';
  return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2019 - 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: Natasha Sharma, University of Texas at El Paso,
 *          Guido Kanschat, University of Heidelberg
 *          Timo Heister, Clemson University
 *          Wolfgang Bangerth, Colorado State University
 *          Zhuroan Wang, Colorado State University
 */

// @sect3{Include files}

// 前面的几个include文件已经在前面的例子中使用过了，所以我们在这里不再解释它们的含义。该程序的主要结构与例如 step-4 的结构非常相似，因此我们包含了许多相同的头文件。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparse_direct.h>

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

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q.h>

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

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

// 最有趣的两个头文件将是这两个。

#include <deal.II/fe/fe_interface_values.h>
#include <deal.II/meshworker/mesh_loop.h>

// 其中第一个文件负责提供FEInterfaceValues类，该类可用于评估单元间界面的形状函数（或其梯度）的跳跃或平均值等数量。这个类在评估C0IP公式中出现的惩罚项时将相当有用。

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

namespace Step47
{
  using namespace dealii;

// 在下面的命名空间中，让我们定义精确解，我们将与数值计算的解进行比较。它的形式是 $u(x,y) = \sin(\pi x) \sin(\pi y)$ （只实现了2d的情况），该命名空间还包含一个对应于产生该解的右手边的类。

  namespace ExactSolution
  {
    using numbers::PI;

    template <int dim>
    class Solution : public Function<dim>
    {
    public:
      static_assert(dim == 2, "Only dim==2 is implemented.");

      virtual double value(const Point<dim> &p,
                           const unsigned int /*component*/ = 0) const override
      {
        return std::sin(PI * p[0]) * std::sin(PI * p[1]);
      }

      virtual Tensor<1, dim>
      gradient(const Point<dim> &p,
               const unsigned int /*component*/ = 0) const override
      {
        Tensor<1, dim> r;
        r[0] = PI * std::cos(PI * p[0]) * std::sin(PI * p[1]);
        r[1] = PI * std::cos(PI * p[1]) * std::sin(PI * p[0]);
        return r;
      }

      virtual void
      hessian_list(const std::vector<Point<dim>> &       points,
                   std::vector<SymmetricTensor<2, dim>> &hessians,
                   const unsigned int /*component*/ = 0) const override
      {
        for (unsigned i = 0; i < points.size(); ++i)
          {
            const double x = points[i][0];
            const double y = points[i][1];

            hessians[i][0][0] = -PI * PI * std::sin(PI * x) * std::sin(PI * y);
            hessians[i][0][1] = PI * PI * std::cos(PI * x) * std::cos(PI * y);
            hessians[i][1][1] = -PI * PI * std::sin(PI * x) * std::sin(PI * y);
          }
      }
    };

    template <int dim>
    class RightHandSide : public Function<dim>
    {
    public:
      static_assert(dim == 2, "Only dim==2 is implemented");

      virtual double value(const Point<dim> &p,
                           const unsigned int /*component*/ = 0) const override

      {
        return 4 * std::pow(PI, 4.0) * std::sin(PI * p[0]) *
               std::sin(PI * p[1]);
      }
    };
  } // namespace ExactSolution

//  @sect3{The main class}

// 以下是本教程程序的主类。它具有许多其他教程程序的结构，其内容和后面的构造函数应该没有什么特别令人惊讶的地方。

  template <int dim>
  class BiharmonicProblem
  {
  public:
    BiharmonicProblem(const unsigned int fe_degree);

    void run();

  private:
    void make_grid();
    void setup_system();
    void assemble_system();
    void solve();
    void compute_errors();
    void output_results(const unsigned int iteration) const;

    Triangulation<dim> triangulation;

    MappingQ<dim> mapping;

    FE_Q<dim>                 fe;
    DoFHandler<dim>           dof_handler;
    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;
  };

  template <int dim>
  BiharmonicProblem<dim>::BiharmonicProblem(const unsigned int fe_degree)
    : mapping(1)
    , fe(fe_degree)
    , dof_handler(triangulation)
  {}

// 接下来是创建初始网格（一次精炼的单元格）和设置每个网格的约束、向量和矩阵的函数。同样，这两个函数与之前的许多教程程序基本没有变化。

  template <int dim>
  void BiharmonicProblem<dim>::make_grid()
  {
    GridGenerator::hyper_cube(triangulation, 0., 1.);
    triangulation.refine_global(1);

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "Total number of cells: " << triangulation.n_cells()
              << std::endl;
  }

  template <int dim>
  void BiharmonicProblem<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);

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

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);

    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             ExactSolution::Solution<dim>(),
                                             constraints);
    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp, constraints, true);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }

//  @sect4{Assembling the linear system}

// 下面的几段代码更有意思。它们都与线性系统的组装有关。虽然组装单元格内部项的难度不大--这在本质上就像组装拉普拉斯方程的相应项一样，你已经在 step-4 或 step-6 中看到了这是如何工作的，例如，困难在于公式中的惩罚项。这需要在单元格的界面上对形状函数的梯度进行评估。因此，至少需要使用两个FEFaceValues对象，但如果其中一个面是自适应细化的，那么实际上需要一个FEFaceValues和一个FESubfaceValues对象；我们还需要跟踪哪些形状函数在哪里，最后我们需要确保每个面只被访问一次。所有这些对于我们真正想要实现的逻辑（即双线性形式中的惩罚项）来说都是一笔不小的开销。因此，我们将使用FEInterfaceValues类--这是deal.II中的一个辅助类，它允许我们抽象出两个FEFaceValues或FESubfaceValues对象，直接访问我们真正关心的东西：跳跃、平均等。

// 但这还没有解决我们的问题，即当我们在所有单元格和它们的所有面中循环时，必须跟踪我们已经访问过哪些面。为了使这个过程更简单，我们使用了 MeshWorker::mesh_loop() 函数，它为这个任务提供了一个简单的接口：基于WorkStream命名空间文档中概述的想法， MeshWorker::mesh_loop() 需要三个函数对单元、内部面和边界面进行工作。这些函数在抓取对象上工作，以获得中间结果，然后将其计算结果复制到复制数据对象中，由一个复制器函数将其复制到全局矩阵和右侧对象中。

// 然后，下面的结构提供了这种方法所需的从头开始和复制对象。你可以查阅WorkStream命名空间以及 @ref threads "多处理器并行计算 "模块，了解更多关于它们通常如何工作的信息。

  template <int dim>
  struct ScratchData
  {
    ScratchData(const Mapping<dim> &      mapping,
                const FiniteElement<dim> &fe,
                const unsigned int        quadrature_degree,
                const UpdateFlags         update_flags,
                const UpdateFlags         interface_update_flags)
      : fe_values(mapping, fe, QGauss<dim>(quadrature_degree), update_flags)
      , fe_interface_values(mapping,
                            fe,
                            QGauss<dim - 1>(quadrature_degree),
                            interface_update_flags)
    {}

    ScratchData(const ScratchData<dim> &scratch_data)
      : fe_values(scratch_data.fe_values.get_mapping(),
                  scratch_data.fe_values.get_fe(),
                  scratch_data.fe_values.get_quadrature(),
                  scratch_data.fe_values.get_update_flags())
      , fe_interface_values(scratch_data.fe_values.get_mapping(),
                            scratch_data.fe_values.get_fe(),
                            scratch_data.fe_interface_values.get_quadrature(),
                            scratch_data.fe_interface_values.get_update_flags())
    {}

    FEValues<dim>          fe_values;
    FEInterfaceValues<dim> fe_interface_values;
  };

  struct CopyData
  {
    CopyData(const unsigned int dofs_per_cell)
      : cell_matrix(dofs_per_cell, dofs_per_cell)
      , cell_rhs(dofs_per_cell)
      , local_dof_indices(dofs_per_cell)
    {}

    CopyData(const CopyData &) = default;

    CopyData(CopyData &&) = default;

    ~CopyData() = default;

    CopyData &operator=(const CopyData &) = default;

    CopyData &operator=(CopyData &&) = default;

    struct FaceData
    {
      FullMatrix<double>                   cell_matrix;
      std::vector<types::global_dof_index> joint_dof_indices;
    };

    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;
    std::vector<FaceData>                face_data;
  };

// 更有趣的部分是我们实际组装线性系统的地方。从根本上说，这个函数有五个部分。

// - `cell_worker`λ函数的定义，这是一个定义在`assemble_system()`函数中的小函数，它将负责计算单个单元上的局部积分。它将在`ScratchData`类的副本上工作，并将其结果放入相应的`CopyData`对象。

// - `face_worker` lambda函数的定义，它将对单元格之间的界面上的所有项进行积分。

// - 定义了`boundary_worker`函数，对位于域的边界上的单元面做同样的工作。

// - `copier`函数的定义，该函数负责将前面三个函数中的所有数据复制到单个单元的复制对象中，并复制到全局矩阵和右侧。

// 第五部分是我们把所有这些都集中在一起。

// 让我们轮流浏览一下这些组装所需的每一块。

  template <int dim>
  void BiharmonicProblem<dim>::assemble_system()
  {
    using Iterator = typename DoFHandler<dim>::active_cell_iterator;

// 第一部分是`cell_worker'，它在细胞内部进行组装。它是一个（lambda）函数，以一个单元格（输入）、一个抓取对象和一个复制对象（输出）为参数。它看起来像许多其他教程程序的装配函数，或者至少是所有单元格的循环主体。

// 我们在这里整合的条款是单元格对全局矩阵的贡献
// @f{align*}{
//     A^K_{ij} = \int_K \nabla^2\varphi_i(x) : \nabla^2\varphi_j(x) dx
//  @f} ，
//  以及对右侧向量的贡献
//  @f{align*}{
//     f^K_i = \int_K \varphi_i(x) f(x) dx
//  @f}

// 我们使用与组装 step-22 相同的技术来加速该函数。我们不在最里面的循环中调用`fe_values.shape_hessian(i, qpoint)`，而是创建一个变量`hessian_i`，在循环中对`i`进行一次评估，在循环中对`j`重新使用如此评估的值。为了对称，我们对变量`hessian_j`也做了同样的处理，尽管它确实只用了一次，而且我们可以在计算两个项之间标量乘积的指令中留下对`fe_values.shape_hessian(j,qpoint)`的调用。

    auto cell_worker = [&](const Iterator &  cell,
                           ScratchData<dim> &scratch_data,
                           CopyData &        copy_data) {
      copy_data.cell_matrix = 0;
      copy_data.cell_rhs    = 0;

      FEValues<dim> &fe_values = scratch_data.fe_values;
      fe_values.reinit(cell);

      cell->get_dof_indices(copy_data.local_dof_indices);

      const ExactSolution::RightHandSide<dim> right_hand_side;

      const unsigned int dofs_per_cell =
        scratch_data.fe_values.get_fe().n_dofs_per_cell();

      for (unsigned int qpoint = 0; qpoint < fe_values.n_quadrature_points;
           ++qpoint)
        {
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              const Tensor<2, dim> &hessian_i =
                fe_values.shape_hessian(i, qpoint);

              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                {
                  const Tensor<2, dim> &hessian_j =
                    fe_values.shape_hessian(j, qpoint);

                  copy_data.cell_matrix(i, j) +=
                    scalar_product(hessian_i,   // nabla^2 phi_i(x)
                                   hessian_j) * // nabla^2 phi_j(x)
                    fe_values.JxW(qpoint);      // dx
                }

              copy_data.cell_rhs(i) +=
                fe_values.shape_value(i, qpoint) * // phi_i(x)
                right_hand_side.value(
                  fe_values.quadrature_point(qpoint)) * // f(x)
                fe_values.JxW(qpoint);                  // dx
            }
        }
    };

// 下一个构建模块是在网格的每个内部面组装惩罚项。正如 MeshWorker::mesh_loop(), 文档中所描述的，这个函数接收到的参数表示一个单元和它的相邻单元，以及（对于这两个单元中的每一个）我们必须整合的面（以及潜在的子面）。同样地，我们也得到了一个从头开始的对象，以及一个用于放置结果的拷贝对象。

// 这个函数本身有三个部分。在顶部，我们初始化FEInterfaceValues对象，并创建一个新的 `CopyData::FaceData` 对象来存储我们的输入。这将被推到`copy_data.face_data`变量的末尾。我们需要这样做，因为我们对一个给定单元进行积分的面（或子面）的数量因单元而异，而且这些矩阵的大小也不同，取决于面或子面相邻的自由度。正如 MeshWorker::mesh_loop(), 文档中所讨论的，每次访问一个新的单元时，复制对象都会被重置，所以我们推到`copy_data.face_data()`末尾的内容实际上就是后来的`copier`函数在复制每个单元的贡献到全局矩阵和右侧对象时所能看到的。

    auto face_worker = [&](const Iterator &    cell,
                           const unsigned int &f,
                           const unsigned int &sf,
                           const Iterator &    ncell,
                           const unsigned int &nf,
                           const unsigned int &nsf,
                           ScratchData<dim> &  scratch_data,
                           CopyData &          copy_data) {
      FEInterfaceValues<dim> &fe_interface_values =
        scratch_data.fe_interface_values;
      fe_interface_values.reinit(cell, f, sf, ncell, nf, nsf);

      copy_data.face_data.emplace_back();
      CopyData::FaceData &copy_data_face = copy_data.face_data.back();

      copy_data_face.joint_dof_indices =
        fe_interface_values.get_interface_dof_indices();

      const unsigned int n_interface_dofs =
        fe_interface_values.n_current_interface_dofs();
      copy_data_face.cell_matrix.reinit(n_interface_dofs, n_interface_dofs);

// 第二部分涉及到确定惩罚参数应该是什么。通过观察双线性形式中各种项的单位，很明显，惩罚必须具有 $\frac{\gamma}{h_K}$ 的形式（即，超过长度尺度的一个），但如何选择无维数 $\gamma$ 并不是先验的。从拉普拉斯方程的不连续Galerkin理论来看，人们可能猜想正确的选择是 $\gamma=p(p+1)$ 是正确的选择，其中 $p$ 是所用有限元的多项式程度。我们将在本程序的结果部分更详细地讨论这个选择。

// 在上面的公式中， $h_K$  是单元格  $K$  的大小。但这也不是很简单的事情。如果使用高度拉伸的单元格，那么一个更复杂的理论说， $h$ 应该被单元格 $K$ 的直径取代，该直径是有关边缘方向的法线。 事实证明，在deal.II中有一个函数用于此。其次，当从一个面的两个不同侧面看时， $h_K$ 可能是不同的。

// 为了安全起见，我们取这两个值的最大值。我们将注意到，如果使用自适应网格细化所产生的悬空节点，有可能需要进一步调整这一计算方法。

      const unsigned int p = fe.degree;
      const double       gamma_over_h =
        std::max((1.0 * p * (p + 1) /
                  cell->extent_in_direction(
                    GeometryInfo<dim>::unit_normal_direction[f])),
                 (1.0 * p * (p + 1) /
                  ncell->extent_in_direction(
                    GeometryInfo<dim>::unit_normal_direction[nf])));

// 最后，像往常一样，我们在正交点和指数`i`和`j`上循环，把这个面或子面的贡献加起来。然后将这些数据存储在上面创建的`copy_data.face_data`对象中。至于单元格工作者，如果可能的话，我们将平均数和跳跃的评估从循环中拉出来，引入局部变量来存储这些结果。然后组件只需要在最里面的循环中使用这些局部变量。关于这段代码实现的具体公式，回顾一下，双线性形式的接口项如下。
// @f{align*}{
//   -\sum_{e \in \mathbb{F}} \int_{e}
//   \jump{ \frac{\partial v_h}{\partial \mathbf n}}
//   \average{\frac{\partial^2 u_h}{\partial \mathbf n^2}} \ ds
//  -\sum_{e \in \mathbb{F}} \int_{e}
//  \average{\frac{\partial^2 v_h}{\partial \mathbf n^2}}
//  \jump{\frac{\partial u_h}{\partial \mathbf n}} \ ds
//  + \sum_{e \in \mathbb{F}}
//  \frac{\gamma}{h_e}
//  \int_e
//  \jump{\frac{\partial v_h}{\partial \mathbf n}}
//  \jump{\frac{\partial u_h}{\partial \mathbf n}} \ ds.
//  @f}

      for (unsigned int qpoint = 0;
           qpoint < fe_interface_values.n_quadrature_points;
           ++qpoint)
        {
          const auto &n = fe_interface_values.normal(qpoint);

          for (unsigned int i = 0; i < n_interface_dofs; ++i)
            {
              const double av_hessian_i_dot_n_dot_n =
                (fe_interface_values.average_hessian(i, qpoint) * n * n);
              const double jump_grad_i_dot_n =
                (fe_interface_values.jump_gradient(i, qpoint) * n);

              for (unsigned int j = 0; j < n_interface_dofs; ++j)
                {
                  const double av_hessian_j_dot_n_dot_n =
                    (fe_interface_values.average_hessian(j, qpoint) * n * n);
                  const double jump_grad_j_dot_n =
                    (fe_interface_values.jump_gradient(j, qpoint) * n);

                  copy_data_face.cell_matrix(i, j) +=
                    (-av_hessian_i_dot_n_dot_n       // - {grad^2 v n n }
                       * jump_grad_j_dot_n           // [grad u n]
                     - av_hessian_j_dot_n_dot_n      // - {grad^2 u n n }
                         * jump_grad_i_dot_n         // [grad v n]
                     +                               // +
                     gamma_over_h *                  // gamma/h
                       jump_grad_i_dot_n *           // [grad v n]
                       jump_grad_j_dot_n) *          // [grad u n]
                    fe_interface_values.JxW(qpoint); // dx
                }
            }
        }
    };

// 第三块是对处于边界的面做同样的装配。当然，想法和上面一样，唯一不同的是，现在有惩罚条款也进入了右手边。

// 和以前一样，这个函数的第一部分只是设置了一些辅助对象。

    auto boundary_worker = [&](const Iterator &    cell,
                               const unsigned int &face_no,
                               ScratchData<dim> &  scratch_data,
                               CopyData &          copy_data) {
      FEInterfaceValues<dim> &fe_interface_values =
        scratch_data.fe_interface_values;
      fe_interface_values.reinit(cell, face_no);
      const auto &q_points = fe_interface_values.get_quadrature_points();

      copy_data.face_data.emplace_back();
      CopyData::FaceData &copy_data_face = copy_data.face_data.back();

      const unsigned int n_dofs =
        fe_interface_values.n_current_interface_dofs();
      copy_data_face.joint_dof_indices =
        fe_interface_values.get_interface_dof_indices();

      copy_data_face.cell_matrix.reinit(n_dofs, n_dofs);

      const std::vector<double> &JxW = fe_interface_values.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals =
        fe_interface_values.get_normal_vectors();

      const ExactSolution::Solution<dim> exact_solution;
      std::vector<Tensor<1, dim>>        exact_gradients(q_points.size());
      exact_solution.gradient_list(q_points, exact_gradients);

// 从正面看，由于我们现在只处理与面相邻的一个单元（因为我们在边界上），惩罚因子 $\gamma$ 的计算大大简化了。

      const unsigned int p = fe.degree;
      const double       gamma_over_h =
        (1.0 * p * (p + 1) /
         cell->extent_in_direction(
           GeometryInfo<dim>::unit_normal_direction[face_no]));

// 第三块是术语的组合。由于这些条款包含了矩阵的条款和右手边的条款，所以现在稍微有些麻烦。前者与上面所说的内部面完全相同，如果我们只是适当地定义了跳跃和平均（这就是FEInterfaceValues类所做的）。后者需要我们评估边界条件 $j(\mathbf x)$ ，在当前情况下（我们知道确切的解决方案），我们从 $j(\mathbf x) = \frac{\partial u(\mathbf x)}{\partial {\mathbf n}}$ 中计算出来。然后，要添加到右侧向量的项是  $\frac{\gamma}{h_e}\int_e \jump{\frac{\partial v_h}{\partial \mathbf n}} j \ ds$  。

      for (unsigned int qpoint = 0; qpoint < q_points.size(); ++qpoint)
        {
          const auto &n = normals[qpoint];

          for (unsigned int i = 0; i < n_dofs; ++i)
            {
              const double av_hessian_i_dot_n_dot_n =
                (fe_interface_values.average_hessian(i, qpoint) * n * n);
              const double jump_grad_i_dot_n =
                (fe_interface_values.jump_gradient(i, qpoint) * n);

              for (unsigned int j = 0; j < n_dofs; ++j)
                {
                  const double av_hessian_j_dot_n_dot_n =
                    (fe_interface_values.average_hessian(j, qpoint) * n * n);
                  const double jump_grad_j_dot_n =
                    (fe_interface_values.jump_gradient(j, qpoint) * n);

                  copy_data_face.cell_matrix(i, j) +=
                    (-av_hessian_i_dot_n_dot_n  // - {grad^2 v n n}
                       * jump_grad_j_dot_n      //   [grad u n]

//

                     - av_hessian_j_dot_n_dot_n // - {grad^2 u n n}
                         * jump_grad_i_dot_n    //   [grad v n]

//

                     + gamma_over_h             //  gamma/h
                         * jump_grad_i_dot_n    // [grad v n]
                         * jump_grad_j_dot_n    // [grad u n]
                     ) *
                    JxW[qpoint]; // dx
                }

              copy_data.cell_rhs(i) +=
                (-av_hessian_i_dot_n_dot_n *       // - {grad^2 v n n }
                   (exact_gradients[qpoint] * n)   //   (grad u_exact . n)
                 +                                 // +
                 gamma_over_h                      //  gamma/h
                   * jump_grad_i_dot_n             // [grad v n]
                   * (exact_gradients[qpoint] * n) // (grad u_exact . n)
                 ) *
                JxW[qpoint]; // dx
            }
        }
    };

// 第四部分是一个小函数，它将上面的单元格、内部和边界面装配程序产生的数据复制到全局矩阵和右手向量中。这里真的没有什么可做的。我们分配单元格矩阵和右侧贡献，就像我们在其他几乎所有的教程程序中使用约束对象那样。然后，我们还必须对面矩阵的贡献做同样的处理，这些贡献已经获得了面（内部和边界）的内容，并且`面_工作`和`边界_工作`已经添加到`copy_data.face_data`阵列中。

    auto copier = [&](const CopyData &copy_data) {
      constraints.distribute_local_to_global(copy_data.cell_matrix,
                                             copy_data.cell_rhs,
                                             copy_data.local_dof_indices,
                                             system_matrix,
                                             system_rhs);

      for (auto &cdf : copy_data.face_data)
        {
          constraints.distribute_local_to_global(cdf.cell_matrix,
                                                 cdf.joint_dof_indices,
                                                 system_matrix);
        }
    };

// 在设置了所有这些之后，剩下的就是创建一个从头开始和复制数据的对象，并调用 MeshWorker::mesh_loop() 函数，然后遍历所有的单元格和面，调用它们各自的工作器，然后是复制器函数，将东西放入全局矩阵和右侧。作为一个额外的好处， MeshWorker::mesh_loop() 以并行方式完成所有这些工作，使用你的机器恰好有多少个处理器核心。

    const unsigned int n_gauss_points = dof_handler.get_fe().degree + 1;
    ScratchData<dim>   scratch_data(mapping,
                                  fe,
                                  n_gauss_points,
                                  update_values | update_gradients |
                                    update_hessians | update_quadrature_points |
                                    update_JxW_values,
                                  update_values | update_gradients |
                                    update_hessians | update_quadrature_points |
                                    update_JxW_values | update_normal_vectors);
    CopyData           copy_data(dof_handler.get_fe().n_dofs_per_cell());
    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_boundary_faces |
                            MeshWorker::assemble_own_interior_faces_once,
                          boundary_worker,
                          face_worker);
  }

//  @sect4{Solving the linear system and postprocessing}

// 到此为止，节目基本上结束了。其余的函数并不太有趣或新颖。第一个函数只是用一个直接求解器来求解线性系统（也见 step-29  ）。

  template <int dim>
  void BiharmonicProblem<dim>::solve()
  {
    std::cout << "   Solving system..." << std::endl;

    SparseDirectUMFPACK A_direct;
    A_direct.initialize(system_matrix);
    A_direct.vmult(solution, system_rhs);

    constraints.distribute(solution);
  }

// 下一个函数评估了计算出的解和精确解之间的误差（在这里是已知的，因为我们选择了右手边和边界值的方式，所以我们知道相应的解）。在下面的前两个代码块中，我们计算了 $L_2$ 准则和 $H^1$ 半准则下的误差。

  template <int dim>
  void BiharmonicProblem<dim>::compute_errors()
  {
    {
      Vector<float> norm_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        ExactSolution::Solution<dim>(),
                                        norm_per_cell,
                                        QGauss<dim>(fe.degree + 2),
                                        VectorTools::L2_norm);
      const double error_norm =
        VectorTools::compute_global_error(triangulation,
                                          norm_per_cell,
                                          VectorTools::L2_norm);
      std::cout << "   Error in the L2 norm           :     " << error_norm
                << std::endl;
    }

    {
      Vector<float> norm_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        ExactSolution::Solution<dim>(),
                                        norm_per_cell,
                                        QGauss<dim>(fe.degree + 2),
                                        VectorTools::H1_seminorm);
      const double error_norm =
        VectorTools::compute_global_error(triangulation,
                                          norm_per_cell,
                                          VectorTools::H1_seminorm);
      std::cout << "   Error in the H1 seminorm       : " << error_norm
                << std::endl;
    }

// 现在也计算一下 $H^2$ 半正态误差的近似值。实际的 $H^2$ 半规范要求我们对解决方案 $u_h$ 的二阶导数进行积分，但是考虑到我们使用的拉格朗日形状函数， $u_h$ 当然在单元间的界面上有结点，因此二阶导数在界面是奇异的。因此，我们实际上只对单元的内部进行积分，而忽略了界面的贡献。这不是*等同于问题的能量准则，但是仍然可以让我们了解误差收敛的速度。

// 我们注意到，我们可以通过定义一个等同于能量准则的准则来解决这个问题。这将涉及到不仅要像我们下面做的那样将细胞内部的积分相加，而且还要为 $u_h$ 的导数在界面上的跳跃添加惩罚项，并对这两种项进行适当的缩放。我们将把这个问题留给以后的工作。

    {
      const QGauss<dim>            quadrature_formula(fe.degree + 2);
      ExactSolution::Solution<dim> exact_solution;
      Vector<double> error_per_cell(triangulation.n_active_cells());

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

      FEValuesExtractors::Scalar scalar(0);
      const unsigned int         n_q_points = quadrature_formula.size();

      std::vector<SymmetricTensor<2, dim>> exact_hessians(n_q_points);
      std::vector<Tensor<2, dim>>          hessians(n_q_points);
      for (auto &cell : dof_handler.active_cell_iterators())
        {
          fe_values.reinit(cell);
          fe_values[scalar].get_function_hessians(solution, hessians);
          exact_solution.hessian_list(fe_values.get_quadrature_points(),
                                      exact_hessians);

          double local_error = 0;
          for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
            {
              local_error +=
                ((exact_hessians[q_point] - hessians[q_point]).norm_square() *
                 fe_values.JxW(q_point));
            }
          error_per_cell[cell->active_cell_index()] = std::sqrt(local_error);
        }

      const double error_norm = error_per_cell.l2_norm();
      std::cout << "   Error in the broken H2 seminorm: " << error_norm
                << std::endl;
    }
  }

// 同样无趣的是生成图形输出的函数。它看起来和  step-6  中的一模一样，比如说。

  template <int dim>
  void
  BiharmonicProblem<dim>::output_results(const unsigned int iteration) const
  {
    std::cout << "   Writing graphical output..." << std::endl;

    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches();

    const std::string filename =
      ("output_" + Utilities::int_to_string(iteration, 6) + ".vtu");
    std::ofstream output_vtu(filename);
    data_out.write_vtu(output_vtu);
  }

// `run()`函数的情况也是如此。就像在以前的程序中一样。

  template <int dim>
  void BiharmonicProblem<dim>::run()
  {
    make_grid();

    const unsigned int n_cycles = 4;
    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        std::cout << "Cycle " << cycle << " of " << n_cycles << std::endl;

        triangulation.refine_global(1);
        setup_system();

        assemble_system();
        solve();

        output_results(cycle);

        compute_errors();
        std::cout << std::endl;
      }
  }
} // namespace Step47

//  @sect3{The main() function}

// 最后是 "main() "函数。同样，这里没有什么可看的。它看起来和以前的教程程序中的一样。有一个变量，可以选择我们要用来解方程的元素的多项式程度。因为我们使用的C0IP公式要求元素的度数至少为2，所以我们用一个断言来检查，无论为多项式度数设置什么都是有意义的。

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

      const unsigned int fe_degree = 2;
      Assert(fe_degree >= 2,
             ExcMessage("The C0IP formulation for the biharmonic problem "
                        "only works if one uses elements of polynomial "
                        "degree at least 2."));

      BiharmonicProblem<2> biharmonic_problem(fe_degree);
      biharmonic_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;
    }

  return 0;
}


// Copyright PinaPL
//
// weights.cpp
// PinaPL
//

#include <Eigen/Dense>
#include <random>
#include "weights.hpp"

Weights::Weights(int input_size, int output_size) {
    this->input_size = input_size;
    this->output_size = output_size;

// We initialize random weights
    this->weight_in_forget_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->input_size);

    this->weight_in_input_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->input_size);

    this->weight_in_input_block = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->input_size);

    this->weight_in_output_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->input_size);

    this->weight_st_forget_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->output_size);

    this->weight_st_input_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->output_size);

    this->weight_st_input_block = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->output_size);

    this->weight_st_output_gate = 0.1 * Eigen::MatrixXd::Random(
        this->output_size,
        this->output_size);

    this->bias_forget_gate = 0.1
        * Eigen::MatrixXd::Random(this->output_size, 1);
    this->bias_input_gate = 0.1
        * Eigen::MatrixXd::Random(this->output_size, 1);
    this->bias_input_block = 0.1
        * Eigen::MatrixXd::Random(this->output_size, 1);
    this->bias_output_gate = 0.1
        * Eigen::MatrixXd::Random(this->output_size, 1);

// We initialize a null gradient

    this->delta_weight_in_forget_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_input_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_input_block = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_output_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_st_forget_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_input_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_input_block = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_output_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_bias_forget_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->delta_bias_input_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->delta_bias_input_block = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->delta_bias_output_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
}

void Weights::apply_gradient(double lambda) {
// We apply the weight variations
    this->weight_in_forget_gate =
        this->weight_in_forget_gate
        - lambda * this->delta_weight_in_forget_gate;

    this->weight_in_input_gate =
        this->weight_in_input_gate
        - lambda * this->delta_weight_in_input_gate;

    this->weight_in_input_block =
        this->weight_in_input_block
        - lambda * this->delta_weight_in_input_block;

    this->weight_in_output_gate =
        this->weight_in_output_gate
        - lambda * this->delta_weight_in_output_gate;

    this->weight_st_forget_gate =
        this->weight_st_forget_gate
        - lambda * this->delta_weight_st_forget_gate;

    this->weight_st_input_gate =
        this->weight_st_input_gate
        - lambda * this->delta_weight_st_input_gate;

    this->weight_st_input_block =
        this->weight_st_input_block
        - lambda * this->delta_weight_st_input_block;

    this->weight_st_output_gate =
        this->weight_st_output_gate
        - lambda * this->delta_weight_st_output_gate;

    this->bias_forget_gate -= lambda * this->delta_bias_forget_gate;
    this->bias_input_gate -= lambda * this->delta_bias_input_gate;
    this->bias_input_block -= lambda * this->delta_bias_input_block;
    this->bias_output_gate -= lambda * this->delta_bias_output_gate;

// We set a null gradient
    this->delta_weight_in_forget_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_input_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_input_block = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_in_output_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->input_size);

    this->delta_weight_st_forget_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_input_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_input_block = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->delta_weight_st_output_gate = Eigen::MatrixXd::Zero(
        this->output_size,
        this->output_size);

    this->bias_forget_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->bias_input_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->bias_input_block = Eigen::MatrixXd::Zero(this->output_size, 1);
    this->bias_output_gate = Eigen::MatrixXd::Zero(this->output_size, 1);
}


/*****************************************************************************
*
* Copyright (C) 2011-2016 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* 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)
*
*****************************************************************************/

// Calculating free energy density of square lattice Ising model

// reference: B. Kastening, Phys. Rev. E 64, 066106 (2001), wrn:2011/02/10

#ifndef ISING_SQUARE_FINITE_HPP
#define ISING_SQUARE_FINITE_HPP

#include <vector>
#include <cmath>
#include <boost/math/differentiation/autodiff.hpp>
// #include <lse/exp_number.hpp>

// namespace {

// inline lse::exp_double cosh_value(double x) {
//   return (lse::exp_value(x) + lse::exp_value(-x)) / 2;
// }

// inline lse::exp_double sinh_value(double x) {
//   return (lse::exp_value(x) - lse::exp_value(-x)) / 2;
// }

// }

namespace ising {
namespace square {

template <typename FVAR>
inline double partition_function_impl(const FVAR& beta, double Jx, double Jy, int Lx, int Ly) {
  auto a = beta * Jx;
  auto b = beta * Jy;
  std::vector<FVAR> gamma(2 * Lx);
  for (int k = 0; k < 2 * Lx; ++k) {
    auto cosh_g =
      (cosh(2*a) * cosh(2*b) - cos(M_PI*k/Lx) * sinh(2*b)) / sinh(2*a);
    gamma[k] = log(cosh_g + sqrt(cosh_g * cosh_g - 1));
  }
  if (sinh(2*a) * sinh(2*b) > 1) gamma[0] = -gamma[0];
  FVAR p0(1), p1(1), p2(1), p3(1);
  for (int k = 1; k <= Lx; ++k) {
    p0 *= 2 * cosh(Ly * gamma[2*k-1] / 2);
    p1 *= 2 * sinh(Ly * gamma[2*k-1] / 2);
    p2 *= 2 * cosh(Ly * gamma[2*k-2] / 2);
    p3 *= 2 * sinh(Ly * gamma[2*k-2] / 2);
  }
  auto z = 0.5 * pow(2 * sinh(2*a), Lx*Ly/2) * (p0 + p1 + p2 - p3);
  auto f = -log(z) / beta / (Lx * Ly);
  // auto e =
  std::cout << f.derivative(0) << ' ' << f.derivative(1) << ' ' << f.derivative(2) << std::endl;
  return z.derivative(0);
}

inline double partition_function(double beta_in, double Jx, double Jy, int Lx, int Ly) {
  using namespace boost::math::differentiation;
  constexpr unsigned Order = 2;
  auto const beta = make_fvar<double, Order>(beta_in);
  return partition_function_impl(beta, Jx, Jy, Lx, Ly);

}

inline double free_energy(double beta, double Jx, double Jy, int Lx, int Ly) {
  return -log(partition_function(beta, Jx, Jy, Lx, Ly)) / beta;
}

inline double free_energy_density(double beta, double Jx, double Jy, int Lx, int Ly) {
  return free_energy(beta, Jx, Jy, Lx, Ly) / (Lx * Ly);
}

} // end namespace square
} // end namespace ising

#endif // ISING_SQUARE_FINITE_HPP


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python.hpp>
#include <rstbx/indexing_api/indexing_api.h>

using namespace boost::python;
using namespace rstbx::indexing_api;

namespace indexing_api{

struct rayleigh_cpp { // a fast C++ version of the Rayleigh distribution class
  /*
  =============================================================================
  Class models a 1-d Rayleigh distribution using one parameter, sigma.

              x                x^2
    pdf = --------- exp(- ------------)
           sigma^2         2 sigma^2

                        x^2
    cdf = 1 - exp(- -----------)
                     2 sigma^2

  The derivative of the cdf with respect to sigma is,

      d(cdf)          x^2               x^2             x
    ---------- = - --------- exp( - -----------) = - ------- pdf
     d(sigma)       sigma^3          2 sigma^2        sigma

  Methods:
    set_parameters
    get_parameters
    estimate_parameters_from_cdf
    pdf
    cdf
    d_cdf_d_sigma
    d_cdf_d_sigma_finite
    cdf_gradients
  -----------------------------------------------------------------------------
  */
  rayleigh_cpp(): sigma(1.),interface("C++"){}
  rayleigh_cpp(const double& s): sigma(s){}

  void set_parameters(scitbx::af::shared<double> p) {
    SCITBX_ASSERT(p.size() == 1);
    sigma = p[0];
  }

  scitbx::af::shared<double> get_parameters(){
    return scitbx::af::shared<double>(1,sigma);
  }

  void estimate_parameters_from_cdf(scitbx::af::shared<double> x_data,scitbx::af::shared<double>y_data){
    //Function estimates the parameter values based on the data (cdf)
    // sigma is the mode of the distribution
    // approximate with the median (cdf = 0.5)
    int midpoint = 0;
    for (int i=0; i < x_data.size(); ++i){
      if (y_data[i] > 0.5){
        midpoint = i;
        break;
      }
    }
    if (midpoint == 0){
      midpoint = x_data.size() - 1;
    }
    sigma = x_data[midpoint];
  }

  double pdf(const double& x){
    //Function returns the probability density function at x
      double x_sigma = x/sigma;
      return (x_sigma/sigma)*std::exp(-0.5*x_sigma*x_sigma);
  }

  scitbx::af::shared<double> pdf(scitbx::af::shared<double> x){
    //Function returns the probability density function at x
    scitbx::af::shared<double> f;
    for (int i = 0; i < x.size(); ++i){
      double x_sigma = x[i]/sigma;
      f.push_back( (x_sigma/sigma)*std::exp(-0.5*x_sigma*x_sigma) );
    }
    return f;
  }

  double cdf(const double& x){
    //Function returns the cumulative distribution function at x
      double x_sigma = x/sigma;
      return 1.0 - std::exp(-0.5*x_sigma*x_sigma);
  }

  scitbx::af::shared<double> cdf(scitbx::af::shared<double> x){
    //Function returns the cumulative distribution function at x
    scitbx::af::shared<double> f;
    for (int i = 0; i < x.size(); ++i){
      double x_sigma = x[i]/sigma;
      f.push_back( 1.0 - std::exp(-0.5*x_sigma*x_sigma) );
    }
    return f;
  }

  double d_cdf_d_sigma(const double& x){
    //Function returns the derivative of the cdf at x with respect to the standard deviation
    double p = pdf(x);
    return -(x/sigma)*p ;
  }

  scitbx::af::shared<double> d_cdf_d_sigma(scitbx::af::shared<double> x){
    //Function returns the derivative of the cdf at x with respect to the standard deviation
    scitbx::af::shared<double> p = pdf(x);
    scitbx::af::shared<double> df;
    for (int i = 0; i < x.size(); ++i){
      df.push_back ( -(x[i]/sigma)*p[i] );
    }
    return df;
  }

  scitbx::af::shared<double> cdf_gradients(const double& x){
    //Function returns a flex.double containing all derivatives
    scitbx::af::shared<double> result;
    result.push_back( d_cdf_d_sigma(x) );
    return result;
  }
  scitbx::af::shared<double> gradients(
    scitbx::af::shared<double> x, const int& nparams, scitbx::af::shared<double>difference){
    //Convenience function to return the gradients in the context of fit_distribution.py
    scitbx::af::shared<double> gradients = scitbx::af::shared<double>(nparams);
    for (int i = 0; i < x.size(); ++i){
      scitbx::af::shared<double> g_i = cdf_gradients(x[i]);
      for (int j = 0; j < nparams; ++j){
        gradients[j] = gradients[j] + difference[i]*g_i[j];
      }
    }
    for (int i = 0; i < gradients.size(); ++i){
      gradients[i] = 2.0*gradients[i];
    }
    return gradients;
  }
  double sigma;
  std::string interface;
};

struct find_green_bar {
  find_green_bar(const scitbx::af::shared<double> rayleigh_cdf_x,
                 const scitbx::af::shared<double> rayleigh_cdf,
                 const scitbx::af::shared<double> dr,
                 const scitbx::af::shared<double> x,
                 const double& sd ):
                 is_set(false){
    const double* ptr_rayleigh_cdf_x = rayleigh_cdf_x.begin();
    const double* ptr_rayleigh_cdf = rayleigh_cdf.begin();
    const double* ptr_x = x.begin();
    const double* ptr_dr = dr.begin();

    for (std::size_t i=0; i < rayleigh_cdf_x.size(); ++i){
      double mx = ptr_rayleigh_cdf_x[i];
      double my = ptr_rayleigh_cdf[i];
      for (std::size_t j=1; j < dr.size(); ++j){
        double upper_x = ptr_dr[j];
        double upper_y = ptr_x[j];
        double lower_x = ptr_dr[j-1];
        double lower_y = ptr_x[j-1];
        if ((my >= lower_y) && (my < upper_y)){
          if ((sd <= (upper_x - mx)) && ((lower_x - mx) > 0.0)){
            //sd_data = ((mx,my),(lower_x,lower_y))
            sd_mx = mx;
            sd_my = my;
            sd_lower_x = lower_x;
            sd_lower_y = lower_y;
            is_set = true;
            radius_outlier_index = j-1;
            limit_outlier = lower_x;
            break;
          }
        }
        if (is_set){
          break;
        }
      }
    }
  }

  bool is_set;
  double sd_mx,sd_my,limit_outlier,sd_lower_x,sd_lower_y;
  int radius_outlier_index;

};

}

BOOST_PYTHON_MODULE(rstbx_indexing_api_ext)
{

   def("cpp_absence_test",cpp_absence_test);

   class_<dps_extended, bases<rstbx::dps_core> >("dps_extended",init< >())
     .def("getData",&dps_extended::getData)
     .def("setData",&dps_extended::setData)
     .def("refine_direction",&dps_extended::refine_direction,
          (arg("candidate"),arg("current_grid"),
           arg("target_grid")))
   ;

   def("raw_spot_positions_mm_to_reciprocal_space_xyz",
     ( scitbx::af::shared< scitbx::vec3<double> > (*) (
       rstbx::pointlist,dxtbx::model::Detector const&, double const&,
       scitbx::vec3<double> const& , scitbx::vec3<double> const&, scitbx::af::shared<int>) )
     raw_spot_positions_mm_to_reciprocal_space_xyz);
   def("raw_spot_positions_mm_to_reciprocal_space_xyz",
     ( scitbx::af::shared< scitbx::vec3<double> > (*) (
       rstbx::pointlist,dxtbx::model::Detector const&, double const&,
       scitbx::vec3<double> const& , scitbx::af::shared<int>) )
     raw_spot_positions_mm_to_reciprocal_space_xyz);

  typedef return_value_policy<return_by_value> rbv;
  class_<indexing_api::find_green_bar>("find_green_bar",
    init<const scitbx::af::shared<double>, const scitbx::af::shared<double>,
              const scitbx::af::shared<double>, const scitbx::af::shared<double>,
              const double& > ((
              arg("rayleigh_cdf_x"),arg("rayleigh_cdf"),arg("dr"),arg("x"),arg("sd"))))
    .add_property("is_set",make_getter(&indexing_api::find_green_bar::is_set, rbv()))
    .add_property("sd_mx",make_getter(&indexing_api::find_green_bar::sd_mx, rbv()))
    .add_property("sd_my",make_getter(&indexing_api::find_green_bar::sd_my, rbv()))
    .add_property("limit_outlier",make_getter(&indexing_api::find_green_bar::limit_outlier, rbv()))
    .add_property("sd_lower_x",make_getter(&indexing_api::find_green_bar::sd_lower_x, rbv()))
    .add_property("sd_lower_y",make_getter(&indexing_api::find_green_bar::sd_lower_y, rbv()))
    .add_property("radius_outlier_index",make_getter(&indexing_api::find_green_bar::radius_outlier_index, rbv()))
  ;
   class_<indexing_api::rayleigh_cpp >("rayleigh_cpp",init< >())
     .def("set_parameters",&indexing_api::rayleigh_cpp::set_parameters, (arg("p")))
     .def("get_parameters",&indexing_api::rayleigh_cpp::get_parameters)
     .def("estimate_parameters_from_cdf",&indexing_api::rayleigh_cpp::estimate_parameters_from_cdf,
         (arg("x_data"),arg("y_data")))
     .def("pdf",(double (indexing_api::rayleigh_cpp::*)(const double&))&indexing_api::rayleigh_cpp::pdf, (arg("x")))
     .def("pdf",(scitbx::af::shared<double> (indexing_api::rayleigh_cpp::*)(scitbx::af::shared<double>))&indexing_api::rayleigh_cpp::pdf, (arg("x")))
     .def("cdf",(double (indexing_api::rayleigh_cpp::*)(const double&))(&indexing_api::rayleigh_cpp::cdf), (arg("x")))
     .def("cdf",(scitbx::af::shared<double> (indexing_api::rayleigh_cpp::*)(scitbx::af::shared<double>))(&indexing_api::rayleigh_cpp::cdf), (arg("x")))
     .def("d_cdf_d_sigma",(double (indexing_api::rayleigh_cpp::*)(const double&))&indexing_api::rayleigh_cpp::d_cdf_d_sigma)
     .def("d_cdf_d_sigma",(scitbx::af::shared<double> (indexing_api::rayleigh_cpp::*)(scitbx::af::shared<double>))&indexing_api::rayleigh_cpp::d_cdf_d_sigma)
     .def("cdf_gradients",(scitbx::af::shared<double> (indexing_api::rayleigh_cpp::*)(const double&))&indexing_api::rayleigh_cpp::cdf_gradients, (arg("x")))
     .add_property("interface",make_getter(&indexing_api::rayleigh_cpp::interface, rbv()))
     .def("gradients",&indexing_api::rayleigh_cpp::gradients, (arg("x"),arg("nparams"),arg("difference")))
   ;
}


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

#include <Eigen/Core>
#include <boost/math/constants/constants.hpp>

using namespace std;
using namespace Eigen;

int main(int argc, char *argv[])
{
	ofstream writing_file;
	const float pi = boost::math::constants::pi<float>();

	/* 遊脚の時間 */
	const float period = 0.30;
	/* 目標足上げ高さ */
	const float h = 0.060;
	/* サンプリングタイム */
	const float dt = 0.01;

	/* 足先軌道 */
	Matrix<float,2,1> p(Matrix<float,2,1>::Zero());
	/* 目標足先接地位置 */
	Matrix<float,2,1> p_goal(Matrix<float,2,1>::Zero());
	/* 初期足先位置 */
	Matrix<float,2,1> p_start(Matrix<float,2,1>::Zero());

	/* 3次補間のための係数 */
	Matrix<float,2,4> A(Matrix<float,2,4>::Zero());

	/* 目標足先接地地点 */
	p_goal << atof(argv[1]), atof(argv[2]);
	/* 初期の足先位置 */
	p_start << atof(argv[3]), atof(argv[4]);

	A << p_start(0), 0, 3*(p_goal(0)-p_start(0))/pow(period,2), -2*(p_goal(0)-p_start(0))/pow(period,3),
		 p_start(1), 0, 3*(p_goal(1)-p_start(1))/pow(period,2), -2*(p_goal(1)-p_start(1))/pow(period,3),

	writing_file.open("swing_foot_trajectory.csv");
	for(float t=0.0f; t<=period;t+=dt){
		/* z方向の遊脚軌道生成(サイクロイド曲線) */
		float z_swing = h*0.5*(1-cos(2*pi/static_cast<int>(period/dt)*(t/dt)));

		/* 時刻tのx, y方向の遊脚軌道生成 */
		p = A * Vector4f(1, t, t*t, t*t*t);

		/* ファイル書き込み */
		writing_file << t << " " << p(0) << " " << p(1) << " "<<z_swing << endl;
	}
	writing_file.close();

	return 0;
}


/**
 *  @file    SparseSolver.hpp
 *  @brief   Solves a finite difference problem.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef SPARSESOLVER_H
#define SPARSESOLVER_H

#include <vector>
#include <Eigen/SparseCore>
#include "spdlog/spdlog.h"
#include "Problem.hpp"

namespace numerical {

namespace fdm {

/*
 * This class is used to solve the diffusion problem in 1D with uniform and
 * constant diffusion coefficient \f$\alpha\f$ and constant Dirichlet boundary
 * conditions.
 */
template <typename T>
class SparseSolver {
typedef Eigen::SparseMatrix<T> SpMat;
typedef Eigen::Triplet<T> Trip;
typedef Eigen::VectorXd Vec;
private:
  SpMat m_A;
  Vec m_b;
  Vec m_u;
  Problem<T>* m_problem;
public:
  SparseSolver(Problem<T>* problem)
    : m_problem(problem)
     {
      // define variables
      T dx, dt;
  }
  ~SparseSolver(){
      // delete m_A;
      // delete m_b;
  }

  /**
  * Assembles sparse coefficient matrix A.
  *
  * \f[
  *    A_{i,i-1}=-F\Theta,~A{i, i}=1+2F\Theta,~A_{i, i+1}=-F\Theta
  * \f].
  *
  */
  void assemble_a(){
    /*
    // The loops are vectorized for efficiency -- see bench/performances
    SpMat A(nx + 1, nx + 1);
    std::vector<Trip> trp;
    // diagonal terms
    // Eigen::VectorXd val = Eigen::VectorXd::Zero(nx + 1);  // initialized to zero
    Vec diagonal = Eigen::VectorXd::Constant(nx + 1, 1.0);
    diagonal[0] = 0.0;
    diagonal[nx] = 0.0;
    // segment(pos, n) the n coeffs in the range [pos : pos + n - 1]
    diagonal.segment(1, diagonal.size()-2) += Dl * m_a.segment(2, m_a.size()-2);
    diagonal.segment(1, diagonal.size()-2) += Dl * 2.0 * m_a.segment(1, m_a.size()-2);
    diagonal.segment(1, diagonal.size()-2) += Dl * m_a.segment(0, m_a.size()-2);
    // lower terms
    Vec lower = Eigen::VectorXd::Zero(nx);
    lower.segment(0, lower.size()-1) += -Dl * m_a.segment(1, m_a.size()-2);
    lower.segment(0, lower.size()-1) += -Dl * m_a.segment(0, m_a.size()-2);
    // upper terms
    Vec upper = Eigen::VectorXd::Zero(nx);
    upper.segment(1, upper.size()-1) += -Dl * m_a.segment(2, m_a.size()-2);
    upper.segment(1, upper.size()-1) += -Dl * m_a.segment(1, m_a.size()-2);

    // boundary conditions
    diagonal[0] = 1.0;
    upper[0] = 0.0;
    diagonal[nx] = 1.0;
    lower[nx-1] = 0.0;

    // std::cout << diagonal << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
        trp.push_back(Trip(i,i,diagonal[i]));
    }
    // std::cout << lower << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
         trp.push_back(Trip(i,i-1,lower[i-1]));
    }
    // std::cout << upper << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
        trp.push_back(Trip(i,i+1,upper[i]));
    }
    // create sparse matrix
    A.setFromTriplets(trp.begin(), trp.end());
    m_A = A;
    */
  }

  /**
  * Assembles RHS vector b.
  *
  * \f[
  *    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
  *        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
  * \f]
  *
  * using vectorization we get:
  *
  * \f[
  *    b[1:n_x-1] = u_n[1:n_x-1] + \left(1-\Theta\right)F
  *        \left(u_n[2:n_x]-2u_n[1:n_x-1]+u_n[0:n_x-2]\right) +
  *        \Theta\Delta t f[1:n_x-1](n+1) +
  *        \left(1-\Theta\right)\Delta t f[1:n_x-1](n)
  * \f]
  *
  */
  void assemble_b(T t){

  }

  /*
  * Solve the time dependent problem.
  */
  virtual Eigen::VectorXf solve(){
      //  Set initial condition
      //for(int i=0; i<u_n.size(); i++){
          // u_n[i] = m_params.init(m_x[i], 0., 0.);
      //}
      // std::cout << u_n << "\n";

      spdlog::info("{}", m_problem->left(0, 1.0, 1.0, 1.0));
      // Time loop

      Eigen::VectorXf solution = Eigen::VectorXf::Unit(4,1);
      return solution;
  }

};

/*
 * For a sparse matrix, return a vector of triplets, such that we can
 * reconstruct the matrix using setFromTriplet function.
 * @param matrix A sparse matrix.
 * @return A triplet with the row, column and value of the non-zero entries.
 */
template <typename Derived>
std::vector<Eigen::Triplet<typename Derived::Scalar>> SparseMatrixToTriplets(
    const Derived& matrix) {
  using Scalar = typename Derived::Scalar;
  std::vector<Eigen::Triplet<Scalar>> triplets;
  triplets.reserve(matrix.nonZeros());
  for (int i = 0; i < matrix.outerSize(); i++) {
    for (typename Derived::InnerIterator it(matrix, i); it; ++it) {
      triplets.push_back(
          Eigen::Triplet<Scalar>(it.row(), it.col(), it.value()));
    }
  }
  return triplets;
}

}  // namespace fdm

}  // namespace numerical

#endif  // SPARSESOLVER_H


/*
 * CubicPolynomialTrajectory.h
 *
 *  Created on: June 21, 2020
 *      Author: Quincy Jones
 *
 * Copyright (c) <2020> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_
#define NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_

// C System Files

// C++ System Files

// Third Party Includes
#include <Eigen/Dense>

// Project Include Files
namespace Common
{
    class CubicPolynomialTrajectory
    {

    public:
        CubicPolynomialTrajectory(double q_f, double t_f);
        CubicPolynomialTrajectory(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);
        CubicPolynomialTrajectory(); // Empty Trajectory

        void Generate(double q_f, double t_f);
        void Generate(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);

        // TODO: Check for valid t between 0<->t_f
        double Position(double t);
        double Velocity(double t);
        double Acceleration(double t);

    protected:
        void ComputeCoeffs();

        Eigen::Vector4d a_; // Coefficients

        double q_0_;
        double v_0_;
        double t_0_;

        double q_f_;
        double v_f_;
        double t_f_;
    };
} // namespace Common

#endif // NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_


/**
 * @file solver.cpp
 * @brief API for ADMM-based formation gain solver
 * @author Parker Lusk <parkerclusk@gmail.com>
 * @date 25 July 2020
 */

#include <iostream>

#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/SparseCholesky>

#include "admm/solver.h"

namespace acl {
namespace aclswarm {
namespace admm {

Solver::Solver(const Params& params)
: params_(params)
{

}

// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve(
                        const Eigen::Matrix<double, 3, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Solve 2D gain design subproblem
  //

  const auto A2d = solve2d(pts.topRows(2), adj);

  //
  // Solve 1D gain design subproblem
  //

  const auto A1d = solve1d(pts.bottomRows(1), adj);

  //
  // Combine for 3D gain design problem
  //

  const size_t n = pts.cols();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*n,3*n);
  for (size_t i=0; i<A.rows(); ++i) {
    for (size_t j=0; j<A.cols(); ++j) {

      // which 3x3 A_ij sub-block are we in?
      const size_t blki = i / 3;
      const size_t blkj = j / 3;

      // map index into 2d sub-block
      const size_t i2d = i - blki;
      const size_t j2d = j - blkj;

      // map index into 1d sub-block
      const size_t i1d = blki;
      const size_t j1d = blkj;

      // determine if we are indexing the 3rd row/col in A_ij
      bool row3 = ((i+1) % 3) == 0;
      bool col3 = ((j+1) % 3) == 0;

      if (!row3 && !col3) {
        A(i,j) = A2d(i2d,j2d);
      } else if (row3 && col3) {
        A(i,j) = A1d(i1d,j1d);
      }
    }
  }

  return A;
}

// ----------------------------------------------------------------------------
// Private Methods
// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve1d(
                        const Eigen::Matrix<double, 1, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Build orthogonal complement of gain matrix kernel
  //

  const size_t n = adj.rows();
  const size_t d = 1; // ambient dimension of the problem

  // xy stacked
  Eigen::Map<const Eigen::VectorXd> qz(pts.data(), pts.size());

  // one vector
  Eigen::VectorXd ez = Eigen::VectorXd::Ones(n);

  // determine if desired formation is actually 2D (flat planar)
  const double stdev = std::sqrt((qz.array() - qz.mean()).array().square().sum()/(n-1));
  bool xyflat = (stdev < params_.thrPlanar);

  // kernel of gain matrix
  size_t dimKer;
  Eigen::MatrixXd N;
  if (xyflat) {
    dimKer = 1;
    N = Eigen::MatrixXd(pts.size(), dimKer);
    N << qz;
  } else {
    dimKer = 2;
    N = Eigen::MatrixXd(pts.size(), dimKer);
    N << qz, ez;
  }
  const size_t m = n - dimKer; // reduced number due to orth. compl. restriction

  // find the orthogonal complement of the kernel
  // recall: N = [U1 U2][S 0; 0 0][V1h; V2h]. We want U2.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(N, Eigen::ComputeFullU);
  Eigen::MatrixXd Q = svd.matrixU().rightCols(svd.matrixU().cols() - dimKer);

  //
  // Build the gain design optimization problem
  //

  SpMat C, A, b, X;
  parse(d, m, n, adj, Q, C, A, b, X);

  //
  // Solve SDP using ADMM on sparse matrices
  //

  admm(C, A, b, X);

  //
  // Recover gain matrix
  //

  Eigen::MatrixXd Aopt = - Q * X.bottomRightCorner(d*m, d*m) * Q.transpose();
  Aopt = (params_.thrSparseZero < Aopt.array().abs()).select(Aopt, 0.0);

  return Aopt;
}

// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve2d(
                        const Eigen::Matrix<double, 2, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Build orthogonal complement of gain matrix kernel
  //

  const size_t n = adj.rows();
  const size_t m = n - 2; // reduced number due to orth. compl. restriction
  const size_t d = 2; // ambient dimension of the problem

  // xy stacked
  Eigen::Map<const Eigen::VectorXd> q(pts.data(), pts.size());

  // 90-degree rotated (-yx stacked)
  Eigen::VectorXd qbar = Eigen::VectorXd::Zero(pts.size());
  Eigen::Map<const Eigen::VectorXd, 0, Eigen::InnerStride<2>> qx(q.data(), q.size()/2);
  Eigen::Map<const Eigen::VectorXd, 0, Eigen::InnerStride<2>> qy(q.data()+1, q.size()/2);
  Eigen::Map<Eigen::VectorXd, 0, Eigen::InnerStride<2>> qbarx(qbar.data(), qbar.size()/2);
  Eigen::Map<Eigen::VectorXd, 0, Eigen::InnerStride<2>> qbary(qbar.data()+1, qbar.size()/2);
  qbarx = -qy;
  qbary =  qx;

  // one vectors
  Eigen::VectorXd ex = Eigen::Vector2d::UnitX().replicate(n, 1);
  Eigen::VectorXd ey = Eigen::Vector2d::UnitY().replicate(n, 1);

  // kernel of gain matrix
  static constexpr size_t dimKer = 4;
  Eigen::Matrix<double, Eigen::Dynamic, dimKer> N = Eigen::MatrixXd(pts.size(), dimKer);
  N << q, qbar, ex, ey;

  // find the orthogonal complement of the kernel
  // recall: N = [U1 U2][S 0; 0 0][V1h; V2h]. We want U2.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(N, Eigen::ComputeFullU);
  Eigen::MatrixXd Q = svd.matrixU().rightCols(svd.matrixU().cols() - dimKer);

  //
  // Build the gain design optimization problem
  //

  SpMat C, A, b, X;
  parse(d, m, n, adj, Q, C, A, b, X);

  //
  // Solve SDP using ADMM on sparse matrices
  //

  admm(C, A, b, X);

  //
  // Recover gain matrix
  //

  Eigen::MatrixXd Aopt = - Q * X.bottomRightCorner(d*m, d*m) * Q.transpose();
  Aopt = (params_.thrSparseZero < Aopt.array().abs()).select(Aopt, 0.0);

  return Aopt;
}

// ----------------------------------------------------------------------------

inline size_t Solver::vecsel(size_t rows, size_t cols, size_t i, size_t j)
{
  return j*rows + i;
}

// ----------------------------------------------------------------------------

inline size_t Solver::blksel(size_t dim, size_t blkidx, size_t subidx)
{
  return dim*blkidx + subidx;
}

// ----------------------------------------------------------------------------

inline void Solver::vectorize(const SpMat& X, SpMat& x)
{
  x.resize(X.size(), 1);
  x.reserve(X.nonZeros());
  x.startVec(0);
  for (size_t j=0; j<X.cols(); ++j) {
    for (SpMat::InnerIterator it(X, j); it; ++it) {
      x.insertBack(j*X.rows() + it.row(), 0) = it.value();
    }
  }
}

// ----------------------------------------------------------------------------

inline void Solver::unvectorize(const SpMat& x, SpMat& X)
{
  X.reserve(x.nonZeros());
  int curj = -1;

  for (SpMat::InnerIterator it(x, 0); it; ++it) {

    // select the correct destination row/col.
    const size_t i = it.row() % X.rows();
    const size_t j = it.row() / X.cols();
    if (j != curj) {
      X.startVec(j);
      curj = j;
    }

    X.insertBack(i, j) = it.value();
  }
}

// ----------------------------------------------------------------------------

void Solver::admm(const SpMat& C, const SpMat& A, const SpMat& b, SpMat& X)
{

  // cached operations
  const SpMat As = A.adjoint(); // dual operator
  Eigen::SimplicialCholesky<SpMat> AAs((A * As).pruned());

  // initialize intermediate variables
  SpMat Xold;
  SpMat S(X.rows(), X.cols());
  SpMat y(b.rows(), 1);

  //
  // ADMM Iterations
  //

  for (size_t i=0; i<params_.maxItr; ++i) {

    // update y
    {
      const SpMat D = C - S - params_.mu * X;
      SpMat Dvec; vectorize(D, Dvec);
      const SpMat e = A * Dvec + params_.mu * b;
      y = AAs.solve(e); // AAs \ e
    }

    // update S
    SpMat W;
    {
      const SpMat d = (As * y).pruned(1, params_.thrSparseZero);
      SpMat dmat(X.rows(), X.cols()); unvectorize(d, dmat);
      const SpMat WW = C - dmat - params_.mu * X;
      W = (WW + SpMat(WW.transpose())) / 2.0;
    }

    // determine index where positive evals start
    Eigen::SelfAdjointEigenSolver<SpMat> es(W);
    size_t k = 0;
    for (size_t i=0; i<W.rows(); ++i) {
      if (es.eigenvalues()(i) > params_.epsEig) {
        k = i;
        break;
      }
    }
    const size_t idxPosStart = W.rows() - k;

    // remove non-positive modes
    const Eigen::MatrixXd V = es.eigenvectors().rightCols(idxPosStart);
    const Eigen::MatrixXd D = es.eigenvalues().tail(idxPosStart).asDiagonal();
    S = (V * D * V.transpose()).sparseView(1, params_.thrSparseZero);

    // update X
    Xold = X;
    X = (S - W) / params_.mu;

    // check stop criteria --- difference in X
    const double diffX = (X - Xold).cwiseAbs().sum();
    if (diffX < params_.thresh) break;

    // check problem specific stop criteria --- trace value of \bar{A}
    const auto Abar = X.bottomRightCorner(X.rows()/2, X.cols()/2);
    const double Etr = Abar.rows(); // expected trace value (d*m)
    double tr = 0;
    for (size_t k=0; k<Abar.rows(); ++k) tr += Abar.coeff(k,k);
    double trPercentErr = (tr - Etr) / Etr;
    if (trPercentErr < params_.threshTr) break;
  }

  //
  // Project soln to ensure graph constraints are satisfied (set S=0)
  //

  const SpMat D = C - params_.mu * X;
  SpMat Dvec; vectorize(D, Dvec);
  const SpMat e = A * Dvec + params_.mu * b;
  y = AAs.solve(e); // AAs \ e

  const SpMat d = (As * y).pruned(1, params_.thrSparseZero);
  SpMat dmat(X.rows(), X.cols()); unvectorize(d, dmat);
  const SpMat WW = C - dmat - params_.mu * X;
  const SpMat W = (WW + SpMat(WW.transpose())) / 2.0;

  X = (- W) / params_.mu;
}

// ----------------------------------------------------------------------------

void Solver::parse(size_t d, size_t m, size_t n,
                      const Eigen::MatrixXd& adj, const Eigen::MatrixXd& Q,
                      SpMat& C, SpMat& A, SpMat& b, SpMat& X)
{
  //
  // Preallocate number of non-zeros
  //

  // block X_11
  const size_t nrA_X11 =
      (d*m-1)*2               // [X_11]_11 can be whatever it wants (t)
                              // but the other diag elements must be == [X_11]_11
    + (d*m)*(d*m-1)/2;        // set upper-triangular elements to zero
  const size_t nrb_X11 = 0;

  // block X_12
  const size_t nrA_X12 =
      d*m                     // each diagonal elem must be 1
    + (d*m)*(d*m-1);          // each off-diagonal elem must be 0
  const size_t nrb_X12 = d*m; // each diagonal elem must be 1

  // structure constraints for each gain matrix block
  const size_t nrA_X22_struct =
  (d == 2) ?
      0.5*m*(m+1)*(2+2)       // structure constraints: A_ij = [a b; -b a]
                              // 0.5*m*(m+1): each blk, including A_ii blks
    - m                       // don't count -b elem on A_ii (below diag)
                              // (2+2) because a-a=0 is 2 and b-b=0 is 2
  : 0; // no structure requirement for 1D subproblem
  const size_t nrb_X22_struct = 0;

  // zero-gain constraints based on given adj mat
  const size_t nr0 = ((adj.array()==0).count() - n)/2; // number of zeros in adj
  const size_t nrA_X22_adjmat =
      nr0*(d*(d*m)*(d*m));    // each 0 in adj creates d constraints on \bar{A}
  const size_t nrb_X22_adjmat = 0;

  // TODO: see MATLAB impl (ADMMGainDesign3D.m). Do we actually need to remove
  // trivial constraints, or was that left over from debugging / designing?

  // trace constraint on \bar{A}
  const size_t nrA_X22_trace =
      d*m;                    // the sum of each [X_22]_ii == d*m*destrace
  const size_t nrb_X22_trace = 1;

  // X must be symmetric: [X]_ij == [X]_ji
  const size_t nrA_X_sym =
      2 * d*m * (2*d*m-1);    //
  const size_t nrb_X_sym = 0;

  // total number of elements from constraints
  const size_t nrA = nrA_X11 + nrA_X12 + nrA_X22_struct + nrA_X22_adjmat + nrA_X22_trace + nrA_X_sym;
  const size_t nrb = nrb_X11 + nrb_X12 + nrb_X22_struct + nrb_X22_adjmat + nrb_X22_trace + nrb_X_sym;

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "nrA_X11: " << nrA_X11 << std::endl;
    std::cout << "nrA_X12: " << nrA_X12 << std::endl;
    std::cout << "nrA_X22_struct: " << nrA_X22_struct << std::endl;
    std::cout << "nr0: " << nr0 << std::endl;
    std::cout << "nrA_X22_adjmat: " << nrA_X22_adjmat << std::endl;
    std::cout << "nrA_X22_trace: " << nrA_X22_trace << std::endl;
    std::cout << "nrA_X_sym: " << nrA_X_sym << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "nrA: " << nrA << std::endl;
    std::cout << "nrb: " << nrb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
  }

  std::vector<Eigen::Triplet<double>> Acoeffs, bcoeffs;
  Acoeffs.reserve(nrA);
  bcoeffs.reserve(nrb);

  size_t itrr = 0; // which row of \mathbf{A} should nz val be in?

  //
  // Build constraints for block X_11
  //

  // diagonal entries of X_11 should be equal to the first diagonal entry
  for (size_t i=1; i<d*m; ++i) {

    // always select first diagonal entry
    Acoeffs.emplace_back(itrr, 0, 1);

    // [1 0 ... -1 ... 0] vec(X) = 0
    //           ^
    //           selects elem corresponding to diagonal, [X_11]_ii
    const size_t itrc = vecsel(2*d*m, 2*d*m, i, i);
    Acoeffs.emplace_back(itrr, itrc, -1);

    // create new row in \mathbf{A} linear constraint matrix
    itrr++;
  }

  // off-diagonal entries should be zero
  for (size_t i=0; i<d*m; ++i) {
    for (size_t j=i+1; j<d*m; ++j) {

      const size_t itrc = vecsel(2*d*m, 2*d*m, i, j);
      Acoeffs.emplace_back(itrr, itrc, 1);

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  size_t tmpA = 0;
  size_t tmpb = 0;

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_11 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Build constraints for block X_12
  //

  // off-diagonal entries should be zero
  for (size_t i=0; i<d*m; ++i) {
    for (size_t j=0; j<d*m; ++j) {

      const size_t jj = d*m + j; // skip first dm cols to get into X_12
      const size_t itrc = vecsel(2*d*m, 2*d*m, i, jj);

      // diagonal entries should be one
      if (i == j) {
        Acoeffs.emplace_back(itrr, itrc, 1);
        bcoeffs.emplace_back(itrr,    0, 1);
      } else { // all other entries should be zero
        Acoeffs.emplace_back(itrr, itrc, 1);
      }

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_12 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Build constraints for block X_22 = \bar{A}
  //

  if (d == 2) {
    // structure constraints A_ij = [a b; -b a]
    for (size_t i=0; i<m; ++i) {
      for (size_t j=i; j<m; ++j) {

        // diagonal entries should be equal
        const size_t ii1 = d*m + blksel(d, i, 0); // skip first dm rows
        const size_t jj1 = d*m + blksel(d, j, 0); // and cols for X_22
        const size_t ii2 = d*m + blksel(d, i, 1);
        const size_t jj2 = d*m + blksel(d, j, 1);
        const size_t itrc1 = vecsel(2*d*m, 2*d*m, ii1, jj1);
        const size_t itrc2 = vecsel(2*d*m, 2*d*m, ii2, jj2);
        Acoeffs.emplace_back(itrr, itrc1,  1);
        Acoeffs.emplace_back(itrr, itrc2, -1);

        // create new row in \mathbf{A} linear constraint matrix
        itrr++;

        // off-diagonal entries should have same value with opposite sign.
        if (i == j) {
          // If operating on a blk on the diag (A_ii), only enfore constraints
          // for the upper triangular portion---sym const enforced later.
          // note that these constraints enforce b = 0.
          const size_t ii = d*m + blksel(d, i, 0); // skip first dm rows
          const size_t jj = d*m + blksel(d, j, 1); // and cols for X_22
          const size_t itrc = vecsel(2*d*m, 2*d*m, ii, jj);
          Acoeffs.emplace_back(itrr, itrc, 1);
        } else {
          const size_t ii1 = d*m + blksel(d, i, 0); // skip first dm rows
          const size_t jj1 = d*m + blksel(d, j, 1); // and cols for X_22
          const size_t ii2 = d*m + blksel(d, i, 1);
          const size_t jj2 = d*m + blksel(d, j, 0);
          const size_t itrc1 = vecsel(2*d*m, 2*d*m, ii1, jj1);
          const size_t itrc2 = vecsel(2*d*m, 2*d*m, ii2, jj2);
          Acoeffs.emplace_back(itrr, itrc1, 1);
          Acoeffs.emplace_back(itrr, itrc2, 1);
        }

        // create new row in \mathbf{A} linear constraint matrix
        itrr++;
      }
    }
  }

  // graph constraints (zero blocks for non-neighbors)
  if (nr0 > 0) {
    for (size_t i=0; i<n; ++i) {
      for (size_t j=i+1; j<n; ++j) {
        if (adj(i,j) == 1) continue;

        // we leverage the structure constraint [a b; -b a] and only
        // create explicit constraints for [A_ij]_11 and [A_ij]_12.

        // two constraint rows are created in \mathbf{A}
        const size_t itrr1 = itrr;
        const size_t itrr2 = itrr + 1;

        // Constraint on [A_ij]_11
        const size_t ii1 = blksel(d, i, 0); // skip first dm rows
        const size_t jj1 = blksel(d, j, 0); // and cols for X_22
        const Eigen::MatrixXd QQ1 = Q.transpose().col(jj1) * Q.row(ii1);

        Eigen::MatrixXd QQ2;
        if (d == 2) {
          // Constraint on [A_ij]_12
          const size_t ii2 = blksel(d, i, 1); // skip first dm rows
          const size_t jj2 = blksel(d, j, 0); // and cols for X_22
          QQ2 = Q.transpose().col(jj2) * Q.row(ii2);
        }

        // Note how the linear transformation using the orthogonal complement Q
        // leaks signal into each element of the gain matrix \bar{A}.
        for (size_t ki=0; ki<d*m; ++ki) {
          for (size_t kj=0; kj<d*m; ++kj) {

            const size_t ii = d*m + ki; // skip first dm rows
            const size_t jj = d*m + kj; // and cols for X_22
            const size_t itrc = vecsel(2*d*m, 2*d*m, ii, jj);

            Acoeffs.emplace_back(itrr1, itrc, QQ1(ki,kj));
            if (d == 2) Acoeffs.emplace_back(itrr2, itrc, QQ2(ki,kj));
          }
        }

        // advance by d rows in \mathbf{A} linear constraint matrix
        itrr += d;
      }
    }
  }

  // trace of \bar{A} matrix must be the specified value
  {
    for (size_t i=0; i<d*m; ++i) {

      const size_t ii = d*m + i; // skip first dm rows/cols for X_22
      const size_t itrc = vecsel(2*d*m, 2*d*m, ii, ii);
      Acoeffs.emplace_back(itrr, itrc, 1);
    }

    // expected trace value
    bcoeffs.emplace_back(itrr, 0, d*m);

    // create new row in \mathbf{A} linear constraint matrix
    itrr++;
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_22 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Symmetry constraints for entire X matrix
  //

  // symmetric entries should be equal
  for (size_t i=0; i<2*d*m; ++i) {
    for (size_t j=i+1; j<2*d*m; ++j) {

      const size_t itrc1 = vecsel(2*d*m, 2*d*m, i, j);
      const size_t itrc2 = vecsel(2*d*m, 2*d*m, j, i);
      Acoeffs.emplace_back(itrr, itrc1,  1);
      Acoeffs.emplace_back(itrr, itrc2, -1);

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X symmetry constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Prepare sparse matrices for ADMM
  //

  C.resize(2*d*m, 2*d*m);
  C.reserve(Eigen::VectorXi::Constant(d*m,1)); // reserve 1 nz per column of X_11
  for (size_t i=0; i<d*m; ++i) C.insert(i,i) = 1; // make [I 0; 0 0]

  // initialize decision variable to something fairly close
  X.resize(2*d*m, 2*d*m); // [I I; I I]
  X.reserve(Eigen::VectorXi::Constant(2*d*m,2)); // reserve 2 nz per column
  for (size_t i=0; i<d*m; ++i) {
    X.insert(i,i) = 1;
    X.insert(d*m+i,i) = 1;
  }
  for (size_t i=d*m; i<2*d*m; ++i) {
    X.insert(i,i) = 1;
    X.insert(i-d*m,i) = 1;
  }

  A.resize(itrr, X.size());
  A.setFromTriplets(Acoeffs.begin(), Acoeffs.end());

  b.resize(itrr, 1);
  b.setFromTriplets(bcoeffs.begin(), bcoeffs.end());
}

} // ns admm
} // ns aclswarm
} // ns acl


// Copyright 2004-5 The Trustees of Indiana University.
// Copyright 2002 Brad King and Douglas Gregor

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#ifndef _ALG_PAGE_RANK_HPP
#define _ALG_PAGE_RANK_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/graph/page_rank.hpp>
#include <vector>

namespace boost { namespace graph {

//~ struct n_iterations
//~ {
  //~ explicit n_iterations(std::size_t n) : n(n) { }

  //~ template<typename RankMap, typename Graph>
  //~ bool
  //~ operator()(const RankMap&, const Graph&)
  //~ {
    //~ return n-- == 0;
  //~ }

 //~ private:
  //~ std::size_t n;
//~ };

namespace detail {
  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
                      incidence_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type rank_type;

    // Set new rank maps
    BGL_FORALL_VERTICES_T(v, g, Graph) put(to_rank, v, rank_type(1 - damping));

    BGL_FORALL_VERTICES_T(u, g, Graph) {
      rank_type u_rank_out = damping * get(from_rank, u) / out_degree(u, g);
      BGL_FORALL_ADJ_T(u, v, g, Graph){
        rank_type ctx_factor = decision_fct(u, v);
        put(to_rank, v, get(to_rank, v) + u_rank_out * ctx_factor);
      }
    }
  }

  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
                      bidirectional_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type damping_type;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      typename property_traits<RankMap>::value_type rank(0);
      BGL_FORALL_INEDGES_T(v, e, g, Graph){
        damping_type ctx_factor = decision_fct(v, source(e, g));
        rank += get(from_rank, source(e, g)) / out_degree(source(e, g), g) * ctx_factor;
      }
      put(to_rank, v, (damping_type(1) - damping) + damping * rank);
    }
  }
} // end namespace detail

template<typename Graph, typename RankMap, typename Done, typename RankMap2>
void
page_rank(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n,
          RankMap2 rank_map2
          BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph, vertex_list_graph_tag))
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  rank_type initial_rank = rank_type(rank_type(1) / n);
  BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, initial_rank);

  bool to_map_2 = true;
  while ((to_map_2 && !done(rank_map, g)) ||
         (!to_map_2 && !done(rank_map2, g))) {
    typedef typename graph_traits<Graph>::traversal_category category;

    if (to_map_2) {
      detail::page_rank_step(g, rank_map, rank_map2, damping, decision_fct, category());
    } else {
      detail::page_rank_step(g, rank_map2, rank_map, damping, decision_fct, category());
    }
    to_map_2 = !to_map_2;
  }

  if (!to_map_2) {
    BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, get(rank_map2, v));
  }
}

template<typename Graph, typename RankMap, typename Done>
void
page_rank(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  std::vector<rank_type> ranks2(num_vertices(g));
  page_rank(g, rank_map, decision_fct, done, damping, n,
            make_iterator_property_map(ranks2.begin(), get(vertex_index, g)));
}

template<typename Graph, typename RankMap, typename Done>
inline void
page_rank_ctx(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping = 0.85)
{
  page_rank(g, rank_map, decision_fct, done, damping, num_vertices(g));
}

template<typename Graph, typename RankMap>
inline void
page_rank_ctx(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct)
{
  page_rank_ctx(g, rank_map, decision_fct, n_iterations(20));
}

} } // end namespace boost::graph

#ifdef BOOST_GRAPH_USE_MPI
#  include <boost/graph/distributed/page_rank.hpp>
#endif

#endif // BOOST_GRAPH_PAGE_RANK_HPP


#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <functional>

#include "../util/assert.hh"
#include "../util/Maybe.hh"

namespace bold
{
  template<typename T>
  class LineSegment2;

  class Math
  {
  public:
    static Maybe<Eigen::Vector3d> intersectRayWithGroundPlane(Eigen::Vector3d const& position,
                                                              Eigen::Vector3d const& direction,
                                                              double const planeZ);

    static Maybe<Eigen::Vector3d> intersectRayWithPlane(Eigen::Vector3d const& position,
                                                        Eigen::Vector3d const& direction,
                                                        Eigen::Vector4d const& plane);

    static Eigen::Vector2d linePointClosestToPoint(LineSegment2<double> const& segment,
                                                   Eigen::Vector2d const& point);

    // TODO what if 'vector' has zero length? should this return 'Maybe<Vector2d>'?
    static Eigen::Vector2d findPerpendicularVector(Eigen::Vector2d const& vector);

    static std::function<double()> createUniformRng(double min, double max, bool randomSeed = true);
    static std::function<double()> createNormalRng(double mean, double stddev, bool randomSeed = true);

    static constexpr double degToRad(double degrees) { return (degrees * M_PI) / 180.0; }
    static constexpr double radToDeg(double radians) { return (radians / M_PI) * 180.0; }

    static double smallestAngleBetween(Eigen::Vector2d v1, Eigen::Vector2d v2);

    static Eigen::Affine3d alignUp(Eigen::Affine3d const& transform);

    template<typename T>
    static constexpr T clamp(T val, T min, T max)
    {
      return val < min
        ? min
        : val > max
          ? max
          : val;
    }

    /**
      * Maps @link input to a value in the range from @link lowerOutput to @link upperOutput.
      * If @link input is outside the range from @link lower to @link upper, then @link lowerOutput or @link upperOutput are returned,
      * otherwise the value is linearly interpolated.
      * Note that @link lower must be less than @link upper, but that there is no restriction on values of @link lowerOutput and @link upperOutput.
      */
    template<typename T>
    static T lerp(double const& input, double const& lower, double const& upper, T const& lowerOutput, T const& upperOutput)
    {
      if (unlikely(upper <= lower))
        throw std::runtime_error("lower must be less than upper");

      double ratio = (input - lower) / (upper - lower);
      ratio = clamp(ratio, 0.0, 1.0);

      return lowerOutput + (upperOutput - lowerOutput) * ratio;
    }

    template<typename T>
    static constexpr T lerp(double const& ratio, T const& lowerOutput, T const& upperOutput)
    {
      return lowerOutput + (upperOutput - lowerOutput) * ratio;
    }

    /** Constrains the angle to range [-PI,PI). */
    static double normaliseRads(double rads)
    {
      rads = fmod(rads + M_PI, 2*M_PI);
      if (rads < 0)
          rads += 2*M_PI;
      return rads - M_PI;
    }

    /** Angle spanned by rotation between two angles
     *
     * The rotation is measured by rotating from @a a1 to @a2 in
     * positive direction (counter clockwise for right hand system).
     */
    static double angleDiffRads(double a1, double a2)
    {
      double rads = a2 - a1;
      if (rads < 0.0)
        rads += 2.0 * M_PI;
      return rads;
    }

    /** Absolute distance between two angles in radians
     *
     * e.g.:
     * |pi - .5 pi| = |.5 pi - pi| = .5 pi
     * | -.1 po - .1pi | = | .1pi - -.1 pi | = .2 pi
     * | -.9 pi - .9 pi | = | .9 pi - -.9pi | = .2 pi
     */
    static double shortestAngleDiffRads(double a1, double a2)
    {
      // The fmod() function computes the floating-point remainder of
      // dividing x by y.  The return value is x - n * y, where n is
      // the quotient of x / y, rounded toward zero to an integer.
      double d = fmod(a2 - a1, 2 * M_PI);

      d += (d > M_PI)
        ? -2 * M_PI
        : d <= -M_PI
          ? 2 * M_PI
          : 0;

      return d;
    }

    /** Returns the angle to a point, as defined in the agent frame,
     * where zero is straight ahead and positive is to the left
     * (counter-clockwise). */
    template<int N>
    static double angleToPoint(Eigen::Matrix<double, N, 1> const& point)
    {
      static_assert(N > 1, "Vector must have at least two dimensions");
      return ::atan2(-point.x(), point.y());
    }

    /** Returns the point at the given angle and distance, as defined
     * in the agent frame, where zero is straight ahead and positive
     * is to the left (counter-clockwise). */
    static inline Eigen::Vector2d pointAtAngle(double angle, double distance)
    {
      return Eigen::Vector2d(cos(angle) * distance, sin(angle) * distance);
    }

    /** Returns mean of angles
     *
     * TODO: Algorithm at:
     * http://www.codeproject.com/Articles/190833/Circular-Values-Math-and-Statistics-with-Cplusplus
     * seems to give more intuitive results
     */
    static double angularMean(std::vector<double> const& angles)
    {
      double x = 0.0;
      double y = 0.0;
      for (auto a : angles)
      {
        x += sin(a);
        y += cos(a);
      }
      return atan2(x / angles.size(), y / angles.size());
    }

  private:
    Math() = delete;
  };
}


// -*- mode: c++; fill-column: 80; indent-tabs-mode: nil; -*-

#include <cassert>
#include <cmath>

#include <iomanip>
#include <iostream>
#include <vector>

#include "expsum/fast_esprit.hpp"
#include <boost/math/special_functions/bessel.hpp>

using size_type = arma::uword;
// using Complex   = std::complex<double>;

//------------------------------------------------------------------------------
// Test functors
//------------------------------------------------------------------------------

struct BesselJ0
{
    double operator()(double x) const
    {
        return boost::math::cyl_bessel_j(0, x);
    }
};

struct rinv
{
    double operator()(double x) const
    {
        return 1.0 / x;
    }
};

template <typename F, typename Vec>
void make_sample(F f, double xmin, double xmax, Vec& result)
{
    auto np = result.n_elem;
    auto h  = (xmax - xmin) / (np - 1);
    for (size_type n = 0; n < np; ++n)
    {
        result(n) = f(xmin + n * h);
    }

    return;
}

template <typename F>
void test_fast_esprit(F fn, double xmin, double xmax, size_type N, size_type L,
                      size_type M, double eps)
{
    using value_type  = decltype(fn(xmin));
    using vector_type = arma::Col<value_type>;
    using esprit_type = expsum::fast_esprit<value_type>;
    using real_type   = typename esprit_type::real_type;

    vector_type exact(N);
    make_sample(fn, xmin, xmax, exact);
    auto delta = (xmax - xmin) / (N - 1);

    // ESPRIT esprit(N, std::min<size_type>(100, n / 2));
    esprit_type esprit(N, L, M);

    esprit.fit(exact, xmin, delta, eps);

    auto nterms = esprit.exponents().n_elem;
    std::cout << "# " << nterms << " terms found\n"
              << "# exponent, weight\n";

    for (size_type i = 0; i < nterms; ++i)
    {
        std::cout << esprit.exponents()(i) << '\t' << esprit.weights()(i)
                  << '\n';
    }

    std::cout << "# x, approx, exact, abserr, relerr\n";

    for (size_type i = 0; i < N; ++i)
    {
        auto x      = xmin + i * delta;
        auto approx = esprit.eval_at(x);
        auto abserr = std::abs(approx - exact(i));
        auto relerr =
            (abserr == real_type()) ? real_type() : abserr / std::abs(exact(i));

        std::cout << x << '\t' << approx << '\t' << exact(i) << '\t' << abserr
                  << '\t' << relerr << '\n';
    }
}

int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    std::cout << "# Approximation of Bessel J0(x): x in [0, 1000] by fast "
                 "ESPRIT method."
              << std::endl;
    size_type N = 1024;  // # of sampling points
    size_type L = N / 2; // window length
    size_type M = 100;   // max # of terms
    double xmin = 0.0;
    double xmax = 1000.0;
    double eps  = 1.0e-10;
    test_fast_esprit(BesselJ0(), xmin, xmax, N, L, M, eps);

    std::cout << "\n\n# Approximation of 1/r: r in [1, 10^{6}] by fast"
                 "ESPRIT method."
              << std::endl;
    N    = (1 << 12);
    L    = N / 2;
    M    = 100;
    xmin = 1.0;
    xmax = 1.0e+6;
    eps  = 1.0e-8;
    test_fast_esprit(rinv(), xmin, xmax, N, L, M, eps);

    // std::cout << "# Exponential sum recovery test" << std::endl;
    // const auto pi = arma::datum::pi;
    // numeric::ExponentialSum<Complex> orig(5);

    // orig.exponents(0) = Complex(0.0,     0.0);
    // orig.exponents(1) = Complex(0.0,  pi / 4);
    // orig.exponents(2) = Complex(0.0, -pi / 4);
    // orig.exponents(3) = Complex(0.0,  pi / 2);
    // orig.exponents(4) = Complex(0.0, -pi / 2);

    // orig.weights(0) = Complex(34.0,  0.0);
    // orig.weights(1) = Complex(300.0, 0.0);
    // orig.weights(2) = Complex(300.0, 0.0);
    // orig.weights(3) = Complex(1.0,   0.0);
    // orig.weights(4) = Complex(1.0,   0.0);

    // N = 1024;
    // L = N / 2;
    // M = 20;
    // eps = 1.0e-10;
    // test_frequency_estimation(orig, N, L, M, eps);

    return 0;
}