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

// 前面几个文件已经在前面的例子中讲过了，因此不再做进一步的评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>

// 这里定义了不连续的有限元。它们的使用方式与所有其他有限元相同，不过--正如你在以前的教程程序中所看到的--用户与有限元类的交互并不多：它们被传递给 DoFHandler 和 FEValues 对象，就这样了。

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

// 我们将使用最简单的求解器，称为Richardson迭代，它代表了一个简单的缺陷修正。这与一个块状SSOR预处理器（定义在precondition_block.h中）相结合，该预处理器使用DG离散化产生的系统矩阵的特殊块状结构。

#include <deal.II/lac/solver_richardson.h>
#include <deal.II/lac/precondition_block.h>

// 我们将使用梯度作为细化指标。

#include <deal.II/numerics/derivative_approximation.h>

// 这里是使用MeshWorker框架的新的包含文件。第一个文件包含了 MeshWorker::DoFInfo, 类，它为局部积分器提供了局部与全局自由度之间的映射。在第二个文件中，我们发现一个类型为 MeshWorker::IntegrationInfo, 的对象，它主要是对一组FEValues对象的封装。文件<tt>meshworker/simple.h</tt>包含了将局部集成数据组装成只包含一个矩阵的全局系统的类。最后，我们将需要在所有的网格单元和面中运行循环的文件。

#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/simple.h>
#include <deal.II/meshworker/loop.h>

// 像所有的程序一样，我们在完成这一部分时要包括所需的C++头文件，并声明我们要使用dealii命名空间中的对象，不加前缀。

#include <iostream>
#include <fstream>

namespace Step12
{
  using namespace dealii;
// @sect3{Equation data}

// 首先，我们定义一个描述不均匀边界数据的类。由于只使用它的值，我们实现value_list()，但不定义Function的所有其他函数。

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    BoundaryValues() = default;
    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int component = 0) const override;
  };

// 考虑到流动方向，单位方块 $[0,1]^2$ 的流入边界为右边界和下边界。我们在x轴上规定了不连续的边界值1和0，在右边界上规定了值0。该函数在流出边界上的值将不会在DG方案中使用。

  template <int dim>
  void BoundaryValues<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int component) const
  {
    (void)component;
    AssertIndexRange(component, 1);
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));

    for (unsigned int i = 0; i < values.size(); ++i)
      {
        if (points[i](0) < 0.5)
          values[i] = 1.;
        else
          values[i] = 0.;
      }
  }

// 最后，一个计算并返回风场的函数  $\beta=\beta(\mathbf x)$  。正如在介绍中所解释的，在2D中我们将使用一个围绕原点的旋转场。在3D中，我们只需不设置 $z$ 分量（即为零），而这个函数在目前的实现中不能用于1D。

  template <int dim>
  Tensor<1, dim> beta(const Point<dim> &p)
  {
    Assert(dim >= 2, ExcNotImplemented());

    Tensor<1, dim> wind_field;
    wind_field[0] = -p[1];
    wind_field[1] = p[0];
    wind_field /= wind_field.norm();

    return wind_field;
  }
// @sect3{The AdvectionProblem class}

// 在这个准备工作之后，我们继续进行这个程序的主类，叫做AdvectionProblem。它基本上是  step-6  的主类。我们没有AffineConstraints对象，因为在DG离散中没有悬挂节点约束。

// 主要的区别只出现在集合函数的实现上，因为在这里，我们不仅需要覆盖面上的通量积分，我们还使用MeshWorker接口来简化涉及的循环。

  template <int dim>
  class AdvectionProblem
  {
  public:
    AdvectionProblem();
    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve(Vector<double> &solution);
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    const MappingQ1<dim> mapping;

// 此外，我们想使用程度为1的DG元素（但这只在构造函数中指定）。如果你想使用不同度数的DG方法，整个程序保持不变，只需在构造函数中用所需的多项式度数替换1。

    FE_DGQ<dim>     fe;
    DoFHandler<dim> dof_handler;

// 接下来的四个成员代表要解决的线性系统。  system_matrix and right_hand_side 是由 assemble_system(), the solution 产生的， solve(). The sparsity_pattern 是用来确定 system_matrix 中非零元素的位置。

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> right_hand_side;

// 最后，我们必须提供集合单元、边界和内表面条款的函数。在MeshWorker框架中，所有单元的循环和大部分操作的设置都将在这个类之外完成，所以我们所要提供的只是这三个操作。他们将在中间对象上工作，首先，我们在这里定义了交给本地集成函数的信息对象的别名，以使我们的生活更轻松。

    using DoFInfo  = MeshWorker::DoFInfo<dim>;
    using CellInfo = MeshWorker::IntegrationInfo<dim>;

// 下面的三个函数是在所有单元和面的通用循环中被调用的。它们是进行实际整合的函数。

// 在我们下面的代码中，这些函数并不访问当前类的成员变量，所以我们可以将它们标记为 static ，并简单地将这些函数的指针传递给MeshWorker框架。然而，如果这些函数想要访问成员变量（或者需要额外的参数，而不是下面指定的参数），我们可以使用lambda函数的设施来为MeshWorker框架提供对象，这些对象就像它们拥有所需的参数数量和类型一样，但实际上已经绑定了其他参数。

    static void integrate_cell_term(DoFInfo &dinfo, CellInfo &info);
    static void integrate_boundary_term(DoFInfo &dinfo, CellInfo &info);
    static void integrate_face_term(DoFInfo & dinfo1,
                                    DoFInfo & dinfo2,
                                    CellInfo &info1,
                                    CellInfo &info2);
  };

// 我们从构造函数开始。 fe 的构造器调用中的1是多项式的度数。

  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem()
    : mapping()
    , fe(1)
    , dof_handler(triangulation)
  {}

  template <int dim>
  void AdvectionProblem<dim>::setup_system()
  {

// 在设置通常的有限元数据结构的函数中，我们首先需要分配DoF。

    dof_handler.distribute_dofs(fe);

// 我们从生成稀疏模式开始。为此，我们首先用系统中出现的耦合物填充一个动态稀疏模式（DynamicSparsityPattern）类型的中间对象。在建立模式之后，这个对象被复制到 sparsity_pattern 并可以被丢弃。

// 为了建立DG离散的稀疏模式，我们可以调用类似于 DoFTools::make_sparsity_pattern, 的函数，它被称为 DoFTools::make_flux_sparsity_pattern:
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

// 最后，我们设置了线性系统的所有组成部分的结构。

    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    right_hand_side.reinit(dof_handler.n_dofs());
  }
// @sect4{The assemble_system function}

// 这里我们看到了与手工组装的主要区别。我们不需要在单元格和面上写循环，而是将这一切交给MeshWorker框架。为了做到这一点，我们只需要定义局部的集成函数，并使用命名空间 MeshWorker::Assembler 中的一个类来构建全局系统。

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

// 这是一个神奇的对象，它知道关于数据结构和局部集成的一切。 这是在函数 MeshWorker::loop(), 中做工作的对象，它被下面的 MeshWorker::integration_loop() 隐式调用。在我们提供指针的函数完成局部积分后， MeshWorker::Assembler::SystemSimple 对象将这些数据分配到全局稀疏矩阵和右手边的向量。

    MeshWorker::IntegrationInfoBox<dim> info_box;

// 首先，我们在工作者基类中初始化正交公式和更新标志。对于正交，我们采取安全措施，使用QGauss公式，其点数比使用的多项式度数高一个。由于单元格、边界和内部面的正交率可以独立选择，我们必须把这个值交给三次。

    const unsigned int n_gauss_points = dof_handler.get_fe().degree + 1;
    info_box.initialize_gauss_quadrature(n_gauss_points,
                                         n_gauss_points,
                                         n_gauss_points);

// 这些是我们整合系统时需要的数值类型。它们被添加到单元格、边界和内部面以及内部邻居面所使用的标志中，这是由四个 @p true 值强制执行的。

    info_box.initialize_update_flags();
    UpdateFlags update_flags =
      update_quadrature_points | update_values | update_gradients;
    info_box.add_update_flags(update_flags, true, true, true, true);

// 在准备好<tt>info_box</tt>中的所有数据后，我们初始化其中的FEValues对象。

    info_box.initialize(fe, mapping);

// 到目前为止创建的对象帮助我们在每个单元和面进行局部积分。现在，我们需要一个对象来接收整合后的（本地）数据，并将它们转发给装配程序。

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 现在，我们必须创建装配器对象，并告诉它将本地数据放在哪里。这些将是我们的系统矩阵和右手边的数据。

    MeshWorker::Assembler::SystemSimple<SparseMatrix<double>, Vector<double>>
      assembler;
    assembler.initialize(system_matrix, right_hand_side);

// 最后，在所有活动单元上进行积分循环（由第一个参数决定，它是一个活动迭代器）。

// 正如在类声明中声明局部积分函数时的讨论中所指出的，装配积分器类所期望的参数实际上不是函数指针。相反，它们是可以像函数一样被调用的对象，有一定数量的参数。因此，我们也可以在这里传递具有适当的operator()实现的对象，或者如果本地集成器是，例如，非静态成员函数，则可以传递lambda函数。

    MeshWorker::loop<dim,
                     dim,
                     MeshWorker::DoFInfo<dim>,
                     MeshWorker::IntegrationInfoBox<dim>>(
      dof_handler.begin_active(),
      dof_handler.end(),
      dof_info,
      info_box,
      &AdvectionProblem<dim>::integrate_cell_term,
      &AdvectionProblem<dim>::integrate_boundary_term,
      &AdvectionProblem<dim>::integrate_face_term,
      assembler);
  }
// @sect4{The local integrators}

// 这些是给上面调用的 MeshWorker::integration_loop() 的函数。它们计算单元格和面中对系统矩阵和右手边的局部贡献。

  template <int dim>
  void AdvectionProblem<dim>::integrate_cell_term(DoFInfo & dinfo,
                                                  CellInfo &info)
  {

// 首先，让我们从 @p info. 中检索这里使用的一些对象。注意，这些对象可以处理更复杂的结构，因此这里的访问看起来比看起来更复杂。

    const FEValuesBase<dim> &  fe_values    = info.fe_values();
    FullMatrix<double> &       local_matrix = dinfo.matrix(0).matrix;
    const std::vector<double> &JxW          = fe_values.get_JxW_values();

// 有了这些对象，我们像往常一样继续进行局部积分。首先，我们在正交点上循环，计算当前点的平流矢量。

    for (unsigned int point = 0; point < fe_values.n_quadrature_points; ++point)
      {
        const Tensor<1, dim> beta_at_q_point =
          beta(fe_values.quadrature_point(point));

// 我们求解的是一个同质方程，因此在单元项中没有显示出右手。 剩下的就是对矩阵项的积分。

        for (unsigned int i = 0; i < fe_values.dofs_per_cell; ++i)
          for (unsigned int j = 0; j < fe_values.dofs_per_cell; ++j)
            local_matrix(i, j) += -beta_at_q_point *                //
                                  fe_values.shape_grad(i, point) *  //
                                  fe_values.shape_value(j, point) * //
                                  JxW[point];
      }
  }

// 现在对边界条款也是如此。注意，现在我们使用FEValuesBase，即FEFaceValues和FESubfaceValues的基类，以便获得法向量。

  template <int dim>
  void AdvectionProblem<dim>::integrate_boundary_term(DoFInfo & dinfo,
                                                      CellInfo &info)
  {
    const FEValuesBase<dim> &fe_face_values = info.fe_values();
    FullMatrix<double> &     local_matrix   = dinfo.matrix(0).matrix;
    Vector<double> &         local_vector   = dinfo.vector(0).block(0);

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

    std::vector<double> g(fe_face_values.n_quadrature_points);

    static BoundaryValues<dim> boundary_function;
    boundary_function.value_list(fe_face_values.get_quadrature_points(), g);

    for (unsigned int point = 0; point < fe_face_values.n_quadrature_points;
         ++point)
      {
        const double beta_dot_n =
          beta(fe_face_values.quadrature_point(point)) * normals[point];
        if (beta_dot_n > 0)
          for (unsigned int i = 0; i < fe_face_values.dofs_per_cell; ++i)
            for (unsigned int j = 0; j < fe_face_values.dofs_per_cell; ++j)
              local_matrix(i, j) += beta_dot_n *                           //
                                    fe_face_values.shape_value(j, point) * //
                                    fe_face_values.shape_value(i, point) * //
                                    JxW[point];
        else
          for (unsigned int i = 0; i < fe_face_values.dofs_per_cell; ++i)
            local_vector(i) += -beta_dot_n *                          //
                               g[point] *                             //
                               fe_face_values.shape_value(i, point) * //
                               JxW[point];
      }
  }

// 最后是内部面的条款。这里的区别是，我们收到了两个信息对象，相邻面的每个单元都有一个，我们组装了四个矩阵，每个单元一个，两个用于来回耦合。

  template <int dim>
  void AdvectionProblem<dim>::integrate_face_term(DoFInfo & dinfo1,
                                                  DoFInfo & dinfo2,
                                                  CellInfo &info1,
                                                  CellInfo &info2)
  {

// 对于正交点、权重等，我们使用第一个参数的FEValuesBase对象。

    const FEValuesBase<dim> &fe_face_values = info1.fe_values();
    const unsigned int       dofs_per_cell  = fe_face_values.dofs_per_cell;

// 对于额外的形状函数，我们必须询问邻居的FEValuesBase。

    const FEValuesBase<dim> &fe_face_values_neighbor = info2.fe_values();
    const unsigned int       neighbor_dofs_per_cell =
      fe_face_values_neighbor.dofs_per_cell;

// 然后我们得到对四个局部矩阵的引用。字母u和v分别指的是试验和测试函数。%的数字表示由info1和info2提供的单元。按照惯例，每个信息对象中的两个矩阵指的是各自单元上的试验函数。第一个矩阵包含该单元的内部耦合，而第二个矩阵包含单元之间的耦合。

    FullMatrix<double> &u1_v1_matrix = dinfo1.matrix(0, false).matrix;
    FullMatrix<double> &u2_v1_matrix = dinfo1.matrix(0, true).matrix;
    FullMatrix<double> &u1_v2_matrix = dinfo2.matrix(0, true).matrix;
    FullMatrix<double> &u2_v2_matrix = dinfo2.matrix(0, false).matrix;

// 在这里，按照前面的函数，我们会有本地的右手边向量。幸运的是，界面条款只涉及到解决方案，右手边没有收到任何贡献。

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

    for (unsigned int point = 0; point < fe_face_values.n_quadrature_points;
         ++point)
      {
        const double beta_dot_n =
          beta(fe_face_values.quadrature_point(point)) * normals[point];
        if (beta_dot_n > 0)
          {

// 这个词我们已经看过了。

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                u1_v1_matrix(i, j) += beta_dot_n *                           //
                                      fe_face_values.shape_value(j, point) * //
                                      fe_face_values.shape_value(i, point) * //
                                      JxW[point];

// 我们另外组装术语  $(\beta\cdot n u,\hat v)_{\partial \kappa_+}$  。

            for (unsigned int k = 0; k < neighbor_dofs_per_cell; ++k)
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                u1_v2_matrix(k, j) +=
                  -beta_dot_n *                                   //
                  fe_face_values.shape_value(j, point) *          //
                  fe_face_values_neighbor.shape_value(k, point) * //
                  JxW[point];
          }
        else
          {

// 这个我们也已经看过了。

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              for (unsigned int l = 0; l < neighbor_dofs_per_cell; ++l)
                u2_v1_matrix(i, l) +=
                  beta_dot_n *                                    //
                  fe_face_values_neighbor.shape_value(l, point) * //
                  fe_face_values.shape_value(i, point) *          //
                  JxW[point];

// 而这是另一个新的。 $(\beta\cdot n \hat u,\hat v)_{\partial \kappa_-}$  :

            for (unsigned int k = 0; k < neighbor_dofs_per_cell; ++k)
              for (unsigned int l = 0; l < neighbor_dofs_per_cell; ++l)
                u2_v2_matrix(k, l) +=
                  -beta_dot_n *                                   //
                  fe_face_values_neighbor.shape_value(l, point) * //
                  fe_face_values_neighbor.shape_value(k, point) * //
                  JxW[point];
          }
      }
  }
// @sect3{All the rest}

// 对于这个简单的问题，我们使用了最简单的求解器，称为Richardson迭代，它代表了简单的缺陷修正。这与一个块状SSOR预处理相结合，该预处理使用DG离散化产生的系统矩阵的特殊块状结构。这些块的大小是每个单元的DoF数量。在这里，我们使用SSOR预处理，因为我们没有根据流场对DoFs进行重新编号。如果在流的下游方向对DoFs进行重新编号，那么块状的Gauss-Seidel预处理（见PreconditionBlockSOR类，放松=1）会做得更好。

  template <int dim>
  void AdvectionProblem<dim>::solve(Vector<double> &solution)
  {
    SolverControl                    solver_control(1000, 1e-12);
    SolverRichardson<Vector<double>> solver(solver_control);

// 这里我们创建了预处理程序。

    PreconditionBlockSSOR<SparseMatrix<double>> preconditioner;

// 然后将矩阵分配给它，并设置正确的块大小。

    preconditioner.initialize(system_matrix, fe.n_dofs_per_cell());

// 做完这些准备工作后，我们就可以启动线性求解器了。

    solver.solve(system_matrix, solution, right_hand_side, preconditioner);
  }

// 我们根据一个非常简单的细化标准来细化网格，即对解的梯度的近似。由于这里我们考虑的是DG(1)方法（即我们使用片状双线性形状函数），我们可以简单地计算每个单元的梯度。但是我们并不希望我们的细化指标只建立在每个单元的梯度上，而是希望同时建立在相邻单元之间的不连续解函数的跳跃上。最简单的方法是通过差分商计算近似梯度，包括考虑中的单元和其相邻的单元。这是由 DerivativeApproximation 类完成的，它计算近似梯度的方式类似于本教程 step-9 中描述的 GradientEstimation 。事实上， DerivativeApproximation 类是在 step-9 的 GradientEstimation 类之后开发的。与  step-9  中的讨论相关，这里我们考虑  $h^{1+d/2}|\nabla_h u_h|$  。此外，我们注意到，我们不考虑近似的二次导数，因为线性平流方程的解一般不在 $H^2$ 中，而只在 $H^1$ 中（或者，更准确地说：在 $H^1_\beta$ 中，即在方向 $\beta$ 中的导数是可平方整除的函数空间）。

  template <int dim>
  void AdvectionProblem<dim>::refine_grid()
  {

//  DerivativeApproximation 类将梯度计算为浮点精度。这已经足够了，因为它们是近似的，只作为细化指标。

    Vector<float> gradient_indicator(triangulation.n_active_cells());

// 现在，近似梯度被计算出来了

    DerivativeApproximation::approximate_gradient(mapping,
                                                  dof_handler,
                                                  solution,
                                                  gradient_indicator);

//并且它们被单元格按比例放大，系数为 $h^{1+d/2}$  。
    unsigned int cell_no = 0;
    for (const auto &cell : dof_handler.active_cell_iterators())
      gradient_indicator(cell_no++) *=
        std::pow(cell->diameter(), 1 + 1.0 * dim / 2);

// 最后它们作为细化指标。

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    gradient_indicator,
                                                    0.3,
                                                    0.1);

    triangulation.execute_coarsening_and_refinement();
  }

// 这个程序的输出包括自适应细化网格的eps文件和gnuplot格式的数值解。

  template <int dim>
  void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
  {

// 首先将网格写成eps格式。

    {
      const std::string filename = "grid-" + std::to_string(cycle) + ".eps";
      deallog << "Writing grid to <" << filename << ">" << std::endl;
      std::ofstream eps_output(filename);

      GridOut grid_out;
      grid_out.write_eps(triangulation, eps_output);
    }

// 然后以gnuplot格式输出解决方案。

    {
      const std::string filename = "sol-" + std::to_string(cycle) + ".gnuplot";
      deallog << "Writing solution to <" << filename << ">" << std::endl;
      std::ofstream gnuplot_output(filename);

      DataOut<dim> data_out;
      data_out.attach_dof_handler(dof_handler);
      data_out.add_data_vector(solution, "u");

      data_out.build_patches();

      data_out.write_gnuplot(gnuplot_output);
    }
  }

// 下面的 run 函数与前面的例子类似。

  template <int dim>
  void AdvectionProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 6; ++cycle)
      {
        deallog << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation);

            triangulation.refine_global(3);
          }
        else
          refine_grid();

        deallog << "Number of active cells:       "
                << triangulation.n_active_cells() << std::endl;

        setup_system();

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

        assemble_system();
        solve(solution);

        output_results(cycle);
      }
  }
} // namespace Step12

// 下面的 main 函数与前面的例子也类似，不需要注释。

int main()
{
  try
    {
      dealii::deallog.depth_console(5);

      Step12::AdvectionProblem<2> dgmethod;
      dgmethod.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;
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include <Eigen/Eigenvalues>
#include <queue>
#include <tuple>

#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TetraMesh.h"
#include "open3d/utility/Console.h"
#include "open3d/utility/Eigen.h"

namespace open3d {

namespace {
using namespace geometry;

Eigen::Vector3d ComputeEigenvector0(const Eigen::Matrix3d &A, double eval0) {
    Eigen::Vector3d row0(A(0, 0) - eval0, A(0, 1), A(0, 2));
    Eigen::Vector3d row1(A(0, 1), A(1, 1) - eval0, A(1, 2));
    Eigen::Vector3d row2(A(0, 2), A(1, 2), A(2, 2) - eval0);
    Eigen::Vector3d r0xr1 = row0.cross(row1);
    Eigen::Vector3d r0xr2 = row0.cross(row2);
    Eigen::Vector3d r1xr2 = row1.cross(row2);
    double d0 = r0xr1.dot(r0xr1);
    double d1 = r0xr2.dot(r0xr2);
    double d2 = r1xr2.dot(r1xr2);

    double dmax = d0;
    int imax = 0;
    if (d1 > dmax) {
        dmax = d1;
        imax = 1;
    }
    if (d2 > dmax) {
        imax = 2;
    }

    if (imax == 0) {
        return r0xr1 / std::sqrt(d0);
    } else if (imax == 1) {
        return r0xr2 / std::sqrt(d1);
    } else {
        return r1xr2 / std::sqrt(d2);
    }
}

Eigen::Vector3d ComputeEigenvector1(const Eigen::Matrix3d &A,
                                    const Eigen::Vector3d &evec0,
                                    double eval1) {
    Eigen::Vector3d U, V;
    if (std::abs(evec0(0)) > std::abs(evec0(1))) {
        double inv_length =
                1 / std::sqrt(evec0(0) * evec0(0) + evec0(2) * evec0(2));
        U << -evec0(2) * inv_length, 0, evec0(0) * inv_length;
    } else {
        double inv_length =
                1 / std::sqrt(evec0(1) * evec0(1) + evec0(2) * evec0(2));
        U << 0, evec0(2) * inv_length, -evec0(1) * inv_length;
    }
    V = evec0.cross(U);

    Eigen::Vector3d AU(A(0, 0) * U(0) + A(0, 1) * U(1) + A(0, 2) * U(2),
                       A(0, 1) * U(0) + A(1, 1) * U(1) + A(1, 2) * U(2),
                       A(0, 2) * U(0) + A(1, 2) * U(1) + A(2, 2) * U(2));

    Eigen::Vector3d AV = {A(0, 0) * V(0) + A(0, 1) * V(1) + A(0, 2) * V(2),
                          A(0, 1) * V(0) + A(1, 1) * V(1) + A(1, 2) * V(2),
                          A(0, 2) * V(0) + A(1, 2) * V(1) + A(2, 2) * V(2)};

    double m00 = U(0) * AU(0) + U(1) * AU(1) + U(2) * AU(2) - eval1;
    double m01 = U(0) * AV(0) + U(1) * AV(1) + U(2) * AV(2);
    double m11 = V(0) * AV(0) + V(1) * AV(1) + V(2) * AV(2) - eval1;

    double absM00 = std::abs(m00);
    double absM01 = std::abs(m01);
    double absM11 = std::abs(m11);
    double max_abs_comp;
    if (absM00 >= absM11) {
        max_abs_comp = std::max(absM00, absM01);
        if (max_abs_comp > 0) {
            if (absM00 >= absM01) {
                m01 /= m00;
                m00 = 1 / std::sqrt(1 + m01 * m01);
                m01 *= m00;
            } else {
                m00 /= m01;
                m01 = 1 / std::sqrt(1 + m00 * m00);
                m00 *= m01;
            }
            return m01 * U - m00 * V;
        } else {
            return U;
        }
    } else {
        max_abs_comp = std::max(absM11, absM01);
        if (max_abs_comp > 0) {
            if (absM11 >= absM01) {
                m01 /= m11;
                m11 = 1 / std::sqrt(1 + m01 * m01);
                m01 *= m11;
            } else {
                m11 /= m01;
                m01 = 1 / std::sqrt(1 + m11 * m11);
                m11 *= m01;
            }
            return m11 * U - m01 * V;
        } else {
            return U;
        }
    }
}

Eigen::Vector3d FastEigen3x3(Eigen::Matrix3d &A) {
    // Previous version based on:
    // https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
    // Current version based on
    // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
    // which handles edge cases like points on a plane

    double max_coeff = A.maxCoeff();
    if (max_coeff == 0) {
        return Eigen::Vector3d::Zero();
    }
    A /= max_coeff;

    double norm = A(0, 1) * A(0, 1) + A(0, 2) * A(0, 2) + A(1, 2) * A(1, 2);
    if (norm > 0) {
        Eigen::Vector3d eval;
        Eigen::Vector3d evec0;
        Eigen::Vector3d evec1;
        Eigen::Vector3d evec2;

        double q = (A(0, 0) + A(1, 1) + A(2, 2)) / 3;

        double b00 = A(0, 0) - q;
        double b11 = A(1, 1) - q;
        double b22 = A(2, 2) - q;

        double p =
                std::sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2) / 6);

        double c00 = b11 * b22 - A(1, 2) * A(1, 2);
        double c01 = A(0, 1) * b22 - A(1, 2) * A(0, 2);
        double c02 = A(0, 1) * A(1, 2) - b11 * A(0, 2);
        double det = (b00 * c00 - A(0, 1) * c01 + A(0, 2) * c02) / (p * p * p);

        double half_det = det * 0.5;
        half_det = std::min(std::max(half_det, -1.0), 1.0);

        double angle = std::acos(half_det) / (double)3;
        double const two_thirds_pi = 2.09439510239319549;
        double beta2 = std::cos(angle) * 2;
        double beta0 = std::cos(angle + two_thirds_pi) * 2;
        double beta1 = -(beta0 + beta2);

        eval(0) = q + p * beta0;
        eval(1) = q + p * beta1;
        eval(2) = q + p * beta2;

        if (half_det >= 0) {
            evec2 = ComputeEigenvector0(A, eval(2));
            if (eval(2) < eval(0) && eval(2) < eval(1)) {
                A *= max_coeff;
                return evec2;
            }
            evec1 = ComputeEigenvector1(A, evec2, eval(1));
            A *= max_coeff;
            if (eval(1) < eval(0) && eval(1) < eval(2)) {
                return evec1;
            }
            evec0 = evec1.cross(evec2);
            return evec0;
        } else {
            evec0 = ComputeEigenvector0(A, eval(0));
            if (eval(0) < eval(1) && eval(0) < eval(2)) {
                A *= max_coeff;
                return evec0;
            }
            evec1 = ComputeEigenvector1(A, evec0, eval(1));
            A *= max_coeff;
            if (eval(1) < eval(0) && eval(1) < eval(2)) {
                return evec1;
            }
            evec2 = evec0.cross(evec1);
            return evec2;
        }
    } else {
        A *= max_coeff;
        if (A(0, 0) < A(1, 1) && A(0, 0) < A(2, 2)) {
            return Eigen::Vector3d(1, 0, 0);
        } else if (A(1, 1) < A(0, 0) && A(1, 1) < A(2, 2)) {
            return Eigen::Vector3d(0, 1, 0);
        } else {
            return Eigen::Vector3d(0, 0, 1);
        }
    }
}

Eigen::Vector3d ComputeNormal(const PointCloud &cloud,
                              const std::vector<int> &indices,
                              bool fast_normal_computation) {
    if (indices.size() == 0) {
        return Eigen::Vector3d::Zero();
    }
    Eigen::Matrix3d covariance =
            utility::ComputeCovariance(cloud.points_, indices);

    if (fast_normal_computation) {
        return FastEigen3x3(covariance);
    } else {
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
        solver.compute(covariance, Eigen::ComputeEigenvectors);
        return solver.eigenvectors().col(0);
    }
}

// Disjoint set data structure to find cycles in graphs
class DisjointSet {
public:
    DisjointSet(size_t size) : parent_(size), size_(size) {
        for (size_t idx = 0; idx < size; idx++) {
            parent_[idx] = idx;
            size_[idx] = 0;
        }
    }

    // find representative element for given x
    // using path compression
    size_t Find(size_t x) {
        if (x != parent_[x]) {
            parent_[x] = Find(parent_[x]);
        }
        return parent_[x];
    }

    // combine two sets using size of sets
    void Union(size_t x, size_t y) {
        x = Find(x);
        y = Find(y);
        if (x != y) {
            if (size_[x] < size_[y]) {
                size_[y] += size_[x];
                parent_[x] = y;
            } else {
                size_[x] += size_[y];
                parent_[y] = x;
            }
        }
    }

private:
    std::vector<size_t> parent_;
    std::vector<size_t> size_;
};

struct WeightedEdge {
    WeightedEdge(size_t v0, size_t v1, double weight)
        : v0_(v0), v1_(v1), weight_(weight) {}
    size_t v0_;
    size_t v1_;
    double weight_;
};

// Minimum Spanning Tree algorithm (Kruskal's algorithm)
std::vector<WeightedEdge> Kruskal(std::vector<WeightedEdge> &edges,
                                  size_t n_vertices) {
    std::sort(edges.begin(), edges.end(),
              [](WeightedEdge &e0, WeightedEdge &e1) {
                  return e0.weight_ < e1.weight_;
              });
    DisjointSet disjoint_set(n_vertices);
    std::vector<WeightedEdge> mst;
    for (size_t eidx = 0; eidx < edges.size(); ++eidx) {
        size_t set0 = disjoint_set.Find(edges[eidx].v0_);
        size_t set1 = disjoint_set.Find(edges[eidx].v1_);
        if (set0 != set1) {
            mst.push_back(edges[eidx]);
            disjoint_set.Union(set0, set1);
        }
    }
    return mst;
}

}  // unnamed namespace

namespace geometry {

void PointCloud::EstimateNormals(
        const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/,
        bool fast_normal_computation /* = true */) {
    bool has_normal = HasNormals();
    if (!has_normal) {
        normals_.resize(points_.size());
    }
    KDTreeFlann kdtree;
    kdtree.SetGeometry(*this);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        std::vector<int> indices;
        std::vector<double> distance2;
        Eigen::Vector3d normal;
        if (kdtree.Search(points_[i], search_param, indices, distance2) >= 3) {
            normal = ComputeNormal(*this, indices, fast_normal_computation);
            if (normal.norm() == 0.0) {
                if (has_normal) {
                    normal = normals_[i];
                } else {
                    normal = Eigen::Vector3d(0.0, 0.0, 1.0);
                }
            }
            if (has_normal && normal.dot(normals_[i]) < 0.0) {
                normal *= -1.0;
            }
            normals_[i] = normal;
        } else {
            normals_[i] = Eigen::Vector3d(0.0, 0.0, 1.0);
        }
    }
}

void PointCloud::OrientNormalsToAlignWithDirection(
        const Eigen::Vector3d &orientation_reference
        /* = Eigen::Vector3d(0.0, 0.0, 1.0)*/) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsToAlignWithDirection] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        auto &normal = normals_[i];
        if (normal.norm() == 0.0) {
            normal = orientation_reference;
        } else if (normal.dot(orientation_reference) < 0.0) {
            normal *= -1.0;
        }
    }
}

void PointCloud::OrientNormalsTowardsCameraLocation(
        const Eigen::Vector3d &camera_location /* = Eigen::Vector3d::Zero()*/) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsTowardsCameraLocation] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        Eigen::Vector3d orientation_reference = camera_location - points_[i];
        auto &normal = normals_[i];
        if (normal.norm() == 0.0) {
            normal = orientation_reference;
            if (normal.norm() == 0.0) {
                normal = Eigen::Vector3d(0.0, 0.0, 1.0);
            } else {
                normal.normalize();
            }
        } else if (normal.dot(orientation_reference) < 0.0) {
            normal *= -1.0;
        }
    }
}

void PointCloud::OrientNormalsConsistentTangentPlane(size_t k) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsConsistentTangentPlane] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }

    // Create Riemannian graph (Euclidian MST + kNN)
    // Euclidian MST is subgraph of Delaunay triangulation
    std::shared_ptr<TetraMesh> delaunay_mesh;
    std::vector<size_t> pt_map;
    std::tie(delaunay_mesh, pt_map) = TetraMesh::CreateFromPointCloud(*this);
    std::vector<WeightedEdge> delaunay_graph;
    std::unordered_set<size_t> graph_edges;
    auto EdgeIndex = [&](size_t v0, size_t v1) -> size_t {
        return std::min(v0, v1) * points_.size() + std::max(v0, v1);
    };
    auto AddEdgeToDelaunayGraph = [&](size_t v0, size_t v1) {
        v0 = pt_map[v0];
        v1 = pt_map[v1];
        size_t edge = EdgeIndex(v0, v1);
        if (graph_edges.count(edge) == 0) {
            double dist = (points_[v0] - points_[v1]).squaredNorm();
            delaunay_graph.push_back(WeightedEdge(v0, v1, dist));
            graph_edges.insert(edge);
        }
    };
    for (const Eigen::Vector4i &tetra : delaunay_mesh->tetras_) {
        AddEdgeToDelaunayGraph(tetra[0], tetra[1]);
        AddEdgeToDelaunayGraph(tetra[0], tetra[2]);
        AddEdgeToDelaunayGraph(tetra[0], tetra[3]);
        AddEdgeToDelaunayGraph(tetra[1], tetra[2]);
        AddEdgeToDelaunayGraph(tetra[1], tetra[3]);
        AddEdgeToDelaunayGraph(tetra[2], tetra[3]);
    }

    std::vector<WeightedEdge> mst = Kruskal(delaunay_graph, points_.size());

    auto NormalWeight = [&](size_t v0, size_t v1) -> double {
        return 1.0 - std::abs(normals_[v0].dot(normals_[v1]));
    };
    for (auto &edge : mst) {
        edge.weight_ = NormalWeight(edge.v0_, edge.v1_);
    }

    // Add k nearest neighbors to Riemannian graph
    KDTreeFlann kdtree(*this);
    for (size_t v0 = 0; v0 < points_.size(); ++v0) {
        std::vector<int> neighbors;
        std::vector<double> dists2;
        kdtree.SearchKNN(points_[v0], int(k), neighbors, dists2);
        for (size_t vidx1 = 0; vidx1 < neighbors.size(); ++vidx1) {
            size_t v1 = size_t(neighbors[vidx1]);
            if (v0 == v1) {
                continue;
            }
            size_t edge = EdgeIndex(v0, v1);
            if (graph_edges.count(edge) == 0) {
                double weight = NormalWeight(v0, v1);
                mst.push_back(WeightedEdge(v0, v1, weight));
                graph_edges.insert(edge);
            }
        }
    }

    // extract MST from Riemannian graph
    mst = Kruskal(mst, points_.size());

    // convert list of edges to graph
    std::vector<std::unordered_set<size_t>> mst_graph(points_.size());
    for (const auto &edge : mst) {
        size_t v0 = edge.v0_;
        size_t v1 = edge.v1_;
        mst_graph[v0].insert(v1);
        mst_graph[v1].insert(v0);
    }

    // find start node for tree traversal
    // init with node that maximizes z
    double max_z = std::numeric_limits<double>::lowest();
    size_t v0;
    for (size_t vidx = 0; vidx < points_.size(); ++vidx) {
        const Eigen::Vector3d &v = points_[vidx];
        if (v(2) > max_z) {
            max_z = v(2);
            v0 = vidx;
        }
    }

    // traverse MST and orient normals consistently
    std::queue<size_t> traversal_queue;
    std::vector<bool> visited(points_.size(), false);
    traversal_queue.push(v0);
    auto TestAndOrientNormal = [&](const Eigen::Vector3d &n0,
                                   Eigen::Vector3d &n1) {
        if (n0.dot(n1) < 0) {
            n1 *= -1;
        }
    };
    TestAndOrientNormal(Eigen::Vector3d(0, 0, 1), normals_[v0]);
    while (!traversal_queue.empty()) {
        v0 = traversal_queue.front();
        traversal_queue.pop();
        visited[v0] = true;
        for (size_t v1 : mst_graph[v0]) {
            if (!visited[v1]) {
                traversal_queue.push(v1);
                TestAndOrientNormal(normals_[v0], normals_[v1]);
            }
        }
    }
}

}  // namespace geometry
}  // namespace open3d


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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 BOOST_MATH_TOOLS_POLYNOMIAL_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_HPP

#include <boost/assert.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/real_cast.hpp>

#include <vector>

namespace boost{ namespace math{ namespace tools{

template <class T>
class polynomial
{
public:
   // typedefs:
   typedef typename std::vector<T>::value_type value_type;
   typedef typename std::vector<T>::size_type size_type;

   // construct:
   polynomial(){}
   template <class U>
   polynomial(const U* data, unsigned order)
      : m_data(data, data + order + 1)
   {
   }
   template <class U>
   polynomial(const U& point)
   {
      m_data.push_back(point);
   }

   // copy:
   polynomial(const polynomial& p)
      : m_data(p.m_data) { }

   template <class U>
   polynomial(const polynomial<U>& p)
   {
      for(unsigned i = 0; i < p.size(); ++i)
      {
         m_data.push_back(boost::math::tools::real_cast<T>(p[i]));
      }
   }

   // access:
   size_type size()const { return m_data.size(); }
   size_type degree()const { return m_data.size() - 1; }
   value_type& operator[](size_type i)
   {
      return m_data[i];
   }
   const value_type& operator[](size_type i)const
   {
      return m_data[i];
   }
   T evaluate(T z)const
   {
      return boost::math::tools::evaluate_polynomial(&m_data[0], z, m_data.size());;
   }

   // operators:
   template <class U>
   polynomial& operator +=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(value);
      else
      {
         m_data[0] += value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator -=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(-value);
      else
      {
         m_data[0] -= value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator *=(const U& value)
   {
      for(size_type i = 0; i < m_data.size(); ++i)
         m_data[i] *= value;
      return *this;
   }
   template <class U>
   polynomial& operator +=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] += value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator -=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] -= value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(-value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator *=(const polynomial<U>& value)
   {
      // TODO: FIXME: use O(N log(N)) algorithm!!!
      BOOST_ASSERT(value.size());
      polynomial base(*this);
      *this *= value[0];
      for(size_type i = 1; i < value.size(); ++i)
      {
         polynomial t(base);
         t *= value[i];
         size_type s = size() - i;
         for(size_type j = 0; j < s; ++j)
         {
            m_data[i+j] += t[j];
         }
         for(size_type j = s; j < t.size(); ++j)
            m_data.push_back(t[j]);
      }
      return *this;
   }

private:
   std::vector<T> m_data;
};

template <class T>
inline polynomial<T> operator + (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T>
inline polynomial<T> operator - (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T>
inline polynomial<T> operator * (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator + (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator - (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator * (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator + (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result += a;
   return result;
}

template <class U, class T>
inline polynomial<T> operator - (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator * (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result *= a;
   return result;
}

template <class charT, class traits, class T>
inline std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& os, const polynomial<T>& poly)
{
   os << "{ ";
   for(unsigned i = 0; i < poly.size(); ++i)
   {
      if(i) os << ", ";
      os << poly[i];
   }
   os << " }";
   return os;
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_POLYNOMIAL_HPP


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      130228    E.D. Brandon      First creation of the code.
 *
 *    References
 *
 *    Notes
 *      The current test cases are used to check the code of the mathematical functions, each case
 *      represents a special case where certain elements of the function can be eliminated.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/improvedInversePolynomialWall.h"
#include "Tudat/Astrodynamics/MissionSegments/oscillatingFunctionNovak.h"

namespace tudat
{
namespace unit_tests
{

//! Test the spherical shape function code.
BOOST_AUTO_TEST_SUITE( test_spherical_shape_function )

//! Test case: inverse polynomial function.
BOOST_AUTO_TEST_CASE( ImprovedInversePolynomialWall )
{
    // Using declaration.
    using mathematical_constants::PI;

    // Initialize parameters of the inverse polynomial function.
    std::pair< Eigen::Vector3d, Eigen::Vector3d > inversePolynomialParameters;
    double timeDepParameter;
    double azimuthalAngle = 0.0;

    // Error tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    //*********************************************************************************************
    // CASE 1: b = 1.0, polar angle = 0.0, all other parameters 0.0
    //*********************************************************************************************

    // Case specific changes.
    inversePolynomialParameters.first = Eigen::Vector3d::Zero(  );
    inversePolynomialParameters.second = Eigen::Vector3d::Zero(  );
    timeDepParameter = 0.0;
    inversePolynomialParameters.first( 1 ) = 1.0; // b

    // Expected results.
    double expectedFunctionValue = 1.0;
    double expectedFirstDerivative = 0.0;
    double expectedSecondDerivative = 1.0;
    double expectedThirdDerivative = 0.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase1(
                boost::lambda::constant( timeDepParameter ) ,
                boost::lambda::constant( inversePolynomialParameters ) );

    // Calculate function value and derivatives.
    double functionValue = myFunctionCase1.evaluate( azimuthalAngle );
    double firstDerivative = myFunctionCase1.computeDerivative( 1 , azimuthalAngle );
    double secondDerivative = myFunctionCase1.computeDerivative( 2 , azimuthalAngle );
    double thirdDerivative = myFunctionCase1.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 2: c = 0.0, polar angle = 0.0, all other parameters 1.0
    //*********************************************************************************************

    // Case-specific changes.
    inversePolynomialParameters.first = Eigen::Vector3d::Ones(  );
    inversePolynomialParameters.second = Eigen::Vector3d::Ones(  );
    timeDepParameter = 1.0;
    inversePolynomialParameters.first( 2 ) = 0.0; // c

    // Expected results.
    expectedFunctionValue = 1.0 / 2.0;
    expectedFirstDerivative = 0.0;
    expectedSecondDerivative = 1.0 / 4.0;
    expectedThirdDerivative = - 6.0 / 4.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase2(
                boost::lambda::constant( timeDepParameter ) ,
                boost::lambda::constant( inversePolynomialParameters ) );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase2.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase2.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase2.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase2.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 3: c = pi - 1.0, polar angle = 1.0, all other parameters 1.0
    //*********************************************************************************************

    // Case-specific changes.
    inversePolynomialParameters.first( 2 ) = PI - 1.0; // c
    azimuthalAngle = 1.0;

    // Expected results.
    expectedFunctionValue = 1.0 / 4.0;
    expectedFirstDerivative = -9.0 / 8.0;
    expectedSecondDerivative = 93.0 / 16.0;
    expectedThirdDerivative = -267.0 / 8.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase3(
                boost::lambda::constant( timeDepParameter ) ,
                boost::lambda::constant( inversePolynomialParameters ) );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase3.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase3.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase3.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase3.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );
}

//! Test case: oscillating function.
BOOST_AUTO_TEST_CASE( OscillatingFunctionNovak )
{
    // Using declaration.
    using mathematical_constants::PI;

    // Initialize parameters of the inverse polynomial function.
    std::pair< Eigen::Vector2d, Eigen::Vector2d > oscillatingFunctionParameters;
    double azimuthalAngle = 0.0;

    // Error tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon(  );

    //*********************************************************************************************
    // CASE 1: theta = 0.0, all parameters = 1.0
    //*********************************************************************************************

    // Case specific changes.
    oscillatingFunctionParameters.first = Eigen::Vector2d::Ones(  );
    oscillatingFunctionParameters.second = Eigen::Vector2d::Ones(  );

    // Expected results.
    double expectedFunctionValue = 1.0;
    double expectedFirstDerivative = 2.0;
    double expectedSecondDerivative = 1.0;
    double expectedThirdDerivative = - 4.0;

    // Initialize mathematical function.
    mission_segments::OscillatingFunctionNovak myFunctionCase1(
                boost::lambda::constant( oscillatingFunctionParameters ) );

    // Calculate function value and derivatives.
    double functionValue = myFunctionCase1.evaluate( azimuthalAngle );
    double firstDerivative = myFunctionCase1.computeDerivative( 1 , azimuthalAngle );
    double secondDerivative = myFunctionCase1.computeDerivative( 2 , azimuthalAngle );
    double thirdDerivative = myFunctionCase1.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 2: theta = pi/2, all parameters = 1.0
    //*********************************************************************************************

    // Case-specific changes.
    azimuthalAngle = PI / 2.0;

    // Expected results.
    expectedFunctionValue = 1.0 + PI / 2.0;
    expectedFirstDerivative = -PI / 2.0;
    expectedSecondDerivative = -3.0 - PI / 2.0;
    expectedThirdDerivative = -2.0 + PI / 2.0;

    // The term cos(PI/2) introduces a small error in the function value, and in the derivative
    // values. [ cos(PI/2) is approximately 6.12323e-017 ]
    // The calculation of the function value, the first derivative and the second derivative are
    // within the numeric limits, however the error in the third derivative is larger than this
    // limit. The tolerance for the third derivative is therefore more flexible than that of the
    // other calculations.
    const double toleranceThirdDerivative = 4.0 * tolerance;

    // The exact value of the third derivative, due to the introduced error. This vaue is checked
    // separately.
    double exactThirdDerivative =
            -2.0 + PI / 2.0 - 1.0 * ( 4.0 + PI / 2.0 ) * std::cos( PI / 2.0 );

    // Initialize mathematical function.
    mission_segments::OscillatingFunctionNovak myFunctionCase2(
                boost::lambda::constant( oscillatingFunctionParameters ) );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase2.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase2.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase2.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase2.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                toleranceThirdDerivative );

    BOOST_CHECK_EQUAL( thirdDerivative, exactThirdDerivative );
}

//! Test case: request wrong derivative or integral of one of the mathematical functions.
BOOST_AUTO_TEST_CASE( wrongRequestMathematicalFunctions )
{
    // Initialize parameters of the mathematical functions.
    // Inverse polynomial parameters.
    std::pair< Eigen::Vector3d, Eigen::Vector3d > inversePolynomialParameters;
    inversePolynomialParameters.first.Zero();
    inversePolynomialParameters.second.Zero();
    double timeDepParameter = 0.0;

    // Oscillating function parameters.
    std::pair< Eigen::Vector2d, Eigen::Vector2d > OscillatingShapeParameters;
    OscillatingShapeParameters.first.Zero(  );
    OscillatingShapeParameters.second.Zero(  );

    // Initialize mathematical functions.
    mission_segments::ImprovedInversePolynomialWall myInversePolynomial(
                boost::lambda::constant( timeDepParameter ) ,
                boost::lambda::constant( inversePolynomialParameters ) );

    mission_segments::OscillatingFunctionNovak myOscillatingFunction(
                boost::lambda::constant( OscillatingShapeParameters ) );

    // Set flags.
    bool isFourthDerivativeInversePolynomial = true;
    bool isFourthDerivativeOscillatingFunction = true;
    bool isDefiniteIntegralInversePolynomial = true;
    bool isDefiniteIntegralOscillatingFunction = true;

    // Try to calculate the fourth derivative of the inverse polynomial function, which should
    // result in a runtime error.
    try
    {
        // Calculate the fourth derivative of the inverse polynomial function.
        myInversePolynomial.computeDerivative( 4 , 0.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isFourthDerivativeInversePolynomial = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isFourthDerivativeInversePolynomial );

    // Try to calculate the definite integral of the inverse polynomial function, which should
    // result in a runtime error.
    try
    {
        // Calculate the definite integral of the inverse polynomial function.
        myInversePolynomial.computeDefiniteIntegral( 1 , 0.0 , 1.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isDefiniteIntegralInversePolynomial = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isDefiniteIntegralInversePolynomial );

    // Try to calculate the fourth derivative of the oscillating function, which should result in a
    // runtime error.
    try
    {
        // Calculate the fourth derivative of the oscillating function.
        myOscillatingFunction.computeDerivative( 4 , 0.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isFourthDerivativeOscillatingFunction = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isFourthDerivativeOscillatingFunction );

    // Try to calculate the definite integral of the oscillating function, which should result in a
    // runtime error.
    try
    {
        // Calculate the definite integral of the oscillating function.
        myOscillatingFunction.computeDefiniteIntegral( 1 , 0.0 , 1.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isDefiniteIntegralOscillatingFunction = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isDefiniteIntegralOscillatingFunction );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include <sophus/se3.h>

// user
#include "pinhole_camera.h"
#include "generate_point_cloud.h"
#include "frame.h"
#include "detect_features.h"
#include "align_image.h"
#include "param_reader.h"
#include "image_reader.h"
#include "visual_odometry.h"

using namespace std;

int main(int argc, char**argv)
{
	int start_index, end_index;

	ParameterReader::Ptr param_reader( new ParameterReader("./param.txt") );
	ImageReader image_reader(param_reader);
	VisualOdometry visual_odometry(param_reader);

	start_index = param_reader->getParam<int>("start_index");
	end_index = param_reader->getParam<int>("end_index");

	// 循环处理其他帧
	for(int index = start_index; index <= end_index; index++) {
		cout << "***********loop " << index-start_index +1<< " ***********index " << index << " *************" << endl;
		image_reader.readImage(index);
		visual_odometry.addImage(image_reader.getRGBImg(), image_reader.getDepthImg());
	}

	// 保存最终的点云文件
// 	point_cloud_generator.savePointCloud();

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 combinatorial toolbox - combs/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of combinatorial components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 07/03/2011
///
#include <nt2/toolbox/combinatorial/include/functions/combs.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/saturate.hpp>
#include <nt2/include/functions/rem.hpp>
#include <nt2/include/functions/find.hpp>
#include <nt2/include/functions/cast.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <nt2/include/functions/colvect.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/tic.hpp>
#include <nt2/include/functions/toc.hpp>
#include <nt2/sdk/meta/type_id.hpp>

NT2_TEST_CASE_TPL ( combs_integer__1_0, NT2_REAL_TYPES)
{

  using nt2::combs;
  using nt2::tag::combs_;

   nt2::table<T, nt2::_2D> a =  combs(nt2::_(T(1), T(5)), 3);
   NT2_DISPLAY(a);
   nt2::table<T, nt2::_2D> b =  combs(nt2::_(T(1), T(5)), 5);
   NT2_DISPLAY(b);
   NT2_TEST(nt2::isequal(b,nt2::_(T(1), T(5)) ));
   nt2::table<T, nt2::_2D> c =  combs(nt2::_(T(1), T(5)), 1);
   NT2_DISPLAY(c);
   NT2_TEST(nt2::isequal(c,nt2::colvect(nt2::_(T(1), T(5)))));

}


/*
 * BooleanFunction.hpp
 *
 *  Created on: Jun 1, 2010
 *      Author: stewie
 */

#ifndef BOOLEANFUNCTION_HPP_
#define BOOLEANFUNCTION_HPP_

#include <cstddef>
#include <cmath>
#include <iosfwd>
#include <utility>
#include <vector>

#include <boost/concept_check.hpp>
#include <boost/range/concepts.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/size.hpp>

#include "network_state.hpp"

namespace bn {

class BooleanFunction {
public:
	template<class RandomAccessRange> BooleanFunction(
			const RandomAccessRange& r) :
		def(boost::size(r)), arity(arityFromSize(def.size())) {
		BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<RandomAccessRange>));
		typename boost::range_iterator<const RandomAccessRange>::type it =
				boost::begin(r);
		for (std::size_t i = 0; i < def.size(); ++i, ++it)
			def[i] = *it;
	}

	BooleanFunction(const bool val = false) :
		def(1, val), arity(0) {
	}

	explicit BooleanFunction(const State& s);

	std::size_t getArity() const {
		return arity;
	}

	std::size_t size() const {
		return def.size();
	}

	std::vector<int> truthTable() const;

	bool isInfluent(const std::size_t i) const;

	BooleanFunction clamped(const std::size_t i, const bool v) const;

	void clamp(const std::size_t i, const bool v);

	std::pair<bool, bool> isConstant() const {
		const bool allZeros = def.none();
		const bool allOnes = (~def).none();
		return std::make_pair(allZeros || allOnes, allOnes);
	}

	bool operator()(const State& x) const;

	bool operator==(const BooleanFunction& other) const {
		return def == other.def;
	}

	bool operator<(const BooleanFunction& other) const;

	friend std::ostream
	& operator<<(std::ostream& out, const BooleanFunction& f);

	friend std::size_t hash_value(const BooleanFunction&);

	friend void swap(BooleanFunction&, BooleanFunction&);

private:
	State def;
	std::size_t arity;

	std::pair<State, State> demultiplex(const std::size_t i) const;

	static std::size_t arityFromSize(const std::size_t n) {
		return static_cast<std::size_t> (log2(n));
	}
};

} // namespace bn

#endif /* BOOLEANFUNCTION_HPP_ */


#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Dense>
#include <cmath>

namespace py = pybind11;

void line(double fx1, double fy1, double fx2, double fy2, const Eigen::MatrixXd& global_map,
          Eigen::MatrixXd& op_map) {
  int x1 = static_cast<int>(round(fx1));
  int y1 = static_cast<int>(round(fy1));
  int x2 = static_cast<int>(round(fx2));
  int y2 = static_cast<int>(round(fy2));

  int dx = abs(x2 - x1);
  int dy = abs(y2 - y1);
  int x = x1, y = y1;
  int error = dx - dy;
  int x_inc = x2 > x1 ? 1 : -1;
  int y_inc = y2 > y1 ? 1 : -1;
  dx *= 2;
  dy *= 2;

  int coll_flag = 0;
  int coll_size = 10;

  int rows = global_map.rows();
  int cols = global_map.cols();

  while (x >= 0 && x < cols && y >= 0 && y < rows) {
    if (x == x2 && y == y2) break;

    int k = global_map(y, x);
    if (k != 1 && coll_flag > 0) break;
    if (k == 1 && coll_flag < coll_size) coll_flag += 1;

    op_map(y, x) = k;

    if (k == 1 && coll_flag == coll_size) break;

    if (error > 0) {
      x += x_inc;
      error -= dy;
    } else {
      y += y_inc;
      error += dx;
    }
  }
}

Eigen::MatrixXd inverse_sensor_model(int x0, int y0, int sensor_range,
                            Eigen::MatrixXd op_map, Eigen::MatrixXd global_map) {
  Eigen::MatrixXd op_map_mod(op_map);
  double sensor_angle_inc = 0.5 / 180.0 * M_PI;
  for (double angle = 0.0; angle < M_PI * 2.0; angle += sensor_angle_inc) {
    double x1 = double(x0) + double(sensor_range) * cos(angle);
    double y1 = double (y0) + double(sensor_range) * sin(angle);
    line(x0, y0, x1, y1, global_map, op_map_mod);
  }
  return op_map_mod;
}

PYBIND11_PLUGIN(inverse_sensor_model) {
  py::module m("inverse_sensor_model", "inverse_sensor_model");
  m.def("inverse_sensor_model", &inverse_sensor_model, "inverse_sensor_model");
  return m.ptr();
}


#include "testsuite.h"
#include <blitz/array.h>
#include <random/discrete-uniform.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
  // try with all kinds of different sizes to make sure it works for
  // aligned and nonaligned lengths.
  for(int i=0;i<20;++i) {
    const int sx=ranlib::DiscreteUniform<int>(10).random();
    const int sy=ranlib::DiscreteUniform<int>(10).random();
    const int sz=ranlib::DiscreteUniform<int>(10).random();
    Array<int,3> A(sx,sy,sz, contiguousArray);

//    A.dumpStructureInformation();

    A.transposeSelf(secondDim, thirdDim, firstDim);

    BZTEST(A.ordering(0) == 1 && A.ordering(1) == 0 && A.ordering(2) == 2);
    BZTEST(A.length(0) == sy && A.length(1) == sz && A.length(2) == sx);
    BZTEST(A.stride(0) == sz && A.stride(1) == 1 && A.stride(2) == sz*sy);
  }

//    A.dumpStructureInformation();

    Array<int,2> B(3,3);
    B = 0, 1, 2,
        3, 4, 5,
        6, 7, 8;

//    cout << B << endl;

    B.transposeSelf(secondDim, firstDim);
    Array<int,2> C(3,3);
    C = 0, 3, 6,
        1, 4, 7,
        2, 5, 8;
    BZTEST(count(B==C) == 9);

//    cout << B << endl;

    Array<int,2> D(3,3,FortranArray<2>());
    D = 0, 1, 2,
        3, 4, 5,
        6, 7, 8;
    D.transposeSelf(secondDim, firstDim);
    Array<int,2> E(3,3,FortranArray<2>());
    E = 0, 3, 6,
        1, 4, 7,
        2, 5, 8;
    BZTEST(count(D==E) == 9);

    return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
using namespace boost::hana;
using namespace literals;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
        return x % 2_c != 0_c;
    };

    BOOST_HANA_CONSTEXPR_ASSERT(any(list(1, 2), odd));
    BOOST_HANA_CONSTANT_ASSERT(!any(list(2_c, 4_c), odd));

    BOOST_HANA_CONSTANT_ASSERT(
        any(list(type<void>, type<char&>), trait<std::is_void>)
    );
    BOOST_HANA_CONSTANT_ASSERT(
        !any(list(type<void>, type<char&>), trait<std::is_integral>)
    );
    //! [main]
}


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/variable_period_sums.hpp>
#include "./is_close.hpp"

using namespace boost;
using namespace unit_test;

///////////////////////////////////////////////////////////////////////////////
// test_variable_period_sums
//
void test_variable_period_sums()
{
    using namespace time_series;
    typedef counting_iterator<int> int_;

    dense_series<int> d;
    std::copy(int_(0), int_(10), make_ordered_inserter(d, 0)).commit();

    std::ptrdiff_t periods[] = { 0, 4, 6, 8, 12 };

    sparse_series<int> result;
    make_ordered_inserter(result)
        (6, 0)(9, 4)(13, 6)(17, 8)
    .commit();

    BOOST_CHECK_EQUAL(result, variable_period_sums(d, periods));
}

///////////////////////////////////////////////////////////////////////////////
// test_shifted_variable_period_sums
//
void test_shifted_variable_period_sums()
{
    using namespace time_series;
    typedef counting_iterator<int> int_;

    dense_series<int> d;
    std::copy(int_(0), int_(10), make_ordered_inserter(d, 0)).commit();

    std::ptrdiff_t periods[] = { 0, 4, 6, 8, 12 };
    int elements[] = { 0, 1, 0, 2, 0 };

    sparse_series<int> result;
    make_ordered_inserter(result)
        (6, 0)(9, 5)(13, 6)(17, 10)
    .commit();

    BOOST_CHECK_EQUAL(result, shifted_variable_period_sums(d, periods, elements));
}

///////////////////////////////////////////////////////////////////////////////
// test_floating_variable_period_sums
//
void test_floating_variable_period_sums()
{
    using namespace time_series;
    piecewise_constant_series<double, int, double> pwc;
    make_ordered_inserter(pwc)
        (3., -2.2, -1.1)
        (4., -.1, .1)
        (5., 1.1, 2.2)
        (6., 10.1, 20.2)
    .commit();

    double periods[] = { -10.0, -2.1, -0.9, 0.9, 2.1, 3.3, 6.6, 12.3, 15.7 };

    sparse_series<double, int, double> expected;
    make_ordered_inserter(expected)
        (0.3, -10)
        (3, -2.1)
        (0.8, -0.9)
        (5, 0.9)
        (0.5, 2.1)
        (13.2, 6.6)
        (20.4, 12.3)
        (27, 15.7)
    .commit();

    sparse_series<double, int, double> actual = variable_period_sums(pwc, periods);
    BOOST_CHECK(is_close(expected, actual, 0.1));
}

///////////////////////////////////////////////////////////////////////////////
// test_floating_variable_period_sums2
//
void test_floating_variable_period_sums2()
{
    using namespace time_series;
    piecewise_constant_series<double> pwc;
    make_ordered_inserter(pwc)
        (3.1, -2, -1)
        (4.2, -1, 0)
        (5.3, 1, 2)
        (6.4, 10, 20)
    .commit();

    std::ptrdiff_t periods[] = { -10, -2, 0, 1, 2, 3, 6, 12, 15 };

    sparse_series<double> expected;
    make_ordered_inserter(expected)
        (7.3, -2)
        (5.3, 1)
        (12.8, 6)
        (19.2, 12)
        (32, 15)
    .commit();

    sparse_series<double> actual = variable_period_sums(pwc, periods);
    BOOST_CHECK(is_close(expected, actual, 0.1));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("variable_period_sums test");

    test->add(BOOST_TEST_CASE(&test_variable_period_sums));
    test->add(BOOST_TEST_CASE(&test_shifted_variable_period_sums));
    test->add(BOOST_TEST_CASE(&test_floating_variable_period_sums));
    test->add(BOOST_TEST_CASE(&test_floating_variable_period_sums2));

    return test;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_math_abs
#endif

#include <cmath>
#include <iostream>

#include <boost/test/included/unit_test.hpp>

#include <boost/config.hpp>

#include "number_types.hpp"

// important: the include above must precede the include below,
// otherwise the test will fail for the custom number type:
// custom_with_global_sqrt

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

namespace bg = boost::geometry;
namespace bgm = boost::geometry::math;

template <typename T>
bool eq(T const& l, T const& r)
{
    return !(l < r || r < l);
}

BOOST_AUTO_TEST_CASE( test_math_abs )
{
    {
        float p1 = bgm::pi<float>();
        double p2 = bgm::pi<double>();
        long double p3 = bgm::pi<long double>();

        BOOST_CHECK(bgm::abs(p1) == p1);
        BOOST_CHECK(bgm::abs(p2) == p2);
        BOOST_CHECK(bgm::abs(p3) == p3);

        float n1 = -p1;
        double n2 = -p2;
        long double n3 = -p3;

        BOOST_CHECK(bgm::abs(n1) == p1);
        BOOST_CHECK(bgm::abs(n2) == p2);
        BOOST_CHECK(bgm::abs(n3) == p3);
    }

    {
        number_types::custom<double> p1(bgm::pi<double>());
        number_types::custom_with_global_sqrt<double> p2(bgm::pi<double>());
        custom_global<double> p3(bgm::pi<double>());
        custom_raw<double> p4(bgm::pi<double>());

        BOOST_CHECK(eq(bgm::abs(p1), p1));
        BOOST_CHECK(eq(bgm::abs(p2), p2));
        BOOST_CHECK(eq(bgm::abs(p3), p3));
        BOOST_CHECK(eq(bgm::abs(p4), p4));

        number_types::custom<double> n1 = -p1;
        number_types::custom_with_global_sqrt<double> n2 = -p2;
        custom_global<double> n3 = -p3;
        custom_raw<double> n4 = -p4;

        BOOST_CHECK(eq(bgm::abs(n1), p1));
        BOOST_CHECK(eq(bgm::abs(n2), p2));
        BOOST_CHECK(eq(bgm::abs(n3), p3));
        BOOST_CHECK(eq(bgm::abs(n4), p4));
    }

#ifdef HAVE_TTMATH
    {
        ttmath_big p1 = bgm::pi<ttmath_big>();
        ttmath::Big<1, 4> p1 = bgm::pi<ttmath::Big<1, 4> >();

        BOOST_CHECK(bgm::abs(p1) == p1);
        BOOST_CHECK(bgm::abs(p2) == p2);

        ttmath_big n1 = -p1;
        ttmath::Big<1, 4> n2 = -p2;

        BOOST_CHECK(bgm::abs(n1) == p1);
        BOOST_CHECK(bgm::abs(n2) == p2);
    }
#endif
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_sensor_iid.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <memory>
#include <type_traits>

#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/util/meta.hpp>
#include <fl/distribution/gaussian.hpp>

#include <fl/model/adaptive_model.hpp>
#include <fl/model/sensor/interface/sensor_function.hpp>

namespace fl
{

// Forward declarations
template <typename...Models> class JointSensor;

/**
 * Traits of JointSensor<MultipleOf<Sensor, Count>>
 */
template <
    typename Sensor,
    int Count
>
struct Traits<JointSensor<MultipleOf<Sensor, Count>>>
{
    enum : signed int { ModelCount = Count };

    typedef Sensor LocalSensor;
    typedef typename Sensor::State State;
    typedef typename Sensor::Obsrv::Scalar Scalar;
    typedef typename Sensor::Obsrv LocalObsrv;
    typedef typename Sensor::Noise LocalNoise;

    enum : signed int
    {
        StateDim = SizeOf<State>::Value,
        ObsrvDim = ExpandSizes<SizeOf<LocalObsrv>::Value, Count>::Value,
        NoiseDim = ExpandSizes<SizeOf<LocalNoise>::Value, Count>::Value
    };

    typedef Eigen::Matrix<Scalar, ObsrvDim, 1> Obsrv;
    typedef Eigen::Matrix<Scalar, NoiseDim, 1> Noise;

    typedef SensorFunction<Obsrv, State, Noise> SensorFunctionBase;
};

/**
 * \ingroup sensors
 *
 * \brief JointSensor itself is an observation model which contains
 * internally multiple models all of the \em same type. The joint model can be
 * simply summarized as \f$ h(x, \theta, w) = [ h_{local}(x, \theta_1, w_1),
 * h_{local}(x, \theta_2, w_2), \ldots, h_{local}(x, \theta_n, w_n) ]^T \f$
 *
 * where \f$x\f$ is the state variate, \f$\theta = [ \theta_1, \theta_2, \ldots,
 * \theta_n ]^T\f$ and \f$w = [ w_1, w_2, \ldots, w_n ]^T\f$  are the the
 * parameters and noise terms of each of the \f$n\f$ models.
 *
 * JointSensor implements the SensorInterface and the
 * AdaptiveModel interface. That being said, the JointSensor can be
 * used as a regular observation model or even as an adaptive observation model
 * which provides a set of parameters that can be changed at any time. This
 * implies that all sub-models must implement the the AdaptiveModel
 * interface. However, if the sub-model is not adaptive, JointSensor
 * applies a decorator call the NotAdaptive operator on the sub-model. This
 * operator enables any model to be treated as if it is adaptive without
 * effecting the model behaviour.
 */
template <
    typename LocalSensor,
    int Count
>
class JointSensor<MultipleOf<LocalSensor, Count>>
    : public Traits<
                 JointSensor<MultipleOf<LocalSensor, Count>>
             >::SensorFunctionBase,
      public Descriptor,
      private internal::JointSensorIidType
{
private:
    typedef JointSensor<MultipleOf<LocalSensor,Count>> This;

public:
    enum : signed int { ModelCount = Count };

    typedef LocalSensor LocalModel;
    typedef typename Traits<This>::LocalObsrv LocalObsrv;
    typedef typename Traits<This>::LocalNoise LocalNoise;

    typedef typename Traits<This>::Obsrv Obsrv;
    typedef typename Traits<This>::Noise Noise;
    typedef typename Traits<This>::State State;

public:
    JointSensor(
            const LocalSensor& local_sensor,
            int count = ToDimension<Count>::Value)
        : local_sensor_(local_sensor),
          count_(count)
    {
        assert(count_ > 0);
    }

    template <typename Model>
    JointSensor(const MultipleOf<Model, Count>& mof)
        : local_sensor_(mof.instance),
          count_(mof.count)
    {
        assert(count_ > 0);
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~JointSensor() noexcept { }

    Obsrv observation(const State& state, const Noise& noise) const override
    {
        Obsrv y = Obsrv::Zero(obsrv_dimension(), 1);

        const int obsrv_dim = local_sensor_.obsrv_dimension();
        const int noise_dim = local_sensor_.noise_dimension();

        for (int i = 0; i < count_; ++i)
        {
            local_sensor_.id(i);

            y.middleRows(i * obsrv_dim, obsrv_dim) =
                local_sensor_.observation(
                    state,
                    noise.middleRows(i * noise_dim, noise_dim));
        }

        return y;
    }

    int obsrv_dimension() const override
    {
        return local_sensor_.obsrv_dimension() * count_;
    }

    int noise_dimension() const override
    {
        return local_sensor_.noise_dimension() * count_;
    }

    int state_dimension() const override
    {
        return local_sensor_.state_dimension();
    }

    LocalSensor& local_sensor()
    {
        return local_sensor_;
    }

    const LocalSensor& local_sensor() const
    {
        return local_sensor_;
    }

    virtual std::string name() const
    {
        return "JointSensor<MultipleOf<"
                    + this->list_arguments(local_sensor_.name()) +
               ", Count>>";
    }

    virtual std::string description() const
    {
        return "Joint observation model of multiple local observation models "
                " with non-additive noise.";
    }

    /**
     *
     * \brief Returns the number of local models within this joint model
     */
    virtual int count_local_models() const
    {
        return count_;
    }

protected:
    mutable LocalSensor local_sensor_;
    int count_;
};

///**
// * Traits of JointSensor<MultipleOf<Sensor, Count>>
// */
//template <
//    typename Sensor,
//    int Count
//>
//struct Traits<
//           JointSensor<MultipleOf<Sensor, Count>>
//        >
//    : public Traits<
//                JointSensor<
//                    MultipleOf<
//                        typename ForwardAdaptive<Sensor>::Type, Count
//                    >,
//                    Adaptive<>>>
//{ };

///**
// * \internal
// * \ingroup sensors
// *
// * Forwards an adaptive LocalSensor type to the JointSensor
// * implementation. \sa ForwardAdaptive for more details.
// */
//template <
//    typename Sensor,
//    int Count
//>
//class JointSensor<MultipleOf<Sensor, Count>>
//    : public JointSensor<
//                MultipleOf<typename ForwardAdaptive<Sensor>::Type, Count>,
//                Adaptive<>>
//{
//public:
//    typedef JointSensor<
//                MultipleOf<typename ForwardAdaptive<Sensor>::Type, Count>,
//                Adaptive<>
//            > Base;

//    typedef typename ForwardAdaptive<Sensor>::Type ForwardedType;

//    JointSensor(
//            const Sensor& local_sensor,
//            int count = ToDimension<Count>::Value)
//        : Base(ForwardedType(local_sensor), count)
//    { }

//    JointSensor(const MultipleOf<Sensor, Count>& mof)
//        : Base(mof)
//    { }
//};

}


#include <cstdio>
#include <memory>
#include <iostream>
#include <vector>
using namespace std;
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
using namespace boost::accumulators;

// 2 + (3+4)
struct Expression
{
  virtual double eval() = 0;
  virtual void collect(vector<double>& v) = 0;
};

struct Literal : Expression
{
  double value;

  explicit Literal(const double value)
    : value{value}
  {
  }

  double eval() override
  {
    return value;
  }

  void collect(vector<double>& v) override
  {
    v.push_back(value);
  }
};

struct AdditionExpression : Expression
{
  shared_ptr<Expression> left, right;

  AdditionExpression(const shared_ptr<Expression>& expression, const shared_ptr<Expression>& expression1)
    : left{expression},
      right{expression1}
  {
  }

  double eval() override
  {
    return left->eval() + right->eval();
  }

  void collect(vector<double>& v) override
  {
    left->collect(v);
    right->collect(v);
  }
};

int main__3(int ac, char* av)
{
  AdditionExpression sum{
    make_shared<Literal>(2),
    make_shared<AdditionExpression>(
      make_shared<Literal>(3),
      make_shared<Literal>(4)
      )
  };
  cout << "2+(3+4) = " << sum.eval() << endl;

  vector<double> v;
  sum.collect(v);
  for (auto x : v)
    cout << x << "\t";
  cout << endl;

  vector<double> values{ 1,2,3,4 };
  double s = 0;
  for (auto x : values) s += x;
  cout << "average is " << (s / values.size()) << endl;

  accumulator_set<double, stats<tag::mean>> acc;
  for (auto x : values) acc(x);
  cout << "average is " << mean(acc) << endl;

  getchar();
  return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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 GENFILE_VCF_GET_SET_EIGEN_HPP
#define GENFILE_VCF_GET_SET_EIGEN_HPP

#include <Eigen/Core>
#include "genfile/VariantDataReader.hpp"
#include "genfile/vcf/get_set.hpp"
#include "genfile/Error.hpp"

namespace genfile {
	namespace vcf {
		// Genotype setter which stores its genotypes in an Eigen matrix with
		// 3 rows and n columns (where n is the number of samples.)
		template< typename Derived >
		struct GenotypeSetter< Eigen::MatrixBase< Derived > >: public GenotypeSetterBase
		{
			GenotypeSetter( Eigen::MatrixBase< Derived >& result, std::string const& scale = "identity" ): GenotypeSetterBase( scale ), m_result( result ) {
				result.setZero() ;
			} ;
			void initialise( std::size_t nSamples, std::size_t nAlleles ) {
				assert( nAlleles ==  2 ) ;
				m_result.derived().resize( nSamples, 3 ) ;
				GenotypeSetterBase::initialise( nSamples, nAlleles ) ;
			}
			void set( std::size_t sample_i, double AA, double AB, double BB ) {
				m_result( sample_i, 0 ) = AA ;
				m_result( sample_i, 1 ) = AB ;
				m_result( sample_i, 2 ) = BB ;
			}
		private:
			Eigen::MatrixBase< Derived >& m_result ;
		} ;

		namespace impl {
			template< typename Data >
			struct ThreshholdedCallGetter1: public GenotypeSetterBase {
				ThreshholdedCallGetter1( ThreshholdedCallGetter1 const& other ):
					m_result( other.m_result ),
					m_threshhold( other.m_threshhold ),
					m_missing_value( other.m_missing_value ),
					m_AA_value( other.m_AA_value ),
					m_AB_value( other.m_AB_value ),
					m_BB_value( other.m_BB_value )
				{}

				ThreshholdedCallGetter1(
					Data& result,
					double threshhold,
					double missing_value = -1,
					double AA_value = 0,
					double AB_value = 1,
					double BB_value = 2
				):
					m_result( result ),
					m_threshhold( threshhold ),
					m_missing_value( missing_value ),
					m_AA_value( AA_value ),
					m_AB_value( AB_value ),
					m_BB_value( BB_value )
				{}

				void initialise( std::size_t nSamples, std::size_t nAlleles ) {
					//m_result.setConstant( n, m_missing_value ) ;
					assert( nAlleles == 2 ) ;
					m_result.resize( nSamples ) ;
					GenotypeSetterBase::initialise( nSamples, nAlleles ) ;
				}

				void set( std::size_t sample_i, double AA, double AB, double BB ) {
					if( AA > m_threshhold ) {
						m_result[ sample_i ] = m_AA_value ;
					}
					else if( AB > m_threshhold ) {
						m_result[ sample_i ] = m_AB_value ;
					}
					else if( BB > m_threshhold ) {
						m_result[ sample_i ] = m_BB_value ;
					}
					else {
						m_result[ sample_i ] = m_missing_value ;
					}
				}
			private:
				Data& m_result ;
				double const m_threshhold ;
				int const m_missing_value ;
				int const m_AA_value ;
				int const m_AB_value ;
				int const m_BB_value ;
			} ;

			template< typename Data, typename Nonmissingness >
			struct ThreshholdedCallGetter2: public GenotypeSetterBase {
				ThreshholdedCallGetter2( ThreshholdedCallGetter2 const& other ):
					m_result( other.m_result ),
					m_nonmissingness( other.m_nonmissingness ),
					m_threshhold( other.m_threshhold ),
					m_missing_value( other.m_missing_value ),
					m_AA_value( other.m_AA_value ),
					m_AB_value( other.m_AB_value ),
					m_BB_value( other.m_BB_value )
				{}

				ThreshholdedCallGetter2(
					Data& result,
					Nonmissingness& nonmissingness,
					double threshhold,
					double missing_value = -1,
					double AA_value = 0,
					double AB_value = 1,
					double BB_value = 2
				):
					m_result( result ),
					m_nonmissingness( nonmissingness ),
					m_threshhold( threshhold ),
					m_missing_value( missing_value ),
					m_AA_value( AA_value ),
					m_AB_value( AB_value ),
					m_BB_value( BB_value )
				{}

				void initialise( std::size_t nSamples, std::size_t nAlleles ) {
					assert( nAlleles == 2 ) ;
					m_result.setConstant( nSamples, m_missing_value ) ;
					m_nonmissingness.setConstant( nSamples, 0 ) ;
					GenotypeSetterBase::initialise( nSamples, nAlleles ) ;
				}

				void set( std::size_t sample_i, double AA, double AB, double BB ) {
					if( AA > m_threshhold ) {
						m_result( sample_i ) = m_AA_value ;
						m_nonmissingness( sample_i ) = 1 ;
					}
					else if( AB > m_threshhold ) {
						m_result( sample_i ) = m_AB_value ;
						m_nonmissingness( sample_i ) = 1 ;
					}
					else if( BB > m_threshhold ) {
						m_result( sample_i ) = m_BB_value ;
						m_nonmissingness( sample_i ) = 1 ;
					}
					else {
						m_result( sample_i ) = m_missing_value ;
						m_nonmissingness( sample_i ) = 0 ;
					}
				}
			private:
				Data& m_result ;
				Nonmissingness& m_nonmissingness;
				double const m_threshhold ;
				int const m_missing_value ;
				int const m_AA_value ;
				int const m_AB_value ;
				int const m_BB_value ;
			} ;
		}

		template< typename Data >
		impl::ThreshholdedCallGetter1< Data > get_threshholded_calls(
			Data& result,
			double threshhold,
			double missing_value = -1,
			double AA_value = 0,
			double AB_value = 1,
			double BB_value = 2
		) {
			return impl::ThreshholdedCallGetter1< Data >( result, threshhold, missing_value, AA_value, AB_value, BB_value ) ;
		}

		template< typename Data, typename Nonmissingness >
		impl::ThreshholdedCallGetter2< Data, Nonmissingness > get_threshholded_calls(
			Data& result,
			Nonmissingness& nonmissingness,
			double threshhold,
			double missing_value = -1,
			double AA_value = 0,
			double AB_value = 1,
			double BB_value = 2
		) {
			return impl::ThreshholdedCallGetter2< Data, Nonmissingness >( result, nonmissingness, threshhold, missing_value, AA_value, AB_value, BB_value ) ;
		}

		// Genotype setter which stores hard genotype calls as values in an Eigen::Vector with specified values for each genotype.
		template< typename HaplotypeAlleles >
		struct PhasedGenotypeSetter: public VariantDataReader::PerSampleSetter
		{
			typedef HaplotypeAlleles NonMissingness ;
			~PhasedGenotypeSetter() throw() {}
			PhasedGenotypeSetter(
				HaplotypeAlleles& result,
				double missing_value = std::numeric_limits< double >::quiet_NaN()
			):
				m_result( result ),
				m_non_missingness( 0 ),
				m_missing_value( missing_value ),
				m_allele_coding( 2, 0 )
			{
				m_result.setConstant( m_missing_value ) ;
				m_allele_coding[0] = 0 ;
				m_allele_coding[1] = 1 ;
			}

			PhasedGenotypeSetter(
				HaplotypeAlleles& result,
				NonMissingness& non_missingness,
				double missing_value = 0,
				double const A_coding = 0,
				double const B_coding = 1
			):
				m_result( result ),
				m_non_missingness( &non_missingness ),
				m_missing_value( missing_value ),
				m_allele_coding( 2, 0 )
			{
				m_result.setConstant( m_missing_value ) ;
				m_allele_coding[0] = A_coding ;
				m_allele_coding[1] = B_coding ;
			}

			void initialise( std::size_t nSamples, std::size_t nAlleles ) {
				assert( nAlleles == 2 ) ;
				m_result.resize( nSamples, 2  ) ;
				if( m_non_missingness ) {
					m_non_missingness->resize( nSamples, 2 ) ;
				}
				m_sample_i = 0 ;
				m_entry_i = 0 ;
			}

			void set_number_of_alleles( std::size_t n ) {
				assert( n == 2 ) ;
			}

			bool set_sample( std::size_t i ) {
				m_sample_i = i ;
				m_entry_i = 0 ;
				return true ;
			}

			void set_number_of_entries( uint32_t, std::size_t n, OrderType const order_type, ValueType const value_type ) {
				if( n != 2 ) {
					throw BadArgumentError( "genfile::vcf::PhasedGenotypeSetter::set_number_of_entries()", "Only a value of 2 is supported." ) ;
				}
				assert( order_type == ePerOrderedHaplotype ) ;
				assert( value_type == eAlleleIndex ) ;
			}

			void set_order_type( OrderType const type ) {
				if( type != eOrderedList ) {
					throw BadArgumentError( "genfile::vcf::PhasedGenotypeSetter::set_order_type()", "Only phased genotypes (an ordered list) is supported." ) ;
				}
			}

			void set_value( std::size_t, MissingValue const value ) {
				m_result( m_sample_i, m_entry_i ) = m_missing_value ;
				if( m_non_missingness ) {
					(*m_non_missingness)( m_sample_i, m_entry_i ) = 0 ;
				}
				++m_entry_i ;
			}

			void set_value( std::size_t, Integer const value ) {
				assert( value >= 0 && value < 2 ) ;
				m_result( m_sample_i, m_entry_i ) = m_allele_coding[ std::size_t( value ) ] ;
				if( m_non_missingness ) {
					(*m_non_missingness)( m_sample_i, m_entry_i ) = 1 ;
				}
				++m_entry_i ;
			}

			void finalise() {} ;

		private:
			HaplotypeAlleles& m_result ;
			NonMissingness* m_non_missingness ;
			double const m_missing_value ;
			std::vector< double > m_allele_coding ;
			int m_sample_i ;
			int m_entry_i ;
		} ;
	}
}

#endif