#ifndef STAN_MATH_PRIM_FUN_GAMMA_P_HPP
#define STAN_MATH_PRIM_FUN_GAMMA_P_HPP

#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/boost_policy.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/is_nan.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 * Return the value of the normalized, lower-incomplete gamma function
 * applied to the specified argument.
 *
 * <p>This function is defined, including error conditions, as follows
   \f[
   \mbox{gamma\_p}(a, z) =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     P(a, z) & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_p}(a, z)}{\partial a} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, P(a, z)}{\partial a} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_p}(a, z)}{\partial z} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, P(a, z)}{\partial z} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   P(a, z)=\frac{1}{\Gamma(a)}\int_0^zt^{a-1}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, P(a, z)}{\partial a} =
 -\frac{\Psi(a)}{\Gamma^2(a)}\int_0^zt^{a-1}e^{-t}dt
   + \frac{1}{\Gamma(a)}\int_0^z (a-1)t^{a-2}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, P(a, z)}{\partial z} = \frac{z^{a-1}e^{-z}}{\Gamma(a)}
   \f]
   *
   * @param z first argument
   * @param a second argument
   * @return value of the normalized, lower-incomplete gamma function
   * applied to z and a
   * @throws std::domain_error if either argument is not positive or
   * if z is at a pole of the function
 */
inline double gamma_p(double z, double a) {
  if (is_nan(z)) {
    return not_a_number();
  }
  if (is_nan(a)) {
    return not_a_number();
  }
  check_positive("gamma_p", "first argument (z)", z);
  check_nonnegative("gamma_p", "second argument (a)", a);
  return boost::math::gamma_p(z, a, boost_policy_t<>());
}

/**
 * Enables the vectorised application of the gamma_p function,
 * when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return gamma_p function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto gamma_p(const T1& a, const T2& b) {
  return apply_scalar_binary(
      a, b, [&](const auto& c, const auto& d) { return gamma_p(c, d); });
}

}  // namespace math
}  // namespace stan
#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 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: Wolfgang Bangerth, 1999,
 *          Guido Kanschat, 2011
 */

// @sect3{Many new include files}

// 这些包含文件已经为你所知。它们声明了处理三角形和自由度枚举的类。

#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>

// 在这个文件中声明了创建网格的函数。

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

// 这个文件包含了对拉格朗日插值有限元的描述。

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

// 而这个文件是创建稀疏矩阵的稀疏模式所需要的，如前面的例子中所示。

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

// 接下来的两个文件是在每个单元上使用正交法组装矩阵所需要的。下面将对其中声明的类进行解释。

#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>

// 以下是我们在处理边界值时需要的三个包含文件。

#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

// 我们现在几乎到了终点。第二组到最后一组include文件是用于线性代数的，我们用它来解决拉普拉斯方程的有限元离散化所产生的方程组。我们将使用向量和全矩阵在每个单元中组装方程组，并将结果转移到稀疏矩阵中。然后我们将使用共轭梯度求解器来解决这个问题，为此我们需要一个预处理程序（在这个程序中，我们使用身份预处理程序，它没有任何作用，但我们还是需要包括这个文件）。

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

// 最后，这是为了输出到文件和控制台。

#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>

// ...这是为了将deal.II命名空间导入到全局范围。

using namespace dealii;
// @sect3{The Step3 class}

// 在这个程序中，我们没有采用以前例子中的程序化编程，而是将所有东西都封装到一个类中。这个类由一些函数组成，这些函数分别执行有限元程序的某些方面，一个`main`函数控制先做什么和后做什么，还有一个成员变量列表。

// 该类的公共部分相当简短：它有一个构造函数和一个从外部调用的函数`run`，其作用类似于`main`函数：它协调该类的哪些操作应以何种顺序运行。该类中的其他东西，即所有真正做事情的函数，都在该类的私有部分。

class Step3
{
public:
  Step3();

  void run();

// 然后，还有一些成员函数，它们主要是做它们名字所暗示的事情，在介绍中已经讨论过了。由于它们不需要从外部调用，所以它们是本类的私有函数。

private:
  void make_grid();
  void setup_system();
  void assemble_system();
  void solve();
  void output_results() const;

// 最后我们还有一些成员变量。有一些变量描述了三角形和自由度的全局编号（我们将在这个类的构造函数中指定有限元的确切多项式程度）...

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

// ...拉普拉斯方程离散化产生的系统矩阵的稀疏模式和数值的变量...

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

// .......以及用于保存右手边和解决方案向量的变量。

  Vector<double> solution;
  Vector<double> system_rhs;
};
// @sect4{Step3::Step3}

// 这里是构造函数。它除了首先指定我们需要双线性元素（由有限元对象的参数表示，它表示多项式的程度），并将dof_handler变量与我们使用的三角形相关联之外，没有做更多的工作。(注意，目前三角结构并没有设置网格，但是DoFHandler并不关心：它只想知道它将与哪个三角结构相关联，只有当你使用distribution_dofs()函数试图在网格上分布自由度时，它才开始关心实际的网格。) Step3类的所有其他成员变量都有一个默认的构造函数，它可以完成我们想要的一切。

Step3::Step3()
  : fe(1)
  , dof_handler(triangulation)
{}
// @sect4{Step3::make_grid}

// 现在，我们要做的第一件事是生成我们想在其上进行计算的三角形，并对每个顶点进行自由度编号。我们之前在 step-1 和 step-2 中分别看到过这两个步骤。

// 这个函数做的是第一部分，创建网格。 我们创建网格并对所有单元格进行五次细化。由于初始网格（也就是正方形 $[-1,1] \times [-1,1]$ ）只由一个单元组成，所以最终的网格有32乘以32个单元，总共是1024个。

// 不确定1024是否是正确的数字？我们可以通过使用三角形上的 n_active_cells() 函数输出单元格的数量来检查。

void Step3::make_grid()
{
  GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(5);

  std::cout << "Number of active cells: " << triangulation.n_active_cells()
            << std::endl;
}
// @note  我们调用 Triangulation::n_active_cells() 函数，而不是 Triangulation::n_cells(). 这里，active指的是没有进一步提炼的单元。我们强调 "活跃 "这个形容词，因为还有更多的单元，即最细的单元的父单元，它们的父单元等等，直到构成初始网格的一个单元为止。当然，在下一个更粗的层次上，单元格的数量是最细层次上的单元格的四分之一，即256，然后是64、16、4和1。如果你在上面的代码中调用 triangulation.n_cells() ，你会因此得到一个1365的值。另一方面，单元格的数量（相对于活动单元格的数量）通常没有什么意义，所以没有很好的理由去打印它。

//  @sect4{Step3::setup_system}

// 接下来我们列举所有的自由度，并建立矩阵和向量对象来保存系统数据。枚举是通过使用 DoFHandler::distribute_dofs(), 来完成的，我们在 step-2 的例子中已经看到了。由于我们使用了FE_Q类，并且在构造函数中设置了多项式的度数为1，即双线性元素，这就将一个自由度与每个顶点联系起来。当我们在生成输出时，让我们也看看有多少自由度被生成。

void Step3::setup_system()
{
  dof_handler.distribute_dofs(fe);
  std::cout << "Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;

// 每个顶点应该有一个DoF。因为我们有一个32乘以32的网格，所以DoFs的数量应该是33乘以33，即1089。

// 正如我们在前面的例子中所看到的，我们通过首先创建一个临时结构，标记那些可能为非零的条目，然后将数据复制到SparsityPattern对象中，然后可以被系统矩阵使用，来设置一个稀疏模式。

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);

// 注意，SparsityPattern对象并不保存矩阵的值，它只保存条目所在的位置。条目本身存储在SparseMatrix类型的对象中，我们的变量system_matrix就是其中之一。

// 稀疏模式和矩阵之间的区别是为了让几个矩阵使用相同的稀疏模式。这在这里似乎并不重要，但是当你考虑到矩阵的大小，以及建立稀疏模式可能需要一些时间时，如果你必须在程序中存储几个矩阵，这在大规模问题中就变得很重要了。

  system_matrix.reinit(sparsity_pattern);

// 在这个函数中要做的最后一件事是将右侧向量和解向量的大小设置为正确的值。

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());
}
// @sect4{Step3::assemble_system}

// 下一步是计算形成线性系统的矩阵和右手边的条目，我们从中计算出解决方案。这是每一个有限元程序的核心功能，我们在介绍中已经讨论了主要步骤。

// 组装矩阵和向量的一般方法是在所有单元上循环，并在每个单元上通过正交计算该单元对全局矩阵和右侧的贡献。现在要认识到的一点是，我们需要实心单元上正交点位置的形状函数值。然而，有限元形状函数和正交点都只定义在参考单元上。因此，它们对我们帮助不大，事实上，我们几乎不会直接从这些对象中查询有关有限元形状函数或正交点的信息。

// 相反，我们需要的是一种将这些数据从参考单元映射到实际单元的方法。能够做到这一点的类都是由Mapping类派生出来的，尽管人们常常不必直接与它们打交道：库中的许多函数都可以将映射对象作为参数，但当它被省略时，它们只是简单地诉诸于标准的双线性Q1映射。我们将走这条路，暂时不打扰它（我们将在 step-10 、 step-11 和 step-12 中再讨论这个问题）。

// 所以我们现在有三个类的集合来处理：有限元、正交、和映射对象。这就太多了，所以有一种类型的类可以协调这三者之间的信息交流：FEValues类。如果给这三个对象各一个实例（或两个，以及一个隐式线性映射），它就能为你提供实心单元上正交点的形状函数值和梯度的信息。

// 利用所有这些，我们将把这个问题的线性系统组装在以下函数中。

void Step3::assemble_system()
{

// 好的，我们开始吧：我们需要一个正交公式来计算每个单元格的积分。让我们采用一个高斯公式，每个方向有两个正交点，即总共有四个点，因为我们是在二维。这个正交公式可以准确地积分三度以下的多项式（在一维）。很容易检查出，这对目前的问题来说是足够的。

  QGauss<2> quadrature_formula(fe.degree + 1);

// 然后我们初始化我们在上面简单谈及的对象。它需要被告知我们要使用哪个有限元，以及正交点和它们的权重（由一个正交对象共同描述）。如前所述，我们使用隐含的Q1映射，而不是自己明确指定一个。最后，我们必须告诉它我们希望它在每个单元上计算什么：我们需要正交点的形状函数值（对于右手 $(\varphi_i,f)$ ），它们的梯度（对于矩阵条目 $(\nabla \varphi_i, \nabla \varphi_j)$ ），以及正交点的权重和从参考单元到实际单元的雅各布变换的行列式。

// 我们实际需要的信息列表是作为FEValues构造函数的第三个参数的标志集合给出的。由于这些值必须重新计算，或者说更新，每次我们进入一个新的单元时，所有这些标志都以前缀 update_ 开始，然后指出我们想要更新的实际内容。如果我们想要计算形状函数的值，那么给出的标志是#update_values；对于梯度，它是#update_gradients。雅各布的行列式和正交权重总是一起使用的，所以只计算乘积（雅各布乘以权重，或者简称 JxW ）；由于我们需要它们，我们必须同时列出#update_JxW_values。

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

// 这种方法的优点是，我们可以指定每个单元上究竟需要什么样的信息。很容易理解的是，这种方法可以大大加快有限元计算的速度，相比之下，所有的东西，包括二阶导数、单元的法向量等都在每个单元上计算，不管是否需要它们。

//  @note  update_values | update_gradients | update_JxW_values的语法对于那些不习惯用C语言编程多年的位操作的人来说不是很明显。首先， operator| 是bitwise or operator，也就是说，它接受两个整数参数，这些参数被解释为比特模式，并返回一个整数，其中每个比特都被设置，因为在两个参数中至少有一个的对应比特被设置。例如，考虑操作 9|10. In binary, 9=0b1001 （其中前缀 0b 表示该数字将被解释为二进制数字）和 10=0b1010  。通过每个比特，看它是否在其中一个参数中被设置，我们得出 0b1001|0b1010=0b1011 ，或者用十进制符号表示， 9|10=11  。你需要知道的第二个信息是，各种 update_* 标志都是有exactly one bit set的整数。例如，假设  update_values=0b00001=1  ,  update_gradients=0b00010=2  ,  update_JxW_values=0b10000=16  。那么update_values | update_gradients | update_JxW_values = 0b10011 = 19。换句话说，我们得到一个数字，即encodes a binary mask representing all of the operations you want to happen，其中每个操作正好对应于整数中的一个位，如果等于1，意味着每个单元格上应该更新一个特定的片断，如果是0，意味着我们不需要计算它。换句话说，即使 operator| 是bitwise OR operation，它真正代表的是I want this AND that AND the other。这样的二进制掩码在C语言编程中很常见，但在C++这样的高级语言中也许不是这样，但对当前的目的有很好的作用。

// 为了在下文中进一步使用，我们为一个将被频繁使用的值定义了一个快捷方式。也就是每个单元的自由度数的缩写（因为我们是在二维，自由度只与顶点相关，所以这个数字是4，但是我们更希望在写这个变量的定义时，不妨碍我们以后选择不同的有限元，每个单元有不同的自由度数，或者在不同的空间维度工作）。

// 一般来说，使用符号名称而不是硬编码这些数字是个好主意，即使你知道它们，因为例如，你可能想在某个时候改变有限元。改变元素就必须在不同的函数中进行，而且很容易忘记在程序的另一部分做相应的改变。最好不要依赖自己的计算，而是向正确的对象索取信息。在这里，我们要求有限元告诉我们每个单元的自由度数，无论我们在程序中的其他地方选择什么样的空间尺寸或多项式程度，我们都会得到正确的数字。

// 这里定义的快捷方式主要是为了讨论基本概念，而不是因为它节省了大量的输入，然后会使下面的循环更容易阅读。在大型程序中，你会在很多地方看到这样的快捷方式，`dofs_per_cell`就是一个或多或少是这类对象的传统名称。

  const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

// 现在，我们说我们想逐个单元地组装全局矩阵和向量。我们可以将结果直接写入全局矩阵，但是这样做的效率并不高，因为对稀疏矩阵元素的访问是很慢的。相反，我们首先在一个小矩阵中计算每个单元的贡献，并在这个单元的计算结束后将其转移到全局矩阵中。我们对右手边的向量也是这样做的。所以我们首先分配这些对象（这些是局部对象，所有的自由度都与所有其他的自由度耦合，我们应该使用一个完整的矩阵对象，而不是一个用于局部操作的稀疏矩阵；以后所有的东西都将转移到全局的稀疏矩阵中）。

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

// 在集合每个单元的贡献时，我们用自由度的局部编号（即从零到dofs_per_cell-1的编号）来做。然而，当我们将结果转移到全局矩阵时，我们必须知道自由度的全局编号。当我们查询它们时，我们需要为这些数字建立一个从头开始的（临时）数组（关于这里使用的类型， types::global_dof_index, ，见介绍末尾的讨论）。

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

// 现在是所有单元格的循环。我们之前已经看到这对一个三角形是如何工作的。DoFHandler的单元格迭代器与Triangulation的迭代器完全类似，但有关于你所使用的有限元的自由度的额外信息。在自由度处理程序的活动单元上进行循环操作的方法与三角法相同。

// 注意，这次我们将单元的类型声明为`const auto &`，而不是`auto`。在第1步中，我们通过用细化指标标记来修改三角形的单元。在这里，我们只检查单元格而不修改它们，所以把`cell`声明为`const`是很好的做法，以便执行这个不变性。

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

// 我们现在坐在一个单元上，我们希望计算形状函数的值和梯度，以及参考单元和真实单元之间映射的雅各布矩阵的行列式，在正交点上。由于所有这些值都取决于单元格的几何形状，我们必须让FEValues对象在每个单元格上重新计算它们。

      fe_values.reinit(cell);

// 接下来，在我们填充之前，将本地单元对全局矩阵和全局右手边的贡献重置为零。

      cell_matrix = 0;
      cell_rhs    = 0;

// 现在是时候开始对单元进行积分了，我们通过对所有的正交点进行循环来完成，我们将用q_index来编号。

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {

// 首先组装矩阵。对于拉普拉斯问题，每个单元格上的矩阵是形状函数i和j的梯度的积分。由于我们不进行积分，而是使用正交，所以这是在所有正交点的积分之和乘以正交点的雅各布矩阵的行列式乘以这个正交点的权重。你可以通过使用 fe_values.shape_grad(i,q_index) 得到形状函数 $i$ 在数字q_index的正交点上的梯度；这个梯度是一个二维向量（事实上它是张量 @<1,dim@>, 类型，这里dim=2），两个这样的向量的乘积是标量乘积，即两个shape_grad函数调用的积是点乘。这又要乘以雅各布行列式和正交点权重（通过调用 FEValues::JxW() 得到）。最后，对所有形状函数 $i$ 和 $j$ 重复上述操作。

          for (const unsigned int i : fe_values.dof_indices())
            for (const unsigned int j : fe_values.dof_indices())
              cell_matrix(i, j) +=
                (fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                 fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                 fe_values.JxW(q_index));           // dx

// 然后我们对右手边做同样的事情。在这里，积分是对形状函数i乘以右手边的函数，我们选择的是常值为1的函数（更有趣的例子将在下面的程序中考虑）。

          for (const unsigned int i : fe_values.dof_indices())
            cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                            1. *                                // f(x_q)
                            fe_values.JxW(q_index));            // dx
        }

// 现在我们有了这个单元的贡献，我们必须把它转移到全局矩阵和右手边。为此，我们首先要找出这个单元上的自由度有哪些全局数字。让我们简单地询问该单元的信息。

      cell->get_dof_indices(local_dof_indices);

// 然后再次循环所有形状函数i和j，并将局部元素转移到全局矩阵中。全局数字可以用local_dof_indices[i]获得。

      for (const unsigned int i : fe_values.dof_indices())
        for (const unsigned int j : fe_values.dof_indices())
          system_matrix.add(local_dof_indices[i],
                            local_dof_indices[j],
                            cell_matrix(i, j));

// 再来，我们对右边的向量做同样的事情。

      for (const unsigned int i : fe_values.dof_indices())
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

// 现在，几乎所有的东西都为离散系统的求解做好了准备。然而，我们还没有照顾到边界值（事实上，没有迪里切特边界值的拉普拉斯方程甚至不是唯一可解的，因为你可以在离散解中加入一个任意的常数）。因此，我们必须对这种情况做一些处理。

// 为此，我们首先获得边界上的自由度列表以及形状函数在那里的值。为了简单起见，我们只对边界值函数进行插值，而不是将其投影到边界上。库中有一个函数正是这样做的。  VectorTools::interpolate_boundary_values(). 它的参数是（省略存在默认值而我们不关心的参数）：DoFHandler对象，用于获取边界上自由度的全局数字；边界上边界值应被内插的部分；边界值函数本身；以及输出对象。

// 边界分量的含义如下：在很多情况下，你可能只想在边界的一部分施加某些边界值。例如，在流体力学中，你可能有流入和流出的边界，或者在身体变形计算中，身体的夹紧和自由部分。那么你就想用指标来表示边界的这些不同部分，并告诉interpolate_boundary_values函数只计算边界的某一部分（例如夹住的部分，或流入的边界）的边界值。默认情况下，所有的边界都有一个0的边界指标，除非另有规定。如果边界的部分有不同的边界条件，你必须用不同的边界指示器为这些部分编号。然后，下面的函数调用将只确定那些边界指标实际上是作为第二个参数指定的0的边界部分的边界值。

// 描述边界值的函数是一个Function类型的对象或一个派生类的对象。其中一个派生类是 Functions::ZeroFunction, ，它描述了一个到处都是零的函数（并不意外）。我们就地创建这样一个对象，并将其传递给 VectorTools::interpolate_boundary_values() 函数。

// 最后，输出对象是一对全局自由度数（即边界上的自由度数）和它们的边界值（这里所有条目都是零）的列表。这种自由度数到边界值的映射是由 std::map 类完成的。

  std::map<types::global_dof_index, double> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<2>(),
                                           boundary_values);

// 现在我们得到了边界DoF的列表和它们各自的边界值，让我们用它们来相应地修改方程组。这可以通过以下函数调用来实现。

  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}
// @sect4{Step3::solve}

// 下面的函数简单地求解了离散化的方程。由于该系统对于高斯消除或LU分解等直接求解器来说是一个相当大的系统，我们使用共轭梯度算法。你应该记住，这里的变量数量（只有1089个）对于有限元计算来说是一个非常小的数字，而100.000是一个比较常见的数字。 对于这个数量的变量，直接方法已经不能使用了，你不得不使用CG这样的方法。

void Step3::solve()
{

// 首先，我们需要有一个对象，知道如何告诉CG算法何时停止。这是通过使用SolverControl对象来实现的，作为停止标准，我们说：在最多1000次迭代后停止（这远远超过了1089个变量的需要；见结果部分以了解真正使用了多少次），如果残差的规范低于 $10^{-12}$ 就停止。在实践中，后一个标准将是停止迭代的一个标准。

  SolverControl solver_control(1000, 1e-12);

// 然后，我们需要解算器本身。SolverCG类的模板参数是向量的类型，留下空的角括号将表明我们采取的是默认参数（即 Vector@<double@>  ）。然而，我们明确地提到了模板参数。

  SolverCG<Vector<double>> solver(solver_control);

// 现在求解方程组。CG求解器的第四个参数是一个预处理程序。我们觉得还没有准备好深入研究这个问题，所以我们告诉它使用身份运算作为预处理。

  solver.solve(system_matrix, solution, system_rhs, PreconditionIdentity());

// 现在求解器已经完成了它的工作，求解变量包含了求解函数的结点值。

}
// @sect4{Step3::output_results}

// 典型的有限元程序的最后一部分是输出结果，也许会做一些后处理（例如计算边界处的最大应力值，或者计算整个流出物的平均通量，等等）。我们这里没有这样的后处理，但是我们想把解决方案写到一个文件里。

void Step3::output_results() const
{

// 为了将输出写入文件，我们需要一个知道输出格式等的对象。这就是DataOut类，我们需要一个该类型的对象。

  DataOut<2> data_out;

// 现在我们必须告诉它从哪里获取它要写的值。我们告诉它使用哪个DoFHandler对象，以及求解向量（以及求解变量在输出文件中的名称）。如果我们有不止一个我们想在输出中查看的向量（例如右手边，每个单元格的错误，等等），我们也要把它们加进去。

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

// 在DataOut对象知道它要处理哪些数据后，我们必须告诉它把它们处理成后端可以处理的数据。原因是我们将前端（知道如何处理DoFHandler对象和数据向量）与后端（知道许多不同的输出格式）分开，使用一种中间数据格式将数据从前端传输到后端。数据通过以下函数转换为这种中间格式。

  data_out.build_patches();

// 现在我们已经为实际输出做好了一切准备。只要打开一个文件，用VTK格式把数据写进去就可以了（在我们这里使用的DataOut类中还有很多其他函数，可以把数据写成postscript、AVS、GMV、Gnuplot或其他一些文件格式）。

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

// 最后，这个类的最后一个函数是主函数，调用 Step3 类的所有其他函数。这样做的顺序类似于大多数有限元程序的工作顺序。由于这些名字大多是不言自明的，所以没有什么可评论的。

void Step3::run()
{
  make_grid();
  setup_system();
  assemble_system();
  solve();
  output_results();
}
// @sect3{The main function}

// 这是程序的主函数。由于主函数的概念大多是C++编程之前的面向对象时代的遗留物，所以它通常不做更多的事情，只是创建一个顶层类的对象并调用其原理函数。

// 最后，函数的第一行是用来启用deal.II可以生成的一些诊断程序的输出。  @p deallog 变量（代表deal-log，而不是de-allog）代表一个流，库的某些部分将输出写入其中。例如，迭代求解器将产生诊断程序（起始残差、求解器步骤数、最终残差），在运行这个教程程序时可以看到。

//  @p deallog 的输出可以写到控制台，也可以写到文件，或者两者都写。两者在默认情况下都是禁用的，因为多年来我们已经知道，一个程序只应该在用户明确要求的时候才产生输出。但这是可以改变的，为了解释如何做到这一点，我们需要解释 @p deallog 是如何工作的。当库的个别部分想要记录输出时，它们会打开一个 "上下文 "或 "部分"，这个输出将被放入其中。在想要写输出的部分结束时，人们再次退出这个部分。由于一个函数可以在这个输出部分打开的范围内调用另一个函数，所以输出实际上可以分层嵌套到这些部分。LogStream类（ @p deallog 是一个变量）将这些部分中的每一个称为 "前缀"，因为所有的输出都以这个前缀打印在行的左端，前缀由冒号分隔。总是有一个默认的前缀叫做 "DEAL"（暗示了deal.II的历史，它是以前一个叫做 "DEAL "的库的继承者，LogStream类是被带入deal.II的少数代码之一）。

// 默认情况下， @p logstream 只输出前缀为零的行--也就是说，所有的输出都是禁用的，因为默认的 "DEAL "前缀总是存在的。但人们可以为应该输出的行设置不同的最大前缀数，以达到更大的效果，事实上在这里我们通过调用 LogStream::depth_console(). 将其设置为两个。这意味着对于所有的屏幕输出，在默认的 "DEAL "之外再推一个前缀的上下文被允许将其输出打印到屏幕上（"控制台"），而所有进一步嵌套的部分将有三个或更多的前缀被激活，会写到 @p deallog, ，但 @p deallog 并不转发这个输出到屏幕。因此，运行这个例子（或者看 "结果 "部分），你会看到解算器的统计数据前缀为 "DEAL:CG"，这是两个前缀。这对于当前程序的上下文来说已经足够了，但是你将在以后看到一些例子（例如，在 step-22 中），其中求解器嵌套得更深，你可能通过设置更高的深度来获得有用的信息。

int main()
{
  deallog.depth_console(2);

  Step3 laplace_problem;
  laplace_problem.run();

  return 0;
}


#include <Eigen/Eigen>

#include "py4dgeo/compute.hpp"
#include "py4dgeo/kdtree.hpp"
#include "py4dgeo/openmp.hpp"
#include "py4dgeo/py4dgeo.hpp"

namespace py4dgeo {

void
compute_distances(EigenPointCloudConstRef corepoints,
                  double scale,
                  const Epoch& epoch1,
                  const Epoch& epoch2,
                  EigenNormalSetConstRef directions,
                  double max_cylinder_length,
                  DistanceVector& distances,
                  UncertaintyVector& uncertainties,
                  const WorkingSetFinderCallback& workingsetfinder,
                  const UncertaintyMeasureCallback& uncertaintycalculator)
{
  // Resize the output data structures
  distances.resize(corepoints.rows());
  uncertainties.resize(corepoints.rows());

  // Instantiate a container for the first thrown exception in
  // the following parallel region.
  CallbackExceptionVault vault;
#ifdef PY4DGEO_WITH_OPENMP
#pragma omp parallel for schedule(dynamic, 1)
#endif
  for (IndexType i = 0; i < corepoints.rows(); ++i) {
    vault.run([&]() {
      // Either choose the ith row or the first (if there is no per-corepoint
      // direction)
      auto dir = directions.row(directions.rows() > 1 ? i : 0);

      WorkingSetFinderParameters params1{
        epoch1, scale, corepoints.row(i), dir, max_cylinder_length
      };
      auto subset1 = workingsetfinder(params1);
      WorkingSetFinderParameters params2{
        epoch2, scale, corepoints.row(i), dir, max_cylinder_length
      };
      auto subset2 = workingsetfinder(params2);

      // Distance calculation
      distances[i] = dir.dot(subset2.cast<double>().colwise().mean() -
                             subset1.cast<double>().colwise().mean());

      // Uncertainty calculation
      UncertaintyMeasureParameters uc_params{ subset1, subset2, dir };
      uncertainties[i] = uncertaintycalculator(uc_params);
    });
  }

  // Potentially rethrow an exception that occurred in above parallel region
  vault.rethrow();
}

EigenPointCloud
radius_workingset_finder(const WorkingSetFinderParameters& params)
{
  // Find the working set in the other epoch
  KDTree::RadiusSearchResult points;
  params.epoch.kdtree.radius_search(
    params.corepoint.data(), params.radius, points);
  return params.epoch.cloud(points, Eigen::all);
}

EigenPointCloud
cylinder_workingset_finder(const WorkingSetFinderParameters& params)
{
  // Cut the cylinder into N segments, perform radius searches around the
  // segment midpoints and create the union of indices. Afterwards, select
  // only those points that are within the cylinder

  // The number of segments - later cast to int
  double N = 1.0;
  double cylinder_length = params.cylinder_length;
  if (cylinder_length > params.radius)
    N = std::ceil(cylinder_length / params.radius);
  else
    cylinder_length = params.radius;

  // The search radius for each segment
  double r_cyl = std::sqrt(params.radius * params.radius +
                           cylinder_length * cylinder_length / (N * N));

  // Perform radius searches and merge results
  std::vector<IndexType> merged;
  for (std::size_t i = 0; i < static_cast<std::size_t>(N); ++i) {
    auto qp = (params.corepoint.row(0) +
               (static_cast<float>(2 * i + 1 - N) / static_cast<float>(N)) *
                 static_cast<float>(cylinder_length) *
                 params.cylinder_axis.cast<float>().row(0))
                .eval();
    KDTree::RadiusSearchResult ball_points;
    params.epoch.kdtree.radius_search(&(qp(0, 0)), r_cyl, ball_points);
    merged.reserve(merged.capacity() + ball_points.size());

    // Extracting points
    auto superset = params.epoch.cloud(ball_points, Eigen::all);

    // Calculate the squared distances to the cylinder axis and to the plane
    // perpendicular to the axis that contains the corepoint
    auto to_midpoint =
      (superset.cast<double>().rowwise() - qp.cast<double>().row(0)).eval();
    auto to_midpoint_plane =
      (to_midpoint * params.cylinder_axis.transpose()).eval();
    auto to_axis2 = (to_midpoint - to_midpoint_plane * params.cylinder_axis)
                      .rowwise()
                      .squaredNorm()
                      .eval();

    // Non-performance oriented version of index extraction. There should
    // be a version using Eigen masks, but I could not find it.
    for (Eigen::Index i = 0; i < superset.rows(); ++i)
      if ((to_axis2(i) <= params.radius * params.radius) &&
          (std::abs(to_midpoint_plane(i)) <= (cylinder_length / N)))
        merged.push_back(ball_points[i]);
  }

  // Select only those indices that are within the cylinder
  return params.epoch.cloud(merged, Eigen::all);
}

double
variance(EigenPointCloudConstRef subset, EigenNormalSetConstRef direction)
{
  auto centered =
    subset.cast<double>().rowwise() - subset.cast<double>().colwise().mean();
  auto cov = (centered.adjoint() * centered) / double(subset.rows() - 1);
  auto multiplied = direction.row(0) * cov * direction.row(0).transpose();
  return multiplied.eval()(0, 0);
}

DistanceUncertainty
standard_deviation_uncertainty(const UncertaintyMeasureParameters& params)
{
  double variance1 = variance(params.workingset1, params.normal);
  double variance2 = variance(params.workingset2, params.normal);

  // Calculate the standard deviations for both point clouds
  double stddev1 = std::sqrt(variance1);
  double stddev2 = std::sqrt(variance2);

  // Calculate the level of  from above variances
  double lodetection =
    1.96 *
    std::sqrt(variance1 / static_cast<double>(params.workingset1.rows()) +
              variance2 / static_cast<double>(params.workingset2.rows()));

  return DistanceUncertainty{ lodetection,
                              stddev1,
                              params.workingset1.rows(),
                              stddev2,
                              params.workingset2.rows() };
}

} // namespace py4dgeo


#include "RCameraPoseEstimation.h"

#include <Eigen/Core>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

#include "theia/sfm/pose/perspective_three_point.h"
#include "theia/sfm/pose/dls_pnp.h"

#include "rce/geometry/RGeometry.h"

//#ifdef RCE_NO_INFO_OUTPUT
//#undef RCE_NO_INFO_OUTPUT
//#endif

#include "dfs/core/DDebug.h"
#include "rce/utility/ROpenCVtoQDebug.h"

#ifndef RCE_POSE_MAX_DISTANCE_TO_SCENE
#define RCE_POSE_MAX_DISTANCE_TO_SCENE (10000)
#endif

void
rce::geometry::RCameraPoseEstimation::
estimateCameraPose(const cv::Mat &cameraMatrix,
                   const cv::Mat &homographyFromWorldToImage,
                   const cv::Point3d &pointInFrontOfCamera,
                   std::vector<cv::Point3d> &positions,
                   std::vector<cv::Mat> &rotations,
                   std::vector<cv::Point3d> &headingVectors,
                   std::vector<cv::Point3d> &toSceneVectors,
                   bool leftHandedCoordinateSystem,
                   bool filterSolutions,
                   bool useAllMethods)
{
    cv::Mat H = getCleanHomography(cameraMatrix,
                                   homographyFromWorldToImage,
                                   leftHandedCoordinateSystem);

    // firstly, dissect homographyFromWorldToImage to transformation between world plane and projection plane
    cv::Mat cameraMatrixInv = cameraMatrix.inv();

    std::vector<cv::Point3d> candidatePositions;
    std::vector<cv::Mat> candidateRotations;

    estimatePoseNADIRApproach(H,
                              candidatePositions,
                              candidateRotations);

    if(useAllMethods)
    {
        // the simplest algorithm:
        estimatePoseBetweenSimple(H,
                                  candidatePositions,
                                  candidateRotations);

        // the dumbest algorithm
        estimatePoseBetweenDumb(H,
                                candidatePositions,
                                candidateRotations);

        // opencv algorithm
        estimatePoseBetweenCVHomography(H,
                                        candidatePositions,
                                        candidateRotations);

        // opencv algorithm 2
        estimatePoseBetweenImagesCVHomography(cameraMatrixInv * homographyFromWorldToImage,
                                              cameraMatrix,
                                              candidatePositions,
                                              candidateRotations);
    }

    // from theia library
    estimatePoseFromHomographyTheiaPNP(H,
                                       pointInFrontOfCamera,
                                       candidatePositions,
                                       candidateRotations);

    if(filterSolutions)
    {
        // get only meaningfull solutions from it
        getReasonableSolutions(candidatePositions,
                               candidateRotations,
                               pointInFrontOfCamera,
                               positions,
                               rotations,
                               headingVectors,
                               toSceneVectors,
                               -1);

    }
    else
    {
        positions = candidatePositions;
        rotations = candidateRotations;
    }

// for debug purposes
//    for(int i = 0;
//        i < positions.size();
//        ++i)
//    {
//        cv::Mat rotInv;
//        cv::Point3d posInv;
//        rce::geometry::invertTranslationAndRotation(positions[i],
//                                                    rotations[i],
//                                                    posInv,
//                                                    rotInv);
//        cv::Mat checkTransform(3,3, CV_64FC1);
//        rotInv.col(0).copyTo(checkTransform.col(0));
//        rotInv.col(1).copyTo(checkTransform.col(1));
//        checkTransform.at<double>(0,2) = posInv.x;
//        checkTransform.at<double>(1,2) = posInv.y;
//        checkTransform.at<double>(2,2) = posInv.z;

//        cv::Mat residualTransform = H * checkTransform.inv();
//        residualTransform = residualTransform / residualTransform.at<double>(2,2);

//        qDebug() << "Residual transform" << i << residualTransform;
//    }
}

void
rce::geometry::RCameraPoseEstimation::
estimateCameraPoseRobust(const cv::Mat &cameraMatrix,
                         const cv::Mat &homographyFromWorldToImage,
                         const cv::Point3d &pointInFrontOfCamera,
                         std::vector<cv::Point3d> &positions,
                         std::vector<cv::Mat> &rotations,
                         std::vector<cv::Point3d> &headingVectors,
                         std::vector<cv::Point3d> &toSceneVectors,
                         bool leftHandedCoordinateSystem)
{
    cv::Mat H = getCleanHomography(cameraMatrix,
                                   homographyFromWorldToImage,
                                   leftHandedCoordinateSystem);

    std::vector<cv::Point3d> candidatePositions;
    std::vector<cv::Mat> candidateRotations;

    // from theia library
    estimatePoseFromHomographyTheiaPNP(H.clone(),
                                       pointInFrontOfCamera,
                                       candidatePositions,
                                       candidateRotations);
    // filter them out
    std::vector<cv::Point3d> filteredPositions;
    std::vector<cv::Mat> filteredRotations;
    std::vector<cv::Point3d> filteredHeadingVectors;
    std::vector<cv::Point3d> filteredToSceneVectors;
    getReasonableSolutions(candidatePositions,
                           candidateRotations,
                           pointInFrontOfCamera,
                           filteredPositions,
                           filteredRotations,
                           filteredHeadingVectors,
                           filteredToSceneVectors,
                           0.1);

    if(filteredPositions.size() > 0)
    {

        cv::Point2d nadir = rce::geometry::calculateNadirPoint(H);

        dDebug() << "NADIR" << qSetRealNumberPrecision(10) << nadir.x << nadir.y;

        double bestDist = std::numeric_limits<double>::max();
        int bestIdx = -1;

        for(int i = 0;
            i < filteredPositions.size();
            ++i)
        {
            double dist = cv::norm(nadir - cv::Point2d(filteredPositions[i].x,
                                                       filteredPositions[i].y));
            if(dist < bestDist)
            {
                bestDist = dist;
                bestIdx = i;
            }
        }

        dDebug() << "Best distance" << bestDist;

        positions.push_back(filteredPositions[bestIdx]);
        rotations.push_back(filteredRotations[bestIdx]);
        headingVectors.push_back(filteredHeadingVectors[bestIdx]);
        toSceneVectors.push_back(filteredToSceneVectors[bestIdx]);
    }
    else
    {
        estimateCameraPose(cameraMatrix,
                           homographyFromWorldToImage,
                           pointInFrontOfCamera,
                           positions,
                           rotations,
                           headingVectors,
                           toSceneVectors,
                           leftHandedCoordinateSystem,
                           true,
                           true);
    }

}

cv::Mat
rce::geometry::RCameraPoseEstimation::
getCleanHomography(const cv::Mat &cameraMatrix,
                   const cv::Mat &_fromWorldToImage,
                   bool flipY)
{
    cv::Mat fromWorldToImage;
    if(flipY)
    { // to make sure that rotation is not mirroring (and thus rotation determinant would be negative)
        // we assume that final image is flipped
        fromWorldToImage = _fromWorldToImage.clone();
        fromWorldToImage.row(1) = fromWorldToImage.row(1) * (-1.0);
    }
    else
    {
        fromWorldToImage = _fromWorldToImage;
    }

    // firstly, get clean homography
    return cameraMatrix.inv() * fromWorldToImage;
}

double estimateZ(const cv::Mat &H,
                 const cv::Vec3d &r3rest,
                 const cv::Point2d &nadirPoint,
                 double coeffW,
                 double coeffR3)
{
    double zEst1 = (H.at<double>(0,2) + coeffW * H.at<double>(0,0) * nadirPoint.x + coeffW * H.at<double>(0,1) * nadirPoint.y) /
                   (-1.0 * coeffW * coeffR3 * r3rest(0));

    double zEst2 = (H.at<double>(1,2) + coeffW * H.at<double>(1,0) * nadirPoint.x + coeffW * H.at<double>(1,1) * nadirPoint.y) /
                   (-1.0 * coeffW * coeffR3 * r3rest(1));

    double zEst = (zEst1 + zEst2) / 2.0;

    dDebug() << "estimated height" << zEst << zEst1 << zEst2 << coeffW << coeffR3;
    return zEst;
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseNADIRApproach(const cv::Mat &cameraMatrix,
                          const cv::Mat &fromWorldToImage,
                          std::vector<cv::Point3d> &positions,
                          std::vector<cv::Mat> &rotations,
                          bool flipY)
{
    // firstly, get clean homography
    cv::Mat H = getCleanHomography(cameraMatrix,
                                   fromWorldToImage,
                                   flipY);

    estimatePoseNADIRApproach(H,
                              positions,
                              rotations);

}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseNADIRApproach(const cv::Mat &_H,
                          std::vector<cv::Point3d> &positions,
                          std::vector<cv::Mat> &rotations)
{
    cv::Mat H = _H.clone();
    // estimate magnitude of scaling and denormalise the H
    double wEst1 = cv::norm(H.col(0));
    double wEst2 = cv::norm(H.col(1));
    double wEst = (wEst1 + wEst2) / 2.0;
    H = H / wEst;
    // w = +- wEst

    // calculate the inverse
    cv::Mat invH = H.inv();

    // calculate NADIR point
    cv::Point2d nadirPoint = rce::geometry::calculateNadirPoint(H,
                                                                invH);

    dDebug() << "NADIR POSE" << qSetRealNumberPrecision(10) << nadirPoint.x << nadirPoint.y << wEst1 << wEst2 << wEst1/wEst2;

    // get two estimates of R
    cv::Mat r3est = H.col(0).cross(H.col(1)); // the sign of w does not have effect in here, as perpendicularity is kept even if we multiply by -1 both vectors
    // r3 = +- r3est

    // estimate Z for all 4 posibilities
    double zPP = estimateZ(H, cv::Vec3d(r3est), nadirPoint, 1,1); // inside this function we can check if both estimated Z are similar or not.
    double zMP = estimateZ(H, cv::Vec3d(r3est), nadirPoint, -1,1);
    double zPM = estimateZ(H, cv::Vec3d(r3est), nadirPoint, 1,-1);
    double zMM = estimateZ(H, cv::Vec3d(r3est), nadirPoint, -1,-1);

    std::vector<cv::Vec3d> coeffSolutions;

    coeffSolutions.push_back(cv::Vec3d(1,1, zPP));
    coeffSolutions.push_back(cv::Vec3d(-1,1, zMP));
    coeffSolutions.push_back(cv::Vec3d(1,-1, zPM));
    coeffSolutions.push_back(cv::Vec3d(-1,-1, zMM));

    for(int i = 0;
        i < coeffSolutions.size();
        ++i)
    {
        cv::Mat fixedH = H * coeffSolutions[i](0);
        // fixedH should now be: [R_r^T(1) R_r^T(2) (-R_r^T*t)]
        cv::Mat rotationT(3,3,CV_64FC1);
        fixedH.col(0).copyTo(rotationT.col(0));
        fixedH.col(1).copyTo(rotationT.col(1));
        cv::Mat(r3est * coeffSolutions[i](1)).copyTo(rotationT.col(2)); // QUESTION: should we multiply here by coeffSolutions[i](0)????
                rotationT = rotationT * coeffSolutions[i](1); // this was added here... because it works when coeffSolutions[i](1) is -1

//        positions.push_back(cv::Point3d(nadirPoint.x,
//                                        nadirPoint.y,
//                                        coeffSolutions[i](2)));

//        rotations.push_back(rotationT.t());

        // make it pure rotation and translation
        Eigen::Matrix3d eigMat;
        cv::cv2eigen(rotationT,
                     eigMat);
        Eigen::Projective3d projTransform = Eigen::Projective3d::Identity();
        projTransform.linear() = eigMat;
        eigMat = projTransform.rotation();
        cv::Mat fixedRotationT;
        cv::eigen2cv(eigMat, fixedRotationT);

        cv::Vec3d fixedPosition2 = cv::Mat(fixedRotationT.t() *  coeffSolutions[i](1) * (-1) * fixedH.col(2));

        //qDebug() << "For solution:" << i << nadirPoint.x << nadirPoint.y << coeffSolutions[i](2) << "the alternative solutions are"
//                                                                 << fixedPosition2(0)
//                                                                 << fixedPosition2(1)
//                                                                 << fixedPosition2(2);

        positions.push_back(cv::Point3d(nadirPoint.x,
                                        nadirPoint.y,
                                        fixedPosition2(2)));

        rotations.push_back(fixedRotationT.t());

        //qDebug() << "Determinants" << cv::determinant(rotationT.t()) << cv::determinant(fixedRotationT.t());
    }
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseBetweenSimple(const cv::Mat &homography,
                          std::vector<cv::Point3d> &positions,
                          std::vector<cv::Mat> &rotations)
{

    // based upon http://ags.cs.uni-kl.de/fileadmin/inf_ags/3dcv-ws11-12/3DCV_WS11-12_lec04.pdf
    // homography = lambda * [r1 r2 t]
    double norm1 = cv::norm(homography.col(0));
    double norm2 = cv::norm(homography.col(1));
    double lambda = (norm1 + norm2) / 2.0;

    cv::Vec3d r1(homography.at<double>(0,0) / norm1,
                 homography.at<double>(1,0) / norm1,
                 homography.at<double>(2,0) / norm1);
    cv::Vec3d r2(homography.at<double>(0,1) / norm2,
                 homography.at<double>(1,1) / norm2,
                 homography.at<double>(2,1) / norm2);
    cv::Point3d t(homography.at<double>(0,2) / lambda,
                  homography.at<double>(1,2) / lambda,
                  homography.at<double>(2,2) / lambda);

    cv::Vec3d r3a = r1.cross(r2);
    double norm3 = cv::norm(r3a);
    r3a *= (1.0 / norm3);

    cv::Vec3d r3b = r3a * (-1);

    cv::Mat rotMatA = rce::geometry::composeRotationMatrix(r1, r2, r3a);
    cv::Mat rotMatB = rce::geometry::composeRotationMatrix(r1, r2, r3b);

    cv::Mat rotMatAInv, rotMatBInv;
    cv::Point3d tAInv, tBInv;
    rce::geometry::invertTranslationAndRotation(t,
                                                rotMatA,
                                                tAInv,
                                                rotMatAInv);
    rce::geometry::invertTranslationAndRotation(t,
                                                rotMatB,
                                                tBInv,
                                                rotMatBInv);
    positions.push_back(tAInv);
    positions.push_back(tBInv);
    rotations.push_back(rotMatAInv);
    rotations.push_back(rotMatBInv);
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseBetweenDumb(const cv::Mat &h,
                        std::vector<cv::Point3d> &positions,
                        std::vector<cv::Mat> &rotations)
{

    // dumbest version based on http://dsp.stackexchange.com/questions/2736/step-by-step-camera-pose-estimation-for-visual-tracking-and-planar-markers

    cv::Mat rotation(3,3,
                     CV_64FC1);

    h.col(0).copyTo(rotation.col(0));
    h.col(1).copyTo(rotation.col(1));
    cv::Mat v3 = h.col(0).cross(h.col(1));
    double norm1 = cv::norm(h.col(0));
    double norm2 = cv::norm(h.col(1));
    double tnorm = (norm1 + norm2) / 2.0;
    //dDebug() << "Rotnorms"<< norm1 << norm2 << cv::norm(v3);
    v3 = v3 / tnorm;
    v3.copyTo(rotation.col(2));

    cv::Point3d position = static_cast<cv::Point3d>(cv::Mat(h.col(2) / tnorm));

    cv::Mat rotMatInv;
    cv::Point3d tInv;
    rce::geometry::invertTranslationAndRotation(position,
                                                rotation / tnorm,
                                                tInv,
                                                rotMatInv);

    positions.push_back(tInv);
    rotations.push_back(rotMatInv);

}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseBetweenCVHomography(const cv::Mat &homography,
                                std::vector<cv::Point3d> &positions,
                                std::vector<cv::Mat> &rotations)
{
    estimatePoseBetweenImagesCVHomography(homography,
                                          cv::Mat::eye(3,3,
                                                       CV_64FC1),
                                          positions,
                                          rotations);
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseBetweenImagesCVHomography(const cv::Mat &homography,
                                      const cv::Mat &cameraMatrix,
                                      std::vector<cv::Point3d> &positions,
                                      std::vector<cv::Mat> &rotations)
{
    std::vector<cv::Mat> rotationsCV, translationsCV, normals;
    cv::decomposeHomographyMat(homography,
                               cameraMatrix,
                               rotationsCV,
                               translationsCV,
                               normals);

    for(int i = 0;
        i < rotationsCV.size();
        ++i)
    {
        //qDebug() << "estimatePoseBetweenCVHomography" << rotationsCV[i].type() << translationsCV[i].type() << translationsCV[i].cols << translationsCV[i].rows  << translationsCV[i].channels();
        positions.push_back(cv::Point3d(translationsCV[i]));
        rotations.push_back(rotationsCV[i]);

        cv::Point3d tInv;
        cv::Mat rotInv;
        rce::geometry::invertTranslationAndRotation(cv::Point3d(translationsCV[i]),
                                                    rotationsCV[i],
                                                    tInv,
                                                    rotInv);
        rotations.push_back(rotInv);
        positions.push_back(tInv);

    }
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseFromHomographyTheiaP3P(const cv::Mat &homography,
                                   const cv::Point3d &sceneCenter,
                                   std::vector<cv::Point3d> &positions,
                                   std::vector<cv::Mat> &rotations,
                                   double featurePointScale)
{
    cv::Vec3d scenePt1(sceneCenter.x - 0.5 * featurePointScale,
                       sceneCenter.y + 1 * featurePointScale,
                       1);
    cv::Vec3d scenePt2(sceneCenter.x - 0.5 * featurePointScale,
                       sceneCenter.y - 1 * featurePointScale,
                       1);
    cv::Vec3d scenePt3(sceneCenter.x + 1 * featurePointScale,
                       sceneCenter.y,
                       1);
    cv::Vec3d camPt1 = cv::Mat(homography * cv::Mat(scenePt1));
    cv::Vec3d camPt2 = cv::Mat(homography * cv::Mat(scenePt2));
    cv::Vec3d camPt3 = cv::Mat(homography * cv::Mat(scenePt3));

    Eigen::Vector2d feature_point[3];
    feature_point[0](0) = camPt1(0) / camPt1(2);
    feature_point[0](1) = camPt1(1) / camPt1(2);
    feature_point[1](0) = camPt2(0) / camPt2(2);
    feature_point[1](1) = camPt2(1) / camPt2(2);
    feature_point[2](0) = camPt3(0) / camPt3(2);
    feature_point[2](1) = camPt3(1) / camPt3(2);

    Eigen::Vector3d world_point[3];
    world_point[0](0) = scenePt1(0);
    world_point[0](1) = scenePt1(1);
    world_point[0](2) = 0;
    world_point[1](0) = scenePt2(0);
    world_point[1](1) = scenePt2(1);
    world_point[1](2) = 0;
    world_point[2](0) = scenePt3(0);
    world_point[2](1) = scenePt3(1);
    world_point[2](2) = 0;

    std::vector<Eigen::Matrix3d> solution_rotations;
    std::vector<Eigen::Vector3d> solution_translations;

    theia::PoseFromThreePoints(feature_point,
                               world_point,
                               &solution_rotations,
                               &solution_translations);

    for(int i = 0;
        i < solution_rotations.size();
        ++i)
    {
        cv::Mat rotation;
        cv::eigen2cv(solution_rotations[i],
                     rotation);
        cv::Point3d position(solution_translations[i](0),
                             solution_translations[i](1),
                             solution_translations[i](2));
//        rotations.push_back(rotation);
//        positions.push_back(position);

        cv::Point3d tInv;
        cv::Mat rotInv;
        rce::geometry::invertTranslationAndRotation(position,
                                                    rotation,
                                                    tInv,
                                                    rotInv);
        rotations.push_back(rotInv);
        positions.push_back(tInv);
    }
}

void
rce::geometry::RCameraPoseEstimation::
estimatePoseFromHomographyTheiaPNP(const cv::Mat &homography,
                                   const cv::Point3d &sceneCenter,
                                   std::vector<cv::Point3d> &positions,
                                   std::vector<cv::Mat> &rotations,
                                   double featurePointScale)
{
    cv::Vec3d scenePt1(sceneCenter.x - 1 * featurePointScale,
                       sceneCenter.y + 1 * featurePointScale,
                       1);
    cv::Vec3d scenePt2(sceneCenter.x - 1 * featurePointScale,
                       sceneCenter.y - 1 * featurePointScale,
                       1);
    cv::Vec3d scenePt3(sceneCenter.x + 1 * featurePointScale,
                       sceneCenter.y + 1 * featurePointScale,
                       1);
    cv::Vec3d scenePt4(sceneCenter.x + 1 * featurePointScale,
                       sceneCenter.y - 1 * featurePointScale,
                       1);
    cv::Vec3d camPt1 = cv::Mat(homography * cv::Mat(scenePt1));
    cv::Vec3d camPt2 = cv::Mat(homography * cv::Mat(scenePt2));
    cv::Vec3d camPt3 = cv::Mat(homography * cv::Mat(scenePt3));
    cv::Vec3d camPt4 = cv::Mat(homography * cv::Mat(scenePt4));

    std::vector<Eigen::Vector2d> feature_points(4);
    feature_points[0](0) = camPt1(0) / camPt1(2);
    feature_points[0](1) = camPt1(1) / camPt1(2);
    feature_points[1](0) = camPt2(0) / camPt2(2);
    feature_points[1](1) = camPt2(1) / camPt2(2);
    feature_points[2](0) = camPt3(0) / camPt3(2);
    feature_points[2](1) = camPt3(1) / camPt3(2);
    feature_points[3](0) = camPt4(0) / camPt4(2);
    feature_points[3](1) = camPt4(1) / camPt4(2);

    std::vector<Eigen::Vector3d> world_points(4);
    world_points[0](0) = scenePt1(0);
    world_points[0](1) = scenePt1(1);
    world_points[0](2) = 0;
    world_points[1](0) = scenePt2(0);
    world_points[1](1) = scenePt2(1);
    world_points[1](2) = 0;
    world_points[2](0) = scenePt3(0);
    world_points[2](1) = scenePt3(1);
    world_points[2](2) = 0;
    world_points[3](0) = scenePt4(0);
    world_points[3](1) = scenePt4(1);
    world_points[3](2) = 0;

    std::vector<Eigen::Quaterniond> solution_rotations;
    std::vector<Eigen::Vector3d> solution_translations;

    theia::DlsPnp(feature_points,
                  world_points,
                  &solution_rotations,
                  &solution_translations);

    dDebug() << "Estimated positions:" << solution_rotations.size();
    for(int i = 0;
        i < solution_rotations.size();
        ++i)
    {
        cv::Mat rotation;
        cv::eigen2cv(solution_rotations[i].matrix(),
                     rotation);
        cv::Point3d position(solution_translations[i](0),
                             solution_translations[i](1),
                             solution_translations[i](2));
//        rotations.push_back(rotation);
//        positions.push_back(position);

        cv::Point3d tInv;
        cv::Mat rotInv;
        rce::geometry::invertTranslationAndRotation(position,
                                                    rotation,
                                                    tInv,
                                                    rotInv);
        rotations.push_back(rotInv);
        positions.push_back(tInv);
    }
}

void
rce::geometry::RCameraPoseEstimation::
getReasonableSolutions(const std::vector<cv::Point3d> &allPositions,
                       const std::vector<cv::Mat> &allRotations,
                       const cv::Point3d &pointInFrontOfCamera,
                       std::vector<cv::Point3d> &positions,
                       std::vector<cv::Mat> &rotations,
                       std::vector<cv::Point3d> &headingVectors,
                       std::vector<cv::Point3d> &toSceneVectors,
                       double determinantLimit)
{
    for(int i = 0;
        i < allPositions.size();
        ++i)
    {
        //qDebug() << "Checking solution" << qSetRealNumberPrecision(10) << i << allPositions[i].x << allPositions[i].y << allPositions[i].z;
        // position must be above ground
        if(allPositions[i].z < 0)
        {
            dInfo() << "RCameraPoseEstimation::getReasonableSolutions: solution" << i << "does not fullfill height constraint." << allPositions[i].x << allPositions[i].y << allPositions[i].z;

            continue;
        }

        cv::Mat rotInv;
        cv::Point3d posInv;
        rce::geometry::invertTranslationAndRotation(allPositions[i],
                                                    allRotations[i],
                                                    posInv,
                                                    rotInv);

        // check whether the point in front of the camera is in front of the camera
        double camZ = rotInv.at<double>(2,0) * pointInFrontOfCamera.x +
                      rotInv.at<double>(2,1) * pointInFrontOfCamera.y +
                      rotInv.at<double>(2,2) * pointInFrontOfCamera.z +
                      posInv.z * 1.0;

        if(camZ <= 0)
        {
            dInfo() << "RCameraPoseEstimation::getReasonableSolutions: solution" << i << "does not fullfill heading constraint." << camZ;

            continue;
        }

        // calculate heading vector
        cv::Point3d originTransformed = rce::geometry::transformPoint(cv::Point3d(0,0,0),
                                                                      allPositions[i],
                                                                      allRotations[i]);
        cv::Point3d viewVectorTransformed = rce::geometry::transformPoint(cv::Point3d(0,0,1),
                                                                          allPositions[i],
                                                                          allRotations[i]);
        cv::Point3d headingVector = viewVectorTransformed - originTransformed;
        headingVector = headingVector / cv::norm(headingVector);

        // calculate to scene vector
        cv::Point3d toSceneVector = pointInFrontOfCamera - allPositions[i];
        double distToScene = cv::norm(toSceneVector);
        // dInfo() << i << "Distance to scene" << distToScene;
        if(distToScene > RCE_POSE_MAX_DISTANCE_TO_SCENE)
        {
            dInfo() << "RCameraPoseEstimation::getReasonableSolutions: solution" << i << "does not fullfill distance constraint." << distToScene;

            continue;
        }

        toSceneVector = toSceneVector / distToScene;

        if(determinantLimit >= 0)
        {
            double determinant = cv::determinant(allRotations[i]);
            if(std::abs(determinant - 1.0) > determinantLimit)
            {
                dInfo() << "RCameraPoseEstimation::getReasonableSolutions: solution" << i << "does not fullfill determinant constraint(" << determinant << ").";

                continue;
            }

            //qDebug() << "Transformation passes:" << allPositions[i].x << allPositions[i].y << allPositions[i].z << camZ << distToScene << determinant;

        }
        else
        {

            //qDebug() << "Transformation passes:" << allPositions[i].x << allPositions[i].y << allPositions[i].z << camZ << distToScene;
        }

        //qDebug() << "Rotation" << allRotations[i];

        positions.push_back(allPositions[i]);

        rotations.push_back(allRotations[i]);
        headingVectors.push_back(headingVector);
        toSceneVectors.push_back(toSceneVector);

    }
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using namespace boost::multiprecision;
cpp_int power_fm(cpp_int a, cpp_int n, cpp_int m) {
    cpp_int ret = 1;
    while (n > 0) {
        if (n & 1) ret = ret * a % m;
        a = a * a % m;
        n >>= 1;
    }
    return ret;
}
int main() {
    cpp_int x, y, z; cin >> x >> y >> z;
    cout << power_fm(x, z, y) << endl;
}


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

class MatrixReplacement;
using Eigen::SparseMatrix;

namespace Eigen {
namespace internal {
  // MatrixReplacement looks-like a SparseMatrix, so let's inherits its traits:
  template<>
  struct traits<MatrixReplacement> :  public Eigen::internal::traits<Eigen::SparseMatrix<double> >
  {};
}
}

// Example of a matrix-free wrapper from a user type to Eigen's compatible type
// For the sake of simplicity, this example simply wrap a Eigen::SparseMatrix.
class MatrixReplacement : public Eigen::EigenBase<MatrixReplacement> {
public:
  // Required typedefs, constants, and method:
  typedef double Scalar;
  typedef double RealScalar;
  typedef int StorageIndex;
  enum {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false
  };

  Index rows() const { return mp_mat->rows(); }
  Index cols() const { return mp_mat->cols(); }

  template<typename Rhs>
  Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs>& x) const {
    return Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct>(*this, x.derived());
  }

  // Custom API:
  MatrixReplacement() : mp_mat(0) {}

  void attachMyMatrix(const SparseMatrix<double> &mat) {
    mp_mat = &mat;
  }
  const SparseMatrix<double> my_matrix() const { return *mp_mat; }

private:
  const SparseMatrix<double> *mp_mat;
};

// Implementation of MatrixReplacement * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {

  template<typename Rhs>
  struct generic_product_impl<MatrixReplacement, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
  : generic_product_impl_base<MatrixReplacement,Rhs,generic_product_impl<MatrixReplacement,Rhs> >
  {
    typedef typename Product<MatrixReplacement,Rhs>::Scalar Scalar;

    template<typename Dest>
    static void scaleAndAddTo(Dest& dst, const MatrixReplacement& lhs, const Rhs& rhs, const Scalar& alpha)
    {
      // This method should implement "dst += alpha * lhs * rhs" inplace,
      // however, for iterative solvers, alpha is always equal to 1, so let's not bother about it.
      assert(alpha==Scalar(1) && "scaling is not implemented");

      // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs,
      // but let's do something fancier (and less efficient):
      for(Index i=0; i<lhs.cols(); ++i)
        dst += rhs(i) * lhs.my_matrix().col(i);
    }
  };

}
}

int main()
{
  int n = 10;
  Eigen::SparseMatrix<double> S = Eigen::MatrixXd::Random(n,n).sparseView(0.5,1);
  S = S.transpose()*S;

  MatrixReplacement A;
  A.attachMyMatrix(S);

  Eigen::VectorXd b(n), x;
  b.setRandom();

  // Solve Ax = b using various iterative solver with matrix-free version:
  {
    Eigen::ConjugateGradient<MatrixReplacement, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> cg;
    cg.compute(A);
    x = cg.solve(b);
    std::cout << "CG:       #iterations: " << cg.iterations() << ", estimated error: " << cg.error() << std::endl;
  }

  {
    Eigen::BiCGSTAB<MatrixReplacement, Eigen::IdentityPreconditioner> bicg;
    bicg.compute(A);
    x = bicg.solve(b);
    std::cout << "BiCGSTAB: #iterations: " << bicg.iterations() << ", estimated error: " << bicg.error() << std::endl;
  }

  {
    Eigen::GMRES<MatrixReplacement, Eigen::IdentityPreconditioner> gmres;
    gmres.compute(A);
    x = gmres.solve(b);
    std::cout << "GMRES:    #iterations: " << gmres.iterations() << ", estimated error: " << gmres.error() << std::endl;
  }

  {
    Eigen::DGMRES<MatrixReplacement, Eigen::IdentityPreconditioner> gmres;
    gmres.compute(A);
    x = gmres.solve(b);
    std::cout << "DGMRES:   #iterations: " << gmres.iterations() << ", estimated error: " << gmres.error() << std::endl;
  }

  {
    Eigen::MINRES<MatrixReplacement, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> minres;
    minres.compute(A);
    x = minres.solve(b);
    std::cout << "MINRES:   #iterations: " << minres.iterations() << ", estimated error: " << minres.error() << std::endl;
  }
}


#include <iostream>
#include <cstring>
#include <NTL/ZZ.h>
#include <algorithm>
#include <bitset>

using namespace NTL;
using namespace std;
using std::bitset;

string ZZToBits(ZZ num, const size_t n) {
	string s = "";
	ZZ last;

	while (num != 0) {
		last = num % 2;
		if (last == 1) {
			s += '1';
		} else {
			s += '0';
		}
		num /= 2;
	}
	for (size_t i = s.length(); i < n; i++) {
		s += '0';
	}
	reverse(s.begin(), s.end());
	return s;
}

const unsigned char DERTable[] = "0123456789ABCDEF";

void PrintInDER(const string s) {
    cout << "modulus:";
    string pairstr = "";
    for (size_t i = 0; i < s.length(); i += 4) {
        int index = 0;

        for (size_t j = i; j < i + 4; j++) {
            index = index << 1;
            if (s[j] == '1') {
                index += 1;
            }
        }
        pairstr += DERTable[index];
        if (pairstr.length() == 2) {
            cout << pairstr;
            if (i + 4 < s.length()) {
                cout << ":";
            }
            pairstr = "";
        }
        if (i % 120 == 0) {
            cout << endl << '\t';
        }
    }
    cout << endl;
}

const string Base64Map =
	"ABCDEFGHIJKLMNOPQRSTUVWXYZ"
	"abcdefghijklmnopqrstuvwxyz"
	"0123456789+/";

void PrintInPEM(const string s) {
	string res = "";
	size_t len = s.length() / 8;	// 比特串转化为字节的长度
	size_t i;
	unsigned char triBytes[3];		// 存储三个字节

	for (i = 0; i+3 <= len; i += 3) {
		for (int j = 0; j < 3; j++) {
			bitset<8> tmp(s.substr(8*i + 8*j, 8));
			triBytes[j] = tmp.to_ulong();
		}
		res += Base64Map[triBytes[0] >> 2];
		res += Base64Map[((triBytes[0]<<4) & 0x30) | (triBytes[1] >> 4)];
		res += Base64Map[((triBytes[1]<<2) & 0x3c) | (triBytes[2] >> 6)];
		res += Base64Map[triBytes[2] & 0x3f];
	}

	if (i < len) {
		if (len - i == 1) {
			bitset<8> tmp(s.substr(8*i, 8));
			triBytes[0] = tmp.to_ulong();
			res += Base64Map[triBytes[0] >> 2];
			res += Base64Map[(triBytes[0]<<4) & 0x30];
			res += "==";
		} else {
			for (int j = 0; j < 2; j++) {
				bitset<8> tmp(s.substr(8*i + 8*j, 8));
				triBytes[j] = tmp.to_ulong();
			}
			res += Base64Map[triBytes[0] >> 2];
			res += Base64Map[((triBytes[0]<<4) & 0x30) | (triBytes[1] >> 4)];
			res += Base64Map[(triBytes[1]<<2) & 0x3c];
			res += "=";
		}
	}

	cout << res << endl;
}

int main() {
	ZZ num(4164596496416416);

	string s = ZZToBits(num, 64);
	cout << s << endl;
	cout << s.length() << endl;
	PrintInDER(s);
	cout << endl;
	PrintInPEM(s);

	return 0;
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-arithmetic
    Function object function implementing fma capabilities

    Computes the (fused) multiply add of the three parameters.

    @par semantic:
    For any given value @c x,  @c y,  @c z of type @c T:

    @code
    T r = fma(x, y, z);
    @endcode

    The code is similar to:

    @code
    T r = x*y+z;
    @endcode

    @par Note
    Conformant fused multiply/add implies

    - only one rounding

    - no "intermediate" overflow

    fma provides this for all integral types and each time it is reasonable
    in terms of performance for floating ones (i.e. if the system has the hard
    wired capability).

    If you need pedantic fma capabilities in all circumstances in your own
    code you can use the pedantic_ or  std_ decorator
    (although both can can be very expensive).

     @par Decorators

    - std_ for floating entries to call directly std::fma. This implies pedantic
      fma behaviour, but in no way improved performances.
    - pedantic_ ensures the fma properties and allows SIMD acceleration if available.

    @see fms, fnma, fnms
  **/
    Value fma(Value const& v0, Value const& v1, Value const& v2);
  }
} }
#endif

#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/simd/fma.hpp>

#endif


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix A(n, n);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<double>::get();
      for (size_type i=0; i<j; ++i) {
	A(i, j)=rand_normal<double>::get();
	A(j, i)=A(i, j);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    matrix P;
    {
      P=alpha*ublas::outer_prod(x, x);
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<j; ++i)
	  P(i, j)=0;
      matrix A1(P+A);
      matrix A2(A);
      blas::syr(alpha, x, blas::lower(A2));
      std::cout << print_mat(A1) << '\n'
		<< print_mat(A2) << '\n';
    }
    {
      P=alpha*ublas::outer_prod(x, x);
      for (size_type j=0; j<n; ++j)
	for (size_type i=j+1; i<n; ++i)
	  P(i, j)=0;
      matrix A1(P+A);
      matrix A2(A);
      blas::syr(alpha, x, blas::upper(A2));
      std::cout << print_mat(A1) << '\n'
		<< print_mat(A2) << '\n';
    }
  }
  return EXIT_SUCCESS;
}


//
// Created by keszocze on 27.09.18.
//

#include "cudd_helpers.hpp"
#include "string_helpers.hpp"

#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <cudd/cudd/cudd.h>
#include <tuple>
#include <map>
#include <set>
#include <stack>
#include <algorithm>
#include <assert.h>
#include <cmath>

namespace abo::util {

    unsigned int terminal_level(const std::vector<std::vector<BDD>>& bdds) {
        unsigned int max_index = 0;
        for (const auto &f : bdds) {
            for (const BDD &b : f) {
                std::vector<unsigned int> support = b.SupportIndices();
                auto max_support_index = std::max_element(support.begin(), support.end());
                max_index = std::max(max_index, (max_support_index == support.end() ? 0 : *max_support_index) + 1);
            }
        }
        return max_index;
    }

    long eval_adder(const std::vector<BDD> &adder, long input1, long input2, int bits) {
        std::vector<int> bdd_inputs;
        for (int i = 0;i<bits;i++) {
            bdd_inputs.push_back((input1 & (1 << i)) > 0 ? 1 : 0);
            bdd_inputs.push_back((input2 & (1 << i)) > 0 ? 1 : 0);
        }

        long result = 0;
        for (unsigned int i = 0;i<adder.size();i++) {
            if (adder[i].Eval(bdd_inputs.data()).IsOne()) {
                result |= 1 << i;
            }
        }

        return result;
    }

    static double count_minterms_rec(DdNode* node, std::map<DdNode*, double> &minterms_map) {
        auto it = minterms_map.find(node);
        if (it != minterms_map.end()) {
            return it->second;
        }

        if (Cudd_IsConstant(node)) {
            double value = Cudd_V(node);
            if (Cudd_IsComplement(node)) {
                value = value == 0.0 ? 1.0 : 0.0;
            }
            minterms_map[node] = value;
            return value;
        }

        DdNode *N = Cudd_Regular(node);

        DdNode *Nv = Cudd_T(N);
        DdNode *Nnv = Cudd_E(N);

        Nv = Cudd_NotCond(Nv, Cudd_IsComplement(node));
        Nnv = Cudd_NotCond(Nnv, Cudd_IsComplement(node));

        double highResult = count_minterms_rec(Nv, minterms_map);
        double lowResult = count_minterms_rec(Nnv, minterms_map);
        double minTerms = highResult / 2 + lowResult / 2;

        minterms_map[node] = minTerms;
        return minTerms;
    }

    std::map<DdNode*, double> count_minterms(const BDD &bdd) {
        std::map<DdNode*, double> result;

        count_minterms_rec(bdd.getNode(), result);
        return result;
    }

    static double count_solutions_rec(DdNode* node, std::map<DdNode*, double> &solutions_map, int terminal_level) {
        auto it = solutions_map.find(node);
        if (it != solutions_map.end()) {
            return it->second;
        }

        if (Cudd_IsConstant(node)) {
            double value = Cudd_V(node);
            if (Cudd_IsComplement(node)) {
                value = value == 0.0 ? 1.0 : 0.0;
            }
            solutions_map[node] = value;
            return value;
        }

        DdNode *N = Cudd_Regular(node);

        DdNode *Nv = Cudd_T(N);
        DdNode *Nnv = Cudd_E(N);

        Nv = Cudd_NotCond(Nv, Cudd_IsComplement(node));
        Nnv = Cudd_NotCond(Nnv, Cudd_IsComplement(node));

        double high_result = count_solutions_rec(Nv, solutions_map, terminal_level);
        double low_result = count_solutions_rec(Nnv, solutions_map, terminal_level);

        unsigned long high_level = Cudd_IsConstant(Nv) ? terminal_level : Cudd_NodeReadIndex(Nv);
        unsigned long low_level = Cudd_IsConstant(Nnv) ? terminal_level : Cudd_NodeReadIndex(Nnv);
        unsigned long own_level = Cudd_NodeReadIndex(node);

        double solutions = high_result * std::pow(2.0, high_level - own_level - 1) +
                low_result * std::pow(2.0, low_level - own_level - 1);

        solutions_map[node] = solutions;
        return solutions;
    }

    std::map<DdNode*, double> count_solutions(const BDD &bdd) {
        std::map<DdNode*, double> result;

        count_solutions_rec(bdd.getNode(), result, terminal_level({{bdd}}));
        return result;
    }

    std::vector<int> random_satisfying_input(const BDD &bdd, const std::map<DdNode*, double> &minterm_count, int max_level) {
            std::vector<int> result;

            DdNode *node = bdd.getNode();

            for(int level = 0;level < max_level;level++) {
                long node_level = Cudd_IsConstant(node) ? max_level : Cudd_NodeReadIndex(node);
                if (level < node_level) {
                    result.push_back(rand() % 2);
                } else {
                    assert(!Cudd_IsConstant(node));

                    DdNode *N = Cudd_Regular(node);
                    DdNode *then_node = Cudd_T(N);
                    DdNode *else_node = Cudd_E(N);

                    then_node = Cudd_NotCond(then_node, Cudd_IsComplement(node));
                    else_node = Cudd_NotCond(else_node, Cudd_IsComplement(node));

                    double then_weight = minterm_count.at(then_node);
                    double else_weight = minterm_count.at(else_node);

                    double r = rand() / double(RAND_MAX);
                    if (r <= then_weight / (then_weight + else_weight)) {
                        result.push_back(1);
                        node = then_node;
                    } else {
                        result.push_back(0);
                        node = else_node;
                    }
                }
            }

            return result;
        }

    unsigned int const_ADD_value(const ADD &add) {
        DdNode *node = add.getNode();
        if (Cudd_IsConstant(node)) {
            return static_cast<unsigned int>(Cudd_V(node));
        }
        return 0;
    }

    static std::map<DdNode*, unsigned long> count_paths(DdNode* node, unsigned int terminal_level) {
        if (Cudd_IsConstant(node)) {
            return {{node, 1}};
        }

        // do a sort of breadth first search, visiting all variable levels from top to bottom
        std::vector<std::stack<DdNode*>> levels;
        levels.push_back(std::stack<DdNode*>({Cudd_Regular(node)}));

        std::map<DdNode*, unsigned long> node_to_count;
        std::set<DdNode*> visited;
        node_to_count[node] = 1;
        visited.insert(node);
        for (unsigned int level = 0;level<levels.size();level++) {
            // a reference can not be used here since the vector may resize and move the data elsewhere
            std::stack<DdNode*> nodes = levels[level];
            while (nodes.size() > 0) {
                DdNode *current = nodes.top();
                nodes.pop();

                if (!Cudd_IsConstant(current)) {
                    DdNode *then_node = Cudd_Regular(Cudd_T(current));
                    DdNode *else_node = Cudd_Regular(Cudd_E(current));

                    unsigned long current_count = node_to_count[current];

                    unsigned long then_level = Cudd_IsConstant(then_node) ? terminal_level : Cudd_NodeReadIndex(then_node);
                    // the map will automatically use 0 if the node is not yet present
                    node_to_count[then_node] += current_count << (then_level - level - 1);
                    if (visited.find(then_node) == visited.end()) {
                        levels.resize(std::max(levels.size(), then_level+1));
                        levels[then_level].push(then_node);
                        visited.insert(then_node);
                    }

                    unsigned long else_level = Cudd_IsConstant(else_node) ? terminal_level : Cudd_NodeReadIndex(else_node);
                    node_to_count[else_node] += current_count << (else_level - level - 1);
                    if (visited.find(else_node) == visited.end()) {
                        levels.resize(std::max(levels.size(), else_level+1));
                        levels[else_level].push(else_node);
                        visited.insert(else_node);
                    }
                }
            }
        }

        return node_to_count;
    }

    std::vector<std::pair<double, unsigned long>> add_terminal_values(const ADD &add) {
        std::set<DdNode*> visited;
        std::stack<DdNode*> toVisit;
        toVisit.push(add.getNode());
        visited.insert(add.getNode());

        // determine the level that terminal nodes should be interpreted as
        std::vector<unsigned int> support = add.SupportIndices();
        auto max_support_index = std::max_element(support.begin(), support.end());
        unsigned int term_level = (max_support_index == support.end() ? 0 : *max_support_index) + 1;

        auto path_count = count_paths(add.getNode(), term_level);

        std::vector<std::pair<double, unsigned long>> result;
        while (toVisit.size() > 0) {
            DdNode *node = toVisit.top();
            toVisit.pop();

            if (Cudd_IsConstant(node)) {
                result.push_back({Cudd_V(node), path_count[node]});
            } else {
                DdNode *thenNode = Cudd_T(node);
                if (visited.find(thenNode) == visited.end()) {
                    // cppcheck-suppress stlFindInsert
                    visited.insert(thenNode);
                    toVisit.push(thenNode);
                }
                DdNode *elseNode = Cudd_E(node);
                if (visited.find(elseNode) == visited.end()) {
                    // cppcheck-suppress stlFindInsert
                    visited.insert(elseNode);
                    toVisit.push(elseNode);
                }
            }
        }
        return result;
    }

    ADD bdd_forest_to_add(const Cudd &mgr, const std::vector<BDD> &bdds, const NumberRepresentation num_rep) {

        std::vector<BDD> bdds_ = bdds;
        if (num_rep == NumberRepresentation::BaseTwo) {
            bdds_.push_back(mgr.bddZero());
        }

        ADD result = mgr.addZero();
        ADD two = mgr.addOne() + mgr.addOne();
        ADD signBitPower = mgr.addOne();

        for (auto it = bdds_.rbegin() + 1;it != bdds_.rend();it++) {
            result *= two;
            signBitPower *= two;
            result += it->Add();
        }

        result -= bdds.back().Add() * signBitPower;

        return result;
    }

    ADD xor_difference_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat) {
        std::vector<BDD> diff;
        for (unsigned int i = 0;i<f.size();i++) {
            diff.push_back(f[i] ^ f_hat[i]);
        }

        return bdd_forest_to_add(mgr, diff, NumberRepresentation::BaseTwo);
    }

    static DdNode * add_absolute_difference_apply(DdManager * dd, DdNode ** f, DdNode ** g) {
        // basically copied from cuddAddApply.c (the operator is modified of course)
        DdNode *F = *f;
        DdNode *G = *g;
        if (Cudd_IsConstant(F) && Cudd_IsConstant(G)) {
            CUDD_VALUE_TYPE value = std::abs(Cudd_V(F) - Cudd_V(G));
            DdNode * res = Cudd_addConst(dd, value);
            return res;
        }
        if (F > G) { /* swap f and g */
            *f = G;
            *g = F;
        }
        return nullptr;
    }

    ADD absolute_difference_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat, const NumberRepresentation num_rep) {
        ADD a = bdd_forest_to_add(mgr, f, num_rep);
        ADD b = bdd_forest_to_add(mgr, f_hat, num_rep);
        DdNode *result_node = Cudd_addApply(mgr.getManager(), add_absolute_difference_apply, a.getNode(), b.getNode());
        return ADD(mgr, result_node);
    }

    void dump_dot(
            const Cudd &mgr,
            const std::vector<BDD> &bdd,
            const std::vector<std::string> &innames,
            const std::vector<std::string> &outnames) {

        if (bdd.empty()) {
            return;
        }

        char **ins = innames.empty() ? nullptr : vec_to_arr(innames);
        char **outs = outnames.empty() ? nullptr : vec_to_arr(outnames);

        mgr.DumpDot(bdd, ins, outs);

        del_arr(ins, innames.size());
        del_arr(outs, outnames.size());
    }

    void dump_dot(
            const Cudd &mgr,
            const BDD &bdd,
            const std::vector<std::string> &inames,
            const std::string &funname) {
        std::vector<BDD> bddv{bdd};
        std::vector<std::string> funnames{funname};
        dump_dot(mgr, bddv, inames, funnames);
    }

    /*
     * How to get the children of nodes has been taken from StackOverflow:
     * https://stackoverflow.com/questions/47704600/cudd-access-bdd-childs
     */

    BDD high(const Cudd &mgr, const BDD &v) {
        DdNode *n = v.getNode();

        if (Cudd_IsConstant(n)) {
            throw std::invalid_argument("high(mgr,v): Cannot retrieve child of a terminal node");
        }

        if (Cudd_IsComplement(n)) {
            return !BDD(mgr, Cudd_Regular(Cudd_T(n)));
        } else {
            return BDD(mgr, Cudd_T(n));
        }
    }

    BDD low(const Cudd &mgr, const BDD &v) {
        DdNode *n = v.getNode();

        if (Cudd_IsConstant(n)) {
            throw std::invalid_argument("low(mgr,v): Cannot retrieve child of a terminal node");
        }

        if (Cudd_IsComplement(n)) {
            return !BDD(mgr, Cudd_Regular(Cudd_E(n)));
        } else {
            return BDD(mgr, Cudd_E(n));
        }
    }

    /**
     * @
     */
    std::pair<BDD, BDD> full_adder(const BDD &f, const BDD &g, const BDD &carry_in) {
        BDD carry_out = (f * g) | (f * carry_in) | (g * carry_in);
        BDD sum = f ^ g ^carry_in;
        return {sum, carry_out};
    }

    std::vector<BDD>
    bdd_subtract(const Cudd &mgr, const std::vector<BDD> &minuend, const std::vector<BDD> &subtrahend) {
        std::vector<BDD> diff;
        diff.reserve(minuend.size());

        // using one as carry in serves as an implicit method to add one to the subtrahend, which is necessary to
        // change its sign
        BDD carry = mgr.bddOne();

        for (size_t i = 0; i < minuend.size(); ++i) {
            // the subtrahend's bits are negated to change the sign of the subtrahend
            auto tmp  = full_adder(minuend[i], !subtrahend[i], carry);
            diff.push_back(tmp.first);
            carry = tmp.second;

        }

        return diff;
    }

    std::vector<BDD> bdd_absolute_difference(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g,
                                             const NumberRepresentation num_rep) {

        std::vector<BDD> f_= f;
        std::vector<BDD> g_ = g;

        if (num_rep == NumberRepresentation::BaseTwo) {
            // we need to add sign bits to the functions as they don't have one right now
            // these are necessary to be able to add the negative number instead of actually subtracting
            f_.push_back(mgr.bddZero());
            g_.push_back(mgr.bddZero());
        }

        bool smaller = true;
        for (std::size_t i = 0;i<f_.size();i++) {
            if (f_[i] > g_[i]) {
                smaller = false;
                break;
            }
        }

        // use correct order for the difference calculation to minimize computation time
        const std::vector<BDD> &f__ = smaller ? g_ : f_;
        const std::vector<BDD> &g__ = smaller ? f_ : g_;

        std::vector<BDD> difference = abo::util::bdd_subtract(mgr, f__, g__);
        return abo::util::abs(mgr,difference);
    }

    std::vector<BDD> abs(const Cudd &mgr, const std::vector<BDD> &f) {

        // create mask consisting of the sign bit only
        BDD sign_bit = f.back();
        std::vector<BDD> mask(f.size(), sign_bit);

        std::vector<BDD> tmp;

        for (size_t i=0; i < f.size(); ++i) {
            tmp.push_back(f[i] ^ sign_bit);
        }

        std::vector<BDD> abs = abo::util::bdd_subtract(mgr,tmp,mask);

        return abs;
    }

    std::vector<BDD> bdd_shift(const Cudd &mgr, const std::vector<BDD> &f, int bits_to_shift) {
        std::vector<BDD> result;
        result.reserve(f.size());

        for (int i = 0;i<int(f.size());i++) {
            if (i < bits_to_shift || i >= int(f.size()) + bits_to_shift) {
                result.push_back(mgr.bddZero());
            } else {
                result.push_back(f[i - bits_to_shift]);
            }
        }

        return result;
    }

    std::vector<BDD> bdd_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g) {
        std::vector<BDD> sum;
        sum.reserve(f.size());

        BDD carry = mgr.bddZero();

        for (unsigned int i = 0; i < f.size(); ++i) {
            auto tmp  = full_adder(f[i], g[i], carry);
            sum.push_back(tmp.first);
            carry = tmp.second;

        }

        return sum;
    }

    std::vector<BDD> bdd_multiply_constant(const Cudd &mgr, const std::vector<BDD> &f, double factor, const unsigned int num_extra_bits) {

        std::vector<BDD> result(f.size() + size_t(std::ceil(std::log2(factor))) + 2, mgr.bddZero());

        std::vector<BDD> fc = f;
        while (result.size() > fc.size()) {
            fc.push_back(mgr.bddZero());
        }

        assert (factor <= static_cast<double>(std::numeric_limits<boost::multiprecision::uint256_t>::max()));

        boost::multiprecision::uint256_t great_factor = static_cast<boost::multiprecision::uint256_t>(factor);
        boost::multiprecision::uint256_t one = 1;
        for (int i = 0;i<256;i++) {
            if (great_factor & (one << i)) {
                result = bdd_add(mgr, result, bdd_shift(mgr, fc, i));
            }
        }
        unsigned long lesser_factor = static_cast<unsigned long>(std::fmod(factor, 1.0) * (1UL << num_extra_bits));
        for (unsigned int i = 0;i<num_extra_bits;i++) {
            if (lesser_factor & (1UL << i)) {
                result = bdd_add(mgr, result, bdd_shift(mgr, fc, -static_cast<int>(num_extra_bits - i)));
            }
        }

        return result;
    }

    void equalize_vector_size(const Cudd &mgr, std::vector<BDD> &f1, std::vector<BDD> &f2) {
        while (f1.size() < f2.size()) {
            f1.push_back(mgr.bddZero());
        }
        while (f2.size() < f1.size()) {
            f2.push_back(mgr.bddZero());
        }
    }

    std::pair<bool, bool> exists_greater_equals(const Cudd &mgr, const std::vector<BDD> &f1, const std::vector<BDD> &f2) {
        std::vector<BDD> f1_ = f1;
        std::vector<BDD> f2_ = f2;

        equalize_vector_size(mgr, f1_, f2_);

        BDD zero_condition = mgr.bddOne();
        BDD equal_condition = mgr.bddOne();
        for (int i = int(f1_.size())-1;i>=0;i--) {
            zero_condition &= !f2_[i];
            if (!((f1_[i] & zero_condition).IsZero())) {
                return {true, false};
            }
            if (!((f1_[i] & !f2_[i] & equal_condition).IsZero())) {
                return {true, false};
            }
            equal_condition &= (f1_[i] & f2_[i]) | ((!f1_[i]) & (!f2_[i]));
        }
        if (!equal_condition.IsZero()) {
            return {true, true};
        }
        return {false, false};
    }

    std::vector<BDD> bdd_max_one(const Cudd &mgr, const std::vector<BDD> &f) {
        std::vector<BDD> result = f;

        BDD zero_condition = mgr.bddOne();
        for (int i = int(result.size())-1;i>0;i--) {
            zero_condition &= !result[i];
        }
        result[0] |= zero_condition;

        return result;
    }

    BDD greater_equals(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g) {
        std::vector<BDD> f_ = f;
        std::vector<BDD> g_ = g;

        equalize_vector_size(mgr, f_, g_);

        BDD zero_condition = mgr.bddOne();
        BDD equal_condition = mgr.bddOne();

        BDD result = mgr.bddZero();
        for (int i = int(f_.size()) - 1; i >= 0; i--) {
            zero_condition &= !g_[i];
            result |= f_[i] & zero_condition;
            result |= f_[i] & !g_[i] & equal_condition;
            equal_condition &= (f_[i] & g_[i]) | ((!f_[i]) & (!g_[i]));
        }
        result |= equal_condition;
        return result;
    }

    std::vector<BDD> bdd_divide(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &g, unsigned int extra_bits) {

        std::vector<BDD> values(extra_bits, mgr.bddZero());
        std::vector<BDD> temp = f;
        temp.insert(temp.begin(), values.begin(), values.end());

        std::vector<BDD> g_ = g;
        for (unsigned int i = 0;i<f.size()+1;i++) {
            g_.push_back(mgr.bddZero());
        }
        g_.insert(g_.begin(), values.begin(), values.end());

        std::vector<BDD> result;
        for (int i = int(f.size())+1;i >= -int(extra_bits);i--) {
            auto shifted = bdd_shift(mgr, g_, i);
            BDD subtract_condition = greater_equals(mgr, temp, shifted);
            std::vector<BDD> to_subtract = shifted;
            for (BDD &b : to_subtract) {
                b &= subtract_condition;
            }
            equalize_vector_size(mgr, to_subtract, temp);
            temp = bdd_subtract(mgr, temp, to_subtract);
            result.push_back(subtract_condition);
        }
        std::reverse(result.begin(), result.end());
        return result;
    }

}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [direct_hyperbolic]
#include <boost/simd/hyperbolic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = bs::enumerate<pack_ft>(-4.0f, 1.0f);
  std::cout << " p =  " << p << std::endl
            <<  " -> bs::cosh(p) =  " << bs::cosh(p) << std::endl
            <<  " -> bs::sinh(p) =  " << bs::sinh(p) << std::endl
            <<  " -> bs::tanh(p) =  " << bs::tanh(p) << std::endl
            <<  " -> bs::sech(p) =  " << bs::sech(p) << std::endl
            <<  " -> bs::csch(p) =  " << bs::csch(p) << std::endl;
  pack_ft s, c;
  std::tie(s, c) = bs::sinhcosh(p);
  std::cout <<  " using sinhcosh " << std::endl
            <<  " -> bs::cosh(p) =  " << c << std::endl
            <<  " -> bs::sinh(p) =  " << s << std::endl;
  return 0;
}
//! [direct_hyperbolic]


#include "optimization_problem.hpp"
#include "homogeneous.h"
#include "utility.hpp"
#include "jacobian.h"

#include <Eigen/Eigenvalues>

#include <range/v3/all.hpp>

double pointPlaneDistance(const Eigen::Vector3d & w, const Eigen::Vector3d & x)
{
  return std::abs(w.dot(x) + 1.0) / w.norm();
}

bool validatePlane(const Eigen::MatrixXd & X, const Eigen::Vector3d & w)
{
  for (int j = 0; j < X.rows(); j++) {
    const Eigen::Vector3d x = X.row(j);
    if (pointPlaneDistance(w, x) > 0.2) {
      return false;
    }
  }
  return true;
}

Eigen::MatrixXd get(
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
  const std::vector<int> & indices)
{
  Eigen::MatrixXd A(indices.size(), 3);
  for (const auto & [j, index] : ranges::views::enumerate(indices)) {
    const Eigen::Vector3d p = getXYZ(pointcloud->at(index));
    A.row(j) = p.transpose();
  }
  return A;
}

Eigen::Matrix3d calcCovariance(const Eigen::MatrixXd & X)
{
  const Eigen::Vector3d c = X.colwise().mean();
  const Eigen::MatrixXd D = X.rowwise() - c.transpose();
  return D.transpose() * D / X.rows();
}

Eigen::VectorXd solveLinear(const Eigen::MatrixXd & A, const Eigen::VectorXd & b)
{
  return A.householderQr().solve(b);
}

bool checkConvergence(const Vector6d & dx)
{
  const float dr = rad2deg(dx.head(3)).norm();
  const float dt = (100 * dx.tail(3)).norm();
  return dr < 0.05 && dt < 0.05;
}

const int n_neighbors = 5;

std::vector<int> trueIndices(const std::vector<bool> & flags)
{
  return ranges::views::iota(0, static_cast<int>(flags.size())) |
         ranges::views::filter([&](int i) {return flags[i];}) |
         ranges::to_vector;
}

std::vector<Eigen::Vector3d> filteredCoeffs(
  const std::vector<int> & indices,
  const std::vector<Eigen::Vector3d> & coeffs)
{
  return indices | ranges::views::transform([&](int i) {return coeffs[i];}) | ranges::to_vector;
}

std::vector<Eigen::Vector3d> filteredPoints(
  const std::vector<int> & indices,
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud)
{
  const auto f = [&](int i) {return getXYZ(pointcloud->at(i));};
  return indices | ranges::views::transform(f) | ranges::to_vector;
}

std::tuple<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<double>>
OptimizationProblem::fromEdge(const Eigen::Affine3d & point_to_map) const
{
  // f(dx) \approx f(0) + J * dx + dx^T * H * dx
  // dx can be obtained by solving H * dx = -J

  std::vector<Eigen::Vector3d> coeffs(edge_scan_->size());
  std::vector<bool> flags(edge_scan_->size(), false);

  #pragma omp parallel for num_threads(n_threads_)
  for (unsigned int i = 0; i < edge_scan_->size(); i++) {
    const pcl::PointXYZ p = transform(point_to_map, edge_scan_->at(i));
    const auto [indices, squared_distances] = edge_kdtree_.nearestKSearch(p, n_neighbors);
    if (squared_distances.back() >= 1.0) {
      continue;
    }

    const Eigen::Matrix<double, n_neighbors, 3> neighbors = get(edge_map_, indices);
    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(calcCovariance(neighbors));
    const Eigen::Vector3d eigenvalues = solver.eigenvalues();
    const Eigen::Vector3d eigenvector = solver.eigenvectors().col(2);

    if (eigenvalues(2) <= 3 * eigenvalues(1)) {
      continue;
    }

    const Eigen::Vector3d c = neighbors.colwise().mean();
    const Eigen::Vector3d p0 = getXYZ(p);
    const Eigen::Vector3d p1 = c + 0.1 * eigenvector;
    const Eigen::Vector3d p2 = c - 0.1 * eigenvector;

    const Eigen::Vector3d d01 = p0 - p1;
    const Eigen::Vector3d d12 = p1 - p2;
    const Eigen::Vector3d d20 = p2 - p0;

    const Eigen::Vector3d u = d20.cross(d01);

    if (u.norm() >= 1.0) {
      continue;
    }

    coeffs[i] = d12.cross(u);
    flags[i] = true;
  }

  const std::vector<int> indices = trueIndices(flags);
  const std::vector<Eigen::Vector3d> points = filteredPoints(indices, edge_scan_);
  const std::vector<Eigen::Vector3d> coeffs_filtered = filteredCoeffs(indices, coeffs);
  const std::vector<double> b(coeffs_filtered.size(), -1.0);
  return {points, coeffs_filtered, b};
}

Eigen::Vector3d estimatePlaneCoefficients(const Eigen::MatrixXd & X)
{
  const Eigen::VectorXd g = -1.0 * Eigen::VectorXd::Ones(X.rows());
  return solveLinear(X, g);
}

std::tuple<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>, std::vector<double>>
OptimizationProblem::fromSurface(const Eigen::Affine3d & point_to_map) const
{
  std::vector<Eigen::Vector3d> coeffs(surface_scan_->size());
  std::vector<double> b(surface_scan_->size());
  std::vector<bool> flags(surface_scan_->size(), false);

  // surface optimization
  #pragma omp parallel for num_threads(n_threads_)
  for (unsigned int i = 0; i < surface_scan_->size(); i++) {
    const pcl::PointXYZ p = transform(point_to_map, surface_scan_->at(i));
    const auto [indices, squared_distances] = surface_kdtree_.nearestKSearch(p, n_neighbors);

    if (squared_distances.back() >= 1.0) {
      continue;
    }

    const Eigen::MatrixXd X = get(surface_map_, indices);
    const Eigen::Vector3d w = estimatePlaneCoefficients(X);

    if (!validatePlane(X, w)) {
      continue;
    }

    const Eigen::Vector3d q = getXYZ(p);
    const double norm = w.norm();

    coeffs[i] = w / norm;
    b[i] = -(w.dot(q) + 1.0) / norm;
    flags[i] = true;
  }

  const std::vector<int> indices = trueIndices(flags);
  const std::vector<Eigen::Vector3d> points = filteredPoints(indices, surface_scan_);
  const std::vector<Eigen::Vector3d> coeffs_filtered = filteredCoeffs(indices, coeffs);
  const std::vector<double> b_filtered =
    indices | ranges::views::transform([&](int i) {return b[i];}) | ranges::to_vector;
  return {points, coeffs_filtered, b_filtered};
}

Eigen::MatrixXd makeJacobian(
  const std::vector<Eigen::Vector3d> & points,
  const std::vector<Eigen::Vector3d> & coeffs,
  const Eigen::Vector3d & rpy)
{
  const Eigen::Matrix3d JX = dRdx(rpy);
  const Eigen::Matrix3d JY = dRdy(rpy);
  const Eigen::Matrix3d JZ = dRdz(rpy);

  Eigen::MatrixXd J(points.size(), 6);
  for (unsigned int i = 0; i < points.size(); i++) {
    // in camera

    const Eigen::Vector3d point = points.at(i);
    const Eigen::Vector3d coeff = coeffs.at(i);

    const Eigen::Vector3d drpdx = JX * point;
    const Eigen::Vector3d drpdy = JY * point;
    const Eigen::Vector3d drpdz = JZ * point;

    // lidar -> camera
    J(i, 0) = coeff.dot(drpdx);  // d ||residual||^2 / d roll
    J(i, 1) = coeff.dot(drpdy);  // d ||residual||^2 / d pitch
    J(i, 2) = coeff.dot(drpdz);  // d ||residual||^2 / d yaw
    J(i, 3) = coeff(0);          // d ||residual||^2 / d tx
    J(i, 4) = coeff(1);          // d ||residual||^2 / d ty
    J(i, 5) = coeff(2);          // d ||residual||^2 / d tz
  }
  return J;
}

std::tuple<Eigen::MatrixXd, Eigen::VectorXd>
OptimizationProblem::make(const Vector6d & posevec) const
{
  const Eigen::Affine3d point_to_map = getTransformation(posevec);
  const auto [edge_points, edge_coeffs, edge_coeffs_b] = fromEdge(point_to_map);
  const auto [surface_points, surface_coeffs, surface_coeffs_b] = fromSurface(point_to_map);

  const auto points = ranges::views::concat(edge_points, surface_points) | ranges::to_vector;
  const auto coeffs = ranges::views::concat(edge_coeffs, surface_coeffs) | ranges::to_vector;
  auto b_vector = ranges::views::concat(edge_coeffs_b, surface_coeffs_b) | ranges::to_vector;

  assert(points.size() == coeffs.size());
  assert(points.size() == b_vector.size());
  const Eigen::MatrixXd J = makeJacobian(points, coeffs, posevec.head(3));
  const Eigen::Map<Eigen::VectorXd> b(b_vector.data(), b_vector.size());
  return {J, b};
}

bool isDegenerate(const OptimizationProblem & problem, const Vector6d & posevec)
{
  const auto [J, b] = problem.make(posevec);
  const Eigen::MatrixXd JtJ = J.transpose() * J;
  const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(JtJ);
  const Eigen::VectorXd eigenvalues = es.eigenvalues();
  return (eigenvalues.array() < 100.0).any();
}

Eigen::VectorXd calcUpdate(const Eigen::MatrixXd & J, const Eigen::VectorXd & b)
{
  const Eigen::MatrixXd JtJ = J.transpose() * J;
  const Eigen::VectorXd JtB = J.transpose() * b;
  return solveLinear(JtJ, JtB);
}

// This optimization is from the original loam_velodyne by Ji Zhang,
// need to cope with coordinate transformation
// lidar <- camera      ---     camera <- lidar
// x = z                ---     x = y
// y = x                ---     y = z
// z = y                ---     z = x
// roll = yaw           ---     roll = pitch
// pitch = roll         ---     pitch = yaw
// yaw = pitch          ---     yaw = roll

Vector6d optimizePose(const OptimizationProblem & problem, const Vector6d & initial_posevec)
{
  Vector6d posevec = initial_posevec;
  for (int iter = 0; iter < 30; iter++) {
    const auto [J, b] = problem.make(posevec);
    if (J.rows() < 50) {
      continue;
    }

    const Eigen::VectorXd dx = calcUpdate(J, b);

    posevec += dx;

    if (checkConvergence(dx)) {
      break;
    }
  }
  return posevec;
}