/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Luca Heltai, Cataldo Manigrasso, 2009
 */

// @sect3{Include files}

// 程序一开始就包括了一堆include文件，我们将在程序的各个部分使用这些文件。其中大部分在以前的教程中已经讨论过了。

#include <deal.II/base/smartpointer.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/quadrature_selector.h>
#include <deal.II/base/parsed_function.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/precondition.h>

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

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

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

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

// 这里有一些我们需要的C++标准头文件。

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

// 这个序言的最后部分是将dealii命名空间中的所有内容导入到这个程序中的所有内容中。

namespace Step34
{
  using namespace dealii;
// @sect3{Single and double layer operator kernels}

// 首先，让我们定义一下边界积分方程的机制。

// 以下两个函数是单层和双层势能核的实际计算，即  $G$  和  $\nabla G$  。只有当矢量 $R = \mathbf{y}-\mathbf{x}$ 不同于零时，它们才是定义良好的。

  namespace LaplaceKernel
  {
    template <int dim>
    double single_layer(const Tensor<1, dim> &R)
    {
      switch (dim)
        {
          case 2:
            return (-std::log(R.norm()) / (2 * numbers::PI));

          case 3:
            return (1. / (R.norm() * 4 * numbers::PI));

          default:
            Assert(false, ExcInternalError());
            return 0.;
        }
    }

    template <int dim>
    Tensor<1, dim> double_layer(const Tensor<1, dim> &R)
    {
      switch (dim)
        {
          case 2:
            return R / (-2 * numbers::PI * R.norm_square());
          case 3:
            return R / (-4 * numbers::PI * R.norm_square() * R.norm());

          default:
            Assert(false, ExcInternalError());
            return Tensor<1, dim>();
        }
    }
  } // namespace LaplaceKernel
// @sect3{The BEMProblem class}

// 边界元素方法代码的结构与有限元素代码的结构非常相似，所以这个类的成员函数与其他大多数教程程序的成员函数一样。特别是，现在你应该熟悉从外部文件中读取参数，以及将不同的任务分割成不同的模块。这同样适用于边界元素方法，我们不会对其进行过多的评论，只是对其中的差异进行评论。

  template <int dim>
  class BEMProblem
  {
  public:
    BEMProblem(const unsigned int fe_degree      = 1,
               const unsigned int mapping_degree = 1);

    void run();

  private:
    void read_parameters(const std::string &filename);

    void read_domain();

    void refine_and_resize();

// 我们在这里发现的唯一真正不同的函数是装配程序。我们以最通用的方式编写了这个函数，以便能够方便地推广到高阶方法和不同的基本解（例如斯托克斯或麦克斯韦）。

// 最明显的区别是，最终的矩阵是完整的，而且我们在通常的单元格循环内有一个嵌套的循环，访问所有自由度的支持点。 此外，当支持点位于我们所访问的单元内时，我们所执行的积分就会变成单数。

// 实际的结果是，我们有两套正交公式、有限元值和临时存储，一套用于标准积分，另一套用于奇异积分，在必要时使用。

    void assemble_system();

// 对于这个问题的解决有两种选择。第一个是使用直接求解器，第二个是使用迭代求解器。我们选择了第二种方案。

// 我们组装的矩阵不是对称的，我们选择使用GMRES方法；然而为边界元素方法构建一个有效的预处理程序并不是一个简单的问题。这里我们使用一个非预处理的GMRES求解器。迭代求解器的选项，如公差、最大迭代次数等，都是通过参数文件选择的。

    void solve_system();

// 一旦我们得到了解决方案，我们将计算计算出的势的 $L^2$ 误差，以及实体角的近似值的 $L^\infty$ 误差。我们使用的网格是平滑曲线的近似值，因此计算出的角的分量或实体角的对角线矩阵  $\alpha(\mathbf{x})$  应该一直等于  $\frac 12$  。在这个例程中，我们输出势的误差和计算角度的近似值的误差。注意，后者的误差实际上不是计算角度的误差，而是衡量我们对球体和圆的近似程度。

// 对角度的计算做一些实验，对于较简单的几何形状，可以得到非常准确的结果。为了验证这一点，你可以在read_domain()方法中注释掉tria.set_manifold(1, manifold)一行，并检查程序生成的alpha。通过删除这个调用，每当细化网格时，新的节点将沿着构成粗略网格的直线放置，而不是被拉到我们真正想要近似的表面。在三维案例中，球体的粗网格是从一个立方体开始得到的，得到的字母值正好是面的节点上的 $\frac 12$ ，边的节点上的 $\frac 34$ 和顶点的8个节点上的 $\frac 78$ 。

    void compute_errors(const unsigned int cycle);

// 一旦我们在一维领域得到了一个解决方案，我们就想把它插值到空间的其他部分。这可以通过在compute_exterior_solution()函数中再次进行解与核的卷积来实现。

// 我们想绘制速度变量，也就是势解的梯度。势解只在边界上是已知的，但我们使用与基本解的卷积在标准的二维连续有限元空间上进行插值。外推解的梯度图将给我们提供我们想要的速度。

// 除了外域上的解，我们还在output_results()函数中输出域的边界上的解，当然了。

    void compute_exterior_solution();

    void output_results(const unsigned int cycle);

// 为了实现不受维度限制的编程，我们对这个单一的函数进行了专业化处理，以提取整合单元内部的奇异核所需的奇异正交公式。

    const Quadrature<dim - 1> &get_singular_quadrature(
      const typename DoFHandler<dim - 1, dim>::active_cell_iterator &cell,
      const unsigned int index) const;

// 通常的deal.II类可以通过指定问题的 "二维 "来用于边界元素方法。这是通过将Triangulation, FiniteElement和DoFHandler的可选第二模板参数设置为嵌入空间的维度来实现的。在我们的例子中，我们生成了嵌入在二维或三维空间的一维或二维网格。

// 可选参数默认等于第一个参数，并产生我们在之前所有例子中看到的通常的有限元类。

// 该类的构造方式是允许任意的域（通过高阶映射）和有限元空间的逼近顺序。有限元空间和映射的顺序可以在该类的构造函数中选择。

    Triangulation<dim - 1, dim> tria;
    FE_Q<dim - 1, dim>          fe;
    DoFHandler<dim - 1, dim>    dof_handler;
    MappingQ<dim - 1, dim>      mapping;

// 在BEM方法中，生成的矩阵是密集的。根据问题的大小，最终的系统可能通过直接的LU分解来解决，或者通过迭代方法来解决。在这个例子中，我们使用了一个无条件的GMRES方法。为BEM方法建立一个预处理程序是不容易的，我们在此不做处理。

    FullMatrix<double> system_matrix;
    Vector<double>     system_rhs;

// 接下来的两个变量将表示解决方案 $\phi$ 以及一个向量，它将保存 $\alpha(\mathbf x)$ 的值（从一个点 $\mathbf x$ 可见的 $\Omega$ 的部分）在我们形状函数的支持点。

    Vector<double> phi;
    Vector<double> alpha;

// 收敛表是用来输出精确解和计算出的字母的误差的。

    ConvergenceTable convergence_table;

// 下面的变量是我们通过参数文件来填充的。 本例中我们使用的新对象是 Functions::ParsedFunction 对象和QuadratureSelector对象。

//  Functions::ParsedFunction 类允许我们通过参数文件方便快捷地定义新的函数对象，自定义的定义可以非常复杂（关于所有可用的选项，见该类的文档）。

// 我们将使用QuadratureSelector类来分配正交对象，该类允许我们根据一个识别字符串和公式本身的可能程度来生成正交公式。我们用它来允许自定义选择标准积分的正交公式，并定义奇异正交规则的顺序。

// 我们还定义了几个参数，这些参数是在我们想把解决方案扩展到整个领域的情况下使用的。

    Functions::ParsedFunction<dim> wind;
    Functions::ParsedFunction<dim> exact_solution;

    unsigned int                         singular_quadrature_order;
    std::shared_ptr<Quadrature<dim - 1>> quadrature;

    SolverControl solver_control;

    unsigned int n_cycles;
    unsigned int external_refinement;

    bool run_in_this_dimension;
    bool extend_solution;
  };
// @sect4{BEMProblem::BEMProblem and BEMProblem::read_parameters}

//构造函数初始化各种对象的方式与有限元程序（如  step-4  或  step-6  ）中的方式基本相同。这里唯一的新成分是ParsedFunction对象，它在构造时需要说明组件的数量。

// 对于精确解来说，向量分量的数量是1，而且不需要任何操作，因为1是ParsedFunction对象的默认值。然而，风需要指定dim组件。注意，在为 Functions::ParsedFunction, 的表达式声明参数文件中的条目时，我们需要明确指定分量的数量，因为函数 Functions::ParsedFunction::declare_parameters 是静态的，对分量的数量没有了解。

  template <int dim>
  BEMProblem<dim>::BEMProblem(const unsigned int fe_degree,
                              const unsigned int mapping_degree)
    : fe(fe_degree)
    , dof_handler(tria)
    , mapping(mapping_degree, true)
    , wind(dim)
    , singular_quadrature_order(5)
    , n_cycles(4)
    , external_refinement(5)
    , run_in_this_dimension(true)
    , extend_solution(true)
  {}

  template <int dim>
  void BEMProblem<dim>::read_parameters(const std::string &filename)
  {
    deallog << std::endl
            << "Parsing parameter file " << filename << std::endl
            << "for a " << dim << " dimensional simulation. " << std::endl;

    ParameterHandler prm;

    prm.declare_entry("Number of cycles", "4", Patterns::Integer());
    prm.declare_entry("External refinement", "5", Patterns::Integer());
    prm.declare_entry("Extend solution on the -2,2 box",
                      "true",
                      Patterns::Bool());
    prm.declare_entry("Run 2d simulation", "true", Patterns::Bool());
    prm.declare_entry("Run 3d simulation", "true", Patterns::Bool());

    prm.enter_subsection("Quadrature rules");
    {
      prm.declare_entry(
        "Quadrature type",
        "gauss",
        Patterns::Selection(
          QuadratureSelector<(dim - 1)>::get_quadrature_names()));
      prm.declare_entry("Quadrature order", "4", Patterns::Integer());
      prm.declare_entry("Singular quadrature order", "5", Patterns::Integer());
    }
    prm.leave_subsection();

// 对于二维和三维，我们将默认的输入数据设置为：解为  $x+y$  或  $x+y+z$  。实际计算出的解在无穷大时的数值为零。在这种情况下，这与精确解相吻合，不需要额外的修正，但是你应该注意，我们任意设置了 $\phi_\infty$ ，而我们传递给程序的精确解需要在无穷远处有相同的值，才能正确计算出误差。

//  Functions::ParsedFunction 对象的使用是非常直接的。 Functions::ParsedFunction::declare_parameters 函数需要一个额外的整数参数，指定给定函数的分量数量。它的默认值是1。当相应的 Functions::ParsedFunction::parse_parameters 方法被调用时，调用对象必须有与这里定义的相同数量的组件，否则会产生异常。

// 在声明条目时，我们同时声明了二维和三维的函数。然而只有二维的最终被解析。这使得我们对二维和三维问题都只需要一个参数文件。

// 注意，从数学的角度来看，边界上的风函数应该满足条件 $\int_{\partial\Omega} \mathbf{v}\cdot \mathbf{n} d \Gamma = 0$  ，这样问题才会有解。如果不满足这个条件，那么就找不到解，求解器也就不会收敛。

    prm.enter_subsection("Wind function 2d");
    {
      Functions::ParsedFunction<2>::declare_parameters(prm, 2);
      prm.set("Function expression", "1; 1");
    }
    prm.leave_subsection();

    prm.enter_subsection("Wind function 3d");
    {
      Functions::ParsedFunction<3>::declare_parameters(prm, 3);
      prm.set("Function expression", "1; 1; 1");
    }
    prm.leave_subsection();

    prm.enter_subsection("Exact solution 2d");
    {
      Functions::ParsedFunction<2>::declare_parameters(prm);
      prm.set("Function expression", "x+y");
    }
    prm.leave_subsection();

    prm.enter_subsection("Exact solution 3d");
    {
      Functions::ParsedFunction<3>::declare_parameters(prm);
      prm.set("Function expression", "x+y+z");
    }
    prm.leave_subsection();

// 在求解器部分，我们设置所有的SolverControl参数。然后，该对象将在solve_system()函数中被送入GMRES求解器。

    prm.enter_subsection("Solver");
    SolverControl::declare_parameters(prm);
    prm.leave_subsection();

// 在向ParameterHandler对象声明了所有这些参数后，让我们读取一个输入文件，该文件将为这些参数提供其值。然后我们继续从ParameterHandler对象中提取这些值。

    prm.parse_input(filename);

    n_cycles            = prm.get_integer("Number of cycles");
    external_refinement = prm.get_integer("External refinement");
    extend_solution     = prm.get_bool("Extend solution on the -2,2 box");

    prm.enter_subsection("Quadrature rules");
    {
      quadrature = std::shared_ptr<Quadrature<dim - 1>>(
        new QuadratureSelector<dim - 1>(prm.get("Quadrature type"),
                                        prm.get_integer("Quadrature order")));
      singular_quadrature_order = prm.get_integer("Singular quadrature order");
    }
    prm.leave_subsection();

    prm.enter_subsection("Wind function " + std::to_string(dim) + "d");
    {
      wind.parse_parameters(prm);
    }
    prm.leave_subsection();

    prm.enter_subsection("Exact solution " + std::to_string(dim) + "d");
    {
      exact_solution.parse_parameters(prm);
    }
    prm.leave_subsection();

    prm.enter_subsection("Solver");
    solver_control.parse_parameters(prm);
    prm.leave_subsection();

// 最后，这里是另一个如何在独立维度编程中使用参数文件的例子。 如果我们想关闭两个模拟中的一个，我们可以通过设置相应的 "运行2D模拟 "或 "运行3D模拟 "标志为假来实现。

    run_in_this_dimension =
      prm.get_bool("Run " + std::to_string(dim) + "d simulation");
  }
// @sect4{BEMProblem::read_domain}

// 边界元素法三角剖分与（dim-1）维三角剖分基本相同，不同之处在于顶点属于（dim）维空间。

// deal.II中支持的一些网格格式默认使用三维点来描述网格。这些格式与deal.II的边界元素方法功能兼容。特别是我们可以使用UCD或GMSH格式。在这两种情况下，我们必须特别注意网格的方向，因为与标准有限元的情况不同，这里没有进行重新排序或兼容性检查。 所有的网格都被认为是有方向性的，因为它们被嵌入到一个高维空间中。参见GridIn和Triangulation的文档，以进一步了解三角结构中单元的方向。在我们的例子中，网格的法线是外在于2D的圆或3D的球体。

// 对边界元素网格进行适当细化所需要的另一个细节是对网格所逼近的流形的准确描述。对于标准有限元网格的边界，我们已经多次看到了这一点（例如在 step-5 和 step-6 中），这里的原理和用法是一样的，只是SphericalManifold类需要一个额外的模板参数来指定嵌入空间维度。

  template <int dim>
  void BEMProblem<dim>::read_domain()
  {
    const Point<dim>                      center = Point<dim>();
    const SphericalManifold<dim - 1, dim> manifold(center);

    std::ifstream in;
    switch (dim)
      {
        case 2:
          in.open("coarse_circle.inp");
          break;

        case 3:
          in.open("coarse_sphere.inp");
          break;

        default:
          Assert(false, ExcNotImplemented());
      }

    GridIn<dim - 1, dim> gi;
    gi.attach_triangulation(tria);
    gi.read_ucd(in);

    tria.set_all_manifold_ids(1);

// 对  Triangulation::set_manifold  的调用复制了流形（通过  Manifold::clone()),  所以我们不需要担心对  manifold  的无效指针。

    tria.set_manifold(1, manifold);
  }
// @sect4{BEMProblem::refine_and_resize}

// 这个函数对网格进行全局细化，分配自由度，并调整矩阵和向量的大小。

  template <int dim>
  void BEMProblem<dim>::refine_and_resize()
  {
    tria.refine_global(1);

    dof_handler.distribute_dofs(fe);

    const unsigned int n_dofs = dof_handler.n_dofs();

    system_matrix.reinit(n_dofs, n_dofs);

    system_rhs.reinit(n_dofs);
    phi.reinit(n_dofs);
    alpha.reinit(n_dofs);
  }
// @sect4{BEMProblem::assemble_system}

// 下面是这个程序的主要功能，组装与边界积分方程相对应的矩阵。

  template <int dim>
  void BEMProblem<dim>::assemble_system()
  {

// 首先我们用正交公式初始化一个FEValues对象，用于在非奇异单元中进行内核积分。这个正交公式是通过参数文件选择的，并且需要相当精确，因为我们要积分的函数不是多项式函数。

    FEValues<dim - 1, dim> fe_v(mapping,
                                fe,
                                *quadrature,
                                update_values | update_normal_vectors |
                                  update_quadrature_points | update_JxW_values);

    const unsigned int n_q_points = fe_v.n_quadrature_points;

    std::vector<types::global_dof_index> local_dof_indices(
      fe.n_dofs_per_cell());

    std::vector<Vector<double>> cell_wind(n_q_points, Vector<double>(dim));
    double                      normal_wind;

// 与有限元方法不同的是，如果我们使用拼合边界元方法，那么在每个装配循环中，我们只装配与一个自由度（与支撑点 $i$ 相关的自由度）和当前单元之间的耦合信息。这是用fe.dofs_per_cell元素的向量完成的，然后将其分配到全局行的矩阵中  $i$  。以下对象将持有这些信息。

    Vector<double> local_matrix_row_i(fe.n_dofs_per_cell());

// 索引  $i$  运行在拼合点上，这是  $i$  第三个基函数的支持点，而  $j$  运行在内部积分点上。

// 我们构建一个支持点的向量，它将用于局部积分。

    std::vector<Point<dim>> support_points(dof_handler.n_dofs());
    DoFTools::map_dofs_to_support_points<dim - 1, dim>(mapping,
                                                       dof_handler,
                                                       support_points);

// 这样做之后，我们就可以开始对所有单元进行积分循环，首先初始化FEValues对象，得到正交点的 $\mathbf{\tilde v}$ 的值（这个向量场应该是常数，但更通用也无妨）。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_v.reinit(cell);
        cell->get_dof_indices(local_dof_indices);

        const std::vector<Point<dim>> &q_points = fe_v.get_quadrature_points();
        const std::vector<Tensor<1, dim>> &normals = fe_v.get_normal_vectors();
        wind.vector_value_list(q_points, cell_wind);

// 然后我们在当前单元上形成所有自由度的积分（注意，这包括不在当前单元上的自由度，这与通常的有限元积分有偏差）。如果其中一个局部自由度与支持点 $i$ 相同，我们需要执行的积分是单数。因此，在循环的开始，我们检查是否是这种情况，并存储哪一个是奇异指数。

        for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
          {
            local_matrix_row_i = 0;

            bool         is_singular    = false;
            unsigned int singular_index = numbers::invalid_unsigned_int;

            for (unsigned int j = 0; j < fe.n_dofs_per_cell(); ++j)
              if (local_dof_indices[j] == i)
                {
                  singular_index = j;
                  is_singular    = true;
                  break;
                }

// 然后我们进行积分。如果指数 $i$ 不是局部自由度之一，我们只需将单层项加到右边，将双层项加到矩阵中。

            if (is_singular == false)
              {
                for (unsigned int q = 0; q < n_q_points; ++q)
                  {
                    normal_wind = 0;
                    for (unsigned int d = 0; d < dim; ++d)
                      normal_wind += normals[q][d] * cell_wind[q](d);

                    const Tensor<1, dim> R = q_points[q] - support_points[i];

                    system_rhs(i) += (LaplaceKernel::single_layer(R) *
                                      normal_wind * fe_v.JxW(q));

                    for (unsigned int j = 0; j < fe.n_dofs_per_cell(); ++j)

                      local_matrix_row_i(j) -=
                        ((LaplaceKernel::double_layer(R) * normals[q]) *
                         fe_v.shape_value(j, q) * fe_v.JxW(q));
                  }
              }
            else
              {

// 现在我们处理更微妙的情况。如果我们在这里，这意味着在 $j$ 索引上运行的单元包含support_point[i]。在这种情况下，单层和双层势都是单数，它们需要特殊处理。
//每当在给定单元内进行积分时，就会使用一个特殊的正交公式，允许人们对参考单元上的奇异权重进行任意函数的积分。
//正确的正交公式由get_singular_quadrature函数选择，下面将详细说明。

                Assert(singular_index != numbers::invalid_unsigned_int,
                       ExcInternalError());

                const Quadrature<dim - 1> &singular_quadrature =
                  get_singular_quadrature(cell, singular_index);

                FEValues<dim - 1, dim> fe_v_singular(
                  mapping,
                  fe,
                  singular_quadrature,
                  update_jacobians | update_values | update_normal_vectors |
                    update_quadrature_points);

                fe_v_singular.reinit(cell);

                std::vector<Vector<double>> singular_cell_wind(
                  singular_quadrature.size(), Vector<double>(dim));

                const std::vector<Tensor<1, dim>> &singular_normals =
                  fe_v_singular.get_normal_vectors();
                const std::vector<Point<dim>> &singular_q_points =
                  fe_v_singular.get_quadrature_points();

                wind.vector_value_list(singular_q_points, singular_cell_wind);

                for (unsigned int q = 0; q < singular_quadrature.size(); ++q)
                  {
                    const Tensor<1, dim> R =
                      singular_q_points[q] - support_points[i];
                    double normal_wind = 0;
                    for (unsigned int d = 0; d < dim; ++d)
                      normal_wind +=
                        (singular_cell_wind[q](d) * singular_normals[q][d]);

                    system_rhs(i) += (LaplaceKernel::single_layer(R) *
                                      normal_wind * fe_v_singular.JxW(q));

                    for (unsigned int j = 0; j < fe.n_dofs_per_cell(); ++j)
                      {
                        local_matrix_row_i(j) -=
                          ((LaplaceKernel::double_layer(R) *
                            singular_normals[q]) *
                           fe_v_singular.shape_value(j, q) *
                           fe_v_singular.JxW(q));
                      }
                  }
              }

// 最后，我们需要将当前单元格的贡献添加到全局矩阵中。

            for (unsigned int j = 0; j < fe.n_dofs_per_cell(); ++j)
              system_matrix(i, local_dof_indices[j]) += local_matrix_row_i(j);
          }
      }

// 积分运算符的第二部分是术语  $\alpha(\mathbf{x}_i) \phi_j(\mathbf{x}_i)$  。由于我们使用的是配位方案， $\phi_j(\mathbf{x}_i)=\delta_{ij}$  而相应的矩阵是一个对角线的矩阵，其条目等于 $\alpha(\mathbf{x}_i)$  。

// 计算这个实体角的对角矩阵的一个快速方法是使用诺伊曼矩阵本身。只需将该矩阵与一个元素都等于-1的向量相乘，就可以得到阿尔法角或实体角的对角线矩阵（见介绍中的公式）。然后将这个结果加回到系统矩阵对象上，得到矩阵的最终形式。

    Vector<double> ones(dof_handler.n_dofs());
    ones.add(-1.);

    system_matrix.vmult(alpha, ones);
    alpha.add(1);
    for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
      system_matrix(i, i) += alpha(i);
  }
// @sect4{BEMProblem::solve_system}

// 下一个函数简单地解决了线性系统。

  template <int dim>
  void BEMProblem<dim>::solve_system()
  {
    SolverGMRES<Vector<double>> solver(solver_control);
    solver.solve(system_matrix, phi, system_rhs, PreconditionIdentity());
  }
// @sect4{BEMProblem::compute_errors}

// 误差的计算在其他所有的例子程序中都是完全一样的，我们就不做过多的评论。请注意，在有限元方法中使用的方法在这里也可以使用。

  template <int dim>
  void BEMProblem<dim>::compute_errors(const unsigned int cycle)
  {
    Vector<float> difference_per_cell(tria.n_active_cells());
    VectorTools::integrate_difference(mapping,
                                      dof_handler,
                                      phi,
                                      exact_solution,
                                      difference_per_cell,
                                      QGauss<(dim - 1)>(2 * fe.degree + 1),
                                      VectorTools::L2_norm);
    const double L2_error =
      VectorTools::compute_global_error(tria,
                                        difference_per_cell,
                                        VectorTools::L2_norm);

//可以直接使用 Vector::linfty_norm() 函数来计算α向量的误差，因为在每个节点上，该值应该是 $\frac 12$  。然后，所有的误差都会被输出并附加到我们的ConvergenceTable对象中，以便以后计算收敛率。

    Vector<double> difference_per_node(alpha);
    difference_per_node.add(-.5);

    const double       alpha_error    = difference_per_node.linfty_norm();
    const unsigned int n_active_cells = tria.n_active_cells();
    const unsigned int n_dofs         = dof_handler.n_dofs();

    deallog << "Cycle " << cycle << ':' << std::endl
            << "   Number of active cells:       " << n_active_cells
            << std::endl
            << "   Number of degrees of freedom: " << n_dofs << std::endl;

    convergence_table.add_value("cycle", cycle);
    convergence_table.add_value("cells", n_active_cells);
    convergence_table.add_value("dofs", n_dofs);
    convergence_table.add_value("L2(phi)", L2_error);
    convergence_table.add_value("Linfty(alpha)", alpha_error);
  }

// 奇异积分需要仔细选择正交规则。特别是deal.II库提供了为对数奇异性（QGaussLog, QGaussLogR）以及1/R奇异性（QGaussOneOverR）量身定制的正交规则。

// 奇异积分通常是通过构建具有奇异权重的加权正交公式得到的，因此可以写成

// \f[ \int_K f(x) s(x) dx = \sum_{i=1}^N w_i f(q_i) \f]

// 其中 $s(x)$ 是一个给定的奇点，权重和正交点 $w_i,q_i$ 是精心选择的，以使上述公式对某类函数 $f(x)$ 是一个等式。

// 在我们目前看到的所有有限元例子中，正交点本身的权重（即函数  $s(x)$  ），总是不断地等于1。 对于奇异积分，我们有两个选择：我们可以使用上面的定义，从积分中剔除奇异性（即用特殊的正交规则对 $f(x)$ 进行积分），或者我们可以要求正交规则用 $s(q_i)$ 对权重 $w_i$ 进行 "标准化"。

// \f[ \int_K f(x) s(x) dx = \int_K g(x) dx = \sum_{i=1}^N \frac{w_i}{s(q_i)} g(q_i) \f]

// 我们通过QGaussLogR和QGaussOneOverR的 @p factor_out_singularity 参数来使用这第二种选择。

// 这些积分有些微妙，特别是在二维空间，由于从实数到参考单元的转换，积分的变量是以转换的行列式为尺度的。

// 在二维空间中，这个过程不仅会导致一个因子作为常数出现在整个积分上，而且还会导致一个需要评估的额外积分。

// \f[ \int_0^1 f(x)\ln(x/\alpha) dx = \int_0^1 f(x)\ln(x) dx - \int_0^1  f(x) \ln(\alpha) dx.  \f]

// 这个过程由QGaussLogR类的构造函数来处理，它增加了额外的正交点和权重，以考虑到积分的第二部分。

// 类似的推理应该在三维情况下进行，因为奇异正交是在参考单元的半径 $r$ 的逆上定制的，而我们的奇异函数生活在实空间，然而在三维情况下一切都更简单，因为奇异性与变换的行列式成线性比例。这使我们可以只建立一次奇异的二维正交规则，并在所有单元格中重复使用。

// 在一维的奇异积分中，这是不可能的，因为我们需要知道正交的缩放参数，而这个参数并不是先验的。这里，正交规则本身也取决于当前单元格的大小。出于这个原因，有必要为每个单数积分创建一个新的正交。

// 不同的正交规则是在get_singular_quadrature中建立的，它专门用于dim=2和dim=3，它们在assemble_system函数中被检索。作为参数给出的索引是奇异点所在的单位支持点的索引。

  template <>
  const Quadrature<2> &BEMProblem<3>::get_singular_quadrature(
    const DoFHandler<2, 3>::active_cell_iterator &,
    const unsigned int index) const
  {
    Assert(index < fe.n_dofs_per_cell(),
           ExcIndexRange(0, fe.n_dofs_per_cell(), index));

    static std::vector<QGaussOneOverR<2>> quadratures;
    if (quadratures.size() == 0)
      for (unsigned int i = 0; i < fe.n_dofs_per_cell(); ++i)
        quadratures.emplace_back(singular_quadrature_order,
                                 fe.get_unit_support_points()[i],
                                 true);
    return quadratures[index];
  }

  template <>
  const Quadrature<1> &BEMProblem<2>::get_singular_quadrature(
    const DoFHandler<1, 2>::active_cell_iterator &cell,
    const unsigned int                            index) const
  {
    Assert(index < fe.n_dofs_per_cell(),
           ExcIndexRange(0, fe.n_dofs_per_cell(), index));

    static Quadrature<1> *q_pointer = nullptr;
    if (q_pointer)
      delete q_pointer;

    q_pointer = new QGaussLogR<1>(singular_quadrature_order,
                                  fe.get_unit_support_points()[index],
                                  1. / cell->measure(),
                                  true);
    return (*q_pointer);
  }

//  @sect4{BEMProblem::compute_exterior_solution}

// 我们还想知道一些关于外域中电势 $\phi$ 的值：毕竟我们考虑边界积分问题的动机是想知道外域中的速度!

// 为此，我们在此假设边界元素域包含在盒子 $[-2,2]^{\text{dim}}$ 中，我们用与基本解的卷积来推算这个盒子内的实际解。这方面的公式在介绍中已经给出。

// 整个空间的解的重建是在一个连续的、尺寸为dim的有限元网格上完成的。这些都是常用的，我们不做进一步评论。在函数的最后，我们再次以通常的方式输出这个外部解。

  template <int dim>
  void BEMProblem<dim>::compute_exterior_solution()
  {
    Triangulation<dim> external_tria;
    GridGenerator::hyper_cube(external_tria, -2, 2);

    FE_Q<dim>       external_fe(1);
    DoFHandler<dim> external_dh(external_tria);
    Vector<double>  external_phi;

    external_tria.refine_global(external_refinement);
    external_dh.distribute_dofs(external_fe);
    external_phi.reinit(external_dh.n_dofs());

    FEValues<dim - 1, dim> fe_v(mapping,
                                fe,
                                *quadrature,
                                update_values | update_normal_vectors |
                                  update_quadrature_points | update_JxW_values);

    const unsigned int n_q_points = fe_v.n_quadrature_points;

    std::vector<types::global_dof_index> dofs(fe.n_dofs_per_cell());

    std::vector<double>         local_phi(n_q_points);
    std::vector<double>         normal_wind(n_q_points);
    std::vector<Vector<double>> local_wind(n_q_points, Vector<double>(dim));

    std::vector<Point<dim>> external_support_points(external_dh.n_dofs());
    DoFTools::map_dofs_to_support_points<dim>(StaticMappingQ1<dim>::mapping,
                                              external_dh,
                                              external_support_points);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_v.reinit(cell);

        const std::vector<Point<dim>> &q_points = fe_v.get_quadrature_points();
        const std::vector<Tensor<1, dim>> &normals = fe_v.get_normal_vectors();

        cell->get_dof_indices(dofs);
        fe_v.get_function_values(phi, local_phi);

        wind.vector_value_list(q_points, local_wind);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            normal_wind[q] = 0;
            for (unsigned int d = 0; d < dim; ++d)
              normal_wind[q] += normals[q][d] * local_wind[q](d);
          }

        for (unsigned int i = 0; i < external_dh.n_dofs(); ++i)
          for (unsigned int q = 0; q < n_q_points; ++q)
            {
              const Tensor<1, dim> R = q_points[q] - external_support_points[i];

              external_phi(i) +=
                ((LaplaceKernel::single_layer(R) * normal_wind[q] +
                  (LaplaceKernel::double_layer(R) * normals[q]) *
                    local_phi[q]) *
                 fe_v.JxW(q));
            }
      }

    DataOut<dim> data_out;

    data_out.attach_dof_handler(external_dh);
    data_out.add_data_vector(external_phi, "external_phi");
    data_out.build_patches();

    const std::string filename = std::to_string(dim) + "d_external.vtk";
    std::ofstream     file(filename);

  }
// @sect4{BEMProblem::output_results}

// 输出我们的计算结果是一个相当机械的任务。这个函数的所有组成部分之前已经讨论过了。

  template <int dim>
  void BEMProblem<dim>::output_results(const unsigned int cycle)
  {
    DataOut<dim - 1, dim> dataout;

    dataout.attach_dof_handler(dof_handler);
    dataout.add_data_vector(phi, "phi", DataOut<dim - 1, dim>::type_dof_data);
    dataout.add_data_vector(alpha,
                            "alpha",
                            DataOut<dim - 1, dim>::type_dof_data);
    dataout.build_patches(mapping,
                          mapping.get_degree(),
                          DataOut<dim - 1, dim>::curved_inner_cells);

    const std::string filename = std::to_string(dim) + "d_boundary_solution_" +
                                 std::to_string(cycle) + ".vtk";
    std::ofstream file(filename);

    dataout.write_vtk(file);

    if (cycle == n_cycles - 1)
      {
        convergence_table.set_precision("L2(phi)", 3);
        convergence_table.set_precision("Linfty(alpha)", 3);

        convergence_table.set_scientific("L2(phi)", true);
        convergence_table.set_scientific("Linfty(alpha)", true);

        convergence_table.evaluate_convergence_rates(
          "L2(phi)", ConvergenceTable::reduction_rate_log2);
        convergence_table.evaluate_convergence_rates(
          "Linfty(alpha)", ConvergenceTable::reduction_rate_log2);
        deallog << std::endl;
        convergence_table.write_text(std::cout);
      }
  }
// @sect4{BEMProblem::run}

// 这是最主要的功能。它应该是不言自明的。

  template <int dim>
  void BEMProblem<dim>::run()
  {
    read_parameters("parameters.prm");

    if (run_in_this_dimension == false)
      {
        deallog << "Run in dimension " << dim
                << " explicitly disabled in parameter file. " << std::endl;
        return;
      }

    read_domain();

    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        refine_and_resize();
        assemble_system();
        solve_system();
        compute_errors(cycle);
        output_results(cycle);
      }

    if (extend_solution == true)
      compute_exterior_solution();
  }
} // namespace Step34
// @sect3{The main() function}

// 这是本程序的主要功能。它与以前所有的教程程序完全一样。

int main()
{
  try
    {
      using namespace Step34;

      const unsigned int degree         = 1;
      const unsigned int mapping_degree = 1;

      deallog.depth_console(3);
      BEMProblem<2> laplace_problem_2d(degree, mapping_degree);
      laplace_problem_2d.run();

      BEMProblem<3> laplace_problem_3d(degree, mapping_degree);
      laplace_problem_3d.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;
}


#define BOOST_TEST_MODULE lue framework core domain_decomposition
#include <hpx/config.hpp>
#include <boost/test/unit_test.hpp>
#include "lue/framework/core/domain_decomposition.hpp"
#include "lue/framework/test/stream.hpp"

BOOST_AUTO_TEST_CASE(nr_2d_partitions)
{
    using Index = std::uint64_t;
    std::size_t const rank = 2;
    using Shape = lue::Shape<Index, rank>;

    Shape area_shape{6, 4};

    {
        Shape partition_shape{1, 1};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 24);
    }

    {
        Shape partition_shape{2, 2};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 6);
    }

    {
        Shape partition_shape{3, 3};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 4);
    }

    {
        Shape partition_shape{4, 4};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 2);
    }

    {
        Shape partition_shape{5, 5};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 2);
    }

    {
        Shape partition_shape{10, 10};
        auto nr_partitions = lue::nr_partitions(area_shape, partition_shape);
        BOOST_CHECK_EQUAL(nr_partitions, 1);
    }
}

BOOST_AUTO_TEST_CASE(linear_to_1d_shape_index)
{
    using Index = std::uint64_t;
    std::size_t const rank = 1;
    using Shape = lue::Shape<Index, rank>;
    using Indices = lue::Indices<Index, rank>;

    Shape area_shape{10};
    Index index;

    {
        index = 0;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{0}));
    }

    {
        index = 5;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{5}));
    }

    {
        index = 9;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{9}));
    }
}

BOOST_AUTO_TEST_CASE(linear_to_2d_shape_index)
{
    using Index = std::uint64_t;
    std::size_t const rank = 2;
    using Shape = lue::Shape<Index, rank>;
    using Indices = lue::Indices<Index, rank>;

    Shape area_shape{6, 4};
    Index index;

    {
        index = 0;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{0, 0}));
    }

    {
        index = 3;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{0, 3}));
    }

    {
        index = 4;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{1, 0}));
    }

    {
        index = 23;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{5, 3}));
    }
}

BOOST_AUTO_TEST_CASE(linear_to_3d_shape_index)
{
    using Index = std::uint64_t;
    std::size_t const rank = 3;
    using Shape = lue::Shape<Index, rank>;
    using Indices = lue::Indices<Index, rank>;

    Shape area_shape{2, 6, 4};
    Index index;

    {
        index = 0;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{0, 0, 0}));
    }

    {
        index = 47;
        auto indices = lue::linear_to_shape_index(area_shape, index);
        BOOST_CHECK(indices == (Indices{1, 5, 3}));
    }
}

BOOST_AUTO_TEST_CASE(clamp_area_shape)
{
    using Index = std::uint64_t;
    std::size_t const rank = 2;
    using Shape = lue::Shape<Index, rank>;

    Shape area_shape{6, 4};

    {
        Shape partition_shape{1, 1};
        auto shape = lue::clamp_area_shape(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{6, 4}));
    }

    {
        Shape partition_shape{2, 2};
        auto shape = lue::clamp_area_shape(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{6, 4}));
    }

    {
        Shape partition_shape{3, 3};
        auto shape = lue::clamp_area_shape(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{6, 6}));
    }

    {
        Shape partition_shape{4, 4};
        auto shape = lue::clamp_area_shape(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{8, 4}));
    }

    {
        Shape partition_shape{5, 5};
        auto shape = lue::clamp_area_shape(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{10, 5}));
    }
}

BOOST_AUTO_TEST_CASE(shape_in_partitions)
{
    using Index = std::uint64_t;
    std::size_t const rank = 2;
    using Shape = lue::Shape<Index, rank>;

    Shape area_shape{6, 4};

    {
        Shape partition_shape{1, 1};
        auto shape = lue::shape_in_partitions(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{6, 4}));
    }

    {
        Shape partition_shape{2, 2};
        auto shape = lue::shape_in_partitions(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{3, 2}));
    }

    {
        Shape partition_shape{3, 3};
        auto shape = lue::shape_in_partitions(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{2, 2}));
    }

    {
        Shape partition_shape{4, 4};
        auto shape = lue::shape_in_partitions(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{2, 1}));
    }

    {
        Shape partition_shape{5, 5};
        auto shape = lue::shape_in_partitions(area_shape, partition_shape);
        BOOST_CHECK(shape == (Shape{2, 1}));
    }
}

BOOST_AUTO_TEST_CASE(partitions)
{
    using Index = std::uint64_t;
    std::size_t const rank = 2;
    using Partition = lue::ArrayPartitionDefinition<Index, rank>;
    using Start = typename Partition::Start;
    using Shape = typename Partition::Shape;

    {
        Shape area_shape{6, 4};
        Shape partition_shape{6, 4};

        std::size_t nr_localities = 2;
        std::uint32_t locality_id = 0;

        auto partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 1);
        BOOST_CHECK(partitions[0] == Partition{partition_shape});

        locality_id = 1;
        partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 0);
    }

    {
        // Grains fit perfectly in area
        Shape area_shape{60, 40};
        Shape partition_shape{10, 10};

        std::size_t nr_localities = 6;

        std::uint32_t locality_id = 0;
        auto partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 4);
        BOOST_CHECK(partitions[0] == (Partition{Start{0,  0}, Shape{10, 10}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{0, 10}, Shape{10, 10}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{0, 20}, Shape{10, 10}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{0, 30}, Shape{10, 10}}));

        locality_id = 5;
        partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 4);
        BOOST_CHECK(partitions[0] == (Partition{Start{50,  0}, Shape{10, 10}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{50, 10}, Shape{10, 10}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{50, 20}, Shape{10, 10}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{50, 30}, Shape{10, 10}}));
    }

    {
        // Grains don't fit perfectly in area
        Shape area_shape{60, 40};
        Shape partition_shape{9, 9};

        std::size_t nr_localities = 6;

        std::uint32_t locality_id = 0;
        auto partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 6);
        BOOST_CHECK(partitions[0] == (Partition{Start{0,  0}, Shape{9, 9}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{0,  9}, Shape{9, 9}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{0, 18}, Shape{9, 9}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{0, 27}, Shape{9, 9}}));
        BOOST_CHECK(partitions[4] == (Partition{Start{0, 36}, Shape{9, 4}}));
        BOOST_CHECK(partitions[5] == (Partition{Start{9,  0}, Shape{9, 9}}));

        locality_id = 4;
        partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 6);
        BOOST_CHECK(partitions[0] == (Partition{Start{36, 36}, Shape{9, 4}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{45,  0}, Shape{9, 9}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{45,  9}, Shape{9, 9}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{45, 18}, Shape{9, 9}}));
        BOOST_CHECK(partitions[4] == (Partition{Start{45, 27}, Shape{9, 9}}));
        BOOST_CHECK(partitions[5] == (Partition{Start{45, 36}, Shape{9, 4}}));

        locality_id = 5;
        partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 5);
        BOOST_CHECK(partitions[0] == (Partition{Start{54,  0}, Shape{6, 9}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{54,  9}, Shape{6, 9}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{54, 18}, Shape{6, 9}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{54, 27}, Shape{6, 9}}));
        BOOST_CHECK(partitions[4] == (Partition{Start{54, 36}, Shape{6, 4}}));
    }

    {
        // Grains don't fit perfectly in area
        Shape area_shape{60, 40};
        Shape partition_shape{11, 11};

        std::size_t nr_localities = 6;

        std::uint32_t locality_id = 0;
        auto partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 4);
        BOOST_CHECK(partitions[0] == (Partition{Start{0,  0}, Shape{11, 11}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{0, 11}, Shape{11, 11}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{0, 22}, Shape{11, 11}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{0, 33}, Shape{11,  7}}));

        locality_id = 5;
        partitions = lue::partitions(
            area_shape, partition_shape, nr_localities, locality_id);

        BOOST_REQUIRE_EQUAL(partitions.size(), 4);
        BOOST_CHECK(partitions[0] == (Partition{Start{55,  0}, Shape{5, 11}}));
        BOOST_CHECK(partitions[1] == (Partition{Start{55, 11}, Shape{5, 11}}));
        BOOST_CHECK(partitions[2] == (Partition{Start{55, 22}, Shape{5, 11}}));
        BOOST_CHECK(partitions[3] == (Partition{Start{55, 33}, Shape{5,  7}}));
    }
}

BOOST_AUTO_TEST_CASE(max_partition_shape_1d)
{
    using Shape = lue::Shape<std::uint32_t, 1>;

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 1u);
        Shape const shape_we_want = Shape{10};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 2u);
        Shape const shape_we_want = Shape{5};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 3u);
        Shape const shape_we_want = Shape{3};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 4u);
        Shape const shape_we_want = Shape{2};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 5u);
        Shape const shape_we_want = Shape{2};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 6u);
        Shape const shape_we_want = Shape{1};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 10u);
        Shape const shape_we_want = Shape{1};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }
}

BOOST_AUTO_TEST_CASE(max_partition_shape_2d)
{
    using Shape = lue::Shape<std::uint32_t, 2>;

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 1u);
        Shape const shape_we_want = Shape{10, 10};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 2u);
        Shape const shape_we_want = Shape{5, 10};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 3u);
        Shape const shape_we_want = Shape{5, 5};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 4u);
        Shape const shape_we_want = Shape{5, 5};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 25u);
        Shape const shape_we_want = Shape{2, 2};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }

    {
        Shape const shape{10, 10};
        Shape const shape_we_got = lue::max_partition_shape(shape, 100u);
        Shape const shape_we_want = Shape{1, 1};

        BOOST_CHECK_EQUAL(shape_we_got, shape_we_want);
    }
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Twoto31 Twoto31 (function template)

  Generates the constant \f$2^{31}\f$

  @headerref{<boost/simd/constant/twoto31.hpp>}

  @par Description

  1.  @code
      template<typename T> T Twoto31();
      @endcode

  2.  @code
      template<typename T> T Twoto31( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to (\f$2^{31}\f$).

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to <tt>T(2147483648)</tt>.

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/twoto31.hpp>
#include <boost/simd/constant/simd/twoto31.hpp>

#endif


#ifndef EXPSUM_VANDERMONDE_HPP
#define EXPSUM_VANDERMONDE_HPP

#include <cassert>

#include <armadillo>

#include "expsum/fitting/lsqr.hpp"
#include "expsum/numeric.hpp"

namespace expsum
{
//
// Generalized Vandermonde matrix
//
// This class represents a ``$m \times n$`` generalized Vandermonde matrix of
// the form
//
// ``` math
//   \bm{V}(\bm{t}) = \left[ \begin{array}{}
//     1         & 1         & \dots  & 1         \\
//     t_{1}^{}  & t_{2}^{}  & \dots  & t_{n}^{}  \\
//     \vdots    & \vdots    & \ddots & \vdots    \\
//     t_{1}^{m} & t_{2}^{m} & \dots  & t_{n}^{m} \\
//   \end{array} \right]
// ```
template <typename T>
class vandermonde_matrix
{
public:
    using value_type  = T;
    using real_type   = typename arma::get_pod_type<value_type>::result;
    using size_type   = arma::uword;
    using vector_type = arma::Col<value_type>;
    using matrix_type = arma::Mat<value_type>;

private:
    size_type nrows_;
    size_type ncols_;

    vector_type coeffs_;
    mutable vector_type work_;

public:
    vandermonde_matrix()                          = default;
    vandermonde_matrix(const vandermonde_matrix&) = default;
    vandermonde_matrix(vandermonde_matrix&&)      = default;
    ~vandermonde_matrix()                         = default;
    vandermonde_matrix& operator=(const vandermonde_matrix&) = default;
    vandermonde_matrix& operator=(vandermonde_matrix&&) = default;

    vandermonde_matrix(size_type nrows, size_type ncols)
        : nrows_(nrows), ncols_(ncols), coeffs_(ncols), work_(ncols)
    {
    }

    template <typename T1>
    vandermonde_matrix(
        size_type nrows, const T1& coeffs,
        typename std::enable_if<arma::is_arma_type<T1>::value>::type* = 0)
        : nrows_(nrows), ncols_(coeffs.n_elem), coeffs_(coeffs), work_(ncols_)
    {
    }

    size_type nrows() const
    {
        return nrows_;
    }

    size_type ncols() const
    {
        return ncols_;
    }

    // Returns size of vector that defines the Vandermonde matrix
    size_type size() const
    {
        return ncols();
    }

    // Resize matrix
    void resize(size_type nrows, size_type ncols)
    {
        nrows_ = nrows;
        ncols_ = ncols;
        coeffs_.resize(ncols);
        work_.resize(ncols);
    }

    // Set elements of Vandermonde matrix
    template <typename T1>
    typename std::enable_if<arma::is_arma_type<T1>::value>::type
    set_coeffs(const T1& coeffs)
    {
        ncols_ = coeffs.n_elem;
        if (coeffs_.size() < ncols_)
        {
            coeffs_.resize(ncols_);
            work_.resize(ncols_);
        }
        coeffs_.head(ncols_) = coeffs;
    }

    auto coeffs() const -> decltype(coeffs_.head(ncols_))
    {
        return coeffs_.head(ncols_);
    }

    matrix_type as_dense_matrix() const
    {
        matrix_type ret(nrows(), ncols());
        for (size_type j = 0; j < ncols(); ++j)
        {
            auto x = coeffs_(j);
            auto v = value_type(1);
            ret(0, j) = v;
            for (size_type i = 1; i < nrows(); ++i)
            {
                v *= x;
                ret(i, j) = v;
            }
        }
        return ret;
    }
    ///
    /// Compute `y = A * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply(const U1& x, value_type beta, U2& y) const
    {
        assert(x.n_elem == ncols());
        assert(y.n_elem == nrows());

        auto w = work_.head(ncols_);
        w      = x;

        y(0) = arma::sum(w) + beta * y(0);
        for (size_type i = 1; i < nrows(); ++i)
        {
            w %= coeffs();
            y(i) = arma::sum(w) + beta * y(i);
        }
    }
    ///
    /// Compute `y = A.t() * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply_trans(const U1& x, value_type beta, U2& y) const
    {
        assert(x.n_elem == nrows());
        assert(y.n_elem == ncols());

        for (size_type i = 0; i < ncols(); ++i)
        {
            const auto zi = numeric::conj(coeffs_(i));
            // Evaluate polynomial using Honer's method
            auto s = value_type();
            for (size_type j = 0; j < nrows(); ++j)
            {
                s = s * zi + x(nrows() - j - 1);
            }

            y(i) = s + beta * y(i);
        }
    }
};

//
// Compute LDL^H factorization of the gramian matrix of column Vandermonde
// matrix.
//
template <typename T, typename MatrixT, typename MatrixWork>
void ldlt_vandermonde_gramian(const vandermonde_matrix<T>& V, MatrixT& mat,
                              MatrixWork& work)
{
    using value_type  = typename vandermonde_matrix<T>::value_type;
    using vector_type = typename vandermonde_matrix<T>::vector_type;
    using real_type   = typename arma::get_pod_type<value_type>::result;
    using size_type   = arma::uword;

    using std::abs;
    using std::real;
    using std::sqrt;

    constexpr const auto one = value_type(1);
    static const auto tiny   = sqrt(std::numeric_limits<real_type>::min());

    const auto z      = V.coeffs();
    const size_type m = V.nrows();
    const size_type n = V.ncols();

    assert(mat.n_rows == n && mat.n_cols == n);
    assert(work.n_rows == n && work.n_cols >= 4);

    // ----- Initialization
    auto y1 = work.col(0);
    auto y2 = work.col(1);
    auto x1 = work.col(2);
    auto x2 = work.col(3);

    auto gramian = [&](size_type i, size_type j) {
        const auto arg = numeric::conj(z(i)) * z(j);
        return arg == one ? value_type(m)
                          : (one - std::pow(arg, m)) / (one - arg);
    };

    auto sigma2 = gramian(0, 0);
    auto b0     = mat.col(0);
    b0(0)       = one;
    for (size_type j = 1; j < n; ++j)
    {
        b0(j) = gramian(j, 0) / sigma2;
    }

    y1 = arma::ones<vector_type>(n) - b0;
    y2 = arma::pow(arma::conj(z), m);
    y2 -= y2(0) * b0;
    x1 = value_type(1) / arma::conj(z);
    x1 -= x1(0) * b0;
    x2 = arma::pow(-arma::conj(z), m - 1);
    x2 -= x2(0) * b0;

    b0(0) = sigma2;

    for (size_type k = 1; k < n; ++k)
    {
        auto bk = mat.col(k);

        auto mu1 = x1(k);
        auto mu2 = x2(k);
        auto nu1 = numeric::conj(y1(k));
        auto nu2 = numeric::conj(y2(k));

        auto zk     = z(k);
        auto zk_inv = one / zk;
        auto denom  = numeric::conj(zk_inv) - zk;

        if (abs(denom) < tiny)
        {
            sigma2         = real_type(m);
            const auto bkk = real(mat(k, k));
            for (size_type j = 0; j < k; ++j)
            {
                const auto bkj = mat(k, j);
                sigma2 -= std::norm(bkj) * bkk;
            }
        }
        else
        {
            sigma2 = (mu1 * nu1 + mu2 * nu2) / denom;
        }

        bk(k) = sigma2;

        for (size_type i = k + 1; i < n; ++i)
        {
            const auto d1 =
                (numeric::conj(mu1) * y1(i) + numeric::conj(mu2) * y2(i));
            const auto d2 = sigma2 * (zk_inv - numeric::conj(z(i)));
            bk(i)         = d1 / d2;
        }

        const size_type nt = n - k - 1;

        x1.tail(nt) -= mu1 * bk.tail(nt);
        x2.tail(nt) -= mu2 * bk.tail(nt);
        y1.tail(nt) -= numeric::conj(nu1) * bk.tail(nt);
        y2.tail(nt) -= numeric::conj(nu2) * bk.tail(nt);
    }

    // arma::Mat<T> tmp_V(V.as_dense_matrix());
    // arma::Mat<T> tmp_H(V.t() * V);

    // for (size_type k = 0; k < n; ++k)
    // {
    //     auto bk          = mat.col(k);
    //     const auto sigma = sqrt(real(mat(k, k)));

    //     bk(k) = sigma;
    //     bk.tail(n - k - 1) *= sigma;
    // }

    return;
}

///
/// Solve overdetermined Vandermonde system
///
template <typename T>
struct vandermonde_least_squares
{
    using size_type  = arma::uword;
    using value_type = T;
    using real_type  = typename arma::get_pod_type<value_type>::result;

    using matrix_type             = arma::Mat<value_type>;
    using vector_type             = arma::Col<value_type>;
    using vandermonde_matrix_type = vandermonde_matrix<value_type>;

private:
    struct matvec
    {
        matvec(const vandermonde_matrix_type& mat_V) : mat_V_(mat_V)
        {
        }

        template <typename U1, typename U2>
        void operator()(const U1& x, value_type beta, U2& y) const
        {
            mat_V_.apply(x, beta, y);
        }

        const vandermonde_matrix_type& mat_V_;
    };

    struct matvec_trans
    {
        matvec_trans(const vandermonde_matrix_type& mat_V) : mat_V_(mat_V)
        {
        }

        template <typename U1, typename U2>
        void operator()(const U1& x, value_type beta, U2& y) const
        {
            mat_V_.apply_trans(x, beta, y);
        }

        const vandermonde_matrix_type& mat_V_;
    };

    struct preconditioner
    {
        // ldlt -- Result of Cholesky (LDL^T) decomposition of V.t() * V
        preconditioner(const matrix_type& ldlt) : ldlt_(ldlt){};

        template <typename Rhs, typename Dest>
        void operator()(const Rhs& rhs, Dest& dst) const
        {
            constexpr const real_type tol =
                real_type(1) / std::numeric_limits<real_type>::max();
            // auto mat_L = arma::trimatl(chol_VtV_);
            // dst = arma::solve(mat_L, rhs);
            // dst = arma::solve(mat_L.t(), dst);
            char uplo           = 'L';
            char trans1         = 'N';
            char trans2         = 'C';
            char diag           = 'U';
            arma::blas_int n    = arma::blas_int(ldlt_.n_rows);
            arma::blas_int nrhs = dst.n_cols;
            arma::blas_int info = 0;
            dst                 = rhs;

            arma::lapack::trtrs(&uplo, &trans1, &diag, &n, &nrhs,
                                ldlt_.memptr(), &n, &dst[0], &n, &info);

            for (size_type i = 0; i < ldlt_.n_rows; ++i)
            {
                if (std::abs(ldlt_(i, i)) > tol)
                {
                    dst(i) /= ldlt_(i, i);
                }
                else
                {
                    dst(i) = value_type();
                }
            }

            arma::lapack::trtrs(&uplo, &trans2, &diag, &n, &nrhs,
                                ldlt_.memptr(), &n, &dst[0], &n, &info);

            return;
        }

        const matrix_type& ldlt_;
    };

public:
    vandermonde_least_squares()
        : max_iterations_(),
          iterations_(),
          tolerance_(arma::Datum<real_type>::eps),
          error_()
    {
    }

    static size_type
    inquery_workspace_size(const vandermonde_matrix_type& mat_V)
    {
        return inquery_workspace_size(mat_V.nrows(), mat_V.ncols());
    }

    static size_type inquery_workspace_size(size_type nrows, size_type ncols)
    {
        return (ncols + 3) * ncols + std::max(nrows, ncols);
    }

    // Solve overdetermined Vandermonde problem.
    template <typename Rhs, typename Dest>
    void solve(const vandermonde_matrix_type& mat_V, const Rhs& b, Dest& x,
               value_type* work)
    {
        const size_type m = mat_V.nrows();
        const size_type n = mat_V.ncols();

        assert(b.n_rows == m && x.n_rows == n);

        //
        // Compute Cholesky decomposition of `V.t() * V = L.t() * L`, where `V`
        // is input Vandermonde matrix while `L` is a lower triangular matrix.
        //
        value_type* ptr_chol = work;
        value_type* ptr_vec1 = ptr_chol + n * n;
        value_type* ptr_vec2 = ptr_vec1 + m;
        matrix_type chol_VtV(ptr_chol, n, n, /* copy_aux_mem */ false,
                             /* strict */ true);
        matrix_type vecs(ptr_vec1, n, 4, /* copy_aux_mem */ false,
                         /* strict */ true);
        ldlt_vandermonde_gramian(mat_V, chol_VtV, vecs);
        //
        // Solve `V * x = b` using preconditioned LSQR
        //
        matvec mv(mat_V);
        matvec_trans mv_trans(mat_V);
        preconditioner precond(chol_VtV);

        vector_type u(ptr_vec1, m, /*copy_aux_mem*/ false, /*strict*/ true);
        matrix_type tmp(ptr_vec2, n, 3, /*copy_aux_mem*/ false,
                        /*strict*/ true);
        u           = arma::conv_to<vector_type>::from(b);
        iterations_ = max_iterations_ ? max_iterations_ : 2 * n;
        error_      = tolerance_;
        lsqr(mv, mv_trans, u, x, precond, tmp, iterations_, error_);

        return;
    }

    /// Check the convergence
    bool converged() const
    {
        return iterations_ <= max_iterations_;
    }

    real_type tolerance() const
    {
        return tolerance_;
    }

    void set_tolerance(real_type tol)
    {
        tolerance_ = tol;
    }

    size_type iterations() const
    {
        return iterations_;
    }

    void set_max_iterations(size_type max_iter)
    {
        max_iterations_ = max_iter;
    }

    /// @return An estimation of residual error
    real_type error() const
    {
        return error_;
    }

private:
    size_type max_iterations_;
    size_type iterations_;
    real_type tolerance_;
    real_type error_;
};

} // namespace expsum

#endif /* EXPSUM_VANDERMONDE_HPP */


#include <stan/math/prim.hpp>
#include <test/unit/math/prim/util.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <gtest/gtest.h>
#include <stdexcept>
#include <vector>

TEST(ProbDistributionsWishart, rng) {
  using Eigen::MatrixXd;
  using stan::math::wishart_rng;

  boost::random::mt19937 rng;

  MatrixXd omega(3, 4);
  EXPECT_THROW(wishart_rng(3.0, omega, rng), std::invalid_argument);

  MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  EXPECT_NO_THROW(wishart_rng(3.0, sigma, rng));
  EXPECT_THROW(wishart_rng(2, sigma, rng), std::domain_error);
  EXPECT_THROW(wishart_rng(-1, sigma, rng), std::domain_error);
  sigma(2, 1) = 100.0;
  EXPECT_THROW(wishart_rng(3.0, sigma, rng), std::domain_error);
}

TEST(ProbDistributionsWishart, rng_pos_def) {
  using Eigen::MatrixXd;
  using stan::math::wishart_rng;

  boost::random::mt19937 rng;

  MatrixXd Sigma(2, 2);
  MatrixXd Sigma_non_pos_def(2, 2);

  Sigma << 1, 0, 0, 1;
  Sigma_non_pos_def << -1, 0, 0, 1;

  unsigned int dof = 5;

  EXPECT_NO_THROW(wishart_rng(dof, Sigma, rng));
  EXPECT_THROW(wishart_rng(dof, Sigma_non_pos_def, rng), std::domain_error);
}

TEST(ProbDistributionsWishart, rng_symmetry) {
  using Eigen::MatrixXd;
  using stan::math::wishart_rng;
  using stan::test::unit::expect_symmetric;
  using stan::test::unit::spd_rng;

  boost::random::mt19937 rng;
  for (int k = 1; k < 20; ++k)
    for (double nu = k - 0.9; nu < k + 10; ++nu)
      for (int n = 0; n < 10; ++n)
        expect_symmetric(wishart_rng(nu, spd_rng(k, rng), rng));
}

TEST(ProbDistributionsWishart, marginalTwoChiSquareGoodnessFitTest) {
  using boost::math::chi_squared;
  using boost::math::digamma;
  using Eigen::MatrixXd;
  using stan::math::determinant;
  using stan::math::wishart_rng;
  using std::log;

  boost::random::mt19937 rng;
  MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 2.0, -3.0, 4.0, 0.0, 2.0, 0.0, 3.0;
  int N = 10000;

  double avg = 0;
  double expect = sigma.rows() * log(2.0) + log(determinant(sigma))
                  + digamma(5.0 / 2.0) + digamma(4.0 / 2.0)
                  + digamma(3.0 / 2.0);

  MatrixXd a(sigma.rows(), sigma.rows());
  for (int count = 0; count < N; ++count) {
    a = wishart_rng(5.0, sigma, rng);
    avg += log(determinant(a));
  }
  avg /= N;
  double chi = (expect - avg) * (expect - avg) / expect;
  chi_squared mydist(1);
  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsWishart, SpecialRNGTest) {
  // For any vector C != 0
  // (C' * W * C) / (C' * S * C)
  // must be chi-square distributed with df = k
  // which has mean = k and variance = 2k

  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using stan::math::wishart_rng;

  boost::random::mt19937 rng(1234);

  MatrixXd sigma(3, 3);

  sigma << 9.0, 2.0, 2.0, 2.0, 4.0, 1.0, 2.0, 1.0, 3.0;

  VectorXd C(3);
  C << 2, 1, 3;

  size_t N = 1e4;
  int k = 20;
  // tolerance for variance
  double tol = 0.2;
  std::vector<double> acum;
  acum.reserve(N);
  for (size_t i = 0; i < N; i++)
    acum.push_back((C.transpose() * wishart_rng(k, sigma, rng) * C)(0)
                   / (C.transpose() * sigma * C)(0));

  EXPECT_NEAR(1, stan::math::mean(acum) / k, tol * tol);
  EXPECT_NEAR(1, stan::math::variance(acum) / (2 * k), tol);
}


#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <math.h>
#include <set>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/number.hpp>

using namespace std;
using namespace boost::multiprecision;

int target = 100;

int main(int argc, char** argv) {
  set<mpz_int> visited;

  for (int a = 2; a <= target; a++) {
    for (int b = 2; b <= target; b++) {
      mpz_int val = 1;
      for (int i = 0; i < b; i++) {
        val *= a;
      }
      visited.insert(val);
      cout << val << endl;
    }
  }

  cout << "Total unique elements: " << visited.size() << endl;

  return 0;
}


/**
    Definitions of functions wrapping lapack.
    Declarations depends on compilations options PROXTV_USE_LAPACK

    @author Pablo Hernandez-Cerdan
*/

#include "lapackFunctionsWrap.h"

#ifndef PROXTV_USE_LAPACK // USE_EIGEN
#include <Eigen/Dense>
void dpttrf_plus_dpttrs_eigen( lapack_int* n, double* d, double* e, double *b)
{
    using EigenMatrix = Eigen::MatrixXd;
    using EigenVector = Eigen::VectorXd;
    using EigenVectorMap = Eigen::Map<EigenVector>;
    // Eigen has to create the full matrix from the diagonal d and subdiagonal e
    int mSize = *n;
    EigenMatrix eigenM(mSize,mSize);
    EigenVectorMap diag(d, mSize);
    EigenVectorMap subAndUpperDiag(e, mSize - 1);
    EigenVectorMap inputB_outputX(b, mSize);
    // Populate matrix
    eigenM.diagonal() = diag;
    eigenM.diagonal( 1) = subAndUpperDiag;
    eigenM.diagonal(-1) = subAndUpperDiag;

    // Factorize using ldlt (ldl is also possible, faster but less accurate)
    // A = LDL'
    Eigen::LDLT<EigenMatrix> factorization(eigenM);
    // A*X = b
    // This modifies the input/output pointer: b
    inputB_outputX = factorization.solve(inputB_outputX);
    // This modifies the input/output pointers: d and e
    EigenMatrix factorized = factorization.matrixLDLT();
    diag = factorized.diagonal();
    subAndUpperDiag = factorized.diagonal(-1);
}
#endif


/**
 * @file velocity_filter.hpp
 * @author Fujii Naomichi
 * @copyright (c) 2021 Fujii Naomichi
 * SPDX-License-Identifier: MIT
 */

#pragma once

#include <Eigen/Core>

/**
 * @brief IMUの測定値とオドメトリから車体速度を推定する
 */
class VelocityFilter {
private:
    using Vector7f = Eigen::Matrix<float, 7, 1>;
    using Matrix7f = Eigen::Matrix<float, 7, 7>;

public:
    /**
     * @brief 内部状態をリセットする
     */
    void reset(void);

    /**
     * @brief フィルタに新たな入力を与えて出力を更新する
     * @param accel 加速度センサーの測定値
     * @param gyro ジャイロスコープの測定値
     * @param wheel_velocity 車輪速度
     * @param wheel_current モーター電流
     */
    void update(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, const Eigen::Vector4f& wheel_velocity, const Eigen::Vector4f& wheel_current);

    /**
     * @brief 車体速度の推定値を取得する
     * @return 車体速度 X [m/s], Y [m/s], ω [rad/s]
     */
    Eigen::Vector3f bodyVelocity(void) const {
        return {_mu(0), _mu(1), _mu(2)};
    }

    /**
     * @brief 摩擦係数の推定値を取得する
     * @return 摩擦係数 [Ns]
     */
    Eigen::Vector4f frictionCoefficients(void) const {
        return {_mu(3), _mu(4), _mu(5), _mu(6)};
    }

    /// 状態変数の最尤値
    Vector7f _mu;

    /// 共分散
    Matrix7f _sigma;

    /// 前回の更新時の車輪速度
    Eigen::Vector4f _last_wheel_velocity;

    /// 線形化された状態方程式
    Matrix7f G;

    /// 線形化された観測方程式
    Matrix7f H;

    /// 逆行列を求める際の途中計算結果
    Matrix7f L, invL;
};


#include <boost/math/distributions/fisher_f.hpp>


#include<boost/numeric/ublas/matrix.hpp>
#include<boost/numeric/ublas/vector.hpp>
#include<boost/numeric/ublas/io.hpp>
#include<complex>

#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/vector_concepts.hpp>

typedef double Type;

namespace ublas = boost::numeric::ublas;

typedef ublas::vector<Type> Vector;

template <typename T>
struct dot
{
  T operator() (const T& v, const T& w)
  {
    using std::conj;
    T tmp= 0;
    for (int i= 0; i < v.size(); i++)
      tmp+= conj(v[i]) * w[i];
    return tmp;
  }
};

namespace math {
  concept_map HilbertSpace< dot<double>, Vector>;
}

int main (int argc, char* argv[])
{
  const int v_size= 10;

  Vector v(v_size), w(v_size), x, y(v_size-1);
  for (int i= 0; i < v_size; i++)
    v[i]= 1.0, w[i]= 2.0;
  for (int i= 0; i < v_size-1; i++)
    y[i]= 3.0;

  w+= v;
  try {
    x= v + y;
  } catch (ublas::bad_argument) {
    std::cout << "caught bad argument " << std::endl;
  }

  std::cout << "v " << v << std::endl;
  std::cout << "w " << w << std::endl;
  std::cout << "x " << x << std::endl;

  return 0;
}


/* Vehicle Vibration Analysis
*	Vehicle dynamics can be modeled as a 2DoF spring-mass-damper system with two ouptuts:
*		1. Vehicle bounce (z motion felt by passengers)
*		2. Vehicle pitch (rotation felt be passengers along front to back)
*	These ouputs are based on vehicle characteristics
*/

// Courtney Armstrong
// Rev 2, 12/06/16

#include <iostream>
#include <fstream>
#include <boost/numeric/odeint.hpp>
#include <vector>
#include <Eigen>
#include <iomanip>
#include <math.h>

using namespace Eigen;
using namespace std;
using namespace boost::numeric::odeint;

typedef std::vector< double > state_type;
double pi = acos(-1);

// Define road class
class Road
{
	friend class coupledODE;
	protected:
		double A;			// 1/2 amplitude of road variation, m
		double L;			// Wavelength of road variation, m
		double V;			// Vehicle velocity, m/s
		double radFreq;		// Radial frequency (rad/s)

	public:
		void getRoadValues()
		{
			cout << "It is efficient to model road variation as a sinusoidal curve, using peak amplitude (bump height) and wavelength (roughness) as defining characteristics." << endl;

			cout << "Enter the peak amplitude of the road variation, A (m): "; cin >> A;
			cout << "Enter the wavelength of the road variation, L (m): "; cin >> L;
			cout << "Enter the velocity of the vehicle, V (m/s): "; cin >> V;
			cout << "\n";
			radFreq = (2 * pi * V) / L;
			cout << setprecision(4) << "The radial frequency is " << radFreq << " rad/s\n";
		}

};

// Define vehicle class
class Vehicle
{
	friend class coupledODE;
	private:
		double mass;			// Vehicle mass, kg
		double inertia;			// Mass moment of inertia, N*sec^2*m
		double stiffness_f;		// Stiffness of front suspension, N/m
		double stiffness_r;		// Stiffness of rear suspension, N/m
		double damping_f;		// Damping of front suspension, N*s/m
		double damping_r;		// Damping of rear suspension, N*s/m
		double frontLength;		// Distance from CG to front suspension, m
		double rearLength;		// Distance from CG to rear suspension, m
		double w1;				// 1st natural frequency, rad/s
		double w2;				// 2nd natural frequency, rad/s
		int classVal;			// User defined class type

	public:
		// Output general information and get user choice (determines vehicle paramenters)
		int setUserChoice()
		{
			int selection;
			cout << "Before the vibration analysis can be completed, modeling parameters for the vehicle need to be defined." << endl;
			cout << "The pre-defined parameters are based on SAE passenger car classifications, or you can define your own parameters.\n" << endl;
			cout << "The SAE classification are based on the wheelbase (length from front to back axle) of the vehicles. They are defined as follows:" << endl;
			cout << "\tClass 1 - Wheelbase = 2.06-2.41 m (80.9-94.8\"); Includes sub-compact vehicles, like the Ford Fiesta and Kia Rio." << endl;
			cout << "\tClass 2 - Wheelbase = 2.41-2.58 m (94.8-101.6\"); Includes compact vehicles, like the Toyota Corolla and Mazda3" << endl;
			cout << "\tClass 3 - Wheelbase = 2.58-2.80 m (101.6-110.4\"); Includes mid-size vehicles, like the Hyundai Sonata and Honda Accord" << endl;
			cout << "\tClass 4 - Wheelbase = 2.80-2.98 m (110.4-117.5\"); Includes full-size vehicles, like the Infiniti Q70 and Jaguar XF" << endl;
			cout << "\tClass 5 - Wheelbase = 2.98+ m (117.5\"+); Unusual for modern passenger cars to fit this class\n" << endl;

			cout << "Enter a value from 1-5 to select one of the SAE classed (enter 1 to select Class 1, 2 to select Class 2, etc.)\nOR\nEnter 0 to define your own parameters: ";
			cin >> selection;

			// Check validity of selection and assign if data is correct
			if (selection >= 0 && selection <= 5)
			{
				classVal = selection;
				if (classVal == 0)
				{
					cout << "\nYou have selected to input your own parameters.\nNote that this may result in failed calculations if not properly defined.\n" << endl;
				}
				else
				{
					cout << "You have selected SAE Class " << classVal << "\n" << endl;
				}
				return 1;
			}
			else
			{
				cout << "You have entered an invalid selection." << endl;
				return 0;
			}
		}

		// Set modeling parameters for the vehicle based on user choice
		void setVehicleParameters()
		{
			if (classVal == 0)
			{
				// User defined parameters
				cout << "Enter the mass, m (kg): "; cin >> mass;
				cout << "Enter the moment of inertia, J (N*s^2*m): "; cin >> inertia;
				cout << "Enter the front suspension stiffness, k_f (N/m): "; cin >> stiffness_f;
				cout << "Enter the rear suspension stiffness, k_r (N/m): "; cin >> stiffness_r;
				cout << "Enter the front damping coefficient, c_f (N*s/m): "; cin >> damping_f;
				cout << "Enter the rear damping coefficient, c_r (N*s/m): "; cin >> damping_r;
				cout << "Enter the length from the center of gravity to the front axle, L1 (m): "; cin >> frontLength;
				cout << "Enter the length from the center of gravity to the rear axle, L2 (m); "; cin >> rearLength;
				cout << "\n";
			}
			else if (classVal == 1)
			{
				// Parameters for SAE class 1
				mass = 998.8104;
				inertia = 1399.0933;
				stiffness_f = 16836.6944;
				stiffness_r = 17616.0088;
				damping_f = 1733.7550;
				damping_r = 1399.2629;
				frontLength = 0.6998;
				rearLength = 0.6934;
			}
			else if (classVal == 2)
			{
				// Parameters for SAE class 2
				mass = 1384.8175;
				inertia = 1790.2473;
				stiffness_f = 17931.2371;
				stiffness_r = 17048.5979;
				damping_f = 1616.4201;
				damping_r = 1227.6387;
				frontLength = 0.7191;
				rearLength = 0.7280;
			}
			else if (classVal == 3)
			{
				// Parameters for SAE class 3
				mass = 1608.8921;
				inertia = 2259.8130;
				stiffness_f = 19465.3483;
				stiffness_r = 20032.7592;
				damping_f = 1521.8516;
				damping_r = 1213.6285;
				frontLength = 0.7457;
				rearLength = 0.7346;
			}
			else if (classVal == 4)
			{
				// Parameters for SAE class 4
				mass = 1926.4068;
				inertia = 2822.3653;
				stiffness_f = 16680.8315;
				stiffness_r = 23204.3063;
				damping_f = 1229.3899;
				damping_r = 1218.8823;
				frontLength = 0.7605;
				rearLength = 0.7854;
			}
			else
			{
				// Parameters for SAE class 5
				mass = 2206.7269;
				inertia = 2709.9452;
				stiffness_f = 16812.1766;
				stiffness_r = 30354.7352;
				damping_f = 1201.3697;
				damping_r = 1337.9685;
				frontLength = 0.7623;
				rearLength = 0.7620;
			}
		}

		// Calculate the two natural frequencies
		void calcNatFreq()
		{

			cout << "-- Natural Frequencies --" << endl;
			// Create a 2x2 matrix using Eigen
			// Mass matrix of the system, from eqtns of motion
			Matrix2d M;
			M(0, 0) = mass;
			M(1, 0) = 0;
			M(0, 1) = 0;
			M(1, 1) = inertia;
			cout << "The mass matrix, M, of this system is: \n" << M << "\n" << endl;

			// Create a 2x2 matrix using Eigen
			// Stiffness matrix of the system, from eqtns of motion
			Matrix2d K;
			K(0, 0) = stiffness_f + stiffness_r;
			K(1, 0) = (stiffness_r*rearLength) - (stiffness_f*frontLength);
			K(0, 1) = K(1, 0);
			K(1, 1) = (stiffness_f*pow(frontLength, 2)) + (stiffness_r*pow(rearLength, 2));
			cout << "The stiffness matrix, K, is: \n" << K << "\n" << endl;

			// Find eigenvalues of M,K
			GeneralizedSelfAdjointEigenSolver<MatrixXd> es(K, M);
			double lambda1 = es.eigenvalues()[0];
			double lambda2 = es.eigenvalues()[1];

			// Natural frequencies equal the sqrt of the eigenvalues
			w1 = sqrt(lambda1);
			w2 = sqrt(lambda2);

			cout << setprecision(4) << "The undamped natural frequencies of the system are " << w1 << " rad/s and " << w2 << " rad/s.\n" << endl;
		}

};

// ODE integration parameters class used to solve equations (inherits data from Vehicle & Road class)
class analysisParam
{
	public:
		double t0, tf, dt;
		double x_init1, x_init2, x_init3, x_init4;

		void getInterval()
		{
			cout << "-- Forced Excitation --" << endl;
			cout << "Enter the starting time of the interval (s): "; cin >> t0;
			cout << "Enter the ending time of the interval (s): "; cin >> tf;
			cout << "Enter the desired time increment (s): "; cin >> dt;
		}

		void getIntitialCond()
		{
			cout << "\nEnter the initial value of the bounce, (m): "; cin >> x_init1;
			cout << "Enter the initial value of the rate of change of the bounce (m/s): "; cin >> x_init2;
			cout << "Enter the initial value of the pitch (rad): "; cin >> x_init3;
			cout << "Enter the initial value of the rate of change of the pitch (rad/s): "; cin >> x_init4;
		}
};

class coupledODE
{
	//double m_param, J_param, kf_param, kr_param, cf_param, cr_param, l1_param, l2_param, A_param, radF_param, L_param;
	//coupledODE(double m, double J, double kf, double kr, double cf, double cr, double l1, double l2, double Amp, double radialF, double waveL) : m_param(m), J_param(J), kf_param(kf), kr_param(kr), cf_param(cf), cr_param(cr), l1_param(l1), l2_param(l2), A_param(Amp), radF_param(radialF), L_param(waveL) {};

	public:
		Vehicle car; Road road;
		coupledODE(const Vehicle& car1, const Road& road1) : car(car1), road(road1) {};

		void operator()(state_type &x, state_type &dxdt, double t)
		{
			double wave_f = car.stiffness_f*road.A*sin((road.radFreq)*t);
			double wave_r = car.stiffness_r*road.A*sin((road.radFreq)*t - (2 * pi*(car.frontLength + car.rearLength)) / road.L);

			double term1f = car.stiffness_f*x[0] + car.damping_f*x[1];
			double term1r = car.stiffness_r*x[0] + car.damping_r*x[1];
			double term2f = car.stiffness_f*x[2] + car.damping_f*x[3];
			double term2r = car.stiffness_r*x[2] + car.damping_r*x[3];
			double term3f = -term1f + term2f*car.frontLength + wave_f;
			double term3r = -term1r - term2r*car.rearLength + wave_r;

			dxdt[0] = x[1];
			dxdt[1] = (1 / car.mass)*(term3f + term3r);
			dxdt[2] = x[3];
			dxdt[3] = (1 / car.inertia)*(-term3f*car.frontLength + term3r*car.rearLength);
		}
};

// Structure to write values generated by integration to vectors
struct writeVals
{
	std::vector< state_type >& m_states;
	std::vector< double >& m_time;

	writeVals(std::vector< state_type > &states, std::vector< double > &time)
		: m_states(states), m_time(time) { }

	void operator()(const state_type &x, double t)
	{
		m_states.push_back(x);
		m_time.push_back(t);
	}
};

int main()
{
	analysisParam calcParams;
	Vehicle car;
	Road road;

	cout << "2-DoF Vehicle Vibration Analysis\n" << endl;
	cout << "\n------ Vehicle Properties ------\n" << endl;
	if (car.setUserChoice() == 0)
	{
		exit(0);
	}
	else
	{
		car.setVehicleParameters();
		cout << "\n------ Driving Conditions ------\n" << endl;
		road.getRoadValues();

		cout << "\n------ System Analysis ------\n" << endl;
		car.calcNatFreq();
		calcParams.getInterval();
		calcParams.getIntitialCond();

		// Redefine input from analysisParams to useable values for odeint evaluation
		state_type x(4);		// Define state vector to hold initial values
		x[0] = calcParams.x_init1;		// value of x[0] (bounce)
		x[1] = calcParams.x_init2;		// value of x[1] (rate of change of bounce, x')
		x[2] = calcParams.x_init3;		// value of x[2] (pitch)
		x[3] = calcParams.x_init4;		// value of x[3] (rate of change of pitch, theta')
		vector<state_type> x_vec;
		vector<double> times;

		const double timeStep = calcParams.dt;		// Time step as defined by user
		double tStart = calcParams.t0;				// Start time as defined by user
		double tEnd = calcParams.tf;				// End time as defined by user

		// Initialize odeint using standard rk5 integration (DOES NOT WORK)
		//typedef runge_kutta_dopri5<state_type> rk5;
		//ntegrate_const(make_dense_output(1E-3, 1E-6, rk5()), coupledODE(car,road), x, tStart, tEnd, timeStep, writeVals(x_vec, times));

		// Initialize odeint using Burlisch Stoer
		bulirsch_stoer_dense_out< state_type > stepper(1E-8, 0.0, 0.0, 0.0);
		integrate_const(stepper, coupledODE(car, road), x, tStart, tEnd, timeStep, writeVals(x_vec, times));

		ofstream out("results.txt");
		if (out.is_open())
		{
			double n = (tEnd - tStart) / timeStep;
			out << "System Analysis Results\n";
			out << "Time" << "\t" << "Bounce" << "\t" << "Pitch" << endl;
			for (int i = 0; i <= n; i++)
			{
				out << setprecision(8) << times[i] << "\t" << x_vec[i][0] << "\t" << x_vec[i][2] << endl;
			}
			out.close();
		}

		cout << "\n------ System results written to file ------\n" << endl;

	}

	return 0;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2004 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Test the FastSLAM algorithm
 */

		// Bayes++ Bayesian filtering schemes
#include "BayesFilter/SIRFlt.hpp"
#include "BayesFilter/covFlt.hpp"
#include "BayesFilter/unsFlt.hpp"
#include "BayesFilter/models.hpp"
		// Types required for SLAM classes
#include <vector>
#include <map>
		// Bayes++ SLAM
#include "SLAM.hpp"
#include "fastSLAM.hpp"
#include "kalmanSLAM.hpp"

#include "Test/random.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/lexical_cast.hpp>

using namespace SLAM_filter;

class SLAM_random : public Bayesian_filter_test::Boost_random, public BF::SIR_random
/*
 * Random numbers for SLAM test
 */
{
public:
	FM::Float normal (const FM::Float mean, const FM::Float sigma)
	{
		return Boost_random::normal (mean, sigma);
	}
	void normal (FM::DenseVec& v)
	{
		Boost_random::normal (v);
	}
	void uniform_01 (FM::DenseVec& v)
	{
		Boost_random::uniform_01 (v);
	}
	void seed ()
	{
		Boost_random::seed();
	}
};

/*
 * Demonstrate a SLAM example
 */
struct SLAMDemo
{
	const unsigned nParticles;

	SLAMDemo (unsigned setnParticles) : nParticles(setnParticles)
	{}
	void OneDExperiment ();
	void InformationLossExperiment ();

	SLAM_random goodRandom;

	// Relative Observation with  Noise model
	struct Simple_observe : BF::Linear_uncorrelated_observe_model
	{
		Simple_observe (Float i_Zv) : Linear_uncorrelated_observe_model(2,1)
		// Construct a linear model with const Hx
		{
			Hx(0,0) = -1.;	// Location
			Hx(0,1) = 1.;	// Map
			Zv[0] = i_Zv;
		}
	};
	struct Simple_observe_inverse : BF::Linear_uncorrelated_observe_model
	{
		Simple_observe_inverse (Float i_Zv) : Linear_uncorrelated_observe_model(2,1)
		{
			Hx(0,0) = 1.;	// location
			Hx(0,1) = 1.;	// observation
			Zv[0] = i_Zv;
		}
	};

	struct Kalman_statistics : public BF::Kalman_state_filter
	// Kalman_statistics without any filtering
	{
		Kalman_statistics (std::size_t x_size) : Kalman_state_filter(x_size) {}
		void init() {}
		void update() {}
	};

	template <class Filter>
	struct Generic_kalman_generator : public Kalman_filter_generator
	// Generate and dispose of generic kalman filter type
	{
		Filter_type* generate( unsigned full_size )
		{
			return new Filter(full_size);
		}
		void dispose( Filter_type* filter )
		{
			delete filter;
		}
	};

	void display( const std::string label, const BF::Kalman_state_filter& stats)
	{
		std::cout << label << stats.x << stats.X << std::endl;
	}
};

void SLAMDemo::OneDExperiment ()
// Experiment with a one dimensional problem
//  Use to look at implication of highly correlated features
{
	// State size
	const unsigned nL = 1;	// Location
	const unsigned nM = 2;	// Map

	// Construct simple Prediction models
	BF::Sampled_LiAd_predict_model location_predict(nL,1, goodRandom);
	// Stationary Prediction model (Identity)
	FM::identity(location_predict.Fx);
				// Constant Noise model
	location_predict.q[0] = 1000.;
	location_predict.G.clear();
	location_predict.G(0,0) = 1.;

	// Relative Observation with  Noise model
	Simple_observe observe0(5.), observe1(3.);
	Simple_observe_inverse observe_new0(5.), observe_new1(3.);

	// Setup the initial state and covariance
	// Location with no uncertainty
	FM::Vec x_init(nL); FM::SymMatrix X_init(nL, nL);
	x_init[0] = 20.;
	X_init(0,0) = 0.;

	// Truth model : location plus one map feature
	FM::Vec true0(nL+1), true1(nL+1);
	true0.sub_range(0,nL) = x_init; true0[nL] = 50.;
	true1.sub_range(0,nL) = x_init; true1[nL] = 70.;
	FM::Vec z(1);

	// Filter statistics for display
	Kalman_statistics stat(nL+nM);

	// Kalman_SLAM filter:
	Generic_kalman_generator<BF::Covariance_scheme> full_gen;
	Kalman_SLAM kalm (full_gen);
	kalm.init_kalman (x_init, X_init);

	// Fast_SLAM filter
	BF::SIR_kalman_scheme fast_location (nL, nParticles, goodRandom);
	fast_location.init_kalman (x_init, X_init);
	Fast_SLAM_Kstatistics fast (fast_location);

	// Initial feature states
	z = observe0.h(true0);		// Observe a relative position between location and map landmark
	z[0] += 0.5;
	kalm.observe_new (0, observe_new0, z);
	fast.observe_new (0, observe_new0, z);

	z = observe1.h(true1);
	z[0] += -1.0;
	kalm.observe_new (1, observe_new1, z);
	fast.observe_new (1, observe_new1, z);

	fast.update(); fast.statistics_sparse(stat); display("Feature Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("Feature Kalm", stat);

	// Predict the location state forward
	fast_location.predict (location_predict);
	kalm.predict (location_predict);
	fast.update(); fast.statistics_sparse(stat); display("Predict Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("Predict Kalm", stat);

	// Observation feature 0
	z = observe0.h(true0);
	z[0] += 0.5;			// Observe a relative position between location and map landmark
	fast.observe( 0, observe0, z );
	kalm.observe( 0, observe0, z );
	fast.update(); fast.statistics_sparse(stat); display("ObserveA Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("ObserveA Kalm", stat);

	// Observation feature 1
	z = observe1.h(true1);
	z[0] += 1.0;			// Observe a relative position between location and map landmark
	fast.observe( 1, observe1, z );
	kalm.observe( 1, observe1, z );
	fast.update(); fast.statistics_sparse(stat); display("ObserveB Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("ObserveB Kalm", stat);

	// Observation feature 0
	z = observe0.h(true0);
	z[0] += 0.5;			// Observe a relative position between location and map landmark
	fast.observe( 0, observe0, z );
	kalm.observe( 0, observe0, z );
	fast.update(); fast.statistics_sparse(stat); display("ObserveC Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("ObserveC Kalm", stat);

	// Forget feature 0
	fast.forget(0);
	kalm.forget(0);
	fast.update(); fast.statistics_sparse(stat); display("Forget Fast", stat);
	kalm.update(); kalm.statistics_sparse(stat); display("Forget Kalm", stat);
}

void SLAMDemo::InformationLossExperiment ()
// Experiment with information loss due to resampling
{
	// State size
	const unsigned nL = 1;	// Location
	const unsigned nM = 2;	// Map

	// Construct simple Prediction models
	BF::Sampled_LiAd_predict_model location_predict(nL,1, goodRandom);
	// Stationary Prediction model (Identity)
	FM::identity(location_predict.Fx);
				// Constant Noise model
	location_predict.q[0] = 1000.;
	location_predict.G.clear();
	location_predict.G(0,0) = 1.;

	// Relative Observation with  Noise model
	Simple_observe observe0(5.), observe1(3.);
	Simple_observe_inverse observe_new0(5.), observe_new1(3.);

	// Setup the initial state and covariance
	// Location with no uncertainty
	FM::Vec x_init(nL); FM::SymMatrix X_init(nL, nL);
	x_init[0] = 20.;
	X_init(0,0) = 0.;

	// Truth model : location plus one map feature
	FM::Vec true0(nL+1), true1(nL+1);
	true0.sub_range(0,nL) = x_init; true0[nL] = 50.;
	true1.sub_range(0,nL) = x_init; true1[nL] = 70.;
	FM::Vec z(1);

	// Filter statistics for display
	Kalman_statistics stat(nL+nM);

	// Kalman_SLAM filter
	Generic_kalman_generator<BF::Unscented_scheme> full_gen;
	Kalman_SLAM kalm (full_gen);
	kalm.init_kalman (x_init, X_init);

	// Fast_SLAM filter
	BF::SIR_kalman_scheme fast_location (nL, nParticles, goodRandom);
	fast_location.init_kalman (x_init, X_init);
	Fast_SLAM_Kstatistics fast (fast_location);

	// Initial feature states
	z = observe0.h(true0);		// Observe a relative position between location and map landmark
	z[0] += 0.5;
	kalm.observe_new (0, observe_new0, z);
	fast.observe_new (0, observe_new0, z);

	z = observe1.h(true1);
	z[0] += -1.0;
	kalm.observe_new (1, observe_new1, z);
	fast.observe_new (1, observe_new1, z);

	unsigned it = 0;
	for (;;) {
		++it;
		std::cout << it << std::endl;

		// Groups of observations without resampling
		{
			// Predict the filter forward
			kalm.predict (location_predict);
			fast_location.predict (location_predict);

			// Observation feature 0 with bias
			z = observe0.h(true0);		// Observe a relative position between location and map landmark
			z[0] += 0.5;
			kalm.observe( 0, observe0, z );
			fast.observe( 0, observe0, z );

			// Predict the filter forward
			kalm.predict (location_predict);
			fast_location.predict (location_predict);

			// Observation feature 1 with bias
			z = observe1.h(true1);		// Observe a relative position between location and map landmark
			z[0] += -1.0;
			kalm.observe( 1, observe1, z );
			fast.observe( 1, observe1, z );
		}

		// Update and resample
		kalm.update();
		fast.update();

		kalm.statistics_sparse(stat); display("Kalm", stat);
		fast.statistics_sparse(stat); display("Fast", stat);
		std::cout << fast_location.stochastic_samples <<','<< fast_location.unique_samples()
			<<' '<< fast.feature_unique_samples(0) <<','<< fast.feature_unique_samples(1) <<std::endl;
		std::cout.flush();
	}
}

int main (int argc, char* argv[])
{
	// Global setup for test output
	std::cout.flags(std::ios::fixed); std::cout.precision(4);

	unsigned nParticles = 1000;
	if (argv[1])
	{
    	try {
			nParticles = boost::lexical_cast<unsigned>(argv[1]);
		}
		catch (boost::bad_lexical_cast) {
			// ignore error and use default
		}
	}
	std::cout << "nParticles = " << nParticles << std::endl;

	// Create test and run experiments
	try {
		SLAMDemo test(nParticles);
		test.OneDExperiment();
		//test.InformationLossExperiment();
	}
	catch (const BF::Filter_exception& ne)
	{
		std::cout << ne.what() << std::endl;
	}
}


#include <string>
#include <NTL/ZZ.h>
#include <sstream>

inline std::string
StringFromZZ(const NTL::ZZ &x)
{
    std::string s;
    s.resize(NumBytes(x), 0);
    NTL::BytesFromZZ((uint8_t*) &s[0], x, s.length());
    return s;
}

// returns ZZ from a string representing the ZZ in base 256
inline NTL::ZZ
ZZFromString(const std::string &s)
{
    return NTL::ZZFromBytes((const uint8_t *) s.data(), s.length());
}

// converts ZZ to and from a string representing the number in decimal

inline std::string
DecStringFromZZ(const NTL::ZZ & x) {
    std::stringstream ss;
    ss << x;
    return ss.str();
}

inline NTL::ZZ
ZZFromDecString(const std::string &s) {
    NTL::ZZ v;
    std::stringstream ss(s);
    ss >> v;
    return v;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014 by Synge Todo <wistaria@comp-phys.org>
*
* 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)
*
*****************************************************************************/

// C++ version of TITPACK Ver.2 by H. Nishimori

#ifndef TITPACK_COMMON_HPP
#define TITPACK_COMMON_HPP

#include "subspace.hpp"
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/tuple/tuple.hpp>

typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> matrix_type;
typedef boost::numeric::ublas::matrix<int, boost::numeric::ublas::column_major> i_matrix_type;

//
// eigenvalues by the bisection method
//
// return value # m and nsplit
// alpha  @ diagonal element
// beta   @ subdiagonal element
// ndim   @ matrix dimension
// E      # eigenvalues
// ne     @ number of eigenvalues to calculate
// eps    @ limit of error

boost::tuple<int, int>
bisec(std::vector<double> const& alpha, std::vector<double> const& beta, int ndim,
      std::vector<double>& E, int ne, double eps, std::vector<int>& iblock,
      std::vector<int>& isplit, double *w);

//
// eigenvector of a tridiagonal matrix by inverse iteration for the large/medium routines
//
// E(4)       @  4 lowest eigenvalues
// ndim       @  matrix dimension
// nvec       @  number of vectors to calculate

void vec12(std::vector<double> const& alpha, std::vector<double> const& beta, int ndim,
           std::vector<double> const& E, int nvec, matrix_type& z,
           std::vector<int>& iblock, std::vector<int>& isplit, double *w);

//
// xx correlation function
//
// n           @ lattice size
// npair       @ pair of sites (k,l) <Sx(k)Sx(l)>
// x           @ eigenvetor
// sxx         # xx correlation function
// list1,list2 @ spin configurations generated in 'sz'

void xcorr(subspace const& ss, std::vector<int> const& npair, const double *x,
           std::vector<double>& sxx);

void xcorr(subspace const& ss, std::vector<int> const& npair,
           std::vector<double> const& x, std::vector<double>& sxx);

void xcorr(subspace const& ss, std::vector<int> const& npair,
           matrix_type const& x, int xindex, std::vector<double>& sxx);

//
// ************* zz correlation function **************
//
// n           @ lattice size
// npair       @ pair of sites (k,l) <Sz(k)Sz(l)>
// x           @ eigenvetor
// szz         # zz correlation function
// list1,list2 @ spin configurations generated in 'sz'

void zcorr(subspace const& ss, std::vector<int> const& npair, const double *x,
           std::vector<double>& szz);

void zcorr(subspace const& ss, std::vector<int> const& npair,
           matrix_type const& x, int xindex, std::vector<double>& szz);

void zcorr(subspace const& ss, std::vector<int> const& npair,
           std::vector<double> const& x, std::vector<double>& szz);

//
// Orthogonalization of the eigenvectors
//
// return value #  degree of degenearcy
// ev      @# vectors to be orthogonalized / orthogonalized vectors
// norm(j) #  norm of the j-th vector returned
// numvec  @  number of vectors to be checked

int orthg(matrix_type& ev, std::vector<double>& norm, int numvec);

//
// configurations with the specified sz
//
// n          @  lattice size
// idim       @  dimension of the matrix
// szval      @  total sz
// list1(i)   #  i-th spin configuration
// list2      #  inverse list of list1 expressed by the
//               2-dim search method of M. Ogata and H.Q. Lin.
// ==============================================================
//      This routine is equivalent to sz but is faster than sz.
//      This routine has been developed by Daijiro Yoshioka,
//      University of Tokyo.  The copyright of szdy belongs to him.
//                                              1993/5/10
// ==============================================================

void szdy(int n, int idim, double szval, std::vector<int>& list1,
          std::vector<std::vector<int> >& list2);

#endif


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, lbeta) {
  using stan::math::lbeta;

  EXPECT_FLOAT_EQ(0.0, lbeta(1.0, 1.0));
  EXPECT_FLOAT_EQ(2.981361, lbeta(0.1, 0.1));
  EXPECT_FLOAT_EQ(-4.094345, lbeta(3.0, 4.0));
  EXPECT_FLOAT_EQ(-4.094345, lbeta(4.0, 3.0));
}

TEST(MathFunctions, lbeta_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::lbeta(nan, 1.0));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::lbeta(1.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::lbeta(nan, nan));
}


#include <iostream>
#include <vector>
#include <fstream>
#include <Windows.h>

#ifdef min
#undef min
#endif

#ifdef max
#undef max
#endif

#include <Eigen\Dense>
#include <Image.h>

#include "CorrelationMat.h"

#include "GDMPCA.h"
#include "SPLH.h"

int main(int argc, char **argv)
{
	Eigen::setNbThreads(4);
	std::clog << "Eigen::nbThreads() = " << Eigen::nbThreads() << std::endl;

	std::clog << "Eigen::nbThreads() = " << Eigen::nbThreads() << std::endl;

	std::vector<Image> images;

	std::clog << "Loading data... " << std::endl;

	int M = 32 * 32 * 3;

	//M = 1024;

	for (int i = 1; i < argc; ++i)
	{
		std::clog << "\t" << argv[i] << std::endl;
		std::ifstream input(argv[i], std::ios::binary);
		auto imgs = LoadBatch(input);
		images.insert(images.end(), imgs.begin(), imgs.end());

		//LoadBatch(input, subCorrMat);
	}

	//while (images.size() > 100)
	//	images.pop_back();

	std::clog << "DONE" << std::endl;

	int N = images.size();

	std::clog << "Creating matrix X(" << M << "x" << N << ")... ";

	int L = 0;
	for (auto img : images)
		if (img.label != 255)
			++L;

	Eigen::MatrixXf X(M, N);
	Eigen::MatrixXf Xl(M, L);

	//Eigen::MatrixXf mi = subCorrMat.Mean();
	Eigen::MatrixXf mi = Eigen::MatrixXf::Zero(M, 1);

	for (int i = 0; i < M; ++i)
		for (int j = 0; j < N; ++j)
		{
			double v = images[j].v[i] / 255.0f;
			X(i, j) = v;
			mi(i, 0) += v;
		}

	int tl = 0;
	for (auto img : images)
		if (img.label != 255)
		{
			for (int i = 0; i < M; ++i)
				Xl(i, tl) = img.v[i] / 255.0f;
			++tl;
		}

	for (int i = 0; i < M; ++i)
		mi(i, 0) /= N;

	for (int i = 0; i < M; ++i)
		for (int j = 0; j < N; ++j)
		{
			X(i, j) -= mi(i,0);
		}

	std::clog << "DONE" << std::endl;

	std::clog << "Creating matrix S("<<L<<"x"<<L<<")... ";

	Eigen::MatrixXf S(L, L);
	for (int i = 0; i < L; ++i)
		for (int j = 0; j < L; ++j)
			S(i, j) = images[i].label == images[j].label ? 1 : -1;

	std::clog << "DONE" << std::endl;

	/*
	std::clog << "Creating matrix M... ";

	Eigen::MatrixXf Me(M, M);
	//Eigen::MatrixXf Me = subCorrMat.CorrelationMatrix();
	double beta = 0.3;

	Me = Xl * S * Xl.transpose();
	Me += beta * (X * X.transpose());

	std::clog << "DONE" << std::endl;
	*/
	std::clog << "Matrix deflation... " << std::endl;

	int H = 64;

	//auto x = OrtogonalDeflation(Me, H);
	//auto x = GDMPCA(Me, 10);
	auto x = SPLH(X, Xl, S, H, 1.0 / M, 0.3);

	std::clog << "DONE" << std::endl;

	std::cout << H << " " << M << std::endl;
	for (auto &v : x)
		std::cout << -mi.transpose() * v << "\t" << v.transpose() << std::endl;

	return 0;
}