/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2019 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Natasha Sharma, University of Texas at El Paso,
 *          Guido Kanschat, University of Heidelberg
 *          Timo Heister, Clemson University
 *          Wolfgang Bangerth, Colorado State University
 *          Zhuroan Wang, Colorado State University
 */

// @sect3{Include files}

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

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

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

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

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

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

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

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

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

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

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

namespace Step47
{
  using namespace dealii;

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

  namespace ExactSolution
  {
    using numbers::PI;

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

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

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

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

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

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

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

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

//  @sect3{The main class}

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

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

    void run();

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

    Triangulation<dim> triangulation;

    MappingQ<dim> mapping;

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

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

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

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

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

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

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

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

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

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

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

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

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

//  @sect4{Assembling the linear system}

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

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

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

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

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

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

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

    CopyData(const CopyData &) = default;

    CopyData(CopyData &&) = default;

    ~CopyData() = default;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

      cell->get_dof_indices(copy_data.local_dof_indices);

      const ExactSolution::RightHandSide<dim> right_hand_side;

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

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

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

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

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

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

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

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

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

      copy_data_face.joint_dof_indices =
        fe_interface_values.get_interface_dof_indices();

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

      copy_data_face.cell_matrix.reinit(n_dofs, n_dofs);

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

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

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

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

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

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

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

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

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

//

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

//

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

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

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

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

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

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

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

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

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

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

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

    constraints.distribute(solution);
  }

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

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

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

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

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

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

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

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

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

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

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

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

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

    DataOut<dim> data_out;

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

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

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

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

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

        triangulation.refine_global(1);
        setup_system();

        assemble_system();
        solve();

        output_results(cycle);

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

//  @sect3{The main() function}

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

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

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

      BiharmonicProblem<2> biharmonic_problem(fe_degree);
      biharmonic_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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

  return 0;
}


#ifndef H_MPC_DESIRED_TRAJECTORY_MANAGER
#define H_MPC_DESIRED_TRAJECTORY_MANAGER

#include <Utils/Math/cubicfit_one_dim.hpp>
#include <Utils/Math/linearfit_one_dim.hpp>
#include <Eigen/Dense>
#include <vector>
#include <cmath>

// A container that holds the state trajectory within the time horizon bounded by time_start and time_end

#define STATE_TRAJECTORY_WITHIN_HORIZON_POSITION 0
#define STATE_TRAJECTORY_WITHIN_HORIZON_VELOCITY 1
#define STATE_TRAJECTORY_WITHIN_HORIZON_ACCELERATION 2

#define STATE_TRAJECTORY_WITHIN_HORIZON_CUBIC_FIT 2
#define STATE_TRAJECTORY_WITHIN_HORIZON_LINEAR_FIT 0

class StateTrajectoryWithinHorizon{
public:
    // for a given state [x, xdot], the size of dimension_in is equal to the dimension of x.
    StateTrajectoryWithinHorizon(const int dimension_in);
    ~StateTrajectoryWithinHorizon();

    // init_boundary is the state [x, xdot] at the start time
    // end_boundary is the state [x, xdot] at the end boundary
    void setParams(const Eigen::VectorXd init_boundary,
                   const Eigen::VectorXd end_boundary,
                   const double time_start, double const time_end);

    Eigen::VectorXd getVal(const int index, const double time,
                           int fit_type = STATE_TRAJECTORY_WITHIN_HORIZON_CUBIC_FIT);

    Eigen::VectorXd getPos(const double time, const int fit_type = STATE_TRAJECTORY_WITHIN_HORIZON_CUBIC_FIT);
    Eigen::VectorXd getVel(const double time, const int fit_type = STATE_TRAJECTORY_WITHIN_HORIZON_CUBIC_FIT);
    Eigen::VectorXd getAcc(const double time, const int fit_type = STATE_TRAJECTORY_WITHIN_HORIZON_CUBIC_FIT);

private:
    int dimension;
    // A vector of a polynomial cubic fit for the state within the horizon
    std::vector<CubicFit_OneDimension> x_cubic;
    // A vector of a linear fit for the state within the horizon
    std::vector<LinearFit_OneDimension> x_linear;
};

class InputTrajectoryWithinHorizon{
public:
    // for a given state [x, xdot], the size of dimension_in is equal to the dimension of x.
    InputTrajectoryWithinHorizon(const int dimension_in);
    ~InputTrajectoryWithinHorizon();

    // init_boundary is the input u at the start time
    // end_boundary is the input u at the end boundary
    void setParams(const Eigen::VectorXd init_boundary,
                   const Eigen::VectorXd end_boundary,
                   const double time_start, double const time_end);

    Eigen::VectorXd getVal(const double time);

private:
    int dimension;
    // A vector of a linear fit for the input within the horizon
    std::vector<LinearFit_OneDimension> u_linear;
};

class MPCDesiredTrajectoryManager{
public:
    MPCDesiredTrajectoryManager(const int state_size_in, const int horizon_in, const double dt_in);
    MPCDesiredTrajectoryManager(const int input_size_in, const int state_size_in, const int horizon_in, const double dt_in);
    ~MPCDesiredTrajectoryManager();

    // Accepts a concatenated state vector which lists the knot points of the state over the horizon
    // Assumes that the state has the form X = [x, \dot{x}]
    // For CMPC, the state has the form X = [x, \dot{x}, g] where g is the gravitational constant.
    //    this class automatically handles the CMPC case due to the implementation.

    // Therefore, X_pred = [X_1, X_2, ..., X_horizon].

    // Warning. It's important that X_pred has the right dimension
    // double t_start_in, starting time of the reference trajectory

    // Eigen::VectorXd X_start = [x_start, \dot{x}_start]; // starting state of the system
    void setStateKnotPoints(const double t_start_in,
                            const Eigen::VectorXd & X_start,
                            const Eigen::VectorXd & X_pred);

    // U_sequence = [U_0, U_1, ... U_{horizon-1}]
    // set the input knot points with U_0 the input that should be taken now.
    void setInputKnotPoints(const double t_start_in,
                            const Eigen::VectorXd & U_sequence);

    // Simultaenously sets input and state knotpoints
    void setStateAndInputKnotPoints(const double t_start_in,
                                    const Eigen::VectorXd & X_start,
                                    const Eigen::VectorXd & X_pred,
                                    const Eigen::VectorXd & U_sequence);

    // int horizon: number of time steps
    void setHorizon(const int horizon_in);
    // double dt: time interval between knot points. Usually equal to the MPC dt
    void setDt(const double dt_in);

    // outputs the state value at the specified time
    // x_out = [x_cubic(t), \dot{x}_cubic(t)]

    // time_in is clamped between (t_start and t_start + horizon*dt_internal)
    void getState(const double time_in, Eigen::VectorXd & x_out);

    // time_in is clamped between (t_start and t_start + horizon*dt_internal)
    void getInput(const double time_in, Eigen::VectorXd & u_out);

    // Returns the input state vector
    Eigen::VectorXd getXStartVector();
    // Returns the input state knot points
    Eigen::VectorXd getXpredVector();
    // Returns the global start time of the trajectories
    double getStartTime();

    // Returns the input sequence
    Eigen::VectorXd getUSequence();

    // Returns the knot points that are after this time.
    Eigen::VectorXd getTruncatedXpredVector(const double time);

    // Returns a vector of knotpoints, equal to the horizon, spaced by dt_internal excluding the input time
    Eigen::VectorXd getInterpolatedXpredVector(const double time);

    // helper function which gives the index to use for the piecewise cubic function
    int getHorizonIndex(const double time);

    Eigen::VectorXd getPos(const double time);
    Eigen::VectorXd getVel(const double time);
    Eigen::VectorXd getAcc(const double time);

    void setLinearInterpolate(bool val);
    void setCubicInterpolate(bool val);
    void setLinearInterpolateFirstStatetoNext(bool val);

private:
    double t_start; // global start time of the trajectories
    double dt_internal;

    int input_size;
    int state_size;
    int dim;
    int horizon;

    // option:
    bool linear_interpolate_states;

    // store initial start value
    Eigen::VectorXd X_start_internal;
    // vector containing the knot points
    Eigen::VectorXd X_pred_internal;

    // vector containing all the polynomial fits from t_start to t_start + horizon*dt
    std::vector< StateTrajectoryWithinHorizon > x_trajectory;

    // vector containing the input knot points
    Eigen::VectorXd U_sequence_internal;
    std::vector< InputTrajectoryWithinHorizon > u_trajectory;

};

namespace mpc_trajectory_manager_test{
    // Function which tests this object
    void test_object();
}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN2_SUPPORT_STAGE15_RESOLVE_API_CONFLICTS_WARN

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template<typename Scalar> void geometry(void)
{
  /* this test covers the following files:
     Cross.h Quaternion.h, Transform.cpp
  */

  typedef Matrix<Scalar,2,2> Matrix2;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,4,4> Matrix4;
  typedef Matrix<Scalar,2,1> Vector2;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Matrix<Scalar,4,1> Vector4;
  typedef eigen2_Quaternion<Scalar> Quaternionx;
  typedef eigen2_AngleAxis<Scalar> AngleAxisx;
  typedef eigen2_Transform<Scalar,2> Transform2;
  typedef eigen2_Transform<Scalar,3> Transform3;
  typedef eigen2_Scaling<Scalar,2> Scaling2;
  typedef eigen2_Scaling<Scalar,3> Scaling3;
  typedef eigen2_Translation<Scalar,2> Translation2;
  typedef eigen2_Translation<Scalar,3> Translation3;

  Scalar largeEps = test_precision<Scalar>();
  if (ei_is_same_type<Scalar,float>::ret)
    largeEps = 1e-2f;

  Vector3 v0 = Vector3::Random(),
    v1 = Vector3::Random(),
    v2 = Vector3::Random();
  Vector2 u0 = Vector2::Random();
  Matrix3 matrot1;

  Scalar a = ei_random<Scalar>(-Scalar(M_PI), Scalar(M_PI));

  // cross product
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).eigen2_dot(v1), Scalar(1));
  Matrix3 m;
  m << v0.normalized(),
      (v0.cross(v1)).normalized(),
      (v0.cross(v1).cross(v0)).normalized();
  VERIFY(m.isUnitary());

  // Quaternion: Identity(), setIdentity();
  Quaternionx q1, q2;
  q2.setIdentity();
  VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
  q1.coeffs().setRandom();
  VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());

  // unitOrthogonal
  VERIFY_IS_MUCH_SMALLER_THAN(u0.unitOrthogonal().eigen2_dot(u0), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().eigen2_dot(v0), Scalar(1));
  VERIFY_IS_APPROX(u0.unitOrthogonal().norm(), Scalar(1));
  VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), Scalar(1));

  VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
  VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0);
  VERIFY_IS_APPROX(ei_cos(a)*v0.squaredNorm(), v0.eigen2_dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
  m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
  VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
  VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);

  q1 = AngleAxisx(a, v0.normalized());
  q2 = AngleAxisx(a, v1.normalized());

  // angular distance
  Scalar refangle = ei_abs(AngleAxisx(q1.inverse()*q2).angle());
  if (refangle>Scalar(M_PI))
    refangle = Scalar(2)*Scalar(M_PI) - refangle;

  if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps)
  {
    VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps));
  }

  // rotation matrix conversion
  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
  VERIFY_IS_APPROX(q1 * q2 * v2,
    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);

  VERIFY( (q2*q1).isApprox(q1*q2, largeEps) || !(q2 * q1 * v2).isApprox(
    q1.toRotationMatrix() * q2.toRotationMatrix() * v2));

  q2 = q1.toRotationMatrix();
  VERIFY_IS_APPROX(q1*v1,q2*v1);

  matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX())
          * AngleAxisx(Scalar(0.2), Vector3::UnitY())
          * AngleAxisx(Scalar(0.3), Vector3::UnitZ());
  VERIFY_IS_APPROX(matrot1 * v1,
       AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix()
    * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix()
    * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));

  // angle-axis conversion
  AngleAxisx aa = q1;
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);

  // from two vector creation
  VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
  VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());

  // inverse and conjugate
  VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
  VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);

  // AngleAxis
  VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
    Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix());

  AngleAxisx aa1;
  m = q1.toRotationMatrix();
  aa1 = m;
  VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(),
    Quaternionx(m).toRotationMatrix());

  // Transform
  // TODO complete the tests !
  a = 0;
  while (ei_abs(a)<Scalar(0.1))
    a = ei_random<Scalar>(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI));
  q1 = AngleAxisx(a, v0.normalized());
  Transform3 t0, t1, t2;
  // first test setIdentity() and Identity()
  t0.setIdentity();
  VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
  t0.matrix().setZero();
  t0 = Transform3::Identity();
  VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());

  t0.linear() = q1.toRotationMatrix();
  t1.setIdentity();
  t1.linear() = q1.toRotationMatrix();

  v0 << 50, 2, 1;//= ei_random_matrix<Vector3>().cwiseProduct(Vector3(10,2,0.5));
  t0.scale(v0);
  t1.prescale(v0);

  VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x());
  //VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x()));

  t0.setIdentity();
  t1.setIdentity();
  v1 << 1, 2, 3;
  t0.linear() = q1.toRotationMatrix();
  t0.pretranslate(v0);
  t0.scale(v1);
  t1.linear() = q1.conjugate().toRotationMatrix();
  t1.prescale(v1.cwise().inverse());
  t1.translate(-v0);

  VERIFY((t0.matrix() * t1.matrix()).isIdentity(test_precision<Scalar>()));

  t1.fromPositionOrientationScale(v0, q1, v1);
  VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
  VERIFY_IS_APPROX(t1*v1, t0*v1);

  t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix());
  t1.setIdentity(); t1.scale(v0).rotate(q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix());
  VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix());

  // More transform constructors, operator=, operator*=

  Matrix3 mat3 = Matrix3::Random();
  Matrix4 mat4;
  mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
  Transform3 tmat3(mat3), tmat4(mat4);
  tmat4.matrix()(3,3) = Scalar(1);
  VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix());

  Scalar a3 = ei_random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
  Vector3 v3 = Vector3::Random().normalized();
  AngleAxisx aa3(a3, v3);
  Transform3 t3(aa3);
  Transform3 t4;
  t4 = aa3;
  VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
  t4.rotate(AngleAxisx(-a3,v3));
  VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
  t4 *= aa3;
  VERIFY_IS_APPROX(t3.matrix(), t4.matrix());

  v3 = Vector3::Random();
  Translation3 tv3(v3);
  Transform3 t5(tv3);
  t4 = tv3;
  VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
  t4.translate(-v3);
  VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
  t4 *= tv3;
  VERIFY_IS_APPROX(t5.matrix(), t4.matrix());

  Scaling3 sv3(v3);
  Transform3 t6(sv3);
  t4 = sv3;
  VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
  t4.scale(v3.cwise().inverse());
  VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
  t4 *= sv3;
  VERIFY_IS_APPROX(t6.matrix(), t4.matrix());

  // matrix * transform
  VERIFY_IS_APPROX(Transform3(t3.matrix()*t4).matrix(), Transform3(t3*t4).matrix());

  // chained Transform product
  VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix());

  // check that Transform product doesn't have aliasing problems
  t5 = t4;
  t5 = t5*t5;
  VERIFY_IS_APPROX(t5, t4*t4);

  // 2D transformation
  Transform2 t20, t21;
  Vector2 v20 = Vector2::Random();
  Vector2 v21 = Vector2::Random();
  for (int k=0; k<2; ++k)
    if (ei_abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3);
  t21.setIdentity();
  t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix();
  VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
    t21.pretranslate(v20).scale(v21).matrix());

  t21.setIdentity();
  t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix();
  VERIFY( (t20.fromPositionOrientationScale(v20,a,v21)
        * (t21.prescale(v21.cwise().inverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) );

  // Transform - new API
  // 3D
  t0.setIdentity();
  t0.rotate(q1).scale(v0).translate(v0);
  // mat * scaling and mat * translation
  t1 = (Matrix3(q1) * Scaling3(v0)) * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // mat * transformation and scaling * translation
  t1 = Matrix3(q1) * (Scaling3(v0) * Translation3(v0));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity();
  t0.prerotate(q1).prescale(v0).pretranslate(v0);
  // translation * scaling and transformation * mat
  t1 = (Translation3(v0) * Scaling3(v0)) * Matrix3(q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // scaling * mat and translation * mat
  t1 = Translation3(v0) * (Scaling3(v0) * Matrix3(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity();
  t0.scale(v0).translate(v0).rotate(q1);
  // translation * mat and scaling * transformation
  t1 = Scaling3(v0) * (Translation3(v0) * Matrix3(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // transformation * scaling
  t0.scale(v0);
  t1 = t1 * Scaling3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // transformation * translation
  t0.translate(v0);
  t1 = t1 * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // translation * transformation
  t0.pretranslate(v0);
  t1 = Translation3(v0) * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // transform * quaternion
  t0.rotate(q1);
  t1 = t1 * q1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // translation * quaternion
  t0.translate(v1).rotate(q1);
  t1 = t1 * (Translation3(v1) * q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // scaling * quaternion
  t0.scale(v1).rotate(q1);
  t1 = t1 * (Scaling3(v1) * q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * transform
  t0.prerotate(q1);
  t1 = q1 * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * translation
  t0.rotate(q1).translate(v1);
  t1 = t1 * (q1 * Translation3(v1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * scaling
  t0.rotate(q1).scale(v1);
  t1 = t1 * (q1 * Scaling3(v1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // translation * vector
  t0.setIdentity();
  t0.translate(v0);
  VERIFY_IS_APPROX(t0 * v1, Translation3(v0) * v1);

  // scaling * vector
  t0.setIdentity();
  t0.scale(v0);
  VERIFY_IS_APPROX(t0 * v1, Scaling3(v0) * v1);

  // test transform inversion
  t0.setIdentity();
  t0.translate(v0);
  t0.linear().setRandom();
  VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse());
  t0.setIdentity();
  t0.translate(v0).rotate(q1);
  VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse());

  // test extract rotation and scaling
  t0.setIdentity();
  t0.translate(v0).rotate(q1).scale(v1);
  VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1);

  Matrix3 mat_rotation, mat_scaling;
  t0.setIdentity();
  t0.translate(v0).rotate(q1).scale(v1);
  t0.computeRotationScaling(&mat_rotation, &mat_scaling);
  VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
  t0.computeScalingRotation(&mat_scaling, &mat_rotation);
  VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));

  // test casting
  eigen2_Transform<float,3> t1f = t1.template cast<float>();
  VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1);
  eigen2_Transform<double,3> t1d = t1.template cast<double>();
  VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1);

  Translation3 tr1(v0);
  eigen2_Translation<float,3> tr1f = tr1.template cast<float>();
  VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1);
  eigen2_Translation<double,3> tr1d = tr1.template cast<double>();
  VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1);

  Scaling3 sc1(v0);
  eigen2_Scaling<float,3> sc1f = sc1.template cast<float>();
  VERIFY_IS_APPROX(sc1f.template cast<Scalar>(),sc1);
  eigen2_Scaling<double,3> sc1d = sc1.template cast<double>();
  VERIFY_IS_APPROX(sc1d.template cast<Scalar>(),sc1);

  eigen2_Quaternion<float> q1f = q1.template cast<float>();
  VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
  eigen2_Quaternion<double> q1d = q1.template cast<double>();
  VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);

  eigen2_AngleAxis<float> aa1f = aa1.template cast<float>();
  VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1);
  eigen2_AngleAxis<double> aa1d = aa1.template cast<double>();
  VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1);

  eigen2_Rotation2D<Scalar> r2d1(ei_random<Scalar>());
  eigen2_Rotation2D<float> r2d1f = r2d1.template cast<float>();
  VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
  eigen2_Rotation2D<double> r2d1d = r2d1.template cast<double>();
  VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);

  m = q1;
//   m.col(1) = Vector3(0,ei_random<Scalar>(),ei_random<Scalar>()).normalized();
//   m.col(0) = Vector3(-1,0,0).normalized();
//   m.col(2) = m.col(0).cross(m.col(1));
  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Vector3 ea = m.eulerAngles(I,J,K); \
    Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
    VERIFY_IS_APPROX(m, m1); \
    VERIFY_IS_APPROX(m,  Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
  VERIFY_EULER(0,1,0, X,Y,X);
  VERIFY_EULER(0,2,1, X,Z,Y);
  VERIFY_EULER(0,2,0, X,Z,X);

  VERIFY_EULER(1,2,0, Y,Z,X);
  VERIFY_EULER(1,2,1, Y,Z,Y);
  VERIFY_EULER(1,0,2, Y,X,Z);
  VERIFY_EULER(1,0,1, Y,X,Y);

  VERIFY_EULER(2,0,1, Z,X,Y);
  VERIFY_EULER(2,0,2, Z,X,Z);
  VERIFY_EULER(2,1,0, Z,Y,X);
  VERIFY_EULER(2,1,2, Z,Y,Z);

  // colwise/rowwise cross product
  mat3.setRandom();
  Vector3 vec3 = Vector3::Random();
  Matrix3 mcross;
  int i = ei_random<int>(0,2);
  mcross = mat3.colwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
  mcross = mat3.rowwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));

}

void test_eigen2_geometry_with_eigen2_prefix()
{
  std::cout << "eigen2 support: " << EIGEN2_SUPPORT_STAGE << std::endl;
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( geometry<float>() );
    CALL_SUBTEST_2( geometry<double>() );
  }
}


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

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

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

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

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

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

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

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

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

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

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

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

// We initialize a null gradient

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


/*****************************************************************************
*
* Copyright (C) 2011-2016 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

// Calculating free energy density of square lattice Ising model

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

#ifndef ISING_SQUARE_FINITE_HPP
#define ISING_SQUARE_FINITE_HPP

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

// namespace {

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

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

// }

namespace ising {
namespace square {

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

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

}

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

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

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

#endif // ISING_SQUARE_FINITE_HPP


#include <cctbx/boost_python/flex_fwd.h>

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

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

namespace indexing_api{

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

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

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

  The derivative of the cdf with respect to sigma is,

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

};

}

BOOST_PYTHON_MODULE(rstbx_indexing_api_ext)
{

   def("cpp_absence_test",cpp_absence_test);

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

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

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


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

struct Generator1D {
  Generator1D() { }

  float operator()(const array<Eigen::DenseIndex, 1>& coordinates) const {
    return coordinates[0];
  }
};

template <int DataLayout>
static void test_1D()
{
  Tensor<float, 1> vec(6);
  Tensor<float, 1> result = vec.generate(Generator1D());

  for (int i = 0; i < 6; ++i) {
    VERIFY_IS_EQUAL(result(i), i);
  }
}

struct Generator2D {
  Generator2D() { }

  float operator()(const array<Eigen::DenseIndex, 2>& coordinates) const {
    return 3 * coordinates[0] + 11 * coordinates[1];
  }
};

template <int DataLayout>
static void test_2D()
{
  Tensor<float, 2> matrix(5, 7);
  Tensor<float, 2> result = matrix.generate(Generator2D());

  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 5; ++j) {
      VERIFY_IS_EQUAL(result(i, j), 3*i + 11*j);
    }
  }
}

template <int DataLayout>
static void test_gaussian()
{
  int rows = 32;
  int cols = 48;
  array<float, 2> means;
  means[0] = rows / 2.0f;
  means[1] = cols / 2.0f;
  array<float, 2> std_devs;
  std_devs[0] = 3.14f;
  std_devs[1] = 2.7f;
  internal::GaussianGenerator<float, Eigen::DenseIndex, 2> gaussian_gen(means, std_devs);

  Tensor<float, 2> matrix(rows, cols);
  Tensor<float, 2> result = matrix.generate(gaussian_gen);

  for (int i = 0; i < rows; ++i) {
    for (int j = 0; j < cols; ++j) {
      float g_rows = powf(rows/2.0f - i, 2) / (3.14f * 3.14f) * 0.5f;
      float g_cols = powf(cols/2.0f - j, 2) / (2.7f * 2.7f) * 0.5f;
      float gaussian = expf(-g_rows - g_cols);
      VERIFY_IS_EQUAL(result(i, j), gaussian);
    }
  }
}

void test_cxx11_tensor_generator()
{
  CALL_SUBTEST(test_1D<ColMajor>());
  CALL_SUBTEST(test_1D<RowMajor>());
  CALL_SUBTEST(test_2D<ColMajor>());
  CALL_SUBTEST(test_2D<RowMajor>());
  CALL_SUBTEST(test_gaussian<ColMajor>());
  CALL_SUBTEST(test_gaussian<RowMajor>());
}


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/mpl/list.hpp>

#include <ORUtils/SE3Pose.h>

#include <orx/geometry/DualQuaternion.h>
#include <orx/geometry/GeometryUtil.h>

#include "HelperFunctions.h"

using namespace ORUtils;
using namespace orx;

//#################### TESTS ####################

typedef boost::mpl::list<double,float> TS;

BOOST_AUTO_TEST_SUITE(test_DualQuaternion)

BOOST_AUTO_TEST_CASE_TEMPLATE(test_apply, T, TS)
{
  const T TOL = static_cast<T>(1e-4);

  DualQuaternion<T> rot = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), -T(M_PI_2));
  DualQuaternion<T> trans = DualQuaternion<T>::from_translation(Vector3<T>(1,2,3));
  DualQuaternion<T> dq = trans * rot;

  check_close(dq.apply(Vector3<T>(1,0,0)), Vector3<T>(1,1,3), TOL);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_from_point, T, TS)
{
  DualQuaternion<T> dq = DualQuaternion<T>::from_point(Vector3<T>(3,4,5));
  BOOST_CHECK(DualQuaternion<T>::close(dq.conjugate(), DualQuaternion<T>(DualNumber<T>(1,0), DualNumber<T>(0,-3), DualNumber<T>(0,-4), DualNumber<T>(0,-5))));
  BOOST_CHECK(DualNumber<T>::close(dq.norm(), DualNumber<T>(1,0)));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_from_rotation, T, TS)
{
  Vector3<T> axis(0,1,0);
  DualQuaternion<T> dq = DualQuaternion<T>::from_rotation(axis, T(M_PI_4));

  BOOST_CHECK_SMALL(length(dq.get_rotation() - axis * T(M_PI_4)), T(1e-4));
  BOOST_CHECK(DualQuaternion<T>::close(dq.get_rotation_part(), dq));
  BOOST_CHECK_SMALL(length(dq.get_translation()), T(1e-4));

  Vector3<T> v(1,0,0);
  Vector3<T> w1 = dq.apply(v);

  SE3Pose pose(0, 0, 0, 0, static_cast<float>(M_PI_4), 0);
  Vector3f r = GeometryUtil::to_rotation_vector(pose.GetR());
  Vector3<T> w2 = DualQuaternion<T>::from_rotation(r).apply(v);

  BOOST_CHECK_SMALL(length(w2 - w1), T(1e-4));

  DualQuaternion<T> expected = DualQuaternion<T>(T(0.707107), T(0), T(0), T(0.707107));

  dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(dq, expected));

  dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,-1), -T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(dq, expected));

  dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,2), T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(dq, expected));

  expected = DualQuaternion<T>(T(0.707107), T(0), T(0), -T(0.707107));

  dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), -T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(dq, expected));

  dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,-1), T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(dq, expected));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_from_se3, T, TS)
{
  DualQuaternion<T> rot = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  DualQuaternion<T> trans = DualQuaternion<T>::from_translation(Vector3<T>(3,4,5));
  Vector3<T> v(1,0,0);

  BOOST_CHECK(DualQuaternion<T>::close(rot, DualQuaternion<T>(DualNumber<T>(0.707107f,0), DualNumber<T>(0,0), DualNumber<T>(0,0), DualNumber<T>(0.707107f,0))));
  BOOST_CHECK(DualQuaternion<T>::close(trans, DualQuaternion<T>(DualNumber<T>(1,0), DualNumber<T>(0,1.5f), DualNumber<T>(0,2), DualNumber<T>(0,2.5f))));
  BOOST_CHECK(DualQuaternion<T>::close(trans * rot, DualQuaternion<T>(DualNumber<T>(0.707107f,-1.76777f), DualNumber<T>(0,2.47487f), DualNumber<T>(0,0.353553f), DualNumber<T>(0.707107f,1.76777f))));
  BOOST_CHECK_SMALL(length(rot.apply(v) - Vector3<T>(0,1,0)), T(1e-4));
  BOOST_CHECK_SMALL(length(trans.apply(v) - Vector3<T>(4,4,5)), T(1e-4));
  BOOST_CHECK_SMALL(length((trans * rot).apply(v) - Vector3<T>(3,5,5)), T(1e-4));
  BOOST_CHECK_SMALL(length((rot * trans).apply(v) - Vector3<T>(-4,4,5)), T(1e-4));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_from_translation, T, TS)
{
  Vector3<T> v(1,2,3), t(3,4,5);
  DualQuaternion<T> dq = DualQuaternion<T>::from_translation(t);
  BOOST_CHECK_SMALL(length(dq.apply(v) - (v + t)), T(1e-4));
  BOOST_CHECK_SMALL(length(dq.get_rotation()), T(1e-4));
  BOOST_CHECK(DualQuaternion<T>::close(dq.get_rotation_part(), DualQuaternion<T>::identity()));
  BOOST_CHECK_SMALL(length(dq.get_translation() - t), T(1e-4));
  BOOST_CHECK(DualQuaternion<T>::close(dq.get_translation_part(), dq));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_linear_blend, T, TS)
{
  DualQuaternion<T> p = DualQuaternion<T>::from_translation(Vector3<T>(2,3,4));
  DualQuaternion<T> q = DualQuaternion<T>::from_translation(Vector3<T>(3,4,4)) * DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  Vector3<T> v(1,0,0);

  std::vector<DualQuaternion<T> > dqs;
  dqs.push_back(p);
  dqs.push_back(q);

  std::vector<T> weights(2);

  weights[0] = 1.0f, weights[1] = 0.0f;
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::linear_blend(&dqs[0], &weights[0], static_cast<int>(dqs.size())).apply(v) - Vector3<T>(3,3,4)), T(1e-4));

  weights[0] = 0.5f, weights[1] = 0.5f;
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::linear_blend(&dqs[0], &weights[0], static_cast<int>(dqs.size())).apply(v) - Vector3<T>(3.41421f,4,4)), T(1e-4));

  weights[0] = 0.0f, weights[1] = 1.0f;
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::linear_blend(&dqs[0], &weights[0], static_cast<int>(dqs.size())).apply(v) - Vector3<T>(3,5,4)), T(1e-4));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_pow, T, TS)
{
  DualQuaternion<T> rot = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  DualQuaternion<T> tripleRot = rot.pow(3);
  Vector3<T> v(1,0,0);
  BOOST_CHECK_SMALL(length(tripleRot.apply(v) - Vector3<T>(0,-1,0)), T(1e-4));

  DualQuaternion<T> trans = DualQuaternion<T>::from_translation(Vector3<T>(3,4,5));
  DualQuaternion<T> tr = trans * rot;
  BOOST_CHECK_SMALL(length(tr.pow(3).apply(v) - (tr * tr * tr).apply(v)), T(1e-4));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_sclerp, T, TS)
{
  DualQuaternion<T> p = DualQuaternion<T>::from_translation(Vector3<T>(2,3,4));
  DualQuaternion<T> q = DualQuaternion<T>::from_translation(Vector3<T>(3,4,4)) * DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  BOOST_CHECK(DualQuaternion<T>::close(p, DualQuaternion<T>(DualNumber<T>(1,0), DualNumber<T>(0,1), DualNumber<T>(0,1.5f), DualNumber<T>(0,2))));
  BOOST_CHECK(DualQuaternion<T>::close(q, DualQuaternion<T>(DualNumber<T>(0.707107f,-1.41421f), DualNumber<T>(0,2.47487f), DualNumber<T>(0,0.353553f), DualNumber<T>(0.707107f,1.41421f))));

  Vector3<T> v(1,0,0);
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::sclerp(p, q, 0.0).apply(v) - Vector3<T>(3,3,4)), T(1e-4));
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::sclerp(p, q, 0.5).apply(v) - Vector3<T>(3.41421f,4,4)), T(1e-4));
  BOOST_CHECK_SMALL(length(DualQuaternion<T>::sclerp(p, q, 1.0).apply(v) - Vector3<T>(3,5,4)), T(1e-4));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_screw, T, TS)
{
  DualQuaternion<T> rot = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));
  DualQuaternion<T> trans = DualQuaternion<T>::from_translation(Vector3<T>(3,4,5));
  DualQuaternion<T> tr = trans * rot;
  BOOST_CHECK(DualQuaternion<T>::close(DualQuaternion<T>::from_screw(tr.to_screw()), tr));
}

BOOST_AUTO_TEST_SUITE_END()


#include "conex/block_triangular_operations.h"
#include "conex/supernodal_solver.h"

#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using T = TriangularMatrixOperations;
using B = BlockTriangularOperations;

int GetMax(const std::vector<Clique>& cliques) {
  int max = cliques.at(0).at(0);
  for (const auto& c : cliques) {
    for (const auto ci : c) {
      if (ci > max) {
        max = ci;
      }
    }
  }
  return max;
}

void DoCholeskyTest(const std::vector<Clique>& cliques) {
  auto mat = GetFillInPattern(GetMax(cliques) + 1, cliques);
  for (auto& sn : mat.supernodes) {
    sn.diagonal().array() += 100;
  }

  Eigen::MatrixXd x = T::ToDense(mat);
  Eigen::LLT<MatrixXd> llt(x);
  MatrixXd L = llt.matrixL();
  EXPECT_TRUE(llt.info() == Eigen::Success);

  B::BlockCholeskyInPlace(&mat.workspace_);
  MatrixXd error = T::ToDense(mat) - L;
  error = error.triangularView<Eigen::Lower>();
  EXPECT_NEAR(error.norm(), 0, 1e-12);
}

TEST(LowerTri, Cholesky) {
  DoCholeskyTest({{0, 1, 2}, {2}});
  DoCholeskyTest({{0, 1, 2, 4, 7}, {3, 4}, {5, 6, 7}});
  DoCholeskyTest({{0, 1, 5}, {1, 2, 5}, {3, 4, 5}});
  DoCholeskyTest({{0, 1, 2}, {1, 2, 3}, {3, 4, 2}});
  DoCholeskyTest({{0, 1}, {2, 4}, {3, 4}, {5, 6, 7}, {7, 8, 9, 10}});
}

void DoInverseTest(const std::vector<Clique>& cliques) {
  auto mat = GetFillInPattern(GetMax(cliques) + 1, cliques);
  for (auto& sn : mat.supernodes) {
    sn.diagonal().array() += 10;
  }

  Eigen::MatrixXd L = T::ToDense(mat).triangularView<Eigen::Lower>();
  Eigen::VectorXd b;
  b.setLinSpaced(L.rows(), -1, 1);

  Eigen::VectorXd y2 = b;
  B::ApplyBlockInverseInPlace(mat.workspace_, &y2);
  EXPECT_NEAR((L * y2 - b).norm(), 0, 1e-12);
}

TEST(LowerTri, InverseTest) {
  DoInverseTest({{0, 1, 2, 3}, {3, 4, 5}});
  DoInverseTest({{0, 1, 2, 3}});
  DoInverseTest({{0, 1, 2, 3}, {3, 4}, {4, 5, 6}});
}

void DoInverseOfTransposeTest(const std::vector<Clique>& cliques) {
  auto mat = GetFillInPattern(GetMax(cliques) + 1, cliques);
  for (auto& sn : mat.supernodes) {
    sn.diagonal().array() += 10;
  }

  Eigen::MatrixXd L = T::ToDense(mat).triangularView<Eigen::Lower>();
  Eigen::VectorXd b;
  b.setLinSpaced(L.rows(), -1, 1);

  Eigen::VectorXd y2 = b;
  B::ApplyBlockInverseOfTransposeInPlace(mat.workspace_, &y2);
  EXPECT_NEAR((L.transpose() * y2 - b).norm(), 0, 1e-12);
}

TEST(LowerTri, InverseOfTranspose) {
  DoInverseOfTransposeTest({{0, 1, 2, 5}, {3, 4, 5}});
  DoInverseOfTransposeTest({{0, 1, 2, 5}, {3, 4, 5}, {5, 6}});
  DoInverseOfTransposeTest({{0, 1, 2, 3}});
}

MatrixXd Submatrix(const MatrixXd& T, const Clique& c) {
  MatrixXd y(c.size(), c.size());
  int i = 0;
  for (auto ci : c) {
    int j = 0;
    for (auto cj : c) {
      y(i, j) = T(ci, cj);
      j++;
    }
    i++;
  }
  return y;
}

void DoLDLTTest(bool diagonal, const std::vector<Clique>& cliques) {
  auto mat = GetFillInPattern(GetMax(cliques) + 1, cliques);

  // Set to identity.
  for (auto& sn : mat.workspace_.diagonal) {
    if (diagonal) {
      sn.setZero();
    }
    int n = sn.diagonal().size();
    for (int i = 0; i < n; i++) {
      sn.diagonal()(i) = -101 + i * 100;
    }
  }

  if (diagonal) {
    for (auto& sn : mat.workspace_.off_diagonal) {
      sn.setZero();
    }
  }

  Eigen::MatrixXd X = T::ToDense(mat).selfadjointView<Eigen::Lower>();

  std::vector<Eigen::LDLT<Eigen::Ref<MatrixXd>>> factorization;
  B::BlockLDLTInPlace(&mat.workspace_, &factorization);

  Eigen::VectorXd z = Eigen::VectorXd::Random(X.cols());
  z.setConstant(0);
  z(1) = 1;

  Eigen::VectorXd y = X * z;
  // X = M D M ^T z = y
  // z = inv(M^{T}) (MD)^{-1} y
  B::ApplyBlockInverseOfMD(mat.workspace_, factorization, &y);
  B::ApplyBlockInverseOfMTranspose(mat.workspace_, factorization, &y);
  EXPECT_NEAR((z - y).norm(), 0, 1e-12);
}

TEST(LowerTri, LDLT) {
  bool diagonal = true;
  DoLDLTTest(diagonal, {{0, 1}});
  DoLDLTTest(diagonal, {{0, 1, 2}, {2}});
  DoLDLTTest(diagonal, {{0, 1, 2, 4, 7}, {3, 4}, {5, 6, 7}});
  DoLDLTTest(diagonal, {{0, 1, 5}, {1, 2, 5}, {3, 4, 5}});
  DoLDLTTest(diagonal, {{0, 1, 2}, {1, 2, 3}, {3, 4, 2}});
  DoLDLTTest(diagonal, {{0, 1}, {2, 4}, {3, 4}, {5, 6, 7}, {7, 8, 9, 10}});

  diagonal = false;
  DoLDLTTest(diagonal, {{0, 1, 2}, {2}});
  DoLDLTTest(diagonal, {{0, 1, 2, 4, 7}, {3, 4}, {5, 6, 7}});
  DoLDLTTest(diagonal, {{0, 1, 5}, {1, 2, 5}, {3, 4, 5}});
  DoLDLTTest(diagonal, {{0, 1, 2}, {1, 2, 3}, {3, 4, 2}});
  DoLDLTTest(diagonal, {{0, 1}, {2, 4}, {3, 4}, {5, 6, 7}, {7, 8, 9, 10}});
}

}  // namespace conex


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_signed_distance_to_vertex_edge_voronoi_plane.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_signed_distance_to_vertex_edge_vp);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  vector3_type a = vector3_type(1.0, 0.0, 0.0);
  vector3_type b = vector3_type(0.0, 0.0, 0.0);

  // First we use a test point that does not lie on the line

  // Front side of A voronoi plane
  {
    vector3_type p = vector3_type( 2.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of A voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In A voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Front side of B voronoi plane
  {
    vector3_type p = vector3_type( -1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of B voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In B voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Second we use a test point that lies on the line

  // Front side of A voronoi plane
  {
    vector3_type p = vector3_type( 2.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of A voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In A voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Front side of B voronoi plane
  {
    vector3_type p = vector3_type( -1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of B voronoi plane
  {
    vector3_type p = vector3_type( 1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In B voronoi plane
  {
    vector3_type p = vector3_type( 0.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = OpenTissue::gjk::detail::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


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

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

using namespace std;
using namespace Eigen;

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

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

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

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

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

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

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

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

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

	return 0;
}


/**
 * @file
 *
 * @copyright
 * SPDX-License-Identifier: Apache-2.0
 *
 * @test @b eigen_gemm_cdouble_dynamic_square
 * @parblock
 * This piece of code aims to stress test the exec (in particular FMA)
 * by repetitively solving general matrix matrix multiplication.  The
 * multiplication function is from the 3rd party library Eigen.  A
 * pair of random matrices are generated as inputs and then multiplied
 * using Eigen's gemm. The multiplication result is compared against a
 * golden result that is computed during init.  This particular
 * version of the test goes for double precision, complex numbers
 * input matrices.  Although the test should run fine on a single
 * thread, it is only expected to catch defects if run on at least 2
 * cores.
 *
 * @note This test requires at least 2 threads to run.
 * @endparblock
 */

#include <sandstone.h>

#include <Eigen/Core>
using namespace Eigen;

#define M_DIM 221 // weird dim on purpose

typedef Matrix < std::complex < double >, Dynamic, Dynamic > Mat;

namespace {
struct eigen_test_data {
    Mat lhs;
    Mat rhs;
    Mat prod;
};
}

#define CAST(_x) static_cast<struct eigen_test_data *>(_x)

static int eigen_gemm_cdouble_dynamic_square_init(struct test *test) {
    test->data = new(eigen_test_data);
    try {
        CAST(test->data)->lhs = Mat::Random(M_DIM, M_DIM);
        CAST(test->data)->rhs = Mat::Random(M_DIM, M_DIM);
        CAST(test->data)->prod = CAST(test->data)->lhs * CAST(test->data)->rhs;
    } catch (...) {
        report_fail_msg("Exception on Eigen code, most probably OOM");
    }
    return EXIT_SUCCESS;
}

static int eigen_gemm_cdouble_dynamic_square_run(struct test *test, int cpu) {
    //int i=0;
    do {
        //++i;
        auto testdata = CAST(test->data);
        Mat x;
        x = testdata->lhs * testdata->rhs;

        memcmp_or_fail(reinterpret_cast<double *>(x.data()),
                       reinterpret_cast<double *>(testdata->prod.data()), 2 * M_DIM * M_DIM);
    } while (test_time_condition(test));
    //log_info("Num iters = %i\n", i);
    return EXIT_SUCCESS;
}

static int eigen_gemm_cdouble_dynamic_square_finish(struct test *test) {
    delete(CAST(test->data));
    return EXIT_SUCCESS;
}

DECLARE_TEST(eigen_gemm_cdouble_dynamic_square, "Eigen GEMM payload (cplx double, dynamic, square)")
  .groups = DECLARE_TEST_GROUPS(&group_math),
  .test_init = eigen_gemm_cdouble_dynamic_square_init,
  .test_run = eigen_gemm_cdouble_dynamic_square_run,
  .test_cleanup = eigen_gemm_cdouble_dynamic_square_finish,
  .fracture_loop_count = 5,
  .quality_level = TEST_QUALITY_PROD,
END_DECLARE_TEST


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "zero_weighted_alpha_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epeck_d.h>

#include <vector>
#include <random>
#include <cmath> // for std::fabs

#include <gudhi/Alpha_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Unitary_tests_utils.h>

using list_of_exact_kernel_variants = boost::mpl::list<CGAL::Epeck_d< CGAL::Dynamic_dimension_tag >,
                                                       CGAL::Epeck_d< CGAL::Dimension_tag<4> >
                                                       > ;

BOOST_AUTO_TEST_CASE_TEMPLATE(Zero_weighted_alpha_complex, Kernel, list_of_exact_kernel_variants) {
  // Check that in exact mode for static dimension 4 the code for dD unweighted and for dD weighted with all weights
  // 0 give exactly the same simplex tree (simplices and filtration values).

  // Random points construction
  using Point_d = typename Kernel::Point_d;
  std::vector<Point_d> points;
  std::uniform_real_distribution<double> rd_pts(-10., 10.);
  std::random_device rand_dev;
  std::mt19937 rand_engine(rand_dev());
  for (int idx = 0; idx < 20; idx++) {
    std::vector<double> point {rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine)};
    points.emplace_back(point.begin(), point.end());
  }

  // Alpha complex from points
  Gudhi::alpha_complex::Alpha_complex<Kernel, false> alpha_complex_from_points(points);
  Gudhi::Simplex_tree<> simplex;
  Gudhi::Simplex_tree<>::Filtration_value infty = std::numeric_limits<Gudhi::Simplex_tree<>::Filtration_value>::infinity();
  BOOST_CHECK(alpha_complex_from_points.create_complex(simplex, infty, true));
  std::clog << "Iterator on alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : simplex.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : simplex.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << simplex.filtration(f_simplex) << "] " << std::endl;
  }

  // Alpha complex from zero weighted points
  std::vector<typename Kernel::FT> weights(20, 0.);
  Gudhi::alpha_complex::Alpha_complex<Kernel, true> alpha_complex_from_zero_weighted_points(points, weights);
  Gudhi::Simplex_tree<> zw_simplex;
  BOOST_CHECK(alpha_complex_from_zero_weighted_points.create_complex(zw_simplex, infty, true));

  std::clog << "Iterator on zero weighted alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : zw_simplex.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : zw_simplex.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << zw_simplex.filtration(f_simplex) << "] " << std::endl;
  }

  BOOST_CHECK(zw_simplex == simplex);
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE mf test
#include <boost/test/unit_test.hpp>

#include <mmf/mfPrior.hpp>
#include <mmf/mf.hpp>

BOOST_AUTO_TEST_CASE(mf_test)
{
  boost::mt19937 rndGen(1);
  double nu = 4;
  MatrixXd Delta = MatrixXd::Identity(2,2);
  Delta *= pow(5.0*M_PI/180.,2)*nu;
  IW<double> iw0(Delta,nu,&rndGen);
  std::vector<shared_ptr<BaseMeasure<double> > >thetas;
  for(uint32_t k=0; k<6; ++k)
    thetas.push_back(shared_ptr<IwTangent<double> >(
          new IwTangent<double>(iw0,&rndGen)));
  VectorXd alpha = VectorXd::Ones(6);
  Dir<Cat<double>, double> dir(alpha,&rndGen);
  DirMM<double> dirMM(dir,thetas);
  MfPrior<double> mfPrior(dirMM,10);

  MatrixXd Sigma = MatrixXd::Identity(2,2);
  Sigma *= pow(1.0*M_PI/180.,2);
  uint32_t N = 100;
  MatrixXd x(3,N*6);
  MatrixXd R(3,3);
  double theta = 30.*M_PI/180.;
  R<<cos(theta), -sin(theta), 0.,
     sin(theta),cos(theta) ,0.,
     0,0,1.;
  cout<<R<<endl;
  cout<<" ..................... "<<endl;
  for(uint32_t k=0; k<6; ++k)
  {
    NormalSphere<double> g(R*mfPrior.M().col(k),Sigma,&rndGen);
    for(uint32_t i =0; i< N; ++i)
      x.col(i+N*k) = g.sample();
  }
  VectorXu z = VectorXu::Zero(N*6);
  MF<double> mf = mfPrior.posteriorSample(x,z,0);

  mf.print();

  for(uint32_t i =0; i< N*6; i+=N)
  {
    cout<<mf.logPdf(x.col(i))<<endl;
  }
  MF<double> mf2(mf);
  mf2.print();
};


// Array DAXPY benchmark

#include <blitz/benchext.h>
#include <blitz/array.h>
#include <blitz/vector2.h>
#include <random/uniform.h>

BZ_NAMESPACE(blitz)
extern void sink();
BZ_NAMESPACE_END

BZ_USING_NAMESPACE(blitz)

#ifdef BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES
  #define arrdaxpyf arrdaxpyf_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
  #define arrdaxpyf arrdaxpyf__
#endif

extern "C" {
    void arrdaxpyf(double* A, double* B, int& N, double& a);
}

void arrdaxpyFortran77Version(BenchmarkExt<int>& bench);
void arrdaxpyBlitzVersion(BenchmarkExt<int>& bench);

int main()
{
    BenchmarkExt<int> bench("Array DAXPY", 2);

    const int numSizes = 8;

    bench.setNumParameters(numSizes);
    bench.setDependentVariable("flops");

    Vector<int> parameters(numSizes);
    Vector<long> iters(numSizes);
    Vector<double> flops(numSizes);

    parameters = pow(2.,tensor::i);
    cout << parameters;
    iters = 100*16*32*8*8*8/pow3(parameters);
    cout << iters;
    flops = pow3(parameters) * 2 * 2;
    cout << flops;

    bench.setParameterVector(parameters);
    bench.setParameterDescription("3D Array size");
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);

    bench.beginBenchmarking();
    arrdaxpyBlitzVersion(bench);
    arrdaxpyFortran77Version(bench);
    bench.endBenchmarking();

    bench.saveMatlabGraph("arrdaxpy.m");

    return 0;
}

void initializeRandomDouble(double* data, int numElements)
{
  ranlib::Uniform<double> rnd;

    for (int i=0; i < numElements; ++i)
        data[i] = rnd.random();
}

void arrdaxpyBlitzVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N);
        initializeRandomDouble(B.data(), N*N*N);
        TinyVector<int,2> size = N-2;
        double a = 0.34928313;
        double b = - a;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            A += a * B;
            A += b * B;
	    sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}
        bench.stopOverhead();
    }

    bench.endImplementation();
}

void arrdaxpyFortran77Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 77: N = " << N << endl;
        cout.flush();

        int iters = (int)bench.getIterations();

        size_t arraySize = size_t(N) * size_t(N) * N;

        double* A = new double[arraySize];
        double* B = new double[arraySize];

        initializeRandomDouble(A, arraySize);
        initializeRandomDouble(B, arraySize);

        double a = 0.34928313;

        for (long i=0; i < iters; ++i)
        {
	  arrdaxpyf(A,B,N,a);
	  sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}
        bench.stopOverhead();

        delete [] A;
        delete [] B;
    }

    bench.endImplementation();
}