/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2005 - 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.
 *
 * ---------------------------------------------------------------------
 */

// @sect3{Include files}

// 由于这个程序只是对 step-4 的改编，所以在头文件方面没有太多的新东西。在deal.II中，我们通常按照base-lac-grid-dofs-fe-numerics的顺序列出包含文件，然后是C++标准包含文件。

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

#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

// 唯一值得关注的两个新头文件是LinearOperator和PackagedOperation类的文件。

#include <deal.II/lac/linear_operator.h>
#include <deal.II/lac/packaged_operation.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// 这是唯一重要的新标题，即声明Raviart-Thomas有限元的标题。

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

// 最后，作为本程序中的一项奖励，我们将使用一个张量系数。由于它可能具有空间依赖性，我们认为它是一个张量值的函数。下面的include文件提供了 TensorFunction 类，提供了这样的功能。

#include <deal.II/base/tensor_function.h>

// 最后一步和以前所有的程序一样。我们把所有与这个程序相关的代码放到一个命名空间中。(这个想法在  step-7  中首次提出) 。

namespace Step20
{
  using namespace dealii;
// @sect3{The MixedLaplaceProblem class template}

// 同样，由于这是对 step-6 的改编，主类与该教程程序中的主类几乎相同。就成员函数而言，主要区别在于构造函数将Raviart-Thomas元素的度数作为参数（并且有一个相应的成员变量来存储这个值），并且增加了 compute_error 函数，在这个函数中，不出意外，我们将计算精确解和数值解之间的差异，以确定我们计算的收敛性。

  template <int dim>
  class MixedLaplaceProblem
  {
  public:
    MixedLaplaceProblem(const unsigned int degree);
    void run();

  private:
    void make_grid_and_dofs();
    void assemble_system();
    void solve();
    void compute_errors() const;
    void output_results() const;

    const unsigned int degree;

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

// 第二个区别是疏散模式、系统矩阵、解和右手向量现在被封锁了。这意味着什么，人们可以用这些对象做什么，在本程序的介绍中已经解释过了，下面我们在解释这个问题的线性求解器和预处理器时也会进一步解释。

    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;

    BlockVector<double> solution;
    BlockVector<double> system_rhs;
  };
// @sect3{Right hand side, boundary values, and exact solution}

// 我们的下一个任务是定义我们问题的右手边（即原始拉普拉斯方程中压力的标量右手边），压力的边界值，以及一个描述压力和精确解的速度的函数，以便以后计算误差。请注意，这些函数分别有一个、一个和 dim+1 个分量，我们将分量的数量传递给 Function@<dim@> 基类。对于精确解，我们只声明实际一次性返回整个解向量（即其中的所有成分）的函数。下面是各自的声明。

  namespace PrescribedSolution
  {
    constexpr double alpha = 0.3;
    constexpr double beta  = 1;

    template <int dim>
    class RightHandSide : public Function<dim>
    {
    public:
      RightHandSide()
        : Function<dim>(1)
      {}

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

    template <int dim>
    class PressureBoundaryValues : public Function<dim>
    {
    public:
      PressureBoundaryValues()
        : Function<dim>(1)
      {}

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

    template <int dim>
    class ExactSolution : public Function<dim>
    {
    public:
      ExactSolution()
        : Function<dim>(dim + 1)
      {}

      virtual void vector_value(const Point<dim> &p,
                                Vector<double> &  value) const override;
    };

// 然后我们还必须定义这些各自的函数，当然了。鉴于我们在介绍中讨论了解决方案应该是怎样的，下面的计算应该是很简单的。

    template <int dim>
    double RightHandSide<dim>::value(const Point<dim> & /*p*/,
                                     const unsigned int /*component*/) const
    {
      return 0;
    }

    template <int dim>
    double
    PressureBoundaryValues<dim>::value(const Point<dim> &p,
                                       const unsigned int /*component*/) const
    {
      return -(alpha * p[0] * p[1] * p[1] / 2 + beta * p[0] -
               alpha * p[0] * p[0] * p[0] / 6);
    }

    template <int dim>
    void ExactSolution<dim>::vector_value(const Point<dim> &p,
                                          Vector<double> &  values) const
    {
      Assert(values.size() == dim + 1,
             ExcDimensionMismatch(values.size(), dim + 1));

      values(0) = alpha * p[1] * p[1] / 2 + beta - alpha * p[0] * p[0] / 2;
      values(1) = alpha * p[0] * p[1];
      values(2) = -(alpha * p[0] * p[1] * p[1] / 2 + beta * p[0] -
                    alpha * p[0] * p[0] * p[0] / 6);
    }

//  @sect3{The inverse permeability tensor}

// 除了其他方程数据外，我们还想使用渗透性张量，或者更好的是--因为这是在弱形式中出现的全部内容--渗透性张量的逆，  KInverse  。对于验证解的精确性和确定收敛顺序的目的来说，这个张量的作用大于帮助。因此，我们将简单地把它设置为同一矩阵。

// 然而，在现实生活中的多孔介质流动模拟中，空间变化的渗透率张量是不可缺少的，我们想利用这个机会来展示使用张量值函数的技术。

// 可能不足为奇，deal.II也有一个基类，不仅适用于标量和一般的矢量值函数（ Function 基类），也适用于返回固定维度和等级的张量的函数， TensorFunction 模板。在这里，所考虑的函数返回一个dim-by-dim矩阵，即一个等级为2、维度为 dim 的张量。然后我们适当地选择基类的模板参数。

//  TensorFunction 类提供的接口本质上等同于 Function 类。特别是，存在一个 value_list 函数，它接收一个评估函数的点的列表，并在第二个参数中返回函数的值，一个张量的列表。

    template <int dim>
    class KInverse : public TensorFunction<2, dim>
    {
    public:
      KInverse()
        : TensorFunction<2, dim>()
      {}

      virtual void
      value_list(const std::vector<Point<dim>> &points,
                 std::vector<Tensor<2, dim>> &  values) const override;
    };

// 实现起来就不那么有趣了。和以前的例子一样，我们在类的开头添加一个检查，以确保输入和输出参数的大小是相同的（关于这个技术的讨论见 step-5 ）。然后我们在所有的评估点上循环，对于每一个评估点，将输出张量设置为身份矩阵。

// 在函数的顶部有一个奇怪的地方（`(void)point;`语句），值得讨论。我们放到输出`values`数组中的值实际上并不取决于函数被评估的坐标`points`数组。换句话说，`points'参数实际上是不用的，如果我们想的话，可以不给它起名字。但是我们想用`points`对象来检查`values`对象是否有正确的大小。问题是，在发布模式下，`AssertDimension`被定义为一个宏，扩展为空；然后编译器会抱怨`points`对象没有使用。消除这个警告的习惯方法是有一个评估（读取）变量的语句，但实际上不做任何事情：这就是`(void)points;`所做的：它从`points`中读取，然后将读取的结果转换为`void`，也就是什么都没有。换句话说，这句话是完全没有意义的，除了向编译器解释是的，这个变量事实上是被使用的，即使是在发布模式下。(在调试模式下，`AssertDimension`宏会扩展为从变量中读出的东西，所以在调试模式下，这个有趣的语句是没有必要的)。

    template <int dim>
    void KInverse<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<Tensor<2, dim>> &  values) const
    {
      (void)points;
      AssertDimension(points.size(), values.size());

      for (auto &value : values)
        value = unit_symmetric_tensor<dim>();
    }
  } // namespace PrescribedSolution

//  @sect3{MixedLaplaceProblem class implementation}
// @sect4{MixedLaplaceProblem::MixedLaplaceProblem}

// 在这个类的构造函数中，我们首先存储传入的关于我们将使用的有限元的度数的值（例如，度数为0，意味着使用RT(0)和DG(0)），然后构造属于介绍中描述的空间 $X_h$ 的向量值的元素。构造函数的其余部分与早期的教程程序一样。

// 这里唯一值得描述的是，这个变量所属的 fe variable. The FESystem 类的构造函数调用有很多不同的构造函数，它们都是指将较简单的元素绑定在一起，成为一个较大的元素。在目前的情况下，我们想把一个RT(度)元素与一个DQ(度)元素结合起来。这样做的 FESystem 构造函数要求我们首先指定第一个基本元素（给定程度的 FE_RaviartThomas 对象），然后指定这个基本元素的副本数量，然后类似地指定 FE_DGQ 元素的种类和数量。注意Raviart-Thomas元素已经有 dim 个矢量分量，所以耦合元素将有 dim+1 个矢量分量，其中第一个 dim 个对应于速度变量，最后一个对应于压力。

// 我们从基本元素中构建这个元素的方式与我们在 step-8 中的方式也值得比较：在那里，我们将其构建为 fe (FE_Q@<dim@>(1), dim) ，即我们简单地使用 dim copies of the FE_Q(1) 元素，每个坐标方向上的位移都有一份。

  template <int dim>
  MixedLaplaceProblem<dim>::MixedLaplaceProblem(const unsigned int degree)
    : degree(degree)
    , fe(FE_RaviartThomas<dim>(degree), 1, FE_DGQ<dim>(degree), 1)
    , dof_handler(triangulation)
  {}

//  @sect4{MixedLaplaceProblem::make_grid_and_dofs}

// 接下来的函数开始于众所周知的函数调用，创建和细化一个网格，然后将自由度与之关联。

  template <int dim>
  void MixedLaplaceProblem<dim>::make_grid_and_dofs()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(5);

    dof_handler.distribute_dofs(fe);

// 然而，接下来事情就变得不同了。正如介绍中提到的，我们要将矩阵细分为对应于速度和压力这两种不同的变量的块。为此，我们首先要确保与速度和压力相对应的指数不会混在一起。首先是所有速度自由度，然后是所有压力自由度。这样一来，全局矩阵就很好地分离成一个 $2 \times 2$ 系统。为了达到这个目的，我们必须根据自由度的矢量分量对其重新编号，这个操作已经很方便地实现了。

    DoFRenumbering::component_wise(dof_handler);

// 接下来，我们要弄清楚这些块的大小，以便我们可以分配适当的空间量。为此，我们调用了 DoFTools::count_dofs_per_fe_component() 函数，该函数计算了某个向量分量的形状函数非零的数量。我们有 dim+1 个向量分量， DoFTools::count_dofs_per_fe_component() 将计算有多少个形状函数属于这些分量中的每个。

// 这里有一个问题。正如该函数的文档所描述的，它 wants 将  $x$  -速度形状函数的数量放入  dofs_per_component[0]  中，将  $y$  -速度形状函数的数量放入  dofs_per_component[1]  中（以及类似的3d），并将压力形状函数的数量放入  dofs_per_component[dim]  中 。但是，Raviart-Thomas元素的特殊性在于它是非 @ref GlossPrimitive "原始 "的，也就是说，对于Raviart-Thomas元素，所有的速度形状函数在所有分量中都是非零。换句话说，该函数不能区分 $x$ 和 $y$ 速度函数，因为is没有这种区分。因此，它将速度的总体数量放入 dofs_per_component[c]  ,  $0\le c\le \text{dim}$ 中的每一个。另一方面，压力变量的数量等于在dim-th分量中不为零的形状函数的数量。

// 利用这些知识，我们可以从 dofs_per_component 的第一个 dim 元素中的任何一个得到速度形状函数的数量，然后用下面这个来初始化向量和矩阵块的大小，以及创建输出。

//  @note  如果你觉得这个概念难以理解，你可以考虑用函数  DoFTools::count_dofs_per_fe_block()  来代替，就像我们在  step-22  的相应代码中做的那样。你可能还想阅读一下术语表中 @ref GlossBlock "块 "和 @ref GlossComponent "组件 "的区别。

    const std::vector<types::global_dof_index> dofs_per_component =
      DoFTools::count_dofs_per_fe_component(dof_handler);
    const unsigned int n_u = dofs_per_component[0],
                       n_p = dofs_per_component[dim];

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "Total number of cells: " << triangulation.n_cells()
              << std::endl
              << "Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << n_u << '+' << n_p << ')' << std::endl;

// 下一个任务是为我们将要创建的矩阵分配一个稀疏模式。我们使用与前面步骤一样的压缩稀疏模式，但是由于 system_matrix 是一个块状矩阵，我们使用 BlockDynamicSparsityPattern 类，而不仅仅是 DynamicSparsityPattern  。这种块状稀疏模式在 $2 \times 2$ 模式下有四个块。块的大小取决于 n_u and n_p ，它持有速度和压力变量的数量。在第二步中，我们必须指示块系统更新它所管理的块的大小的知识；这发生在 dsp.collect_sizes () 的调用中。

    BlockDynamicSparsityPattern dsp(2, 2);
    dsp.block(0, 0).reinit(n_u, n_u);
    dsp.block(1, 0).reinit(n_p, n_u);
    dsp.block(0, 1).reinit(n_u, n_p);
    dsp.block(1, 1).reinit(n_p, n_p);
    dsp.collect_sizes();
    DoFTools::make_sparsity_pattern(dof_handler, dsp);

// 我们以与非区块版本相同的方式使用压缩的区块稀疏模式，以创建稀疏模式，然后创建系统矩阵。

    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);

// 然后，我们必须以与块压缩稀疏度模式完全相同的方式调整解决方案和右侧向量的大小。

    solution.reinit(2);
    solution.block(0).reinit(n_u);
    solution.block(1).reinit(n_p);
    solution.collect_sizes();

    system_rhs.reinit(2);
    system_rhs.block(0).reinit(n_u);
    system_rhs.block(1).reinit(n_p);
    system_rhs.collect_sizes();
  }
// @sect4{MixedLaplaceProblem::assemble_system}

// 同样地，组装线性系统的函数在这个例子的介绍中已经讨论过很多了。在它的顶部，发生的是所有常见的步骤，此外，我们不仅为单元项分配正交和 FEValues 对象，而且还为面项分配。之后，我们为变量定义通常的缩写，并为本地矩阵和右手贡献分配空间，以及保存当前单元的全局自由度数的数组。

  template <int dim>
  void MixedLaplaceProblem<dim>::assemble_system()
  {
    QGauss<dim>     quadrature_formula(degree + 2);
    QGauss<dim - 1> face_quadrature_formula(degree + 2);

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

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

    FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     local_rhs(dofs_per_cell);

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

// 下一步是声明代表方程中源项、压力边界值和系数的对象。除了这些代表连续函数的对象外，我们还需要数组来保存它们在各个单元格（或面，对于边界值）的正交点的值。请注意，在系数的情况下，数组必须是矩阵的一种。

    const PrescribedSolution::RightHandSide<dim> right_hand_side;
    const PrescribedSolution::PressureBoundaryValues<dim>
                                            pressure_boundary_values;
    const PrescribedSolution::KInverse<dim> k_inverse;

    std::vector<double>         rhs_values(n_q_points);
    std::vector<double>         boundary_values(n_face_q_points);
    std::vector<Tensor<2, dim>> k_inverse_values(n_q_points);

// 最后，我们需要几个提取器，用来获取矢量值形状函数的速度和压力成分。它们的功能和使用在 @ref vector_valued报告中有详细描述。基本上，我们将把它们作为下面FEValues对象的下标：FEValues对象描述了形状函数的所有矢量分量，而在订阅后，它将只指速度（一组从零分量开始的 dim 分量）或压力（位于 dim 位置的标量分量）。

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);

// 有了这些，我们就可以继续对所有单元进行循环。这个循环的主体已经在介绍中讨论过了，这里就不再做任何评论了。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_values.reinit(cell);
        local_matrix = 0;
        local_rhs    = 0;

        right_hand_side.value_list(fe_values.get_quadrature_points(),
                                   rhs_values);
        k_inverse.value_list(fe_values.get_quadrature_points(),
                             k_inverse_values);

        for (unsigned int q = 0; q < n_q_points; ++q)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              const Tensor<1, dim> phi_i_u = fe_values[velocities].value(i, q);
              const double div_phi_i_u = fe_values[velocities].divergence(i, q);
              const double phi_i_p     = fe_values[pressure].value(i, q);

              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                {
                  const Tensor<1, dim> phi_j_u =
                    fe_values[velocities].value(j, q);
                  const double div_phi_j_u =
                    fe_values[velocities].divergence(j, q);
                  const double phi_j_p = fe_values[pressure].value(j, q);

                  local_matrix(i, j) +=
                    (phi_i_u * k_inverse_values[q] * phi_j_u //
                     - phi_i_p * div_phi_j_u                 //
                     - div_phi_i_u * phi_j_p)                //
                    * fe_values.JxW(q);
                }

              local_rhs(i) += -phi_i_p * rhs_values[q] * fe_values.JxW(q);
            }

        for (const auto &face : cell->face_iterators())
          if (face->at_boundary())
            {
              fe_face_values.reinit(cell, face);

              pressure_boundary_values.value_list(
                fe_face_values.get_quadrature_points(), boundary_values);

              for (unsigned int q = 0; q < n_face_q_points; ++q)
                for (unsigned int i = 0; i < dofs_per_cell; ++i)
                  local_rhs(i) += -(fe_face_values[velocities].value(i, q) * //
                                    fe_face_values.normal_vector(q) *        //
                                    boundary_values[q] *                     //
                                    fe_face_values.JxW(q));
            }

// 循环所有单元的最后一步是将局部贡献转移到全局矩阵和右手向量中。请注意，我们使用的接口与之前的例子完全相同，尽管我们现在使用的是块状矩阵和向量，而不是常规的。换句话说，对于外界来说，块对象具有与矩阵和向量相同的接口，但它们还允许访问单个块。

        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            system_matrix.add(local_dof_indices[i],
                              local_dof_indices[j],
                              local_matrix(i, j));
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          system_rhs(local_dof_indices[i]) += local_rhs(i);
      }
  }
// @sect3{Implementation of linear solvers and preconditioners}

// 我们在这个例子中使用的线性求解器和预处理器已经在介绍中进行了详细的讨论。因此，我们在这里不再讨论我们的方法的原理，而只是对剩下的一些实现方面进行评论。

//  @sect4{MixedLaplace::solve}

// 正如在介绍中所概述的那样，求解函数基本上由两个步骤组成。首先，我们必须形成涉及舒尔补数的第一个方程，并求解压力（解决方案的第一部分）。然后，我们可以从第二个方程（解的第0部分）中重构速度。

  template <int dim>
  void MixedLaplaceProblem<dim>::solve()
  {

// 作为第一步，我们声明对矩阵的所有块状成分、右手边和我们将需要的解向量的引用。

    const auto &M = system_matrix.block(0, 0);
    const auto &B = system_matrix.block(0, 1);

    const auto &F = system_rhs.block(0);
    const auto &G = system_rhs.block(1);

    auto &U = solution.block(0);
    auto &P = solution.block(1);

// 然后，我们将创建相应的LinearOperator对象并创建 op_M_inv 运算器。

    const auto op_M = linear_operator(M);
    const auto op_B = linear_operator(B);

    ReductionControl         reduction_control_M(2000, 1.0e-18, 1.0e-10);
    SolverCG<Vector<double>> solver_M(reduction_control_M);
    PreconditionJacobi<SparseMatrix<double>> preconditioner_M;

    preconditioner_M.initialize(M);

    const auto op_M_inv = inverse_operator(op_M, solver_M, preconditioner_M);

// 这样我们就可以声明舒尔补数  op_S  和近似舒尔补数  op_aS  。

    const auto op_S = transpose_operator(op_B) * op_M_inv * op_B;
    const auto op_aS =
      transpose_operator(op_B) * linear_operator(preconditioner_M) * op_B;

// 我们现在从 op_aS 中创建一个预处理程序，应用固定数量的30次（便宜的）CG迭代。

    IterationNumberControl   iteration_number_control_aS(30, 1.e-18);
    SolverCG<Vector<double>> solver_aS(iteration_number_control_aS);

    const auto preconditioner_S =
      inverse_operator(op_aS, solver_aS, PreconditionIdentity());

// 现在来看看第一个方程。它的右边是 $B^TM^{-1}F-G$  ，这就是我们在前几行计算的结果。然后我们用CG求解器和我们刚刚声明的预处理程序来解决第一个方程。

    const auto schur_rhs = transpose_operator(op_B) * op_M_inv * F - G;

    SolverControl            solver_control_S(2000, 1.e-12);
    SolverCG<Vector<double>> solver_S(solver_control_S);

    const auto op_S_inv = inverse_operator(op_S, solver_S, preconditioner_S);

    P = op_S_inv * schur_rhs;

    std::cout << solver_control_S.last_step()
              << " CG Schur complement iterations to obtain convergence."
              << std::endl;

// 得到压力后，我们可以计算速度。方程为 $MU=-BP+F$  ，我们通过首先计算右手边，然后与代表质量矩阵逆的对象相乘来解决这个问题。

    U = op_M_inv * (F - op_B * P);
  }
// @sect3{MixedLaplaceProblem class implementation (continued)}
// @sect4{MixedLaplace::compute_errors}

// 在我们处理完线性求解器和预处理器之后，我们继续实现我们的主类。特别是，下一个任务是计算我们数值解的误差，包括压力和速度。

// 为了计算解的误差，我们已经在  step-7  和  step-11  中介绍了  VectorTools::integrate_difference  函数。然而，在那里我们只处理了标量解，而在这里我们有一个矢量值的解，其组成部分甚至表示不同的量，并且可能有不同的收敛阶数（由于所使用的有限元的选择，这里不是这种情况，但在混合有限元应用中经常出现这种情况）。因此，我们要做的是 "掩盖 "我们感兴趣的成分。这很容易做到： VectorTools::integrate_difference 函数将一个指向权重函数的指针作为其参数之一（该参数默认为空指针，意味着单位权重）。我们要做的是传递一个函数对象，在我们感兴趣的成分中等于1，而在其他成分中等于0。例如，为了计算压力误差，我们应该传入一个函数，该函数在分量 dim 中代表单位值的常数向量，而对于速度，常数向量在第一个 dim 分量中应该是1，而在压力的位置是0。

// 在deal.II中， ComponentSelectFunction 正是这样做的：它想知道它要表示的函数应该有多少个向量分量（在我们的例子中，这将是 dim+1 ，用于联合速度-压力空间），哪个个体或范围的分量应该等于1。因此，我们在函数的开头定义了两个这样的掩码，接下来是一个代表精确解的对象和一个向量，我们将在其中存储由 integrate_difference 计算的单元误差。

  template <int dim>
  void MixedLaplaceProblem<dim>::compute_errors() const
  {
    const ComponentSelectFunction<dim> pressure_mask(dim, dim + 1);
    const ComponentSelectFunction<dim> velocity_mask(std::make_pair(0, dim),
                                                     dim + 1);

    PrescribedSolution::ExactSolution<dim> exact_solution;
    Vector<double> cellwise_errors(triangulation.n_active_cells());

// 正如在 step-7 中已经讨论过的那样，我们必须认识到，不可能精确地整合误差。我们所能做的就是用正交法对这个积分进行近似。这实际上在这里提出了一个小小的转折：如果我们像人们可能倾向于做的那样天真地选择一个 QGauss@<dim@>(degree+1) 类型的对象（这就是我们用于积分线性系统的对象），就会发现误差非常小，根本不遵循预期的收敛曲线。现在的情况是，对于这里使用的混合有限元，高斯点恰好是超收敛点，其中的点误差要比其他地方小得多（而且收敛的阶数更高）。因此，这些点不是特别好的积分点。为了避免这个问题，我们只需使用梯形法则，并在每个坐标方向上迭代 degree+2 次（同样如 step-7 中的解释）。

    QTrapezoid<1>  q_trapez;
    QIterated<dim> quadrature(q_trapez, degree + 2);

// 有了这个，我们就可以让库计算出误差并将其输出到屏幕上。

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      exact_solution,
                                      cellwise_errors,
                                      quadrature,
                                      VectorTools::L2_norm,
                                      &pressure_mask);
    const double p_l2_error =
      VectorTools::compute_global_error(triangulation,
                                        cellwise_errors,
                                        VectorTools::L2_norm);

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      exact_solution,
                                      cellwise_errors,
                                      quadrature,
                                      VectorTools::L2_norm,
                                      &velocity_mask);
    const double u_l2_error =
      VectorTools::compute_global_error(triangulation,
                                        cellwise_errors,
                                        VectorTools::L2_norm);

    std::cout << "Errors: ||e_p||_L2 = " << p_l2_error
              << ",   ||e_u||_L2 = " << u_l2_error << std::endl;
  }
// @sect4{MixedLaplace::output_results}

// 最后一个有趣的函数是我们生成图形输出的函数。请注意，所有的速度分量都得到相同的解名 "u"。再加上使用 DataComponentInterpretation::component_is_part_of_vector ，这将导致 DataOut<dim>::write_vtu() 生成各个速度分量的矢量表示，更多信息请参见 step-22 或 @ref VVOutput 模块中的 "生成图形输出 "部分。最后，对于高阶元素来说，在图形输出中每个单元只显示一个双线性四边形似乎不合适。因此，我们生成大小为(度数+1)x(度数+1)的斑块来捕捉解决方案的全部信息内容。有关这方面的更多信息，请参见 step-7 的教程程序。

  template <int dim>
  void MixedLaplaceProblem<dim>::output_results() const
  {
    std::vector<std::string> solution_names(dim, "u");
    solution_names.emplace_back("p");
    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      interpretation(dim,
                     DataComponentInterpretation::component_is_part_of_vector);
    interpretation.push_back(DataComponentInterpretation::component_is_scalar);

    DataOut<dim> data_out;
    data_out.add_data_vector(dof_handler,
                             solution,
                             solution_names,
                             interpretation);

    data_out.build_patches(degree + 1);

    std::ofstream output("solution.vtu");
    data_out.write_vtu(output);
  }

//  @sect4{MixedLaplace::run}

// 这是我们主类的最后一个函数。它唯一的工作是按照自然顺序调用其他函数。

  template <int dim>
  void MixedLaplaceProblem<dim>::run()
  {
    make_grid_and_dofs();
    assemble_system();
    solve();
    compute_errors();
    output_results();
  }
} // namespace Step20
// @sect3{The main function}

// 我们从  step-6  而不是  step-4  那里偷来的主函数。它几乎等同于 step-6 中的函数（当然，除了改变的类名），唯一的例外是我们将有限元空间的度数传递给混合拉普拉斯问题的构造函数（这里，我们使用零阶元素）。

int main()
{
  try
    {
      using namespace Step20;

      const unsigned int     fe_degree = 0;
      MixedLaplaceProblem<2> mixed_laplace_problem(fe_degree);
      mixed_laplace_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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

  return 0;
}


/**
 This file is part of Poisson Image Editing.

 Copyright Christoph Heindl 2015

 Poisson Image Editing is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Poisson Image Editing is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Poisson Image Editing.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <blend/poisson_solver.h>
#include <opencv2/opencv.hpp>
#pragma warning (push)
#pragma warning (disable: 4244)
#include <Eigen/Sparse>
#include <Eigen/Dense>
#pragma warning (pop)
#include <bitset>

namespace blend {

    bool isSameSize(cv::Size a, cv::Size b) {
        return a.width == b.width && a.height == b.height;
    }

    /* Make matrix memory continuous. */
    cv::Mat makeContinuous(cv::Mat m) {
        if (!m.isContinuous()) {
            m = m.clone();
        }
        return m;
    }

    /* Build a one dimensional index lookup for element in mask. */
    cv::Mat buildPixelToIndexLookup(cv::InputArray mask, int &npixel)
    {
        cv::Mat_<uchar> m = makeContinuous(mask.getMat());

        cv::Mat_<int> pixelToIndex(mask.size());
        npixel = 0;

        int *pixelToIndexPtr = pixelToIndex.ptr<int>();
        const uchar *maskPtr = m.ptr<uchar>();

        for (int id = 0; id < (m.rows * m.cols); ++id) {
            pixelToIndexPtr[id] = (maskPtr[id] == constants::DIRICHLET_BD) ? -1 : npixel++;
        }

        return pixelToIndex;
    }

    void solvePoissonEquations(
        cv::InputArray f_,
        cv::InputArray bdMask_,
        cv::InputArray bdValues_,
        cv::OutputArray result_)
    {
        // Input validation

        CV_Assert(
            !f_.empty() &&
            isSameSize(f_.size(), bdMask_.size()) &&
            isSameSize(f_.size(), bdValues_.size())
        );

        CV_Assert(
            f_.depth() == CV_32F &&
            bdMask_.depth() == CV_8U &&
            bdValues_.depth() == CV_32F &&
            f_.channels() == bdValues_.channels() &&
            bdMask_.channels() == 1);

        // We assume continuous memory on input
        cv::Mat f = makeContinuous(f_.getMat());
        cv::Mat_<uchar> bm = makeContinuous(bdMask_.getMat());
        cv::Mat bv = makeContinuous(bdValues_.getMat());

        // Allocate output
        result_.create(f.size(), f.type());
        cv::Mat r = result_.getMat();
        bv.copyTo(r, bm == constants::DIRICHLET_BD);

        // The number of unknowns correspond to the number of pixels on the rectangular region
        // that don't have a Dirichlet boundary condition.
        int nUnknowns = 0;
        cv::Mat_<int> unknownIdx = buildPixelToIndexLookup(bm, nUnknowns);

        if (nUnknowns == 0) {
            // No unknowns left, we're done
            return;
        } else if (nUnknowns == f.size().area()) {
            // All unknowns, will not lead to a unique solution
            // TODO emit warning
        }

        const cv::Rect bounds(0, 0, f.cols, f.rows);

        // Directional indices
        const int center = 0;
        const int north = 1;
        const int east = 2;
        const int south = 3;
        const int west = 4;

        // Neighbor offsets in all directions
        const int offsets[5][2] = { { 0, 0 }, { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };

        // Directional opposite
        const int opposite[5] = { center, south, west, north, east };
        const int channels = f.channels();

        std::vector< Eigen::Triplet<float> > lhsTriplets;
        lhsTriplets.reserve(nUnknowns * 5);

        Eigen::MatrixXf rhs(nUnknowns, channels);
        rhs.setZero();

        // Loop over domain once. The coefficient matrix A is the same for all
        // channels, the right hand side is channel dependent.

        for (int y = 0; y < f.rows; ++y) {
            for (int x = 0; x < r.cols; ++x) {

                const cv::Point p(x, y);
                const int pid = unknownIdx(p);

                if (pid == -1) {
                    // Current pixel is not an unknown, skip
                    continue;
                }

                // Start coefficients of left hand side. Based on discrete Laplacian with central difference.
                float lhs[] = { -4.f, 1.f, 1.f, 1.f, 1.f };

                const bool hasNeumann = (bm(p) == constants::NEUMANN_BD);

                if (hasNeumann) {

                    // Implementation note:
                    //
                    // We first sweep over all neighbors and apply Neumann boundary (NB) conditions if necessary.
                    // NBs are currently only applied if the neighbor is not in the domain or it has Dirichlet
                    // boundary condition (DB).
                    //
                    // When the neighbor is not available we introduce ghost points which are immediately
                    // removed by substitution. Assume that we are at a pixel C at the top border (not corner)
                    // and that pixel is assigned a NB = 1. Denoting the pixels C, N, E, S, W we have for C
                    // the Laplacian
                    //      1: -4C + N + E + S + W = f(x)
                    // From NB we have
                    //      2: (N - S) * 0.5 = 1
                    // As N is not in the domain we need to get rid of it through substitution. Rewriting 2:
                    //      N = 2 + S
                    // and substituting in 1:
                    //      -4C + (2 + S) + E + S + W = f(x)
                    //      -4C + E + 2S + W = f(x) - 2

                    for (int n = 1; n < 5; ++n) {
                        const cv::Point q(x + offsets[n][0], y + offsets[n][1]);

                        if (!bounds.contains(q) || bm(q) == constants::DIRICHLET_BD) {
                            lhs[opposite[n]] += 1.0f;
                            lhs[n] = 0.f;
                            rhs.row(pid) += 2.f * Eigen::Map<Eigen::VectorXf>(bv.ptr<float>(p.y, p.x), channels);
                        }
                    }
                }

                for (int n = 1; n < 5; ++n) {
                    const cv::Point q(x + offsets[n][0], y + offsets[n][1]);

                    const bool hasNeighbor = bounds.contains(q);
                    const bool isNeighborDirichlet = hasNeighbor && (bm(q) == constants::DIRICHLET_BD);

                    if (!hasNeumann && !hasNeighbor) {
                        lhs[center] += lhs[n];
                        lhs[n] = 0.f;
                    } else if (isNeighborDirichlet) {

                        // Implementation note:
                        //
                        // Dirichlet boundary conditions (DB) turn neighbor unknowns into knowns (data) and
                        // are therefore moved to the right hand side. Alternatively, we could add more
                        // equations for these pixels setting the lhs 1 and rhs to the Dirichlet value, but
                        // that would unnecessarily blow up the equation system.

                        rhs.row(pid) -= lhs[n] * Eigen::Map<Eigen::VectorXf>(bv.ptr<float>(q.y, q.x), channels);
                        lhs[n] = 0.f;
                    }
                }

                // Add f to rhs.
                rhs.row(pid) += Eigen::Map<Eigen::VectorXf>(f.ptr<float>(p.y, p.x), channels);

                // Build triplets for row
                for (int n = 0; n < 5; ++n) {
                    if (lhs[n] != 0.f) {
                        const cv::Point q(x + offsets[n][0], y + offsets[n][1]);
                        lhsTriplets.push_back(Eigen::Triplet<float>(pid, unknownIdx(q), lhs[n]));
                    }
                }

            }
        }

        // Solve the sparse linear system of equations

        Eigen::SparseMatrix<float> A(nUnknowns, nUnknowns);
        A.setFromTriplets(lhsTriplets.begin(), lhsTriplets.end());

        Eigen::SparseLU< Eigen::SparseMatrix<float> > solver;
        solver.analyzePattern(A);
        solver.factorize(A);

        Eigen::MatrixXf result(nUnknowns, channels);
        for (int c = 0; c < channels; ++c)
            result.col(c) = solver.solve(rhs.col(c));

        // Copy results back

        for (int y = 0; y < f.rows; ++y) {
            for (int x = 0; x < f.cols; ++x) {
                const cv::Point p(x, y);
                const int pid = unknownIdx(p);

                if (pid > -1) {
                    Eigen::Map<Eigen::VectorXf>(r.ptr<float>(p.y, p.x), channels) = result.row(pid);
                }

            }
        }

    }

}


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

/**
 * \file AbsCoeffGas.cpp
 */

#define PI 3.14159265

#include <radprops/AbsCoeffGas.h>
#include <radprops/RadiativeSpecies.h>
#include <iostream>
#include <fstream>
#include <iomanip>   // format manipulation
#include <vector>
#include <stdexcept>
#include <cmath>
#include <cassert>
#include <limits>
#include <algorithm>

#include <radprops/GPUHelper.h>

#ifdef RadProps_ENABLE_PREPROCESSOR
#include <boost/filesystem.hpp>
#include <boost/regex.hpp>
#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>
#endif

namespace RadProps {
using namespace std;

/**
 * @brief Implements the composite trapezoid rule for uniformly spaced data.
 * @fn double trapz( const double dx, const vector<double>& fx )
 * @param dx the spacing (assumed constant)
 * @param fx the function values
 * @return the integral
 */
double trapz( const double dx, const vector<double>& fx )
{
  const size_t n = fx.size();  assert( n>2 );
  // composite trapezoid rule
  double integral = 0.5*(fx[0] + fx[n-1]);
  for( size_t i=1; i<n-1; ++i ){
    integral += fx[i];
  }
  integral *= dx;
  return integral;
}

/**
 * @fn double planck_blackbody_intensity( const double eta, const double tref )
 * @param eta  wavenumber (1/cm)
 * @param tref temperature (K)
 * @return the Planck blackbody intensity (J/cm^2)
 *  calculates
 *  \f[
 *  I(\nu,T) = 2 h c^2 \nu^3 \frac{1}{\exp(\frac{h c \nu}{k T})-1}
 *  \f]
 *  where c is the speed of sound, k is the Boltzmann constant, h is the Planck constant.
 *  See  this link for more information.
 */
inline double planck_blackbody_intensity( const double eta, const double tref )
{
  const double h = 6.626070e-34;   // Planck constant (J s);
  const double c = 2.997925e10;    // Speed of light in vacuum (cm/s);
  const double k = 1.380658e-23;   // Boltzmann constant (J/K);
  return 2*h*c*c*pow(eta,3.0) / ( (exp(h*c*eta/(k*tref)) - 1.0) );
}

namespace detail{

template< typename IndexT, typename ValT >
IndexT
index_finder( const ValT& x,
              const std::vector<ValT>& xgrid,
              const bool allowClipping=false )
{
  const size_t nx = xgrid.size();
  IndexT ilo = 0;
  IndexT ihi = nx-1;

  if( allowClipping && x<xgrid.front() ) return ilo;
  if( allowClipping && x>xgrid.back()  ) return ihi;

  // sanity check
  if( x<xgrid.front() || x>xgrid.back() ){
    std::ostringstream msg;
    msg << __FILE__ << " : " << __LINE__ << endl
        << "root is not bracketed!" << endl;
    throw std::runtime_error(msg.str());
  }

  // regula falsi method to find lower index

  while( ihi-ilo > 1 ){
    const ValT m = ( xgrid[ihi]-xgrid[ilo] ) / ValT( ihi-ilo );
    const IndexT c = std::max( ilo+1, IndexT( ihi - (xgrid[ihi]-x)/m ) );
    assert( c>0 && c<xgrid.size() );
    if( x >= xgrid[c] )
      ilo = std::min(ihi-1,c);
    else
      ihi = std::max(ilo+1,c);
  }
  // error checking:
  if( !allowClipping && (x > xgrid[ihi] || x < xgrid[ilo]) ){
    std::ostringstream msg;
    msg << __FILE__ << " : " << __LINE__ << std::endl
        << " Regula falsi failed to converge properly!" << std::endl
        << " Target x=" << x << std::endl
        << " xlo=" << xgrid[ilo] << ", xhi=" << xgrid[ihi]
        << std::endl << std::endl;
    throw std::runtime_error( msg.str() );
  }

  return ilo;
}

}

struct RadiationData{
  double waveNumber;    ///< wavenumber, cm
  double absCoeff;      ///< absorption coefficient, 1/cm
};

std::ostream& operator<<(std::ostream& out, const RadiationData& data ){
  out << data.waveNumber << "," << data.absCoeff;
  return out;
}

std::ifstream& operator>>( std::ifstream& in, RadiationData& data ){
  in >> data.waveNumber >> data.absCoeff;
  return in;
}

SpeciesAbsCoeff::SpeciesAbsCoeff( const string& filename, const double temperature )
 : temperature_( temperature )
{

  std::ifstream file;
  file.open(filename.c_str(),std::ios::in); // opens as ASCII!
  if( file.bad() ){
    std::ostringstream msg;
    msg << "ERROR! Could not open file '" << filename << "'";
    throw std::runtime_error( msg.str() );
  }

  RadiationData rdata;
  file >> rdata;
  npts_ = 1;
  loWaveNum_ = rdata.waveNumber;        // the lowest wavenumber in the dataset
  double wvn = rdata.waveNumber;        // the previous wave number we read
  double wvnSpc = 0;                    // the wavenumber spacing between
  while( !file.eof() ){
    ++npts_;
    file >> rdata;                      // read the next entry
    absCoeff_.push_back( rdata.absCoeff );
    wvnSpc = rdata.waveNumber - wvn;
    if( std::abs((rdata.waveNumber-wvn)-wvnSpc) > 1e-10 ){
      ostringstream msg;
      msg << __FILE__ << " : " << __LINE__
          << "\nInconsistency in wavenumber spacing! " << wvnSpc << "\n";
      throw runtime_error( msg.str() );
    }
    wvn = rdata.waveNumber;             // update the previous wavenumber
  }
  hiWaveNum_  = rdata.waveNumber;       // the highest wavenumber in the dataset
  waveNumInc_ = (hiWaveNum_-loWaveNum_)/double(npts_-1);
  file.close();
}

double
SpeciesAbsCoeff::planck_abs_coeff() const
{
  std::vector<double> plnckfunc1(npts_,0.0);
//  std::vector<double> plnckfunc2(npts_,0.0);  // use this for consistency checking
  const double sigma = 5.670373e-12;    // Stefan� Boltzmann constant (J/(cm^2 s K^4))

  for( size_t i=0; i<npts_; ++i ){
    // compute the integrand, which is I * kappa -- see equation 2.21 in Lyubima's thesis
    plnckfunc1[i] = planck_blackbody_intensity( loWaveNum_ + i*waveNumInc_, temperature_ ) * absCoeff_[i];
    //    plnckfunc2[i] = planck_blackbody_intensity(wvnmB_+i*wvnmst_,myTref);
  }

  const double integral = trapz( waveNumInc_, plnckfunc1 );
  const double tt = integral*PI/( sigma * pow(temperature_,4.0) );
//  const double tt2 = trapz(wvnmst_,plnckfunc1)/trapz(wvnmst_,plnckfunc2);
////  std::cout << " original: "<<  tt
////            << " , ratio: " << tt2 << std::endl;
//  if( std::abs(tt-tt2)/tt2 > 0.1 )
//    std::cout << "\tT=" << myTref << ", " << std::abs(tt-tt2)/tt2 << std::endl;

  return tt;
}

double
SpeciesAbsCoeff::rosseland_abs_coeff() const
{
  const double sigma = 5.67e-12;   // Stefan� Boltzmann constant (J/(cm^2 s K^4))
  const double h = 6.626070e-34;   // Planck constant (J s);
  const double c = 2.997925e10;    // Speed of light in vacuum (cm/s);
  const double k = 1.380658e-23;   // Boltzmann constant (J/K);
  const double tmp = 2.0*c*c*c*h*h/(k*temperature_*temperature_);

  std::vector<double> rossfunc1(npts_,0.0);
//  std::vector<double> rossfunc2(N_,0.0);  // use this for consistency checking
  for( int i=0; i<npts_; ++i ){
    if( absCoeff_[i] > 1e-16 ){
      /* Matlab commands to get dI/dT:
          syms h c k T v;
          I = 2*h*c^2*v^3/(exp(h*c*v/k/T)-1);
          pretty(I)
          pretty(diff(I,T))
       */
      const double wvn = (loWaveNum_+i*waveNumInc_);
      const double expTerm = exp(h*c*wvn/(k*temperature_));
      rossfunc1[i] = tmp * pow(wvn,4.0) * expTerm/pow(expTerm-1.0,2.0) / absCoeff_[i];
//      rossfunc2[i] = tmp * pow(wvn,4.0) * expTerm/pow(expTerm-1.0,2.0);
    }
  }

  const double integral = trapz( waveNumInc_,rossfunc1 ) * PI / (4.0*sigma*pow(temperature_,3.0) );
  return 1.0/integral;
//  const double t1 = trapz(wvnmst_,rossfunc1);
//  const double t2 = trapz(wvnmst_,rossfunc2);
//  std::cout << t1 <<" , " << t2 << " , " << t2/t1 << ", " << 4.0*sigma*pow(myTref,3.0)/(PI*t1) << "\n";
////  return trapz(wvnmst_,rossfunc2)/trapz(wvnmst_,rossfunc1);
//  return 4.0*sigma*pow(myTref,3.0)/(PI*t1);
}

double
SpeciesAbsCoeff::effective_abs_coeff( const double opl ) const
{
  std::vector<double> plnckfunc1(npts_,0.0);
  std::vector<double> plnckfunc2(npts_,0.0);
  for( int i=0; i<npts_; i++ ){
    const double tmp = planck_blackbody_intensity(loWaveNum_+i*waveNumInc_,temperature_)*exp(-absCoeff_[i]*opl);
    plnckfunc1[i] = tmp * absCoeff_[i];
    plnckfunc2[i] = tmp;
  }
  return trapz(waveNumInc_,plnckfunc1) / trapz(waveNumInc_,plnckfunc2);
}

//==============================================================================

void
SpeciesGData::read( std::istream& file )
{
  data.clear();
  size_t ntemp;
  file >> speciesName >> ntemp;
  for( size_t j=0; j<ntemp; ++j){
    SpeciesGDataSingleTemperature d;
    d.read(file);
    data.push_back(d);
  }
}

void
SpeciesGData::write( std::ostream& file ) const
{
  file << speciesName << std::endl;
  file << data.size() << std::endl;
  for( size_t j=0; j<data.size(); ++j ){
    data[j].write(file);
  }
}

void
SpeciesGDataSingleTemperature::read( std::istream& file )
{
  size_t ng;
  file >> temperature >> ng;
  gvalues.resize(ng,0.0);
  kvalues.resize(ng,0.0);
  for( size_t i=0; i<ng; ++i ){
    file >> gvalues[i] >> kvalues[i];
  }
}

void
SpeciesGDataSingleTemperature::write( std::ostream& file ) const
{
  file << std::setprecision( std::numeric_limits<double>::digits10 );
  file << temperature << std::endl
       << gvalues.size() << std::endl;
  for( int i=0; i<gvalues.size(); ++i ){
    file << gvalues[i] << " " << kvalues[i]<<endl;
  }
}

SpeciesGDataSingleTemperature
calculate_plank( const SpeciesAbsCoeff& absCoeffMix,
                 const double myTref )
{
  SpeciesGDataSingleTemperature gandk;

  const std::vector<double>& absCoef = absCoeffMix.get_coeffs();
  const size_t nwvnm = absCoef.size();

  const double kmin = *std::min_element( absCoef.begin(), absCoef.end() );
  const double kmax = *std::max_element( absCoef.begin(), absCoef.end() );

  const double pwr = 0.1;
  const double pwrk_min = std::pow(kmin,pwr);
  const double pwrk_max = std::pow(kmax,pwr);

  const int n_pwrk=5;  //Number of g-values

  std::vector<double> pwrk( n_pwrk,   0.0 );
  std::vector<double> ff  ( n_pwrk+1, 0.0 );
  std::vector<double> gg  ( n_pwrk+1, 0.0 );
  std::vector<double>  k  ( n_pwrk+1, 0.0 );

  double sum00 = 0;
  for( size_t j=0; j<n_pwrk; ++j ){
    const double pwrk_step = (pwrk_max - pwrk_min)*(log(j+2)-log(j+1)) / log(n_pwrk+2);
    sum00 += pwrk_step;
    pwrk[j] = sum00+kmin-pwrk_step/2;
    k[j] = std::pow( pwrk[j], 1.0/pwr );
  }

  const double hhh = 6.626076e-34;  // Planck constant (Js);
  const double ccc = 2.997925e10;   // Speed of light in vacuum (cm/s)
  const double kkk = 1.380658e-23;  // Boltzmann constant (J/K);
  const double hck = hhh*ccc/kkk;

  const double c1      = 3.7419e-12; // First radiation constant (W cm^2)
  const double sigma   = 5.67e-12;   // Stefan Boltzmann constant (J/(cm^2 s K^4))

  for( size_t i=0; i<nwvnm; ++i ){
    const double wvnm_b = absCoeffMix.min_wavenumber();    // Min wavenumber (1/cm)
    const double wvnm_e = absCoeffMix.max_wavenumber();    // Max wavenumber (1/cm)
    const double wvnmst = absCoeffMix.wavenumber_step();   // wavenumber step (1/cm)
    const double c1sigt4 = c1/(sigma*std::pow(myTref,4))*wvnmst;
    const double eb = c1sigt4 * std::pow((wvnm_b+wvnmst*i),3) / ( exp(hck/myTref*(wvnm_b+wvnmst*i)) - 1.0 );
    const double kpwri = pow( absCoef[i], pwr );
    vector<double>::const_iterator up = std::upper_bound( pwrk.begin(), pwrk.end(), kpwri );
    const size_t iadd = up - pwrk.begin();
    ff[iadd] += eb;
  }
  double fh=0;
  for( size_t l=0; l<n_pwrk+1; ++l ){
    fh += ff[l];
    gg[l] = fh;
  }

  // jcs why are the temporary arrays of length n_pwrk+1 but the resultant array is length n_pwrk ???
  gandk.temperature=myTref;
  for( size_t i=0; i<n_pwrk-1; ++i ){
    gandk.gvalues.push_back(gg[i]);
    gandk.kvalues.push_back( k[i]);
  }

  gandk.gvalues.push_back(1);
  gandk.kvalues.push_back(kmax);

  return gandk;
}

#ifdef RadProps_ENABLE_PREPROCESSOR

// sorts "data" and populates the vector of indices that indicate the sort pattern
template<typename T>
void
paired_sort( vector<size_t> & index,
             vector<T> & data )
{
  // A vector of a pair which will contain the sorted value and its index in the original array
  vector<pair<T,size_t> > indexedPair;
  indexedPair.resize(data.size());
  for( size_t i=0; i<indexedPair.size(); ++i ){
    indexedPair[i].first = data[i];
    indexedPair[i].second = i;
  }
  sort( indexedPair.begin(), indexedPair.end() );
  index.resize( data.size() );
  for( size_t i = 0; i < index.size(); ++i ){
    index[i] = indexedPair[i].second;
    data [i] = indexedPair[i].first;
  }
}

/**
 *
 * @param specNam the name of the species to load files for
 * @param path the path to search
 * @param tempVec the sorted vector of temperatures (output)
 * @param sortFname the sorted vector of file names (output)
 */
void
get_files_sorted_by_temperature( const string specNam, const string path,
                                 vector<double>& tempVec,
                                 vector<string>& sortFname )
{
  const string firstPart( "AbsCoeff" + specNam + "T" );
  const string  lastPart( "txt" );
  const boost::regex filter( firstPart + ".*" + lastPart );  // the ".*" is a wildcard matching operation.

  vector<string> fnames;
  try{
    boost::filesystem::directory_iterator end;
    for( boost::filesystem::directory_iterator i(path); i!=end; ++i ){

      if( !boost::filesystem::is_regular_file( i->status() ) ) continue; // only look at regular files (not dirs)

      const std::string fname = i->path().filename().string();

      // Skip if no match
      boost::smatch what;
      if( !boost::regex_match( fname, what, filter ) ) continue;
      // grab off the temperature for this file
      const string tmp = fname.substr( firstPart.size(), fname.size()-firstPart.size()-lastPart.size() );
      const double temp = boost::lexical_cast<double>(tmp);

      // File matches. parse it
      tempVec.push_back(temp);
      const string p( path+"/"+fname );
      fnames.push_back( p );
      assert( boost::filesystem::is_regular_file(p) );
    }
  }
  catch( std::exception& err ){
    std::ostringstream msg;
    msg << "ERROR loading files.  Details follow:\n" << err.what();
    throw std::runtime_error( msg.str() );
  }

  // sort by temperature
  vector<size_t> myIndex;
  paired_sort( myIndex, tempVec );

  sortFname.clear();
  for( size_t i=0; i<myIndex.size(); ++i ){
    sortFname.push_back( fnames[ myIndex[i] ] );
  }
}

SpeciesAbsData
load_species_abs_coefs( const string& mySp,
                        const string path="." )
{
  SpeciesAbsData specData;
  specData.speciesName = mySp;
  std::vector<SpeciesAbsCoeff> speciesAbsCoeffTempVector;
  std::vector<double>& tempVec = specData.temperatures;
  std::vector<SpeciesAbsCoeff>& specAbsCoefs = specData.absCoeff;

  vector<string> fnames;
  get_files_sorted_by_temperature( mySp, path, tempVec, fnames );

  for( size_t i=0; i<fnames.size(); ++i ){
    cout << "\t-> loading " << fnames[i] << endl;
    specAbsCoefs.push_back( SpeciesAbsCoeff(fnames[i],tempVec[i]) );
  }
  return specData;
}

FSK::FSK( const std::vector<RadiativeSpecies>& mySpecies,
          const string outputFileName,
          const string path )
  : allowClipping_( true )
{
  speciesOrder_ = mySpecies;
  std::vector<std::string> spnam;
  for( size_t j=0; j<mySpecies.size(); ++j ){
    spnam.push_back( species_name(mySpecies[j]) );
    cout << "Species = " << spnam[j] << endl;
  }

  const size_t nspecies=spnam.size();
  for( size_t isp=0; isp<nspecies; ++isp ){

    cout << "loading data for species " << spnam[isp] << endl;
    const SpeciesAbsData specAbsData = load_species_abs_coefs( spnam[isp], path );

    SpeciesGData gSpIV;
    gSpIV.speciesName = spnam[isp];
    const size_t ntemp = specAbsData.temperatures.size();
    std::vector<SpeciesGDataSingleTemperature>& calFSK = gSpIV.data;
    for( size_t itemp=0; itemp<ntemp; ++itemp ){
      const SpeciesAbsCoeff& sac = specAbsData.absCoeff[itemp];
      calFSK.push_back( calculate_plank( specAbsData.absCoeff[itemp],
                                         specAbsData.temperatures[itemp] ) );
    }
//    cout << endl << "G data for " << spnam[isp] << endl;
//    gSpIV.write(cout);
//    cout << endl << endl;
    spCalFSK_.push_back(gSpIV);
  }

  ofstream gfile( outputFileName.c_str() );
  gfile << nspecies << endl;
  for( size_t ll=0; ll<nspecies; ++ll ){
    spCalFSK_[ll].write(gfile);
  }
  gfile.close();
  cout << endl << "Processed FSK data has been written to " << outputFileName << endl << endl;
}
#endif // RadProps_ENABLE_PREPROCESSOR

FSK::FSK( const string fileN )
  : allowClipping_( true )
{
  cout << "Loading FSK Radiation data file: " << fileN << endl;

  std::ifstream fileM2( fileN.c_str(), std::ios::in ); // opens as ASCII!
  if( !fileM2.good() ){
    std::ostringstream msg;
    msg << "ERROR! Could not open file '" << fileN << "'" << std::endl
        << __FILE__ << " : " << __LINE__;
    throw std::runtime_error( msg.str() );
  }

  int nspecies;
  fileM2 >> nspecies;
  for( size_t isp=0; isp<nspecies; ++isp ){
    cout << "loading " << isp+1 << " of " << nspecies << endl;
    SpeciesGData spGData;
    spGData.read( fileM2 );
    spCalFSK_.push_back( spGData );
    speciesOrder_.push_back(species_enum( spCalFSK_[isp].speciesName ));
  }
  fileM2.close();

  // echo file information back out to disk
//  ofstream gfile("TestGs2.txt");
//  gfile << nspecies << endl;
//  for (int ll=0; ll<nspecies; ll++) {
//    spCalFSK_[ll].write(gfile);
//  }
//  gfile.close();

}

double
FSK::mixture_abs_coeff( const std::vector<double>& mixMoleFrac,
                        const double mixT,
                        const double gp ) const
{
  std::vector<double> mixG;
  std::vector<double> mixK;
  mixG.clear();
  mixK.clear();

  mixture_coeffs( mixG, mixK, mixMoleFrac, mixT );

  // clip if we exceed bounds.
  if( gp <= mixG.front() ) return mixK.front();
  if( gp >= mixG.back()  ) return mixK.back();

  const size_t indexg = detail::index_finder<size_t,double>( gp, mixG, allowClipping_ );
  return mixK[indexg]+(mixK[indexg+1]-mixK[indexg])*(gp-mixG[indexg])/(mixG[indexg+1]-mixG[indexg]);
}

void
FSK::mixture_coeffs( std::vector<double>& gmix,
                     std::vector<double>& kmix,
                     const std::vector<double>& mixMoleFrac,
                     const double mixT ) const
{
  const size_t nspecies = mixMoleFrac.size();
  const size_t ng = spCalFSK_[0].data[0].gvalues.size();
  gmix.resize(ng,1.0);
  kmix.resize(ng,0.0);

  for( size_t isp=0; isp<nspecies; ++isp ){

    const SpeciesGData& spData = spCalFSK_[isp];

    const size_t ntemp = spData.data.size();
    std::vector<double> tvec(ntemp,0.0);  // jcs this is slow - need to fix it...
    for( size_t j=0; j<ntemp; ++j ){
      tvec[j] = spData.data[j].temperature;
    }

    const size_t tindex = detail::index_finder<size_t,double>( mixT, tvec, allowClipping_ );

    const vector<double>& g     = spData.data[tindex  ].gvalues;
    const vector<double>& gPlus = spData.data[tindex+1].gvalues;
    const vector<double>& k     = spData.data[tindex  ].kvalues;
    const vector<double>& kPlus = spData.data[tindex+1].kvalues;
    const double T              = spData.data[tindex  ].temperature;
    const double Tplus          = spData.data[tindex+1].temperature;

    for( size_t ig=0; ig<ng; ++ig ){
      gmix[ig] *= std::abs( g[ig] + ( gPlus[ig]-g[ig] )*( mixT-T )/( Tplus-T ) );
      kmix[ig] += mixMoleFrac[isp]/nspecies*sqrt(k[ig]*kPlus[ig]);
    }
  }
}

void
FSK::a_function( std::vector<double>& a,
                 const std::vector<double>& mixMoleFrac,
                 const double Tmed,
                 const double Twall ) const
{
  std::vector<double> mediumG, mediumK, wallG, wallK;

  mixture_coeffs(mediumG,mediumK,mixMoleFrac, Tmed);
  mixture_coeffs(wallG,wallK,mixMoleFrac, Twall);
  const size_t L = mediumG.size();

  a.clear();
  a.resize(L,0.0);

  a[0]=((wallG[1]-wallG[0])/(mediumG[1]-mediumG[0]));
  for( size_t l=1; l<mediumG.size()-1; l++ ){
    a[l]=(wallG[l+1]-wallG[l-1])/(mediumG[l+1]-mediumG[l-1]);
  }
  a[L-1]=(wallG[L-1]-wallG[L-2])/(mediumG[L-1]-mediumG[L-2]);
}

//==============================================================================

#ifdef RadProps_ENABLE_PREPROCESSOR
GreyGas::GreyGas( const std::vector<RadiativeSpecies>& mySpecies,
                  const double opl,
                  const std::string outputFileName,
                  const string path )
 : allowClipping_( true ),
   opl_( opl ),
   nspecies_( mySpecies.size() ),
   order_(1)
{
  speciesOrder_=mySpecies;
  std::vector<std::string> speciesNames;
  for( int j=0; j<mySpecies.size(); j++ ){
    speciesNames.push_back( species_name(mySpecies[j]) );
  }

  for( size_t isp=0; isp<nspecies_; ++isp ){
    cout << "Species = " << speciesNames[isp] << endl;
    const SpeciesAbsData specData = load_species_abs_coefs( speciesNames[isp], path );

    // process this species information at each temperature to obtain
    // the mean absorption coefficients
    GreyGasData spData;
    spData.speciesName = speciesNames[isp];
    assert( speciesNames[isp] == specData.speciesName );
    spData.ntemp = specData.temperatures.size();

    for( size_t itemp=0; itemp<spData.ntemp; ++itemp ){
      const SpeciesAbsCoeff& spAbsCoeff = specData.absCoeff[itemp];
      spData.temperatures.push_back( specData.temperatures[itemp] );
      spData.planckCoeff.push_back( spAbsCoeff.planck_abs_coeff()        );
      spData.rossCoeff  .push_back( spAbsCoeff.rosseland_abs_coeff()     );
      spData.effAbsCoeff.push_back( spAbsCoeff.effective_abs_coeff(opl_) );
    }
    data_.push_back(spData);

    LagrangeInterpolant1D interpplank( order_, spData.temperatures, spData.planckCoeff, allowClipping_);
    planckCoeff_.push_back( interpplank );

    LagrangeInterpolant1D interpross( order_, spData.temperatures, spData.rossCoeff, allowClipping_);
    rossCoeff_.push_back( interpross );

    LagrangeInterpolant1D interpeff( order_, spData.temperatures, spData.effAbsCoeff, allowClipping_);
    effAbsCoeff_.push_back( interpeff );
  }

  std::cout << "Writing preprocessed grey-gas properties to file: " << outputFileName << std::endl;
  ofstream out( outputFileName.c_str() );
  out << nspecies_ << endl << opl_ << endl;
  for( size_t isp=0; isp<nspecies_; ++isp ){
    data_[isp].write(out);
  }
}
#endif // RadProps_ENABLE_PREPROCESSOR

GreyGas::GreyGas( const std::string fileName )
: allowClipping_( true ), order_(1)
{
  std::ifstream fileMG( fileName.c_str(), std::ios::in ); // opens as ASCII!
  if( !fileMG.good() ){
    std::ostringstream msg;
    msg << "ERROR! Could not open file '" << fileName << "'" << std::endl
        << __FILE__ << " : " << __LINE__;
    throw std::runtime_error( msg.str() );
  }
  fileMG >> nspecies_ >> opl_;
  const std::vector<string> indepvarname(1, "Temperature");
  for( size_t isp=0; isp<nspecies_; ++isp ){
    GreyGasData data;
    data.read( fileMG );
    if (data.temperatures.size() < order_+1 ) {
      std::ostringstream msg;
      msg << "ERROR! In order to make a table, size of independet variable (temperature) must be higher than  " << order_  << std::endl
      << __FILE__ << " : " << __LINE__;
      throw std::runtime_error( msg.str() );
    }

    LagrangeInterpolant1D interpplank( order_, data.temperatures, data.planckCoeff, allowClipping_);
    planckCoeff_.push_back( interpplank );

    LagrangeInterpolant1D interpross( order_, data.temperatures, data.rossCoeff, allowClipping_);
    rossCoeff_.push_back( interpross );

    LagrangeInterpolant1D interpeff( order_, data.temperatures, data.effAbsCoeff, allowClipping_);
    effAbsCoeff_.push_back( interpeff );
    speciesOrder_.push_back(species_enum( data.speciesName ));
  }
  fileMG.close();
}

#ifdef ENABLE_CUDA
void
GreyGas::gpu_mixture_coeffs(double* result,
                            const std::vector<const double*>& mixMoleFrac,
                            const double* mixT,
                            const size_t indepsize,
                            const MixtureProperties coeffname = EFF_ABS_COEFF)
{

  const size_t nspecies = mixMoleFrac.size();

  const std::vector<const double*> tempvec(1, mixT);

  // initialize results with 0
  cudaMemset( result, 0, sizeof(double) * indepsize);
  GPU_ERROR_CHECK;

  double *coeffi;
  cudaMalloc((void**) &coeffi,  sizeof(double) * indepsize);
  GPU_ERROR_CHECK;

  for (int i=0; i< mixMoleFrac.size(); i++) {

    switch (coeffname) {
      case PLANK_COEFF:
        planckCoeff_[i].gpu_value( tempvec, coeffi,  indepsize);
        break;
      case ROSS_COEFF:
        rossCoeff_  [i].gpu_value( tempvec, coeffi,  indepsize);
        break;
      case EFF_ABS_COEFF:
        effAbsCoeff_[i].gpu_value( tempvec, coeffi,  indepsize);
        break;
      case INVALID_RAD_COEFF:
        ostringstream msg;
        msg << __FILE__ << " : " << __LINE__ << endl << endl
        << "Invalid gray gas propety! \n";
        throw invalid_argument( msg.str() );
    }

    gray_gas_gpu_mixture_coeffs<<<NBLOCK(indepsize), NTHREAD(indepsize)>>>(result, coeffi, mixMoleFrac[i], indepsize);
    GPU_ERROR_CHECK;
  }
  cudaThreadSynchronize();
  GPU_ERROR_CHECK;
}

#endif

void
GreyGas::mixture_coeffs( double& result,
                         const std::vector<double>& mixMoleFrac,
                         const double mixT,
                         const MixtureProperties coeffname = EFF_ABS_COEFF) const
{
  if( nspecies_ != mixMoleFrac.size() ){
    ostringstream msg;
    msg << __FILE__ << " : " << __LINE__ << endl << endl
        << "The number of species supplied to 'GreyGas::mixture_coeffs()' is not consistent with the number in the table\n"
        << "  Number in table: " << nspecies_ << endl
        << "  Number supplied: " << mixMoleFrac.size() << endl << endl;
    throw invalid_argument( msg.str() );
  }
  result = 0;
  switch (coeffname) {
    case PLANK_COEFF:
      for( size_t i=0; i<nspecies_; i++ ) result += mixMoleFrac[i] * planckCoeff_[i].value(mixT);
      break;
    case ROSS_COEFF:
      for( size_t i=0; i<nspecies_; i++ ) result += mixMoleFrac[i] * rossCoeff_[i].value(mixT);
      break;
    case EFF_ABS_COEFF:
      for( size_t i=0; i<nspecies_; i++ ) result += mixMoleFrac[i] * effAbsCoeff_[i].value(mixT);
      break;
    case INVALID_RAD_COEFF:
      ostringstream msg;
      msg << __FILE__ << " : " << __LINE__ << endl << endl
      << "Invalid gray gas propety! \n";
      throw invalid_argument( msg.str() );
  }
}

void
GreyGas::GreyGasData::read( std::ifstream& file )
{
  file >> speciesName;
  file >> ntemp;
  planckCoeff.resize(ntemp,0.0);
  rossCoeff.resize(ntemp,0.0);
  effAbsCoeff.resize(ntemp,0.0);
  temperatures.resize(ntemp,0.0);
  for( int i=0; i<ntemp; ++i ){
    file >> temperatures[i] >> planckCoeff[i] >> rossCoeff[i] >> effAbsCoeff[i];
  }
}

void
GreyGas::GreyGasData::write( std::ofstream& file ) const
{
  file << speciesName << std::endl;
  file << ntemp << std::endl;
  for( int ll=0; ll<ntemp; ++ll ){
    file << temperatures[ll] << "  "<< planckCoeff[ll] << "  " << rossCoeff[ll] << "  " << effAbsCoeff[ll] << endl;
  }
}
} // namespace RadPorps


#include <cstddef>
#include <sys/time.h>
#include "FHE.h"
#include "EncryptedArray.h"
#include <NTL/ZZX.h>
#include <NTL/ZZ.h>
#include <gmp.h>
#include <omp.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <iostream>
#include "Ctxt.h"
#include "polyEval.h"
#include <algorithm>
#include <math.h>

//define how many numbers you want to compare at VECTOR_COUNT
#define VECTOR_COUNT 2
#define VECTOR_SIZE 1

// Simple class to measure time for each method
class Timer
{
public:
    void start() { m_start = my_clock(); }
    void stop() { m_stop = my_clock(); }
    double elapsed_time() const {
        return m_stop - m_start;
    }

private:
    double m_start, m_stop;
    double my_clock() const {
        struct timeval tv;
        gettimeofday(&tv, NULL);
        return tv.tv_sec + tv.tv_usec * 1e-6;
    }
};

//Equality recursive function at page 4
Ctxt z(int i,int j, std::vector<Ctxt> x,std::vector<Ctxt> y, Ctxt enc1){
    if (j==1){
        Ctxt g=x[i];
        g+=y[i];
        g+=enc1;
        return g;
    }
    else {
        int l=ceil(j/2);
        Ctxt f= z(i+l,j-l,x,y, enc1);
        f*=z(i,l,x,y,enc1);
        return f;
    }

}

//Inequality recursive function at page 4
Ctxt t(int i,int j, std::vector<Ctxt> x,std::vector<Ctxt> y,  Ctxt enc1){
    if (j==1){
    Ctxt  H=x[i];
    H*=y[i];
    H+=x[i];
    return H;
    }
    else {
        int l1=ceil(j/2);
        Ctxt we=z(i+l1,j-l1,x,y,enc1);
        we*=t(i,l1,x,y,enc1);
        we+=t(i+l1,j-l1,x,y,enc1);
        return we;
    }
 }

//selection  function with input two numbers (extracted to bits) with output the minimum of two numbers
std::vector<Ctxt> sel(std::vector<Ctxt> digits, std::vector<Ctxt> digits1,std::vector<Ctxt> digitsenc1,std::vector<Ctxt> digitsqw){
    for (int i=0;i<digits1.size();i++){
        Ctxt reset=digitsenc1[0];
        digits1[i]*=digitsqw[0];
        digitsenc1[0]-=digitsqw[0];
        digits[i]*=digitsenc1[0];
        digits1[i]+=digits[i];
        digitsenc1[0]=reset;
    }
    return digits1;

}

//function with input a list of encrypted numbers and output the minimum of them
std::vector<Ctxt> minimum(std::vector<Ctxt> list, Ctxt enc1){
    Ctxt mini=list[0];

    std::vector<Ctxt> digits1;
    extractDigits(digits1, mini);

    for (int i=1;i<list.size();i++){

         std::vector<Ctxt> digitsenc1;
        extractDigits(digitsenc1,enc1);

        std::vector<Ctxt> digitsi;
        extractDigits(digitsi, list[i]);

        Ctxt compute_t=t(0,digits1.size(),digits1,digitsi, enc1);

        std::vector<Ctxt> digitsqw;
        extractDigits(digitsqw, compute_t);
        std::vector<Ctxt> mi=sel( digits1, digitsi, digitsenc1, digitsqw);
        digits1=mi;
    }
    return digits1;

}

int main(int argc, char **argv)
{
    /*** BEGIN INITIALIZATION ***/
    long m = 0;                   // Specific modulus
    long p = 2;                 // Plaintext base [default=2], should be a prime number
    long r = 8;                   // Lifting [default=1]
    long L = 21;                  // Number of levels in the modulus chain [default=heuristic]
    long c = 2;                   // Number of columns in key-switching matrix [default=2]
    long w = 5;                  // Hamming weight of secret key
    long d = 1;                   // Degree of the field extension [default=1]
    long k = 80;                  // Security parameter [default=80]
    long s = 0;                   // Minimum number of slots [default=0]

    Timer tInit;
    tInit.start();

    std::cout << "Finding m... " << std::flush;
    m = FindM(k, L, c, p, d, s, 0);           // Find a value for m given the specified values

    std::cout << "m = " << m << std::endl;

    std::cout << "Initializing context... " << std::flush;
    FHEcontext context(m, p, r); 	          // Initialize context
    buildModChain(context, L, c);             // Modify the context, adding primes to the modulus chain
    std::cout << "OK!" << std::endl;

    std::cout << "Generating keys... " << std::flush;

    fstream pubKeyFile("pk.txt", fstream::out|fstream::trunc);
    assert(pubKeyFile.is_open());
    writeContextBase(pubKeyFile,context);
    pubKeyFile << context << std::endl;

    FHESecKey sk(context);                    // Construct a secret key structure
    const FHEPubKey& pk = sk;                 // An "upcast": FHESecKey is a subclass of FHEPubKey
    sk.GenSecKey(w);                          // Actually generate a secret key with Hamming weight
    //addSome1DMatrices(sk);                    // Extra information for relinearization
    std::cout << "OK!" << std::endl;

    pubKeyFile << pk << std::endl;
    pubKeyFile.close();

    /****INITIALIZATION END****/

    std::ifstream infile("message1.txt");

   //open the message.txt file each line of this txt is a vector

    std::vector< std::vector<int> > e;
    e.resize(VECTOR_COUNT);
    for (int i=0; i<VECTOR_COUNT; i++){
        e[i].resize(VECTOR_SIZE);
    }

    for (int i=0; i<VECTOR_COUNT; i++){
        for (int j=0; j<VECTOR_SIZE ;j++){
            infile >> e[i][j];
        }
    }

    std::cout << "starting"<< std::endl;

    //put the first line vector to u and the second line vector to v

    /*******************************/
    /************CLIENT*************/
    /*******************************/
    long int u,v,y,z;
    u=e[0][0];
    v=e[1][0];
    //y=e[2][0];
    //z=e[3][0];
    std::cout << "u:" << u << std::endl;
    std::cout << "v:" << v << std::endl;
    //std::cout << "y:" << y << std::endl;
    //std::cout << "z:" << z << std::endl;
    std::cout << "encryption of two number from the file message1.txt" << std::endl;

    Ctxt encU(pk),encV(pk),encG(pk),enc1(pk),enc11(pk),enc0(pk),encY(pk),encZ(pk);

    pk.Encrypt(encU,to_ZZX(u));
    pk.Encrypt(encV,to_ZZX(v));
    pk.Encrypt(enc0,to_ZZX(0));
     pk.Encrypt(enc1,to_ZZX(1));
     //pk.Encrypt(encY,to_ZZX(y));
     pk.Encrypt(enc11,to_ZZX(1));
     //pk.Encrypt(encZ,to_ZZX(z));
     std::vector<Ctxt> digits0;
     extractDigits(digits0, enc0);

    //extractdigits of encU and store them at vector:digits
    std::vector<Ctxt> digitsU;
    extractDigits(digitsU, encU);

  	 /*************************************************************/
    /***decrypt each digit of encU to see if extractDigits work*****/
    /***************************************************************/

    long res[digitsU.size()];
    for (int i=0;i<digitsU.size();i++){
    ZZX result;
    sk.Decrypt(result,digitsU[i]);
    if (result[0]>(pow(p,r))/2){ result[0]=result[0]-pow(p,r);}
    res[i]=conv<long>(result[0]);
    }

    std::cout<< "U:";
    size_t res_size = sizeof(res)/sizeof(res[0]);
    std::reverse(res, res + res_size);
    for (int i=0;i<digitsU.size();i++){
        std::cout << res[i] << "," ;
    }
    std::cout << std::endl;

    /****CORRECT******/

    //extractdigits of encV and store them at vector:digitsV
    std::vector<Ctxt> digitsV;
    extractDigits(digitsV, encV);

    std::vector<Ctxt> digitsenc1;
    extractDigits(digitsenc1, enc1);

    /*************************************************************/
    /***decrypt each digit of encV to see if extractDigits work*****/
    /***************************************************************/

    long res1[digitsV.size()];
    for (int i=0;i<digitsV.size();i++){
    ZZX result;
    sk.Decrypt(result,digitsV[i]);
    if (result[0]>(pow(p,r))/2){ result[0]=result[0]-pow(p,r);}
    res1[i]=conv<long>(result[0]);
    }
    size_t res1_size = sizeof(res1)/sizeof(res1[0]);
    std::reverse(res1, res1 + res1_size);
    std::cout<< "V:";
    for (int i=0;i<digitsV.size();i++){
        std::cout << res1[i] << ",";
    }
    std::cout<< std::endl;
    /*******CORRECT******/

     //extractdigits of encV and store them at vector:digitsY
    //td::vector<Ctxt> digitsY;
    //extractDigits(digitsY, encY);

    /*************************************************************/
    /***decrypt each digit of encY to see if extractDigits work*****/
    /***************************************************************/

    /*long dig[digitsY.size()];
    for (int i=0;i<digitsY.size();i++){
     ZZX resultY;
    sk.Decrypt(resultY,digitsY[i]);
    if (resultY[0]>(pow(p,r))/2){ resultY[0]=resultY[0]-pow(p,r);}
    dig[i]=conv<long>(resultY[0]);
    }
    size_t dig_size = sizeof(dig)/sizeof(dig[0]);
    std::reverse(dig, dig + dig_size);
    std::cout<< "Y:";
    for (int i=0;i<digitsY.size();i++){
        std::cout << dig[i] << ",";
    }
    std::cout<< std::endl;
    */

    //input all encrypted numbers on list
    std::vector<Ctxt> list;

        list.push_back(encU);
        list.push_back(encV);
        //list.push_back(encY);
        //list.push_back(encZ);

    Timer timecompare;
    timecompare.start();

   	//calculate the encrypted minimum of the list
    std::vector<Ctxt> kappa=minimum( list, enc1);

     timecompare.stop();

     //decryption of the minimum
    long res2[digitsV.size()];
    for (int i=0;i<digitsV.size();i++){

        ZZX result2;
        sk.Decrypt(result2,kappa[i]);

        res2[i]=conv<long>(result2[0]);
    }
    //the number
    std::cout<< "the minimum number is:";
    int min=0,flow;
    for (int i=0;i<digitsV.size();i++){
        flow=pow(2,i)*res2[i];
        min+=flow;
    }
    std::cout << min << std::endl;
    std::cout << "and his binary form is:" ;
    //the binary form of the number
    size_t res2_size = sizeof(res2)/sizeof(res2[0]);
    std::reverse(res2, res2 + res2_size);
    for (int i=0;i<digitsU.size();i++){
        std::cout << res2[i] << "," ;
    }
    std::cout<< std::endl;
    std::cout << "the time to compute minimum is:" << timecompare.elapsed_time() << std::endl;

    std::cout << std::endl;

     return 0;

}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
using adept::adouble;

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#include "fft.h"

void foobar(double* data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

void afoobar(aVector& data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

extern "C" {
  int enzyme_dupnoneed;
}

static double foobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    __enzyme_autodiff<void>(foobar, enzyme_dupnoneed, inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static double afoobar_and_gradient(unsigned len) {
    adept::Stack stack;

    aVector x(2*len);
    for(int i=0; i<2*len; i++) x(i) = 2.0;
    stack.new_recording();
    afoobar(x, len);
    for(int i=0; i<2*len; i++)
      x(i).set_gradient(1.0);
    stack.compute_adjoint();

    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++)
      dinp[i] = x(i).get_gradient();
    double res = dinp[0];
    delete[] dinp;
    return res;
}

static double tfoobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    foobar_b(inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static void adept_sincos(double inp, unsigned len) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector x(2*len);
  for(int i=0; i<2*len; i++) x[i] = 2.0;
 // stack.new_recording();
  afoobar(x, len);
  double res = x(0).value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = afoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void tapenade_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double* x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = tfoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Tapenade combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void enzyme_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = foobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

/* Function to check if x is power of 2*/
bool isPowerOfTwo (int x)
{
    /* First x in the below expression is for the case when x is 0 */
    return x && (!(x&(x-1)));
}

unsigned max(unsigned A, unsigned B){
  if (A>B) return A;
  return B;
}

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

  if (argc < 2) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  unsigned N = atoi(argv[1]);
  if (!isPowerOfTwo(N)) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  double inp = -2.1;

  for(unsigned iters=max(1, N>>5); iters <= N; iters*=2) {
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    tapenade_sincos(inp, iters);
    enzyme_sincos(inp, iters);
  }
}


//==================================================================================================
/*!
  @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_STIRLING_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-euler
    This function object computes an approwimation of the gamma function
    using the Stirling formula:
  \f$\sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.

    @see gamma, gammaln

    @par Header <boost/simd/function/stirling.hpp>

    @par Example:

      @snippet stirling.cpp stirling

    @par Possible output:

      @snippet stirling.txt stirling

  **/
  Value stirling(Value const& x);
} }
#endif

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

#endif


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#ifndef R10_HPP_
#define R10_HPP_

#include <boost/graph/isomorphism.hpp>

namespace unimod
{

  /**
   * Singleton class to test a bipartite graph to be ismorphic to the graphs representing R10.
   */

  class bipartite_r10_graphs
  {
  public:
    typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> graph_t;

  private:

    /**
     * Constructs the R10 graph class.
     */

    bipartite_r10_graphs()
    {
      typedef std::pair <int, int> E;

      E g1_edges[] =
      { E(0, 5), E(0, 8), E(0, 9), E(1, 5), E(1, 6), E(1, 9), E(2, 6), E(2, 7), E(2, 9), E(3, 7), E(3, 8), E(3, 9), E(4, 5), E(4, 6), E(4, 7),
          E(4, 8), E(4, 9) };
      g1 = graph_t(&g1_edges[0], &g1_edges[0] + sizeof(g1_edges) / sizeof(E), 10);

      E g2_edges[] =
      { E(0, 5), E(0, 8), E(0, 9), E(1, 5), E(1, 6), E(1, 8), E(2, 6), E(2, 7), E(2, 9), E(3, 7), E(3, 8), E(3, 9), E(4, 5), E(4, 6), E(4, 7) };
      g2 = graph_t(&g2_edges[0], &g2_edges[0] + sizeof(g2_edges) / sizeof(E), 10);
    }

    /**
     * Singleton instance function.
     *
     * @return The unique instance
     */

    static bipartite_r10_graphs& instance()
    {
      static bipartite_r10_graphs* instance = NULL;
      if (instance == NULL)
        instance = new bipartite_r10_graphs();
      return *instance;
    }

  public:

    /**
     * Checks a given graph to be isomorphic to the ones stored.
     *
     * @param graph Given graph
     * @return true if and only if it is isomorphic to any of the two stored ones.
     */

    template <typename Graph>
    static bool is_r10_graph(const Graph& graph)
    {
      return boost::isomorphism(graph, instance().g1) || boost::isomorphism(graph, instance().g2);
    }

  private:
    graph_t g1, g2;
  };

  /**
   * Tests a given matrix to be matrix-isomorphic to one of the R10-representing matrices
   * by examining the corresponding bipartite graphs.
   *
   * @param matrix A given matrix
   * @return true if and only if this matrix is a represenation matrix for R10
   */

  template <typename MatrixType>
  inline bool is_r10(MatrixType matrix)
  {
    if (matrix.size1() != 5 || matrix.size2() != 5)
      return false;

    bipartite_r10_graphs::graph_t graph(10);

    for (size_t row = 0; row < 5; ++row)
    {
      for (size_t column = 0; column < 5; ++column)
      {
        if (matrix(row, column))
        {
          boost::add_edge(boost::vertex(row, graph), boost::vertex(5 + column, graph), graph);
        }
      }
    }

    return bipartite_r10_graphs::is_r10_graph(graph);
  }
}

#endif /* R10_HPP_ */


#include "aslam/triangulation/triangulation.h"

#include <Eigen/QR>
#include <glog/logging.h>

namespace aslam {

TriangulationResult::Status TriangulationResult::SUCCESSFUL =
    TriangulationResult::Status::kSuccessful;
TriangulationResult::Status TriangulationResult::TOO_FEW_MEASUREMENTS =
    TriangulationResult::Status::kTooFewMeasurments;
TriangulationResult::Status TriangulationResult::UNOBSERVABLE =
    TriangulationResult::Status::kUnobservable;
TriangulationResult::Status TriangulationResult::UNINITIALIZED =
    TriangulationResult::Status::kUninitialized;

TriangulationResult linearTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const aslam::TransformationVector& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  VLOG(200) << "Triangulating from " << T_G_B.size() << " views.";

  const size_t rows = 3 * measurements_normalized.size();
  const size_t cols = 3 + measurements_normalized.size();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(rows, cols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(rows);

  const Eigen::Matrix3d R_B_C = T_B_C.getRotationMatrix();

  // Fill in A and b.
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    Eigen::Vector3d v(measurements_normalized[i](0),
        measurements_normalized[i](1), 1.);
    Eigen::Matrix3d R_G_B = T_G_B[i].getRotationMatrix();
    const Eigen::Vector3d& p_G_B = T_G_B[i].getPosition();
    A.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity();
    A.block<3, 1>(3 * i, 3 + i) = -R_G_B * R_B_C * v;
    b.segment<3>(3 * i) = p_G_B + R_G_B * T_B_C.getPosition();
  }

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr = A.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 0.001;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();

  if ((rank - measurements_normalized.size()) < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *G_point = qr.solve(b).head<3>();

  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult linearTriangulateFromNViews(
    const Eigen::Matrix3Xd& t_G_bv,
    const Eigen::Matrix3Xd& p_G_C,
    Eigen::Vector3d* p_G_P) {
  CHECK_NOTNULL(p_G_P);

  const int num_measurements = t_G_bv.cols();
  if (num_measurements < 2) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  // 1.) Formulate the geometrical problem
  // p_G_P + alpha[i] * t_G_bv[i] = p_G_C[i]      (+ alpha intended)
  // as linear system Ax = b, where
  // x = [p_G_P; alpha[0]; alpha[1]; ... ] and b = [p_G_C[0]; p_G_C[1]; ...]
  //
  // 2.) Apply the approximation AtAx = Atb
  // AtA happens to be composed of mostly more convenient blocks than A:
  // - Top left = N * Eigen::Matrix3d::Identity()
  // - Top right and bottom left = t_G_bv
  // - Bottom right = t_G_bv.colwise().squaredNorm().asDiagonal()

  // - Atb.head(3) = p_G_C.rowwise().sum()
  // - Atb.tail(N) = columnwise dot products between t_G_bv and p_G_C
  //               = t_G_bv.cwiseProduct(p_G_C).colwise().sum().transpose()
  //
  // 3.) Apply the Schur complement to solve after p_G_P only
  // AtA = [E B; C D] (same blocks as above) ->
  // (E - B * D.inverse() * C) * p_G_P = Atb.head(3) - B * D.inverse() * Atb.tail(N)

  const Eigen::MatrixXd BiD = t_G_bv *
      t_G_bv.colwise().squaredNorm().asDiagonal().inverse();
  const Eigen::Matrix3d AxtAx = num_measurements * Eigen::Matrix3d::Identity() -
      BiD * t_G_bv.transpose();
  const Eigen::Vector3d Axtbx = p_G_C.rowwise().sum() - BiD *
      t_G_bv.cwiseProduct(p_G_C).colwise().sum().transpose();

  Eigen::ColPivHouseholderQR<Eigen::Matrix3d> qr = AxtAx.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 1e-5;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();
  if (rank < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *p_G_P = qr.solve(Axtbx);
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult linearTriangulateFromNViewsMultiCam(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const std::vector<size_t>& measurement_camera_indices,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const Aligned<std::vector, aslam::Transformation>& T_B_C,
    Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  CHECK_EQ(measurements_normalized.size(), measurement_camera_indices.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  const size_t rows = 3 * measurements_normalized.size();
  const size_t cols = 3 + measurements_normalized.size();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(rows, cols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(rows);

  // Fill in A and b.
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    size_t cam_index = measurement_camera_indices[i];
    CHECK_LT(cam_index, T_B_C.size());
    Eigen::Vector3d v(measurements_normalized[i](0),
        measurements_normalized[i](1), 1.);
    const Eigen::Vector3d& t_B_C = T_B_C[cam_index].getPosition();

    A.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity();
    A.block<3, 1>(3 * i, 3 + i) = -1.0 * T_G_B[i].getRotation().rotate(
        T_B_C[cam_index].getRotation().rotate(v));
    b.segment<3>(3 * i) = T_G_B[i] * t_B_C;
  }

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr = A.colPivHouseholderQr();
  static constexpr double kRankLossTolerance = 0.001;
  qr.setThreshold(kRankLossTolerance);
  const size_t rank = qr.rank();
  if ((rank - measurements_normalized.size()) < 3) {
    return TriangulationResult(TriangulationResult::UNOBSERVABLE);
  }

  *G_point = qr.solve(b).head<3>();
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

// [1]  A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter
// "for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. on Robotics
// and Automation, pp. 10–14, 2007.
// [2] T. Hinzmann, "Robust Vision-Based Navigation for Micro Air Vehicles", 2014.
TriangulationResult iterativeGaussNewtonTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point) {
  CHECK_NOTNULL(G_point);
  CHECK_EQ(measurements_normalized.size(), T_G_B.size());
  if (measurements_normalized.size() < 2u) {
    return TriangulationResult(TriangulationResult::TOO_FEW_MEASUREMENTS);
  }

  const double kPrecision = 1.0e-5;
  const size_t kIterMax = 10;

  // Initialize minimization variables.
  double alpha = 0.0;
  double beta = 0.0;
  double rho = 0.0;

  double residual_norm_last = 1000.0;
  double residual_norm = 100.0;

  // Camera frame n: camera frame in which feature was observed for the first time.
  const size_t n = 0;

  // Rotation and position of n-th camera in i-th camera frame.
  const aslam::Transformation T_Cn_G = T_B_C.inverse() * T_G_B[n].inverse();
  const Eigen::Matrix3d& R_Cn_G = T_Cn_G.getRotationMatrix();
  const Eigen::Vector3d& p_G_Cn = T_Cn_G.getPosition();

  // Cache matrices that are constant for every iteration.
  Aligned<std::vector, Eigen::Matrix3d> R_Ci_Cn;
  Aligned<std::vector, Eigen::Vector3d> p_Ci_Cn;
  for (size_t i = 0; i < measurements_normalized.size(); ++i) {
    const aslam::Transformation T_Ci_G = T_B_C.inverse() * T_G_B[i].inverse();
    // Rotation from first camera to current camera.
    const Eigen::Matrix3d R_Ci_G = T_Ci_G.getRotationMatrix();
    R_Ci_Cn.emplace_back(R_Ci_G * R_Cn_G.transpose());
    // Translation from first camera to current camera.
    const Eigen::Vector3d p_G_Ci = T_Ci_G.inverse().getPosition();
    p_Ci_Cn.emplace_back(R_Ci_G * p_G_Cn - R_Ci_G * p_G_Ci);
  }

  // [1.] Loop over iterations.
  // Loop while delta residual too large or number of maximum iterations reached.
  size_t iter = 0;
  while (residual_norm_last - residual_norm > kPrecision && iter < kIterMax) {
    const size_t num_measurements = 2 * measurements_normalized.size();
    Eigen::VectorXd residuals(num_measurements);
    Eigen::MatrixXd jacobian(num_measurements, 3);
    residuals.setZero(num_measurements);
    jacobian.setZero(num_measurements, 3);

    // [2.] Loop over camera frames / measurements.
    for (size_t i = 0; i < measurements_normalized.size(); ++i) {
      // Current measurement.
      const Eigen::Vector2d& h_meas = measurements_normalized[i];

      // Predicted measurement.
      const Eigen::Vector3d h_i = R_Ci_Cn[i] *
	  (Eigen::Matrix<double, 3, 1>() << alpha, beta, 1.0).finished() + rho * p_Ci_Cn[i];
      // Normalized predicted measurement.
      const Eigen::Vector2d h  = h_i.head<2>() / h_i(2);

      // Calculate residuals.
      residuals.segment<2>(i * 2) = h_meas - h;

      // Calculate jacobians.
      Eigen::Matrix<double, 2, 3> jacobian_perspective;
      jacobian_perspective << -1.0 / h_i(2), 0.0, h_i(0) / (h_i(2) * h_i(2)),
          0.0, -1.0 / h_i(2), h_i(1) / (h_i(2) * h_i(2));

      const Eigen::Matrix<double, 3, 1> jacobian_alpha =
          R_Ci_Cn[i] * (Eigen::Matrix<double, 3, 1>() << 1.0, 0.0, 0.0).finished();
      const Eigen::Matrix<double, 3, 1> jacobian_beta =
          R_Ci_Cn[i] * (Eigen::Matrix<double, 3, 1>() << 0.0, 1.0, 0.0).finished();
      const Eigen::Matrix<double, 3, 1> jacobian_rho = p_Ci_Cn[i];

      const Eigen::Matrix<double, 2, 1> jacobian_A = jacobian_perspective * jacobian_alpha;
      const Eigen::Matrix<double, 2, 1> jacobian_B = jacobian_perspective * jacobian_beta;
      const Eigen::Matrix<double, 2, 1> jacobian_C = jacobian_perspective * jacobian_rho;

      jacobian.block<1, 3>(i * 2, 0) =
          (Eigen::Matrix<double, 1, 3>() << jacobian_A(0), jacobian_B(0), jacobian_C(0)).finished();
      jacobian.block<1, 3>(i * 2 + 1, 0) =
          (Eigen::Matrix<double, 1, 3>() << jacobian_A(1), jacobian_B(1), jacobian_C(1)).finished();
    }  // Measurement loop.

    // Calculate update using LDLT decomposition.
    Eigen::Vector3d delta = (jacobian.transpose() * jacobian)
	.ldlt().solve(jacobian.transpose() * residuals);

    alpha = alpha - delta(0);
    beta = beta - delta(1);
    rho = rho - delta(2);

    residual_norm_last = residual_norm;
    residual_norm = residuals.squaredNorm();
    ++iter;
  } // Iteration loop.

  // Coordinate of feature in global frame.
  *G_point = 1.0 / rho * R_Cn_G.transpose() *
      (Eigen::Matrix<double, 3, 1>() << alpha, beta, 1.0).finished() + p_G_Cn;
  return TriangulationResult(TriangulationResult::SUCCESSFUL);
}

TriangulationResult triangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark) {
  CHECK_NOTNULL(W_landmark);
  size_t track_length = track.getTrackLength();
  CHECK_GT(track_length, 1u);
  CHECK_EQ(track_length, T_W_Bs.size());

  VLOG(200) << "Triangulating track of length " << track_length;

  const aslam::Camera::ConstPtr& camera = track.getFirstKeypointIdentifier().getCamera();
  CHECK(camera);
  const aslam::CameraId track_camera_id = camera->getId();
  aslam::Transformation T_B_C = track.getFirstKeypointIdentifier().get_T_C_B().inverse();

  // Get the normalized measurements for all observations on the track.
  Aligned<std::vector, Eigen::Vector2d> normalized_measurements;
  normalized_measurements.reserve(track_length);

  size_t index = 0u;
  for (const aslam::KeypointIdentifier& keypoint_on_track : track.getKeypointIdentifiers()) {
    const aslam::Camera::ConstPtr& camera = keypoint_on_track.getCamera();
    CHECK(camera) << "Missing camera for keypoint on track with frame index: "
        << keypoint_on_track.getFrameIndex();

    // Obtain the normalized keypoint measurements.
    const Eigen::Vector2d& keypoint_measurement = keypoint_on_track.getKeypointMeasurement();
    Eigen::Vector3d C_ray;
    camera->backProject3(keypoint_measurement, &C_ray);
    Eigen::Vector2d normalized_measurement = C_ray.head<2>() / C_ray[2];
    normalized_measurements.push_back(normalized_measurement);
  }

  VLOG(200) << "Assembled triangulation data.";

  // Triangulate the landmark.
  CHECK_EQ(track_length, normalized_measurements.size());
  CHECK_EQ(track_length, T_W_Bs.size());
  aslam::TriangulationResult triangulation_result = linearTriangulateFromNViews(
                                                        normalized_measurements,
                                                        T_W_Bs,
                                                        T_B_C,
                                                        W_landmark);

  VLOG(200) << "Triangulation returned the following result:" << std::endl
          << triangulation_result;

  return triangulation_result;
}

TriangulationResult fastTriangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark) {
  CHECK_NOTNULL(W_landmark);
  size_t track_length = track.getTrackLength();
  CHECK_GT(track_length, 1u);
  CHECK_EQ(track_length, T_W_Bs.size());

  VLOG(200) << "Triangulating track of length " << track_length;

  const aslam::Camera::ConstPtr& camera = track.getFirstKeypointIdentifier().getCamera();
  CHECK(camera);
  const aslam::CameraId track_camera_id = camera->getId();
  aslam::Transformation T_B_C = track.getFirstKeypointIdentifier().get_T_C_B().inverse();

  Eigen::Matrix3Xd G_bearing_vectors;
  Eigen::Matrix3Xd p_G_C_vector;

  G_bearing_vectors.resize(Eigen::NoChange, track_length);
  p_G_C_vector.resize(Eigen::NoChange, track_length);

  size_t index = 0u;
  for (const aslam::KeypointIdentifier& keypoint_on_track : track.getKeypointIdentifiers()) {
    const aslam::Camera::ConstPtr& camera = keypoint_on_track.getCamera();
    CHECK(camera) << "Missing camera for keypoint on track with frame index: "
        << keypoint_on_track.getFrameIndex();

    // Obtain the normalized keypoint measurements.
    const Eigen::Vector2d& keypoint_measurement = keypoint_on_track.getKeypointMeasurement();
    Eigen::Vector3d C_ray;
    camera->backProject3(keypoint_measurement, &C_ray);

    aslam::Transformation T_W_C = T_W_Bs[index] * keypoint_on_track.get_T_C_B().inverse();

    G_bearing_vectors.col(index) = T_W_C.getRotationMatrix() * C_ray;
    p_G_C_vector.col(index) = T_W_C.getPosition();
  }

  VLOG(200) << "Assembled triangulation data.";

  // Triangulate the landmark.
  aslam::TriangulationResult triangulation_result = linearTriangulateFromNViews(
      G_bearing_vectors, p_G_C_vector, W_landmark);

  VLOG(200) << "Triangulation returned the following result:" << std::endl
          << triangulation_result;

  return triangulation_result;
}

}  // namespace aslam