/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2018 - 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 at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, Colorado State University
 *         Yong-Yong Cai, Beijing Computational Science Research Center
 */

// @sect3{Include files}  程序以通常的包含文件开始，所有这些文件你现在应该都见过了。

#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>

#include <fstream>
#include <iostream>

// 然后按照惯例将这个程序的所有内容放入一个命名空间，并将deal.II命名空间导入到我们将要工作的命名空间中。

namespace Step58
{
  using namespace dealii;
// @sect3{The NonlinearSchroedingerEquation class}

// 然后是主类。它看起来非常像  step-4  或  step-6  中的相应类，唯一的例外是，矩阵和向量以及其他所有与线性系统相关的元素现在都存储为  `std::complex<double>`  类型，而不仅仅是 `double`。

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

  private:
    void setup_system();
    void assemble_matrices();
    void do_half_phase_step();
    void do_full_spatial_step();
    void output_results() const;

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

    AffineConstraints<std::complex<double>> constraints;

    SparsityPattern                    sparsity_pattern;
    SparseMatrix<std::complex<double>> system_matrix;
    SparseMatrix<std::complex<double>> rhs_matrix;

    Vector<std::complex<double>> solution;
    Vector<std::complex<double>> system_rhs;

    double       time;
    double       time_step;
    unsigned int timestep_number;

    double kappa;
  };

//  @sect3{Equation data}

// 在我们继续填写主类的细节之前，让我们定义与问题相对应的方程数据，即初始值，以及一个右手类。我们将把初始条件也用于边界值，我们只是保持边界值不变）。我们使用派生自Function类模板的类来做这件事，这个模板之前已经用过很多次了，所以下面的内容看起来并不令人惊讶。唯一值得注意的是，我们这里有一个复值问题，所以我们必须提供Function类的第二个模板参数（否则会默认为`double`）。此外，`value()`函数的返回类型当然也是复数。

// 这些函数精确地返回什么，在介绍部分的最后已经讨论过了。

  template <int dim>
  class InitialValues : public Function<dim, std::complex<double>>
  {
  public:
    InitialValues()
      : Function<dim, std::complex<double>>(1)
    {}

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

  template <int dim>
  std::complex<double>
  InitialValues<dim>::value(const Point<dim> & p,
                            const unsigned int component) const
  {
    static_assert(dim == 2, "This initial condition only works in 2d.");

    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    const std::vector<Point<dim>> vortex_centers = {{0, -0.3},
                                                    {0, +0.3},
                                                    {+0.3, 0},
                                                    {-0.3, 0}};

    const double R = 0.1;
    const double alpha =
      1. / (std::pow(R, dim) * std::pow(numbers::PI, dim / 2.));

    double sum = 0;
    for (const auto &vortex_center : vortex_centers)
      {
        const Tensor<1, dim> distance = p - vortex_center;
        const double         r        = distance.norm();

        sum += alpha * std::exp(-(r * r) / (R * R));
      }

    return {std::sqrt(sum), 0.};
  }

  template <int dim>
  class Potential : public Function<dim>
  {
  public:
    Potential() = default;
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <int dim>
  double Potential<dim>::value(const Point<dim> & p,
                               const unsigned int component) const
  {
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return (Point<dim>().distance(p) > 0.7 ? 1000 : 0);
  }

//  @sect3{Implementation of the NonlinearSchroedingerEquation class}

// 我们首先指定了类的构造函数的实现。

  template <int dim>
  NonlinearSchroedingerEquation<dim>::NonlinearSchroedingerEquation()
    : fe(2)
    , dof_handler(triangulation)
    , time(0)
    , time_step(1. / 128)
    , timestep_number(0)
    , kappa(1)
  {}
// @sect4{Setting up data structures and assembling matrices}

// 下一个函数是在程序开始时，也就是在第一个时间步骤之前，设置网格、DoFHandler以及矩阵和向量。如果你已经阅读了至少到 step-6 为止的教程程序，那么前几行是相当标准的。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::setup_system()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(6);

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

    dof_handler.distribute_dofs(fe);

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

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

    system_matrix.reinit(sparsity_pattern);
    rhs_matrix.reinit(sparsity_pattern);

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

    constraints.close();
  }

// 接下来，我们组装相关的矩阵。按照我们对斯特朗分裂的空间步骤（即每个时间步骤中三个部分步骤中的第二个步骤）的Crank-Nicolson离散化的写法，我们被引导到线性系统
// $\left[ -iM  +  \frac 14 k_{n+1} A + \frac 12 k_{n+1} W \right]
//    \Psi^{(n,2)}
//   =
//   \left[ -iM  -  \frac 14 k_{n+1} A - \frac 12 k_{n+1} W \right]
//    \Psi^{(n,1)}$

//     换句话说，这里有两个矩阵在起作用--一个用于左手边，一个用于右手边。我们分别建立这些矩阵。我们可以避免建立右手边的矩阵，而只是在每个时间步长中形成矩阵的*作用* $\Psi^{(n,1)}$ 。这可能更有效，也可能不有效，但是对于这个程序来说，效率并不是最重要的）。)

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::assemble_matrices()
  {
    const QGauss<dim> quadrature_formula(fe.degree + 1);

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

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

    FullMatrix<std::complex<double>> cell_matrix_lhs(dofs_per_cell,
                                                     dofs_per_cell);
    FullMatrix<std::complex<double>> cell_matrix_rhs(dofs_per_cell,
                                                     dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  potential_values(n_q_points);
    const Potential<dim>                 potential;

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix_lhs = std::complex<double>(0.);
        cell_matrix_rhs = std::complex<double>(0.);

        fe_values.reinit(cell);

        potential.value_list(fe_values.get_quadrature_points(),
                             potential_values);

        for (unsigned int q_index = 0; q_index < n_q_points; ++q_index)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              {
                for (unsigned int l = 0; l < dofs_per_cell; ++l)
                  {
                    const std::complex<double> i = {0, 1};

                    cell_matrix_lhs(k, l) +=
                      (-i * fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index) +
                       time_step / 4 * fe_values.shape_grad(k, q_index) *
                         fe_values.shape_grad(l, q_index) +
                       time_step / 2 * potential_values[q_index] *
                         fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index)) *
                      fe_values.JxW(q_index);

                    cell_matrix_rhs(k, l) +=
                      (-i * fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index) -
                       time_step / 4 * fe_values.shape_grad(k, q_index) *
                         fe_values.shape_grad(l, q_index) -
                       time_step / 2 * potential_values[q_index] *
                         fe_values.shape_value(k, q_index) *
                         fe_values.shape_value(l, q_index)) *
                      fe_values.JxW(q_index);
                  }
              }
          }

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(cell_matrix_lhs,
                                               local_dof_indices,
                                               system_matrix);
        constraints.distribute_local_to_global(cell_matrix_rhs,
                                               local_dof_indices,
                                               rhs_matrix);
      }
  }
// @sect4{Implementing the Strang splitting steps}

// 在建立了上述所有数据结构后，我们现在可以实现构成斯特朗分裂方案的部分步骤。我们从推进阶段的半步开始，这被用作每个时间步骤的第一和最后部分。

// 为此，回顾一下，对于第一个半步，我们需要计算  $\psi^{(n,1)} = e^{-i\kappa|\psi^{(n,0)}|^2 \tfrac 12\Delta t} \; \psi^{(n,0)}$  。这里， $\psi^{(n,0)}=\psi^{(n)}$ 和 $\psi^{(n,1)}$ 是空间的函数，分别对应于前一个完整时间步骤的输出和三个部分步骤中第一个步骤的结果。必须为第三个部分步骤计算相应的解决方案，即  $\psi^{(n,3)} = e^{-i\kappa|\psi^{(n,2)}|^2 \tfrac 12\Delta t} \; \psi^{(n,2)}$  ，其中  $\psi^{(n,3)}=\psi^{(n+1)}$  是整个时间步骤的结果，其输入  $\psi^{(n,2)}$  是斯特朗分割的空间步骤的结果。

// 一个重要的认识是，虽然 $\psi^{(n,0)}(\mathbf x)$ 可能是一个有限元函数（即，是片状多项式），但对于我们使用指数因子更新相位的 "旋转 "函数来说，不一定是这样的（回顾一下，该函数的振幅在该步骤中保持不变）。换句话说，我们可以在每一个点 $\psi^{(n,1)}(\mathbf x)$ *计算 $\mathbf x\in\Omega$ ，但我们不能在网格上表示它，因为它不是一个片状多项式函数。在一个离散的环境中，我们能做的最好的事情就是计算一个投影或内插。换句话说，我们可以计算 $\psi_h^{(n,1)}(\mathbf x) = \Pi_h \left(e^{-i\kappa|\psi_h^{(n,0)}(\mathbf x)|^2 \tfrac 12\Delta t} \; \psi_h^{(n,0)}(\mathbf x) \right)$ ，其中 $\Pi_h$ 是一个投影或内插算子。如果我们选择插值，情况就特别简单。那么，我们需要计算的就是*在节点点上的右手边的值，并将这些作为自由度向量 $\Psi^{(n,1)}$ 的节点值。这很容易做到，因为在这里使用的拉格朗日有限元的节点点上评估右手边，需要我们只看节点向量的一个（复值）条目。换句话说，我们需要做的是计算 $\Psi^{(n,1)}_j = e^{-i\kappa|\Psi^{(n,0)}_j|^2 \tfrac 12\Delta t} \; \Psi^{(n,0)}_j$ ，其中 $j$ 在我们的解向量的所有条目上循环。这就是下面的函数所做的--事实上，它甚至没有为 $\Psi^{(n,0)}$ 和 $\Psi^{(n,1)}$ 使用单独的向量，而只是适当地更新同一个向量。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::do_half_phase_step()
  {
    for (auto &value : solution)
      {
        const std::complex<double> i         = {0, 1};
        const double               magnitude = std::abs(value);

        value = std::exp(-i * kappa * magnitude * magnitude * (time_step / 2)) *
                value;
      }
  }

// 下一步是求解每个时间步骤中的线性系统，即我们使用的Strang分割的后半步。记得它的形式是 $C\Psi^{(n,2)} = R\Psi^{(n,1)}$ ，其中 $C$ 和 $R$ 是我们之前组装的矩阵。

// 我们在这里解决这个问题的方法是使用直接求解器。我们首先使用 $r=R\Psi^{(n,1)}$ 函数形成右边的 SparseMatrix::vmult() ，并将结果放入`system_rhs`变量。然后我们调用 SparseDirectUMFPACK::solver() ，该函数以矩阵 $C$ 和右手边的向量为参数，并在同一向量`system_rhs`中返回解。最后一步是将计算出的解放回`solution`变量中。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::do_full_spatial_step()
  {
    rhs_matrix.vmult(system_rhs, solution);

    SparseDirectUMFPACK direct_solver;
    direct_solver.solve(system_matrix, system_rhs);

    solution = system_rhs;
  }

//  @sect4{Creating graphical output}

// 我们应该讨论的最后一个辅助函数和类是那些创建图形输出的函数。对斯特朗分裂的局部和空间部分运行半步和全步的结果是，我们在每个时间步数结束时将`solution`向量 $\Psi^n$ 更新为正确的值。它的条目包含有限元网格节点上的解的复数。

// 复数不容易被视觉化。我们可以输出它们的实部和虚部，即字段 $\text{Re}(\psi_h^{(n)}(\mathbf x))$ 和 $\text{Im}(\psi_h^{(n)}(\mathbf x))$ ，这正是DataOut类在通过 DataOut::add_data_vector() 附加复数向量，然后调用 DataOut::build_patches(). 时所做的事情，这确实是我们下面要做的。

// 但很多时候，我们对解向量的实部和虚部并不特别感兴趣，而是对解的幅度 $|\psi|$ 和相位角 $\text{arg}(\psi)$ 等衍生量感兴趣。在这里这样的量子系统的背景下，幅度本身并不那么有趣，相反，"振幅"， $|\psi|^2$ 才是一个物理属性：它对应于在一个特定的状态场所找到一个粒子的概率密度。将计算出的量放入输出文件以实现可视化的方法--正如在以前的许多教程程序中使用的那样--是使用数据后处理程序和派生类的设施。具体来说，一个复数的振幅和它的相位角都是标量，因此DataPostprocessorScalar类是我们要做的正确工具。

// 因此，我们在这里要做的是实现两个类`ComplexAmplitude`和`ComplexPhase`，为DataOut决定生成输出的每个点计算解决方案的振幅 $|\psi_h|^2$ 和相位 $\text{arg}(\psi_h)$ ，以便进行可视化。下面有大量的模板代码，这两个类中的第一个唯一有趣的部分是它的`evaluate_vector_field()`函数如何计算`computed_quantities`对象。

//（还有一个相当尴尬的事实是，std::norm()函数并没有计算人们天真的想象，即 $|\psi|$  ，而是返回 $|\psi|^2$ 。一个标准函数以这样的方式被错误地命名，这当然是相当令人困惑的......)

  namespace DataPostprocessors
  {
    template <int dim>
    class ComplexAmplitude : public DataPostprocessorScalar<dim>
    {
    public:
      ComplexAmplitude();

      virtual void evaluate_vector_field(
        const DataPostprocessorInputs::Vector<dim> &inputs,
        std::vector<Vector<double>> &computed_quantities) const override;
    };

    template <int dim>
    ComplexAmplitude<dim>::ComplexAmplitude()
      : DataPostprocessorScalar<dim>("Amplitude", update_values)
    {}

    template <int dim>
    void ComplexAmplitude<dim>::evaluate_vector_field(
      const DataPostprocessorInputs::Vector<dim> &inputs,
      std::vector<Vector<double>> &               computed_quantities) const
    {
      Assert(computed_quantities.size() == inputs.solution_values.size(),
             ExcDimensionMismatch(computed_quantities.size(),
                                  inputs.solution_values.size()));

      for (unsigned int q = 0; q < computed_quantities.size(); ++q)
        {
          Assert(computed_quantities[q].size() == 1,
                 ExcDimensionMismatch(computed_quantities[q].size(), 1));
          Assert(inputs.solution_values[q].size() == 2,
                 ExcDimensionMismatch(inputs.solution_values[q].size(), 2));

          const std::complex<double> psi(inputs.solution_values[q](0),
                                         inputs.solution_values[q](1));
          computed_quantities[q](0) = std::norm(psi);
        }
    }

// 这些后处理程序类中的第二个是计算每一个点的复值解决方案的相位角。换句话说，如果我们表示  $\psi(\mathbf x,t)=r(\mathbf x,t) e^{i\varphi(\mathbf x,t)}$  ，那么这个类就会计算  $\varphi(\mathbf x,t)$  。函数 std::arg 为我们做这个，并将角度作为实数返回  $-\pi$  和  $+\pi$  之间。

// 由于我们将在结果部分详细解释的原因，我们实际上没有在产生输出的每个位置输出这个值。相反，我们取相位所有评估点的最大值，然后用这个最大值填充每个评估点的输出字段--实质上，我们将相位角作为一个片状常数字段输出，其中每个单元都有自己的常数值。一旦你读完下面的讨论就会明白其中的原因。

    template <int dim>
    class ComplexPhase : public DataPostprocessorScalar<dim>
    {
    public:
      ComplexPhase();

      virtual void evaluate_vector_field(
        const DataPostprocessorInputs::Vector<dim> &inputs,
        std::vector<Vector<double>> &computed_quantities) const override;
    };

    template <int dim>
    ComplexPhase<dim>::ComplexPhase()
      : DataPostprocessorScalar<dim>("Phase", update_values)
    {}

    template <int dim>
    void ComplexPhase<dim>::evaluate_vector_field(
      const DataPostprocessorInputs::Vector<dim> &inputs,
      std::vector<Vector<double>> &               computed_quantities) const
    {
      Assert(computed_quantities.size() == inputs.solution_values.size(),
             ExcDimensionMismatch(computed_quantities.size(),
                                  inputs.solution_values.size()));

      double max_phase = -numbers::PI;
      for (unsigned int q = 0; q < computed_quantities.size(); ++q)
        {
          Assert(computed_quantities[q].size() == 1,
                 ExcDimensionMismatch(computed_quantities[q].size(), 1));
          Assert(inputs.solution_values[q].size() == 2,
                 ExcDimensionMismatch(inputs.solution_values[q].size(), 2));

          max_phase =
            std::max(max_phase,
                     std::arg(
                       std::complex<double>(inputs.solution_values[q](0),
                                            inputs.solution_values[q](1))));
        }

      for (auto &output : computed_quantities)
        output(0) = max_phase;
    }

  } // namespace DataPostprocessors

// 在这样实现了这些后处理程序后，我们像往常一样创建输出。与其他许多时间相关的教程程序一样，我们给DataOut附加标志，表示时间步数和当前模拟时间。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::output_results() const
  {
    const DataPostprocessors::ComplexAmplitude<dim> complex_magnitude;
    const DataPostprocessors::ComplexPhase<dim>     complex_phase;

    DataOut<dim> data_out;

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

    data_out.set_flags(DataOutBase::VtkFlags(time, timestep_number));

    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{Running the simulation}

// 剩下的步骤是我们如何设置这个程序的整体逻辑。这其实是比较简单的。设置数据结构；将初始条件插值到有限元空间；然后迭代所有时间步长，在每个时间步长上执行斯特朗分割法的三个部分。每隔10个时间步长，我们就生成图形输出。这就是了。

  template <int dim>
  void NonlinearSchroedingerEquation<dim>::run()
  {
    setup_system();
    assemble_matrices();

    time = 0;
    VectorTools::interpolate(dof_handler, InitialValues<dim>(), solution);
    output_results();

    const double end_time = 1;
    for (; time <= end_time; time += time_step)
      {
        ++timestep_number;

        std::cout << "Time step " << timestep_number << " at t=" << time
                  << std::endl;

        do_half_phase_step();
        do_full_spatial_step();
        do_half_phase_step();

        if (timestep_number % 1 == 0)
          output_results();
      }
  }
} // namespace Step58

//  @sect4{The main() function}

// 其余的又是锅炉板，和以前几乎所有的教程程序完全一样。

int main()
{
  try
    {
      using namespace Step58;

      NonlinearSchroedingerEquation<2> nse;
      nse.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;
}


/*
   Based on https://github.com/jackjack-jj/jeeq, GPLv3.
   Specifically designed to use Smileycoin key pairs for encoding/decoding so
   this is not as general as jeeq.py
   All the math here is explained pretty well on this wikipedia page:
   https://en.wikipedia.org/wiki/ElGamal_encryption
 */
#include <string.h>
#include <stdbool.h>
#include <boost/endian/conversion.hpp>

#include <vector>
#include <stdexcept>

#include <openssl/ec.h>
#include <openssl/bn.h>
#include <openssl/obj_mac.h>
#include <openssl/crypto.h>
#include <openssl/rand.h>
#include <openssl/sha.h>
#include <openssl/err.h>

#include "key.h"
#include "util.h"
#include "jeeq.h"

#define PRIVHEADER_LEN          9
#define PUBHEADER_LEN           7
#define PRIVKEY_LEN             32
#define COMPR_PUBKEY_LEN        33
#define UNCOMPR_PUBKEY_LEN      65
#define CHUNK_SIZE              32
#define VERSION                 0x00
#define GX_HEX                  "79BE667EF9DCBBAC55A06295CE870B07029BFCDB2DCE28D959F2815B16F81798"
#define GY_HEX                  "483ADA7726A3C4655DA4FBFC0E1108A8FD17B448A68554199C47D08FFB10D4B8"
#define GORDER_HEX              "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEBAAEDCE6AF48A03BBFD25E8CD0364141"
#define GCOFACTOR_HEX           "01"

using namespace boost::endian;

/* return:  secp256k1 curve with the bitcoin generator, order and cofactor
 * in:      ctx, bignum context
 */
static EC_GROUP *init_curve(BN_CTX *ctx)
{
    EC_GROUP *group = EC_GROUP_new_by_curve_name(NID_secp256k1);

    EC_POINT *generator = EC_POINT_new(group);

    BIGNUM *Gx = BN_new();
    BIGNUM *Gy = BN_new();
    BIGNUM *order = BN_new();
    BIGNUM *cofactor = BN_new();

    BN_hex2bn(&Gx, GX_HEX);
    BN_hex2bn(&Gy, GY_HEX);

    BN_hex2bn(&order, GORDER_HEX);
    BN_hex2bn(&cofactor, GCOFACTOR_HEX);

    EC_POINT_set_affine_coordinates_GFp(group, generator, Gx, Gy, ctx);
    EC_GROUP_set_generator(group, generator, order, cofactor);

    BN_free(Gx);
    BN_free(Gy);
    BN_free(cofactor);
    BN_free(order);

    EC_POINT_free(generator);

    return group;
}

/* return: 1
 * out: m, pointer to message that will be prefaced with the private header
 * in:  nmsg, pointer to the message
 *      msg_length, length of the message
 */
static int write_private_header(uint8_t *m, const uint8_t *nmsg, const uint32_t msg_length)
{
    uint8_t hash[SHA256_DIGEST_LENGTH];
    SHA256(nmsg, msg_length, hash);

    m[0] = VERSION;
    m[1] = 0x00;
    m[2] = 0x06;
    *(uint32_t*)&m[3] = native_to_big(msg_length);
    m[7] = hash[0];
    m[8] = hash[1];

    return 1;
}

/* return: 1
 * out:     enc, the encrypted string to preface the public header with
 * in:      pub, pointer to raw pubkey,
 *          is_compressed, whether the pubkey is compressed NEEDED
 */
static int write_public_header(uint8_t *enc, const uint8_t *pub)
{
    uint8_t hash[SHA256_DIGEST_LENGTH];
    size_t pubkey_length = (pub[0] == 0x04) ? UNCOMPR_PUBKEY_LEN : COMPR_PUBKEY_LEN;

    SHA256(pub, pubkey_length, hash);

    enc[0] = 0x6a;
    enc[1] = 0x6a;
    enc[2] = VERSION;
    enc[3] = 0x00;
    enc[4] = 0x02;
    enc[5] = hash[0];
    enc[6] = hash[1];

    return 1;
}

/* return:  1 on success, 0 otherwise
 * out:     message_length, read message length
 * in:      dec, decrypted string
 */
static int read_private_header(size_t *message_length, const uint8_t *dec)
{
    if (dec[0] != VERSION) return 0;
    if (dec[1] != 0x00) return 0;
    if (dec[2] != 0x06) return 0;

    uint32_t msg_len = big_to_native(*(uint32_t*)&dec[3]);

    /* com_mh stands for computed message hash */
    uint8_t com_mh[SHA256_DIGEST_LENGTH];
    SHA256(&dec[PRIVHEADER_LEN], msg_len, com_mh);

    /* check that our computed hash matches the one in the header */
    if (dec[7] != com_mh[0] || dec[8] != com_mh[1]) return 0;
    *message_length = msg_len;

    return 1;
}

/* return: 1 on success, 0 otherwise
 * in:  group, bitcoin curve
 *      enc, encryted string
 *      bn_privkey, our privkey as a bignum
 *      is_compressed??,  RETHINK
 *      ctx, bignum context
 */
static int read_public_header(const EC_GROUP *group, const uint8_t *enc,
        const uint8_t* our_pubkey, BN_CTX *ctx)
{
    if (enc[0] != 0x6a)     return 0;
    if (enc[1] != 0x6a)     return 0;
    if (enc[2] != VERSION)  return 0;
    if (enc[3] != 0x00)     return 0;
    if (enc[4] != 0x02)     return 0;

    /* computed public key hash = com_pkh */
    uint8_t com_pkh[SHA256_DIGEST_LENGTH];
    SHA256(our_pubkey, (our_pubkey[0]==0x04 ? UNCOMPR_PUBKEY_LEN : COMPR_PUBKEY_LEN), com_pkh);

    if (enc[5] != com_pkh[0] || enc[6] != com_pkh[1])
        return 0;

    return 1;
}

/* return: 1 on success, 0 otherwise
 * out: y, the y value corresponding to x+offset, if it is found
 *      offset, the offset needed to the x value to give a y
 * in:  group, bitcoin curve
 *      x,   our x
 *      odd, whether we want the y value to be odd or even,
 *      ctx, bignum context
 */
static int y_from_x(const EC_GROUP *group, BIGNUM *y, size_t *offset, const BIGNUM *x, const bool odd, BN_CTX *ctx)
{
    EC_POINT *M = EC_POINT_new(group);

    /* try to find y the easy way */
    if (EC_POINT_set_compressed_coordinates_GFp(group, M, x, odd, ctx) == 1)
    {
        EC_POINT_get_affine_coordinates_GFp(group, M, NULL, y, ctx);
        *offset = 0;
        EC_POINT_free(M);
        return 1;
    }

    int ret = 0;
    BN_CTX_start(ctx);

    BIGNUM *p = BN_CTX_get(ctx);
    BIGNUM *a = BN_CTX_get(ctx);
    BIGNUM *b = BN_CTX_get(ctx);

    EC_GROUP_get_curve_GFp(group, p, a, b, ctx);

    BIGNUM *Mx = BN_CTX_get(ctx);
    BN_copy(Mx, x);
    BIGNUM *My = BN_CTX_get(ctx);
    BIGNUM *My2 = BN_CTX_get(ctx);
    BIGNUM *aMx2 = BN_CTX_get(ctx);

    BIGNUM *half = BN_CTX_get(ctx);
    BN_copy(half, p);
    BN_add_word(half, 1);
    BN_div_word(half, 4);

    /* xoffset can be in the range 1-127 since we have 7 bits free,
       we only need 1 bit to discern odd from even points
    */
    for (int i = 1; i < 128; i++)
    {
        BN_add_word(Mx, 1);

        /* My2 = (Mx^2 * Mx mod p) */
        BN_sqr(My2, Mx, ctx);
        BN_mod_mul(My2, My2, Mx, p, ctx);

        BN_mod_sqr(aMx2, Mx, p, ctx);
        BN_mul(aMx2, aMx2, a, ctx);

        BN_mod(b, b, p, ctx);

        BN_add(My2, My2, aMx2);
        BN_add(My2, My2, b);

        BN_mod_exp(My, My2, half, p, ctx);

        /* this function will return 1 on success (point on curve), else 0 */
        if (EC_POINT_set_affine_coordinates_GFp(group, M, Mx, My, ctx) == 1)
        {
            if (odd == BN_is_bit_set(My, 0))
            {
                BN_copy(y, My);
                *offset = i;
            }
            else
            {
                BN_sub(y, p, My);
                *offset = i;
            }

            ret = 1;
            break;
        }
    }

    /* some errors here are expected since set_affine_coordinates logs an error
     * when we try to set an invalid x,y combination when trying offsets
     */
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("y_from_x %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    BN_CTX_end(ctx);
    EC_POINT_free(M);

    return ret;
}

/* return:  a malloc pointer to the encrypted string,
 * out:     enc_len, size of the encrypted string in bytes
 * in:      pubkey, pointer to raw pubkey
 *          msg, pointer to the message to be encrypted
 *          msg_len, length of the message to be encrypted
 */
static uint8_t *encrypt_message(size_t *enc_len, const uint8_t *pubkey,
        const uint8_t *msg, const size_t msg_len)
{
    BN_CTX *ctx = BN_CTX_new();
    /* our secp256k1 curve and bitcoin generator
     * these must be thread local as openssl does not support shared
     * use of its data structures
     */
    EC_GROUP *group = init_curve(ctx);

    uint8_t *ret = NULL;

    EC_POINT *pk = EC_POINT_new(group);

    // get so many blocks of 32B blocks that msg will fit
    int chunk_count = (PRIVHEADER_LEN + msg_len)/CHUNK_SIZE + 1;

    uint8_t *m = (uint8_t*)OPENSSL_zalloc(chunk_count * CHUNK_SIZE);

    write_private_header(m, msg, msg_len);
    memcpy(&m[PRIVHEADER_LEN], msg, msg_len);

    BIGNUM *bn_pubkey = BN_new();
    BN_bin2bn(&pubkey[1], 32, bn_pubkey);

    // pubkey is compressed
    if (pubkey[0] == 0x02 || pubkey[0] == 0x03)
    {
        EC_POINT_set_compressed_coordinates_GFp(group, pk, bn_pubkey, pubkey[0]==0x03, ctx);
    }
    else
    {
        BIGNUM *bn_pubkey_extra = BN_new();

        BN_bin2bn(&pubkey[1+32], 32, bn_pubkey_extra);
        EC_POINT_set_affine_coordinates_GFp(group, pk, bn_pubkey, bn_pubkey_extra, ctx);

        BN_free(bn_pubkey_extra);
    }

    BIGNUM *rand = BN_new();
    BIGNUM *rand_range = BN_new();
    BIGNUM *Mx = BN_new();
    BIGNUM *My = BN_new();
    EC_POINT *M = EC_POINT_new(group);
    EC_POINT *T = EC_POINT_new(group);
    EC_POINT *U = EC_POINT_new(group);
    EC_GROUP_get_order(group, rand_range, ctx);
    BN_sub_word(rand_range, 1);

    uint8_t *enc = (uint8_t*)malloc(PUBHEADER_LEN + chunk_count * 2*COMPR_PUBKEY_LEN);
    write_public_header(enc, pubkey);
    int enc_loc = PUBHEADER_LEN;
    int m_loc = 0;
    size_t xoffset = 0;

    for (int i = 0; i < chunk_count; i++)
    {
        /* since rand must be in [1,...,q-1] */
        BN_rand_range(rand, rand_range);
        BN_add_word(rand, 1);

        if (!BN_bin2bn(&m[m_loc], CHUNK_SIZE, Mx))
            goto err;

        if (!y_from_x(group, My, &xoffset, Mx, true, ctx))
            goto err;

        /* adding our xoffset that we get from y_from_x() */
        BN_add_word(Mx, xoffset);

        EC_POINT_set_affine_coordinates_GFp(group, M, Mx, My, ctx);

        /* see wiki */
        EC_POINT_mul(group, T, rand, NULL, NULL, ctx);
        EC_POINT_mul(group, U, NULL, pk, rand, ctx);
        EC_POINT_add(group, U, U, M, ctx);

        EC_POINT_point2oct(group, T, POINT_CONVERSION_COMPRESSED,
                &enc[enc_loc], COMPR_PUBKEY_LEN, ctx);
        EC_POINT_point2oct(group, U, POINT_CONVERSION_COMPRESSED,
                &enc[enc_loc+COMPR_PUBKEY_LEN], COMPR_PUBKEY_LEN, ctx);

        /* encoding our offset within the odd/even byte (02/03), the first bit represets
         * evenness and other 7 represent the offset */
        enc[enc_loc] = enc[enc_loc] - 2 + (xoffset << 1);

        enc_loc += 2*COMPR_PUBKEY_LEN;
        m_loc += CHUNK_SIZE;
    }

    /* success */
    *enc_len = enc_loc;
    ret = enc;

err:
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("encrypt_message %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    OPENSSL_clear_free(m, chunk_count * CHUNK_SIZE);
    BN_free(rand);
    BN_free(rand_range);
    BN_free(Mx);
    BN_free(My);
    BN_free(bn_pubkey);
    EC_POINT_free(M);
    EC_POINT_free(T);
    EC_POINT_free(U);
    EC_POINT_free(pk);

    EC_GROUP_free(group);
    BN_CTX_free(ctx);

    return ret;
}

/* return:  malloc pointer to the decrypted message
 * out:     dec_len, length of the decrypted message
 * in:      privkey, pointer to raw private key
 *          pubkey, pointer to the raw public key matching privkey
 *          enc, pointer to the encrypted message
 *          enc_len, length of the encrypted message
 */
static uint8_t *decrypt_message(size_t *dec_len, const uint8_t *privkey, const uint8_t *pubkey,
                                const uint8_t *enc, const size_t enc_len)
{
    BN_CTX *ctx = BN_CTX_new();
    EC_GROUP *group = init_curve(ctx);

    /* check that the public header is valid and matches our priv/pubkey pair */
    if (!read_public_header(group, enc, pubkey, ctx))
        return NULL;

    int chunk_count = (enc_len - PUBHEADER_LEN) / (2*COMPR_PUBKEY_LEN);

    uint8_t *r = (uint8_t*)OPENSSL_malloc(PRIVHEADER_LEN + chunk_count * CHUNK_SIZE);

    uint8_t *Tser = (uint8_t*)OPENSSL_malloc(COMPR_PUBKEY_LEN);
    uint8_t *User = (uint8_t*)OPENSSL_malloc(COMPR_PUBKEY_LEN);
    int xoffset = 0;

    EC_POINT *T = EC_POINT_new(group);
    EC_POINT *U = EC_POINT_new(group);
    EC_POINT *M = EC_POINT_new(group);
    EC_POINT *V = EC_POINT_new(group);

    BIGNUM *Mx = BN_new();

    BIGNUM *bn_privkey = BN_new();
    BN_bin2bn(privkey, PRIVKEY_LEN, bn_privkey);

    int enc_loc = PUBHEADER_LEN;
    int r_loc = 0;
    for (int i = 0; i < chunk_count; i++)
    {
        memcpy(Tser, &enc[enc_loc], COMPR_PUBKEY_LEN);
        memcpy(User, &enc[enc_loc+COMPR_PUBKEY_LEN], COMPR_PUBKEY_LEN);

        /* decode the offset and evenness from the first byte */
        xoffset = Tser[0] >> 1;
        Tser[0] = 2 + (Tser[0]&1);

        EC_POINT_oct2point(group, T, Tser, COMPR_PUBKEY_LEN, ctx);
        EC_POINT_oct2point(group, U, User, COMPR_PUBKEY_LEN, ctx);

        EC_POINT_mul(group, V, NULL, T, bn_privkey, ctx);
        EC_POINT_invert(group, V, ctx);
        EC_POINT_add(group, M, U, V, ctx);

        EC_POINT_get_affine_coordinates_GFp(group, M, Mx, NULL, ctx);

        /* substract our offset so that we get the original Mx */
        BN_sub_word(Mx, xoffset);

        BN_bn2binpad(Mx, &r[r_loc], CHUNK_SIZE);

        r_loc += CHUNK_SIZE;
        enc_loc += 2*COMPR_PUBKEY_LEN;
    }

    uint8_t *ret = NULL;

    size_t size = 0;
    if (!read_private_header(&size, r))
        goto err;

    /* success */
    ret = (uint8_t*)malloc(size);
    memcpy(ret, &r[PRIVHEADER_LEN], size);
    *dec_len = size;

err:
    if (ERR_peek_error())
    {
        unsigned long e = 0;
        while ((e = ERR_get_error()))
            LogPrintf("decrypt_message %s: %s", ERR_func_error_string(e), ERR_reason_error_string(e));
    }

    OPENSSL_free(Tser);
    OPENSSL_free(User);

    EC_POINT_clear_free(V);
    EC_POINT_clear_free(U);
    EC_POINT_clear_free(M);
    EC_POINT_clear_free(T);

    BN_free(Mx);
    BN_clear_free(bn_privkey);

    OPENSSL_clear_free(r, r_loc);

    EC_GROUP_free(group);
    BN_CTX_free(ctx);

    return ret;
}

/* wrappers for the C functions
 */
using namespace std;

namespace Jeeq {

vector<uint8_t> EncryptMessage(const CPubKey pubkey, const string msg)
{
    // check pubkey
    if (!pubkey.IsValid())
        throw runtime_error("Jeeq::EncryptMessage(): called with an invalid public key");
    //  ensure that msg is not empty
    if (msg.size() == 0) throw runtime_error("Jeeq::EncryptMessage(): no message to encrypt");

    size_t enc_len = 0;
    uint8_t *benc = encrypt_message(&enc_len, pubkey.begin(), (uint8_t*)&msg[0], msg.size());
    if (benc == NULL || enc_len == 0)
        return vector<uint8_t>{};

    vector<uint8_t> enc(benc, benc+enc_len);
    return enc;
}

string DecryptMessage(const CKey privkey, const vector<uint8_t> enc)
{
    // check privkey
    if (!privkey.IsValid())
        throw runtime_error("Jeeq::DecryptMessage(): called with an invalid private key");
    // ensure that enc is not empty
    if (enc.size() == 0)
        throw runtime_error("Jeeq::DecryptMessage(): no encoded message to decrypt");

    size_t dec_len = 0;
    CPubKey pubkey = privkey.GetPubKey();
    uint8_t *bdec = decrypt_message(&dec_len, privkey.begin(), pubkey.begin(),
                      enc.data(), enc.size());
    if (bdec == NULL || dec_len == 0)
        return string();

    string dec(bdec, bdec+dec_len);
    free(bdec);
    return dec;
}

}

// if we don't do this bignum.h ducks everything up
#include "base58.h"
#include "net.h"
#include "netbase.h"
#include "util.h"
#include "wallet.h"
#include "walletdb.h"

// auxillary function for EncryptMessage
namespace Jeeq {
CPubKey SearchForPubKey(CBitcoinAddress addr)
{
    CScript spk;
    spk.SetDestination(addr.Get());

    CBlock block;
    uint256 blockhash;
    CTransaction txout;

    // beginning at the blockchain tip going back
    for (CBlockIndex* pindex = chainActive.Tip(); pindex && pindex->pprev; pindex = pindex->pprev)
    {
        if (!ReadBlockFromDisk(block, pindex))
            return CPubKey();

        // skip coinbase (vtx[0])
        for (unsigned int i = 1; i < block.vtx.size(); i++)
        {
            for (unsigned int j = 0; j < block.vtx[i].vin.size(); j++)
            {
                // get tx from txid and blockhash and store it in txout
                GetTransaction(block.vtx[i].vin[j].prevout.hash, txout, blockhash);
                int n = block.vtx[i].vin[j].prevout.n;

                if (txout.vout[n].scriptPubKey == spk)
                {
                    CScript ssig = block.vtx[i].vin[j].scriptSig;

                    opcodetype opcode;
                    std::vector<unsigned char> pkdata;
                    auto pc = ssig.begin();

                    // we do it twice since the pubkey is the second item
                    ssig.GetOp(pc, opcode, pkdata);
                    ssig.GetOp(pc, opcode, pkdata);

                    return CPubKey(pkdata.begin(), pkdata.end());
                }
            }
        }
    }

    return CPubKey();
}
} // Namespace Jeeq


/**
 * @file   test.cpp
 * @author Chengyu Liu
 * @date   Tue 08 June 2021
 *
 * @brief  这是个抛物型方程的例子，非常简单，中间就没有解释太多了
 *
 */

#include <iostream>
#include <vector>
#include <iterator>
#include <algorithm>

#include <base/exceptions.h>
#include <lac/full_matrix.h>
#include <lac/sparsity_pattern.h>
#include <lac/sparse_matrix.h>

#include <AFEPack/EasyMesh.h>
#include <AFEPack/TemplateElement.h>
#include <AFEPack/FEMSpace.h>
#include <AFEPack/BilinearOperator.h>
#include <AFEPack/Operator.h>
#include <AFEPack/Geometry.h>
#include <AFEPack/BoundaryCondition.h>

//#include <trace/mintrace.h>
#include <CG/CGSolver.h>
#include <EigenSolver/Miscellaneous.h>
#include <EigenSolver/EigenSolver.h>
#define DIM 2
#define PI (4.0*atan(1.0))

/// 初值和边值的表达式
double _u_(const double * p)
{
  return sin(PI*p[0]) * sin(PI*p[1]);
  //return sin(PI*p[0]) * sin(2*PI*p[1]);
  //return p[0]*exp(p[1]);
}

/// 右端项 In fact, right hand side vector will not be used in Laplacian eigenvalue problems.
double _f_(const double * p)
{
  return 10+5*PI*PI*_u_(p);
  //return p[0]*p[1] + sin(p[1]);
}

double f(const double * p)
{
  return 0;
}

/// Construct the Stiff matrix in the left hand side.
class Stiff_Matrix : public L2InnerProduct<DIM,double>
{
public:
  Stiff_Matrix(FEMSpace<double,DIM>& sp) :
    L2InnerProduct<DIM,double>(sp, sp) {}
  virtual void getElementMatrix(const Element<double,DIM>& e0,
                                const Element<double,DIM>& e1,
                                const ActiveElementPairIterator< DIM >::State s)
  {
    double vol = e0.templateElement().volume();
    u_int acc = algebricAccuracy();
    const QuadratureInfo<DIM>& qi = e0.findQuadratureInfo(acc);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = e0.local_to_global_jacobian(qi.quadraturePoint());
    //AFEPack::Point<DIM> test1;
    //std::vector<AFEPack::Point<DIM>> test2;
    //
    // Here always get an error! vector<Point<DIM> >;
    // Reason: Because there is a same name class in deal.ii: Point class. While
    // using Point, it might use the dealii.Point defaultly!!!! So you just add
    // the namespace to control the class will solve the problem;
    std::vector<AFEPack::Point<DIM>> q_pnt = e0.local_to_global(qi.quadraturePoint());
    //std::cout<<"ATTENTION!This is a test flag!!!"<<std::endl;
    std::vector<std::vector<double> > bas_val = e0.basis_function_value(q_pnt);
    std::vector<std::vector<std::vector<double> > > bas_grad = e0.basis_function_gradient(q_pnt);
    u_int n_ele_dof = e0.dof().size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      for (u_int i = 0;i < n_ele_dof;++ i) {
        for (u_int j = 0;j < n_ele_dof;++ j) {
          //elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]/_dt +
	  //                         innerProduct(bas_grad[i][l], bas_grad[j][l]));
	  elementMatrix(i,j) += Jxw*(innerProduct(bas_grad[i][l], bas_grad[j][l]));
        }
      }
    }
  }
};

// Construct the Mass matrix in the right hand side,
class Mass_Matrix : public L2InnerProduct<DIM,double>
{
public:
  Mass_Matrix(FEMSpace<double,DIM>& sp) :
    L2InnerProduct<DIM,double>(sp, sp){}
  virtual void getElementMatrix(const Element<double,DIM>& e0,
                                const Element<double,DIM>& e1,
                                const ActiveElementPairIterator< DIM >::State s)
  {
    double vol = e0.templateElement().volume();
    u_int acc = algebricAccuracy();
    const QuadratureInfo<DIM>& qi = e0.findQuadratureInfo(acc);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = e0.local_to_global_jacobian(qi.quadraturePoint());
    //AFEPack::Point<DIM> test1;
    //std::vector<AFEPack::Point<DIM>> test2;
    //
    // Here always get an error! vector<Point<DIM> >;
    // Reason: Because there is a same name class in deal.ii: Point class. While
    // using Point, it might use the dealii.Point defaultly!!!! So you just add
    // the namespace to control the class will solve the problem;
    std::vector<AFEPack::Point<DIM>> q_pnt = e0.local_to_global(qi.quadraturePoint());
    //std::cout<<"ATTENTION!This is a test flag!!!"<<std::endl;
    std::vector<std::vector<double> > bas_val = e0.basis_function_value(q_pnt);
    std::vector<std::vector<std::vector<double> > > bas_grad = e0.basis_function_gradient(q_pnt);
    u_int n_ele_dof = e0.dof().size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      for (u_int i = 0;i < n_ele_dof;++ i) {
        for (u_int j = 0;j < n_ele_dof;++ j) {
          /*elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]/_dt +
	    innerProduct(bas_grad[i][l], bas_grad[j][l]));*/
	  elementMatrix(i,j) += Jxw*(bas_val[i][l]*bas_val[j][l]);
        }
      }
    }
  }
};

void boundary_condition_apply(const FEMSpace<double, DIM>& sp,
		SparseMatrix<double>& A,
		BoundaryConditionAdmin<double,DIM> boundary,
		bool preserve_symmetry = true)
{
  u_int n_dof = sp.n_dof();
  const SparsityPattern& spA = A.get_sparsity_pattern();
  const std::size_t * rowstart = spA.get_rowstart_indices();
  const u_int * colnum = spA.get_column_numbers();

  for(u_int i=0; i< n_dof; ++ i)
  {
	  int bm = sp.dofInfo(i).boundary_mark;
	  std::cout<<"The "<<i<<"-th dof corresponding boundary mark is:"<<bm<<std::endl;
  }

  int numOfbm = 0;
  for (u_int i = 0; i < n_dof; ++ i){
    int bm = sp.dofInfo(i).boundary_mark;
    if (bm == 0) continue;
    // For this case only 1 for Drichlet boundary condition.
    if (bm != 1) continue; // Attention, For this case specially.

    numOfbm += 1;

    for (u_int j = rowstart[i]+1; j < rowstart[i+1]; ++ j){
	    A.global_entry(j) -= A.global_entry(j);
    }
    if (preserve_symmetry) {
      for (u_int j = rowstart[i] + 1;j < rowstart[i + 1];++ j) {
        u_int k = colnum[j];
        const u_int * p = std::find(&colnum[rowstart[k] + 1],
                                    &colnum[rowstart[k + 1]], i);
        if (p != &colnum[rowstart[k+1]]) {
          u_int l = p - &colnum[rowstart[0]];
          A.global_entry(l) -= A.global_entry(l);
        }
      }
    }
  }

  std::cout<<"!@@@@@@@@@@The number of Dirchlet is :"<<numOfbm<<std::endl;

}

int main(int argc, char * argv[])
{
  /// 准备网格
  EasyMesh mesh;
  mesh.readData(argv[1]);

  /// 准备参考单元
  TemplateGeometry<DIM> tmp_geo;
  tmp_geo.readData("triangle.tmp_geo");
  CoordTransform<DIM,DIM> crd_trs;
  crd_trs.readData("triangle.crd_trs");
  TemplateDOF<DIM> tmp_dof(tmp_geo);
  tmp_dof.readData("triangle.2.tmp_dof");
  BasisFunctionAdmin<double,DIM,DIM> bas_fun(tmp_dof);
  bas_fun.readData("triangle.2.bas_fun");

  std::vector<TemplateElement<double,DIM> > tmp_ele(1);
  tmp_ele[0].reinit(tmp_geo, tmp_dof, crd_trs, bas_fun);

  /// 定制有限元空间
  FEMSpace<double,DIM> fem_space(mesh, tmp_ele);
  u_int n_ele = mesh.n_geometry(DIM);
  fem_space.element().resize(n_ele);
  for (u_int i = 0;i < n_ele;++ i) {
    fem_space.element(i).reinit(fem_space, i, 0);
  }
  fem_space.buildElement();
  fem_space.buildDof();
  fem_space.buildDofBoundaryMark();

  /// 准备初值
  FEMFunction<double,DIM> u_h(fem_space);
  Operator::L2Interpolate(&_u_, u_h);

  /// 准备边界条件
  Vector<double> rhs;
  Operator::L2Discretize(&f, fem_space, rhs, 4);

  BoundaryFunction<double,DIM> boundary(BoundaryConditionInfo::DIRICHLET,
                                        1,
                                        &_u_);
  BoundaryConditionAdmin<double,DIM> boundary_admin(fem_space);
  boundary_admin.add(boundary);

  // double t;//

  // double dt = 0.01; /// 简单起见，随手取个时间步长算了

  /// 准备线性系统的矩阵
  /*
  Matrix mat(fem_space, dt);
  mat.algebricAccuracy() = 3;
  mat.build();*/

  StiffMatrix<2,double> stiff_matrix(fem_space);
  stiff_matrix.algebricAccuracy() = 4;
  stiff_matrix.build();
  boundary_condition_apply(fem_space, stiff_matrix, boundary_admin);

  MassMatrix<2,double> mass_matrix(fem_space);
  mass_matrix.algebricAccuracy() = 4;
  mass_matrix.build();
  boundary_condition_apply(fem_space, mass_matrix, boundary_admin);

  /*
  /// 准备右端项
  Vector<double> rhs(fem_space.n_dof());
  FEMSpace<double,DIM>::ElementIterator the_ele = fem_space.beginElement();
  FEMSpace<double,DIM>::ElementIterator end_ele = fem_space.endElement();
  for (;the_ele != end_ele;++ the_ele) {
    double vol = the_ele->templateElement().volume();
    const QuadratureInfo<DIM>& qi = the_ele->findQuadratureInfo(3);
    u_int n_q_pnt = qi.n_quadraturePoint();
    std::vector<double> jac = the_ele->local_to_global_jacobian(qi.quadraturePoint());
    std::vector<AFEPack::Point<DIM>> q_pnt = the_ele->local_to_global(qi.quadraturePoint());
    std::vector<std::vector<double> > bas_val = the_ele->basis_function_value(q_pnt);

    /// 当基函数的值已知情况下，可以使用下面的函数来加速
    std::vector<double> u_h_val = u_h.value(bas_val, *the_ele);
    std::vector<std::vector<double> > u_h_grad = u_h.gradient(q_pnt, *the_ele);
    const std::vector<int>& ele_dof = the_ele->dof();
    u_int n_ele_dof = ele_dof.size();
    for (u_int l = 0;l < n_q_pnt;++ l) {
      double Jxw = vol*qi.weight(l)*jac[l];
      double f_val = _f_(q_pnt[l]);
      for (u_int i = 0;i < n_ele_dof;++ i)
	{
	  rhs(ele_dof[i]) += Jxw*bas_val[i][l]*(u_h_val[l]/dt + f_val);
        }
    }
    }*/

  /// 应用边界条件
  //  boundary_admin.apply(mat, u_h, rhs);

  /// 求解线性系统
  /*
  AMGSolver solver;
  solver.lazyReinit(mat);
  solver.solve(u_h, rhs, 1.0e-08, 50);*/
  //TraceSolver soll(stiff_matrix, mass_matrix);
  CGSolver AAAA;
  EigenSolver solver(stiff_matrix, mass_matrix);
  /*
  std::vector<double>rhs(stiff_matrix.m(),0),x(12,0);
  rhs[0]=-0.107343;
  rhs[1]=-0.107920;
  rhs[2]=0.296721;
  rhs[3]=0.136396;
  rhs[4]=-1.038010;
  rhs[5]=-0.456170;
  rhs[6]=0.694243;
  rhs[7]=1.047650;
  rhs[8]=0.488934;
  rhs[9]=0.065605;
  rhs[10]=-0.113350;
  rhs[11]=-0.309275;
  std::vector<double> temp(6,0);
  std::vector<std::vector<double>> tempX(12,temp);
  solver.X=tempX;
  for(int i=0;i<12;i++)
    {
      for(int j=0;j<6;j++)
	{
	  std::cin>>solver.X[i][j];
	}
    }
  solver.get_MX();
  solver.solve(x, rhs, 1.0e-3, 200);
  std::cout<<"The rhs vector before CG is :\n";
  for(int i=0;i<rhs.size();i++)
    {
      std::cout<<rhs[i]<<" ";
    }
  std::cout<<std::endl;
  std::cout<<"The solution of CG\n";
  for(int i=0;i<x.size();i++)
    {
      std::cout<<x[i]<<" ";
    }
  std::cout<<std::endl;
  */

  /////////////////////////////

  //solver.mintrace(10, 1.0e-3, 200);
  std::vector<double> x(stiff_matrix.m(),1);
  double lambda;
  solver.PowerSolve(x, lambda);
  /*
  std::cout<<"This is the maximal eigenvalues of AX=lambda Mx;\n";
  for (int i=0;i < x.size();i++)
  {
    std::cout<<x[i]<<" ";
  }
  std::cout<<"\n";
*/
  std::vector<double> x2(stiff_matrix.m(),1);
  solver.IPowerSolve(x2, lambda, 100, 1.e-3);

  int eig_num=5;
  std::vector<double> tempxx(eig_num,0), lam_3(eig_num,0);
  std::vector<std::vector<double>> x3(stiff_matrix.m(),tempxx);
  /*
  for(int k=0;k<eig_num;k++)
  {
    x3[k][k]=1;
  }*/

  for (int i=0;i<x3.size();i++)
  {
    for(int j=0;j<x3[0].size();j++)
    {
      x3[i][j] = ((double) rand() / (RAND_MAX));
    }
  }

  solver.BIPowerSolve(x3, lam_3, eig_num, 1000, 1.e-3);
  std::cout<<"The "<<eig_num<<" smallest eigenvalues are:\n";

  for (int k=0;k<eig_num;k++)
  {
    std::cout<<lam_3[k]<<" ";
  }
  std::cout<<"\n";

  std::cout<<"The matrix are:\n";
  show_matrix(x3);

//  std::cout<<"This is the minimal eigenvalues of AX=lambda Mx;\n";
  /*
  for (int i=0;i < x2.size();i++)
  {
    std::cout<<x2[i]<<" ";
  }
  std::cout<<"\n";
  */
  /*for (int i=0;i<solver.lambda.size();i++)
    {
      std::cout<<solver.lambda[i]<<std::endl;
    }
  std::cout<<"\n";
*/
  /*
  std::vector<double> exact_eigen={2.0*PI*PI,PI*PI,PI*PI,0.0};
  std::cout<<" The exact eigens are:\n";
  for (int i=0;i<solver.lambda.size();i++)
    {
      std::cout<<fabs(solver.lambda[i]-exact_eigen[i])<<std::endl;
      // std::cout<<exact_eigen[i]<<std::endl;
    }
  std::cout<<"\n";

  std::cout<<"This is the matrix V after the whole process\n";
  for(int i=0;i<solver.X.size();i++)
    {
      for(int j=0;j<solver.X[0].size();j++)
	{
	  std::cout<<solver.X[i][j]<<" ";
	}
      std::cout<<std::endl;
      }*/

  /*
  std::cout<<"Output the solution matrix X!\n";
  for(int i=0;i<solver.X.size();i++)
    {
      for(int j=0;j<solver.X[0].size();j++)
	{
	  std::cout<<solver.X[i][j]<<" ";
	}
      std::cout<<std::endl;
    }
  std::cout<<std::endl;
  */

  /// 输出数据画图
  /* u_h.writeOpenDXData("u_h.dx");*/
  // std::cout << "Press ENTER to continue or CTRL+C to stop ..." << std::flush;
  // getchar();

  // t += dt; /// 更新时间

  //std::cout << "\n\tt = " <<  t << std::endl;

  // Print the stiffness matrix in to .txt and .gnuplot form.
 // std::ofstream sparsematrix2 ("stiff_matrix.1");
 // stiff_matrix.print(sparsematrix2);

  std::filebuf fb;
  fb.open ("stiff_matrix.txt",std::ios::out);
  std::ostream os(&fb);
  stiff_matrix.print_formatted(os, 3, true, 0, "0.0", 1);
  fb.close();

 // Print the mass matrix into .txt and .gnuplot form.
//  std::ofstream sparsematrix  ("mass_matrix.1");
 // mass_matrix.print(sparsematrix);

  std::filebuf fb2;
  fb2.open ("mass_matrix.txt",std::ios::out);
  std::ostream os2(&fb2);
  mass_matrix.print_formatted(os2, 3, true, 0, "0.0", 1);
  fb2.close();

 /*
  std::cout<<"Attention! This is print out the columns of the matrix A and M\n";
  std::vector<double> tempx(stiff_matrix.n(),0), tempAx, tempMx;

  for(int i=0;i<solver.A->m();i++)
    {
      tempx[i]=1;

      tempAx=multiply(solver.A, tempx);
      for(int j=0;j<tempAx.size();j++)
	{
	  std::cout<<tempAx[j]<<" ";
	}
	std::cout<<std::endl;

      tempMx=multiply(solver.M, tempx);
      for(int j=0;j<tempMx.size();j++)
	{
	  std::cout<<tempMx[j]<<" ";
	}
      std::cout<<std::endl;
      tempx[i]=0;
    }
    */

  return 0;
}

/**
 * end of file
 *
 */


// Copyright 2010-2014, D. E. Shaw Research.
// 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 BOOST_RANDOM_DETAIL_MULHILO_HPP
#define BOOST_RANDOM_DETAIL_MULHILO_HPP

#include <limits>
#include <boost/cstdint.hpp>
#include <boost/integer.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/static_assert.hpp>

namespace boost{
namespace random{
namespace detail{

// First, we implement hilo multiplication with "half words".  This is
// the "reference implementation" which should be correct for any
// binary unsigned integral UINT with an even number of bits.  In
// practice, we try to avoid this implementation because it is so slow
// (4 multiplies plus about a dozen xor, or, +, shift, mask and
// compare operations.
template <typename Uint>
inline Uint
mulhilo_halfword(Uint a, Uint b, Uint& hip){
    BOOST_STATIC_ASSERT(std::numeric_limits<Uint>::is_specialized &&
                        std::numeric_limits<Uint>::is_integer &&
                        !std::numeric_limits<Uint>::is_signed &&
                        std::numeric_limits<Uint>::radix == 2 &&
                        std::numeric_limits<Uint>::digits%2 == 0);
    const unsigned WHALF = std::numeric_limits<Uint>::digits/2;
    const Uint LOMASK = ((Uint)(~(Uint)0)) >> WHALF;
    Uint lo = a*b;
    Uint ahi = a>>WHALF;
    Uint alo = a& LOMASK;
    Uint bhi = b>>WHALF;
    Uint blo = b& LOMASK;

    Uint ahbl = ahi*blo;
    Uint albh = alo*bhi;

    Uint ahbl_albh = ((ahbl&LOMASK) + (albh&LOMASK));
    Uint hi = (ahi*bhi) + (ahbl>>WHALF) +  (albh>>WHALF);
    hi += ahbl_albh >> WHALF;
    /* carry from the sum with alo*blo */
    hi += ((lo >> WHALF) < (ahbl_albh&LOMASK));
    hip = hi;
    return lo;
}

// We can formulate a much faster implementation if we can use
// integers of twice the width of Uint (e.g., DblUint).  Such types
// are not always available (e.g., when Uint is uintmax_t), but when
// they are, we find that modern compilers (gcc, MSVC, Intel) pattern
// match the structure of the mulhilo below and turn it into an
// optimized instruction sequence, e.g., mulw or mull.
//
// However, the alternative implementation, which we want to use
// when there *is* a DblUint would be ambiguoous without some enable_if
// hackery.  To support that, we need a has_double_width type trait.
//
// N.B.  It should be possible to do this with some SFINAE wrapped
// around an instantiation of uint_t<2*W>::least.  My attempts to do
// so ran into the problem described in
// https://svn.boost.org/trac/boost/ticket/6169 from Nov 23, 2011
// (still open in April 2014).  So instead just check that twice the
// number of digits in Uint is less than or equal to the number of
// digits in uintmax_t.  FWIW, there is currently (as of 1.53)
// a BOOST_STATIC_ASSERT in integer.hpp that insists on a very
// similar condition:  Bits <= sizeof(boost::uintmax_t)*CHAR_BIT.

template <typename Uint>
class has_double_width{
public:
    static const bool value = std::numeric_limits<Uint>::is_specialized &&
        std::numeric_limits<Uint>::is_integer &&
        !std::numeric_limits<Uint>::is_signed &&
        std::numeric_limits<Uint>::radix == 2 &&
        2*std::numeric_limits<Uint>::digits <= std::numeric_limits< boost::uintmax_t >::digits;
};

// mulhilo using double-width DblUint
template <typename Uint>
inline typename boost::enable_if_c<has_double_width<Uint>::value, Uint>::type
mulhilo(Uint a, Uint b, Uint& hip){
    typedef typename uint_t<2*std::numeric_limits<Uint>::digits>::least DblUint;
    DblUint product = ((DblUint)a)*((DblUint)b);
    hip = product>>std::numeric_limits<Uint>::digits;
    return (Uint)product;
}

// When there is no DblUint and there are no specializations that use
// machine-specific intrinsics (below), fall back to mulhilo_halfword.
template <typename Uint>
inline typename boost::enable_if_c<!has_double_width<Uint>::value, Uint>::type
mulhilo(Uint a, Uint b, Uint& hip){
    return mulhilo_halfword(a, b, hip);
}

// Every ISA I know (x86, ppc, arm, CUDA) has an instruction that
// gives the hi word of the product of two uintmax_t's FAR more
// quickly and succinctly than a call to mulhilo_halfword.  Without
// them, philox<N, uintmax_t> would be impractically slow.
// Unfortunately, they require compiler-and-hardware-specific
// intrinsics or asm statements.
//
// FIXME - add more special cases here, e.g., MSVC intrinsics and
// asm for PowerPC and ARM.
#if defined(__GNUC__) && defined(__x86_64__)
template <>
inline uint64_t
mulhilo(uint64_t ax, uint64_t b, uint64_t& hip){
    uint64_t dx;
    __asm__("\n\t"
        "mulq %2\n\t"
        : "=a"(ax), "=d"(dx)
        : "r"(b), "0"(ax)
        );
    hip = dx;
    return ax;
}
#endif

} // namespace detail
} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_DETAIL_MULHILO_HPP


/*****************************************************************************
*   ExploringSfMWithOpenCV
******************************************************************************
*   by Roy Shilkrot, 5th Dec 2012
*   http://www.morethantechnical.com/
******************************************************************************
*   Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/

#include "FindCameraMatrices.h"
#include "Triangulation.h"

#include <vector>
#include <iostream>

#include <opencv2/calib3d/calib3d.hpp>

using namespace cv;
using namespace std;

#ifdef USE_EIGEN
#include <Eigen/Eigen>
#endif

#define DECOMPOSE_SVD

#ifndef CV_PCA_DATA_AS_ROW
#define CV_PCA_DATA_AS_ROW 0
#endif

void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
	//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
	using namespace Eigen;

	Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
	Matrix3d EEt = E * E.transpose();
	Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(2));
	Vector3d b1,b2;

#if 1
	//Method 1
	Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
	Vector3d bbt_diag = bbt.diagonal();
	if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
		b1 = bbt.row(0) / sqrt(bbt_diag(0));
		b2 = -b1;
	} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
		b1 = bbt.row(1) / sqrt(bbt_diag(1));
		b2 = -b1;
	} else {
		b1 = bbt.row(2) / sqrt(bbt_diag(2));
		b2 = -b1;
	}
#else
	//Method 2
	if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
		b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
		b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else {
		b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	}
#endif

	//Horn90 (19)
	Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
	cofactors.transposeInPlace();

	//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
	Matrix3d B1; B1 <<	0,-b1(2),b1(1),
						b1(2),0,-b1(0),
						-b1(1),b1(0),0;
	Matrix3d B2; B2 <<	0,-b2(2),b2(1),
						b2(2),0,-b2(0),
						-b2(1),b2(0),0;

	Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);

	//Horn90 (24)
	R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2);
	t1 = b1; t2 = b2;

	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}

bool CheckCoherentRotation(cv::Mat_<double>& R) {

	if(fabsf(determinant(R))-1.0 > 1e-07) {
		cerr << "det(R) != +-1.0, this is not a rotation matrix" << endl;
		return false;
	}

	return true;
}

Mat GetFundamentalMat(const vector<KeyPoint>& imgpts1,
					   const vector<KeyPoint>& imgpts2,
					   vector<KeyPoint>& imgpts1_good,
					   vector<KeyPoint>& imgpts2_good,
					   vector<DMatch>& matches
#ifdef __SFM__DEBUG__
					  ,const Mat& img_1,
					  const Mat& img_2
#endif
					  )
{
	//Try to eliminate keypoints based on the fundamental matrix
	//(although this is not the proper way to do this)
	vector<uchar> status(imgpts1.size());

#ifdef __SFM__DEBUG__
	std::vector< DMatch > good_matches_;
	std::vector<KeyPoint> keypoints_1, keypoints_2;
#endif
	//	undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
	//	undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
	//
	imgpts1_good.clear(); imgpts2_good.clear();

	vector<KeyPoint> imgpts1_tmp;
	vector<KeyPoint> imgpts2_tmp;
	if (matches.size() <= 0) {
		//points already aligned...
		imgpts1_tmp = imgpts1;
		imgpts2_tmp = imgpts2;
	} else {
		GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);
	}

	Mat F;
	{
		vector<Point2f> pts1,pts2;
		KeyPointsToPoints(imgpts1_tmp, pts1);
		KeyPointsToPoints(imgpts2_tmp, pts2);
#ifdef __SFM__DEBUG__
		cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_tmp.size() << ")" << endl;
		cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_tmp.size() << ")" << endl;
#endif
		double minVal,maxVal;
		cv::minMaxIdx(pts1,&minVal,&maxVal);
		F = findFundamentalMat(pts1, pts2, FM_RANSAC, 0.006 * maxVal, 0.99, status); //threshold from [Snavely07 4.1]
	}

	vector<DMatch> new_matches;
	cout << "F keeping " << countNonZero(status) << " / " << status.size() << endl;
	for (unsigned int i=0; i<status.size(); i++) {
		if (status[i])
		{
			imgpts1_good.push_back(imgpts1_tmp[i]);
			imgpts2_good.push_back(imgpts2_tmp[i]);

			if (matches.size() <= 0) { //points already aligned...
				new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,matches[i].distance));
			} else {
				new_matches.push_back(matches[i]);
			}

#ifdef __SFM__DEBUG__
			good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
			keypoints_1.push_back(imgpts1_tmp[i]);
			keypoints_2.push_back(imgpts2_tmp[i]);
#endif
		}
	}

	cout << matches.size() << " matches before, " << new_matches.size() << " new matches after Fundamental Matrix\n";
	matches = new_matches; //keep only those points who survived the fundamental matrix

#if 0
	//-- Draw only "good" matches
#ifdef __SFM__DEBUG__
	if(!img_1.empty() && !img_2.empty()) {
		vector<Point2f> i_pts,j_pts;
		Mat img_orig_matches;
		{ //draw original features in red
			vector<uchar> vstatus(imgpts1_tmp.size(),1);
			vector<float> verror(imgpts1_tmp.size(),1.0);
			img_1.copyTo(img_orig_matches);
			KeyPointsToPoints(imgpts1_tmp, i_pts);
			KeyPointsToPoints(imgpts2_tmp, j_pts);
			drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,0,255));
		}
		{ //superimpose filtered features in green
			vector<uchar> vstatus(imgpts1_good.size(),1);
			vector<float> verror(imgpts1_good.size(),1.0);
			i_pts.resize(imgpts1_good.size());
			j_pts.resize(imgpts2_good.size());
			KeyPointsToPoints(imgpts1_good, i_pts);
			KeyPointsToPoints(imgpts2_good, j_pts);
			drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,255,0));
			imshow( "Filtered Matches", img_orig_matches );
		}
		int c = waitKey(0);
		if (c=='s') {
			imwrite("fundamental_mat_matches.png", img_orig_matches);
		}
		destroyWindow("Filtered Matches");
	}
#endif
#endif

	return F;
}

void TakeSVDOfE(Mat_<double>& E, Mat& svd_u, Mat& svd_vt, Mat& svd_w) {
#if 1
	//Using OpenCV's SVD
	SVD svd(E,SVD::MODIFY_A);
	svd_u = svd.u;
	svd_vt = svd.vt;
	svd_w = svd.w;
#else
	//Using Eigen's SVD
	cout << "Eigen3 SVD..\n";
	Eigen::Matrix3f  e = Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> >((double*)E.data).cast<float>();
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(e, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::MatrixXf Esvd_u = svd.matrixU();
	Eigen::MatrixXf Esvd_v = svd.matrixV();
	svd_u = (Mat_<double>(3,3) << Esvd_u(0,0), Esvd_u(0,1), Esvd_u(0,2),
						  Esvd_u(1,0), Esvd_u(1,1), Esvd_u(1,2),
						  Esvd_u(2,0), Esvd_u(2,1), Esvd_u(2,2));
	Mat_<double> svd_v = (Mat_<double>(3,3) << Esvd_v(0,0), Esvd_v(0,1), Esvd_v(0,2),
						  Esvd_v(1,0), Esvd_v(1,1), Esvd_v(1,2),
						  Esvd_v(2,0), Esvd_v(2,1), Esvd_v(2,2));
	svd_vt = svd_v.t();
	svd_w = (Mat_<double>(1,3) << svd.singularValues()[0] , svd.singularValues()[1] , svd.singularValues()[2]);
#endif

	cout << "----------------------- SVD ------------------------\n";
	cout << "U:\n"<<svd_u<<"\nW:\n"<<svd_w<<"\nVt:\n"<<svd_vt<<endl;
	cout << "----------------------------------------------------\n";
}

bool TestTriangulation(const vector<CloudPoint>& pcloud, const Matx34d& P, vector<uchar>& status) {
	vector<Point3d> pcloud_pt3d = CloudPointsToPoints(pcloud);
	vector<Point3d> pcloud_pt3d_projected(pcloud_pt3d.size());

	Matx44d P4x4 = Matx44d::eye();
	for(int i=0;i<12;i++) P4x4.val[i] = P.val[i];

	perspectiveTransform(pcloud_pt3d, pcloud_pt3d_projected, P4x4);

	status.resize(pcloud.size(),0);
	for (int i=0; i<pcloud.size(); i++) {
		status[i] = (pcloud_pt3d_projected[i].z > 0) ? 1 : 0;
	}
	int count = countNonZero(status);

	double percentage = ((double)count / (double)pcloud.size());
	cout << count << "/" << pcloud.size() << " = " << percentage*100.0 << "% are in front of camera" << endl;
	if(percentage < 0.75)
		return false; //less than 75% of the points are in front of the camera

	//check for coplanarity of points
	if(false) //not
	{
		cv::Mat_<double> cldm(pcloud.size(),3);
		for(unsigned int i=0;i<pcloud.size();i++) {
			cldm.row(i)(0) = pcloud[i].pt.x;
			cldm.row(i)(1) = pcloud[i].pt.y;
			cldm.row(i)(2) = pcloud[i].pt.z;
		}
		cv::Mat_<double> mean;
		cv::PCA pca(cldm,mean,CV_PCA_DATA_AS_ROW);

		int num_inliers = 0;
		cv::Vec3d nrm = pca.eigenvectors.row(2); nrm = nrm / norm(nrm);
		cv::Vec3d x0 = pca.mean;
		double p_to_plane_thresh = sqrt(pca.eigenvalues.at<double>(2));

		for (int i=0; i<pcloud.size(); i++) {
			Vec3d w = Vec3d(pcloud[i].pt) - x0;
			double D = fabs(nrm.dot(w));
			if(D < p_to_plane_thresh) num_inliers++;
		}

		cout << num_inliers << "/" << pcloud.size() << " are coplanar" << endl;
		if((double)num_inliers / (double)(pcloud.size()) > 0.85)
			return false;
	}

	return true;
}

bool DecomposeEtoRandT(
	Mat_<double>& E,
	Mat_<double>& R1,
	Mat_<double>& R2,
	Mat_<double>& t1,
	Mat_<double>& t2)
{
#ifdef DECOMPOSE_SVD
	//Using HZ E decomposition
	Mat svd_u, svd_vt, svd_w;
	TakeSVDOfE(E,svd_u,svd_vt,svd_w);

	//check if first and second singular values are the same (as they should be)
	double singular_values_ratio = fabsf(svd_w.at<double>(0) / svd_w.at<double>(1));
	if(singular_values_ratio>1.0) singular_values_ratio = 1.0/singular_values_ratio; // flip ratio to keep it [0,1]
	if (singular_values_ratio < 0.7) {
		cout << "singular values are too far apart\n";
		return false;
	}

	Matx33d W(0,-1,0,	//HZ 9.13
		1,0,0,
		0,0,1);
	Matx33d Wt(0,1,0,
		-1,0,0,
		0,0,1);
	R1 = svd_u * Mat(W) * svd_vt; //HZ 9.19
	R2 = svd_u * Mat(Wt) * svd_vt; //HZ 9.19
	t1 = svd_u.col(2); //u3
	t2 = -svd_u.col(2); //u3
#else
	//Using Horn E decomposition
	DecomposeEssentialUsingHorn90(E[0],R1[0],R2[0],t1[0],t2[0]);
#endif
	return true;
}

bool FindCameraMatrices(const Mat& K,
						const Mat& Kinv,
						const Mat& distcoeff,
						const vector<KeyPoint>& imgpts1,
						const vector<KeyPoint>& imgpts2,
						vector<KeyPoint>& imgpts1_good,
						vector<KeyPoint>& imgpts2_good,
						Matx34d& P,
						Matx34d& P1,
						vector<DMatch>& matches,
						vector<CloudPoint>& outCloud
#ifdef __SFM__DEBUG__
						,const Mat& img_1,
						const Mat& img_2
#endif
						)
{
	//Find camera matrices
	{
		cout << "Find camera matrices...";
		double t = getTickCount();

		Mat F = GetFundamentalMat(imgpts1,imgpts2,imgpts1_good,imgpts2_good,matches
#ifdef __SFM__DEBUG__
								  ,img_1,img_2
#endif
								  );
		if(matches.size() < 100) { // || ((double)imgpts1_good.size() / (double)imgpts1.size()) < 0.25
			cerr << "not enough inliers after F matrix" << endl;
			return false;
		}

		//Essential matrix: compute then extract cameras [R|t]
		Mat_<double> E = K.t() * F * K; //according to HZ (9.12)

		//according to http://en.wikipedia.org/wiki/Essential_matrix#Properties_of_the_essential_matrix
		if(fabsf(determinant(E)) > 1e-07) {
			cout << "det(E) != 0 : " << determinant(E) << "\n";
			P1 = 0;
			return false;
		}

		Mat_<double> R1(3,3);
		Mat_<double> R2(3,3);
		Mat_<double> t1(1,3);
		Mat_<double> t2(1,3);

		//decompose E to P' , HZ (9.19)
		{
			if (!DecomposeEtoRandT(E,R1,R2,t1,t2)) return false;

			if(determinant(R1)+1.0 < 1e-09) {
				//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
				cout << "det(R) == -1 ["<<determinant(R1)<<"]: flip E's sign" << endl;
				E = -E;
				DecomposeEtoRandT(E,R1,R2,t1,t2);
			}
			if (!CheckCoherentRotation(R1)) {
				cout << "resulting rotation is not coherent\n";
				P1 = 0;
				return false;
			}

			P1 = Matx34d(R1(0,0),	R1(0,1),	R1(0,2),	t1(0),
						 R1(1,0),	R1(1,1),	R1(1,2),	t1(1),
						 R1(2,0),	R1(2,1),	R1(2,2),	t1(2));
			cout << "Testing P1 " << endl << Mat(P1) << endl;

			vector<CloudPoint> pcloud,pcloud1; vector<KeyPoint> corresp;
			double reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
			double reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
			vector<uchar> tmp_status;
			//check if pointa are triangulated --in front-- of cameras for all 4 ambiguations
			if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
				P1 = Matx34d(R1(0,0),	R1(0,1),	R1(0,2),	t2(0),
							 R1(1,0),	R1(1,1),	R1(1,2),	t2(1),
							 R1(2,0),	R1(2,1),	R1(2,2),	t2(2));
				cout << "Testing P1 "<< endl << Mat(P1) << endl;

				pcloud.clear(); pcloud1.clear(); corresp.clear();
				reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
				reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

				if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
					if (!CheckCoherentRotation(R2)) {
						cout << "resulting rotation is not coherent\n";
						P1 = 0;
						return false;
					}

					P1 = Matx34d(R2(0,0),	R2(0,1),	R2(0,2),	t1(0),
								 R2(1,0),	R2(1,1),	R2(1,2),	t1(1),
								 R2(2,0),	R2(2,1),	R2(2,2),	t1(2));
					cout << "Testing P1 "<< endl << Mat(P1) << endl;

					pcloud.clear(); pcloud1.clear(); corresp.clear();
					reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
					reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

					if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
						P1 = Matx34d(R2(0,0),	R2(0,1),	R2(0,2),	t2(0),
									 R2(1,0),	R2(1,1),	R2(1,2),	t2(1),
									 R2(2,0),	R2(2,1),	R2(2,2),	t2(2));
						cout << "Testing P1 "<< endl << Mat(P1) << endl;

						pcloud.clear(); pcloud1.clear(); corresp.clear();
						reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
						reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

						if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
							cout << "Shit." << endl;
							return false;
						}
					}
				}
			}
			for (unsigned int i=0; i<pcloud.size(); i++) {
				outCloud.push_back(pcloud[i]);
			}
		}

		t = ((double)getTickCount() - t)/getTickFrequency();
		cout << "Done. (" << t <<"s)"<< endl;
	}
	return true;
}


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

/*
 * Implement a NON-LINEAR range angle observer for many filter schemes
 * The model parameters allow model sizes and the non-linearity.
 * This provides an excellent vehicle to test each scheme. Use for regression testing
 */

#include "BayesFilter/allFilters.hpp"
#include "BayesFilter/schemeFlt.hpp"
#include "Test/random.hpp"
#include "angle.hpp"
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/format.hpp>
#include <boost/limits.hpp>

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;
using namespace angleArith;

const std::size_t NX = 2+1;			// State dimension (x,y) and some more to show noise coupling or singular X
const std::size_t NQ = 2;			// State dimension of noise
const std::size_t NZ = 2+2;			// Observation dimension (r,a) and some empty dummies
const std::size_t NS = 1000;			// Number of samples for a sampled representation

const bool RA_MODEL = true;		// Use Range angle NON-linear model (requires normalising angle)
const bool NOISE_MODEL = true;		// Add noise to truth model
const bool TRUTH_STATIONARY = false;// Truth model setup
const Float INIT_XY[2] = {1.,-0.2};	// XY initial position
const Float TARGET[2] = {-11.,0.};	// XY position of target

const Float RANGE_NOISE = NOISE_MODEL ? Float(0.1) : Float(1e-6);
const Float ANGLE_NOISE = NOISE_MODEL ? Float(5. * angle<Float>::Deg2Rad) : Float(1e-6);
const Float Z_CORRELATION = Float(0e-1);	// (Un)Correlated observation model

const Float X_NOISE = Float(0.05);		// predict model
const Float Y_NOISE = Float(0.09);
const Float XY_NOISE_COUPLING = Float(0.05);
const Float Q_NOISE = Float(1.0);		// Noise in addition Q terms
const Float G_COUPLING = Float(1.0);	// Coupling in addition G terms

const Float INIT_X_NOISE = Float(0.07);
const Float INIT_Y_NOISE = Float(0.10);
const Float INIT_XY_NOISE_CORRELATION = Float(0.4);
const Float INIT_2_NOISE = Float(0.09);	// Use zero for singular X
const Float INIT_2_NOISE_CORRELATION = Float(0.5);

// Square
template <class scalar>
inline scalar sqr(scalar x)
{
	return x*x;
}

class Rtheta_random : public Bayesian_filter_test::Boost_random, public SIR_random
/*
 * Random numbers for filters from Boost
 */
{
public:
	Float normal (const Float mean, const Float sigma)
	{
		Float f = Boost_random::normal (mean, sigma);
		return f;
	}
	void normal (DenseVec& v)
	{
		Boost_random::normal (v);
	}
	void uniform_01 (DenseVec& v)
	{
		Boost_random::uniform_01 (v);
	}
} Random, Random2;

/*
 * Linear predict model
 *  x static and y tending to x
 *  Correlated additive noise
 */
class pred_model : public Sampled_LiInAd_predict_model
{
public:
	pred_model();
};

pred_model::pred_model () :
	Sampled_LiInAd_predict_model(NX,NQ, Random2)
// Construct constant model
{
	// Build Fx, Identity all except active partition
	FM::identity (Fx);
	Fx(0,0) = Float(1);
	Fx(0,1) = Float(0);
	Fx(1,0) = Float(0.1);
	Fx(1,1) = Float(0.9);

	// Build q,G, Test addition parts using coupled noise
	q = ublas::scalar_vector<Float>(NQ, Q_NOISE);
	q[0] = sqr(X_NOISE);
	q[1] = sqr(Y_NOISE);
	G = ublas::scalar_matrix<Float>(NX,NQ, G_COUPLING);
	G(0,0) = 1;
	G(0,1) = XY_NOISE_COUPLING;
	G(1,0) = XY_NOISE_COUPLING;
	G(1,1) = 1;

	// Build inverse Fx, Identity all except active partition
	DenseColMatrix denseinvFx (Fx.size1(), Fx.size2());
	Information_root_scheme::inverse_Fx (denseinvFx, Fx);
	inv.Fx = denseinvFx;
}

/*
 * Observation model: gradient linearised versions
 *  Linearisation state must be set before use
 *  Both uncorrelated and correlated representation
 *  Correlated representation is just the uncorrelated adapted so the
 *  correlation term are added.
 */
#ifndef MODEL_LINEAR
	typedef General_LzUnAd_observe_model Observe_un_special;
	typedef General_LzCoAd_observe_model Observe_co_special;
#else
	typedef General_LiUnAd_observe_model Observe_un_special;
	typedef General_LiCoAd_observe_model Observe_co_special;
#endif

class uobs_model : public Observe_un_special
{
public:
	uobs_model();
	const Vec& h(const Vec& x) const;
	void normalise (Vec& z_denorm, const Vec& z_from) const;
	void state (const Vec& x);
private:
	mutable Vec z_pred;
};

class cobs_model : public Observe_co_special
{
public:
	cobs_model(uobs_model& u);
	const Vec& h(const Vec& x) const;
	void normalise (Vec& z_denorm, const Vec& z_from) const;
	void state (const Vec& x);
private:
	uobs_model& uobs;
};

uobs_model::uobs_model() :
	Observe_un_special(NX,NZ),
	z_pred(NZ)
{
	// Observation covariance uncorrelated
	Zv = ublas::scalar_vector<Float>(Zv.size(), Float(1));
	Zv[0] = sqr(RANGE_NOISE);
	Zv[1] = (RA_MODEL ? sqr(ANGLE_NOISE): sqr(RANGE_NOISE));
}

cobs_model::cobs_model(uobs_model& u) :
	Observe_co_special(NX,NZ),
	uobs(u)
{
	FM::identity (Z);
	// Create the correlation in Z
	Z(0,0) = Float(uobs.Zv[0]); Z(1,1) = Float(uobs.Zv[1]);	// ISSUE mixed type proxy assignment
	Z(1,0) = Float(Z(0,1) = sqrt(Z(0,0))*sqrt(Z(1,1))*Z_CORRELATION);
}

void uobs_model::state (const Vec& x)
{
	Float dx = TARGET[0] - x[0];
	Float dy = TARGET[1] - x[1];

	Hx.clear();
	if (RA_MODEL) {
		Float distSq = dx*dx + dy*dy;
		Float dist = sqrt (distSq);
		Hx(0,0) = -dx / dist;
		Hx(0,1) = -dy / dist;
		Hx(1,0) = +dy / distSq;
		Hx(1,1) = -dx / distSq;
	}
	else {
		Hx(0,0) = -1;
		Hx(0,1) = 0;
		Hx(1,0) = 0;
		Hx(1,1) = -1;
	}
}

void cobs_model::state (const FM::Vec& x)
{
	uobs.state (x);
	Hx = uobs.Hx;
}

const Vec& uobs_model::h (const Vec& x) const
{
	Float dx = TARGET[0] - x[0];
	Float dy = TARGET[1] - x[1];

	z_pred.clear();
	if (RA_MODEL) {
		Float distSq = dx*dx + dy*dy;
		Float dist = sqrt (distSq);

		z_pred[0] = dist;
		z_pred[1] = std::atan2 (dy, dx);
	}
	else {
		z_pred[0] = dx;
		z_pred[1] = dy;
	}
	return z_pred;
}

const Vec& cobs_model::h (const Vec& x) const
{
	return uobs.h(x);
}

void uobs_model::normalise (Vec& z_denorm, const Vec& z_from) const
{
	if (RA_MODEL) {
		z_denorm[1] = angle<Float>(z_denorm[1]).from (z_from[1]);
	}
}

void cobs_model::normalise (Vec& z_denorm, const Vec& z_from) const
{
	uobs.normalise(z_denorm, z_from);
}

/*
 * A dynamic system with noise, or a fixed additive noise
 */
class walk : private pred_model
{
public:
	walk (const Vec start, const bool fixed = false);
	void predict ();
	Vec x, x_pred, rootq;
private:
	bool m_fixed;
	Vec m_base;
};

walk::walk (const Vec start, const bool fixed) : x(NX), x_pred(NX), rootq(NQ), m_base(NX)
{
	m_fixed = fixed;
	m_base = start;
	x = m_base;
	for (Vec::const_iterator qi = q.begin(); qi != q.end(); ++qi) {
		rootq[qi.index()] = std::sqrt(*qi);
	}
}

void walk::predict ()
{
						// Correlated additive random noise
	DenseVec n(rootq.size()), nc(x.size());
	::Random.normal (n);		// independant zero mean normal
								// multiply elements by std dev
	for (DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
		*ni *= rootq[ni.index()];
	}
	nc = prod(G,n);				// correlate

	if (m_fixed) {				// Randomize based on assumed noise
		x = m_base + nc;
	}
	else {
		x_pred = f(x);
		x = x_pred;
		noalias(x) += nc;
	}
}

// Special for Information scheme using Linrz predict
class Information_linrz_scheme : public Information_scheme
{
public:
	Information_linrz_scheme (std::size_t x_size, std::size_t z_initialsize = 0) :
		Kalman_state_filter (x_size),
		Information_state_filter (x_size),
		Information_scheme (x_size, z_initialsize)
	{}
	Float predict (Linrz_predict_model& f)
	// Enforce use of Linrz predict
	{
		return Information_scheme::predict (f);
	}
};

// Filter_scheme Information_linrz_scheme specialisation
namespace Bayesian_filter {
template <>
Filter_scheme<Information_linrz_scheme>::Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) :
	Kalman_state_filter (x_size),
	Information_state_filter (x_size),
	Information_linrz_scheme (x_size, z_initialsize)
{}
}

/*
 * Filter under test. Initialised for state and covariance
 */
template <class TestScheme>
class Filter
{
public:
	Filter (const Vec& x_init, const SymMatrix& X_init);
	Filter_scheme<TestScheme> ts;
	template <class P>
	void predict (P& pmodel)
	{
		ts.predict (pmodel);
	}
	template <class O>
	void observe (O& omodel, const Vec& z)
	{
		ts.observe (omodel, z);
	}
	void update ()
	{
		ts.update ();
	}
	const Vec& x()
	{	return ts.x;
	}
	const SymMatrix& X()
	{	return ts.X;
	}
	void dump_state()
	{	// output any additional state variables
	}
};

template <class TestScheme>
Filter<TestScheme>::Filter (const Vec& x_init, const SymMatrix& X_init) :
	ts (x_init.size(), NQ, NZ)
{
	ts.init_kalman (x_init, X_init);
}

// Specialise for SIR_kalman
template <>
Filter<SIR_kalman_scheme>::Filter (const Vec& x_init, const SymMatrix& X_init) :
	ts (x_init.size(), NS, ::Random2)
{
	ts.init_kalman (x_init, X_init);
}

// dump_state specialisations
template <>
void Filter<Information_root_scheme>::dump_state()
{	// output any additional state variables
	std::cout << ts.r << ts.R << std::endl;
}

template <>
void Filter<Unscented_scheme>::dump_state()
{	// output any additional state variables
	std::cout << ts.XX << std::endl;
}

/*
 * Compare Two filters
 */

template<class Tf1, class Tf2>
class CCompare
{
public:
	CCompare (const Vec x_init, const SymMatrix X_init, unsigned nIterations);
private:
	void doIt (unsigned nIterations);
	// Attributes
	Tf1 f1;
	Tf2 f2;
	walk truth;
	pred_model f;
	uobs_model uh;
	cobs_model ch;

	// Implementation
	void dumpCompare ();

	Vec ztrue, z;

	DenseVec f1_xpred, f2_xpred;
	DenseSymMatrix f1_Xpred;
	DenseSymMatrix f2_Xpred;
};

template<class Tf1, class Tf2>
CCompare<Tf1,Tf2>::CCompare (const Vec x_init, const SymMatrix X_init, unsigned nIterations) :
	f1(x_init, X_init),
	f2(x_init, X_init),
	truth (x_init, TRUTH_STATIONARY),
	uh(),
	ch(uh),
	ztrue(NZ), z(NZ),
	f1_xpred (NX),
	f2_xpred (NX),
	f1_Xpred (NX, NX),
	f2_Xpred (NX, NX)
{
	// Initialises test variables
	z.clear();

	f1_xpred.clear();
	f2_xpred.clear();
	f1_Xpred.clear();
	f2_Xpred.clear();

	doIt (nIterations);
}

template<class Tf1, class Tf2>
void CCompare<Tf1, Tf2>::dumpCompare ()
{
	// Additional Scheme state
	//f1.dump_state(); f2.dump_state();

	Float zx, zy;
	if (RA_MODEL) {
		zx = truth.x[0] + z[0] * cos (z[1]);
		zy = truth.x[1] + z[0] * sin (z[1]);
	}
	else {
		zx = truth.x[0] + z[0];
		zy = truth.x[1] + z[1];
	}

	// Comparison
	{
		using std::cout; using std::endl;
		using boost::format;
		const Vec& f1x = f1.x(); const SymMatrix& f1X = f1.X();
		const Vec& f2x = f2.x(); const SymMatrix& f2X = f2.X();

		// Comparison and truth line
		//	x(0)diff, x(1)diff, truth.x(0), truth.x(1), zx, zy)
		cout << format("*%11.4g %11.4g * %10.3f %10.3f  %10.3f %10.3f")
			 	% (f1x[0]-f2x[0]) % (f1x[1]-f2x[1])
			 	% Float(truth.x[0]) % Float(truth.x[1]) % zx % zy << endl;	// ISSUE mixed type proxy assignment

		format state(" %11.4g %11.4g * %10.3f %10.3f");
		format covariance(" %12.4e %12.4e %12.4e");

		// Filter f1 performance
		//		x[0]err, x[1]err, x[0], x[1],  Xpred*3, X*3
		cout << state % (f1x[0]-truth.x[0]) % (f1x[1]-truth.x[1])
				% f1x[0] % f1x[1] << endl;
		cout << covariance % f1_Xpred(0,0) % f1_Xpred(1,1) % f1_Xpred(1,0);
		cout << covariance % f1X(0,0) % f1X(1,1) % f1X(1,0) << endl;
		// Filter f2 performance
		//		x[0]err, x[1]err, x[0], x[1], x[01]dist,  Xpred*3, X*3
		cout << state % (f2x[0]-truth.x[0]) % (f2x[1]-truth.x[1])
				% f2x[0] % f2x[1] << endl;
		cout << covariance % f2_Xpred(0,0) % f2_Xpred(1,1) % f2_Xpred(1,0);
		cout << covariance % f2X(0,0) % f2X(1,1) % f2X(1,0) << endl;
	}
}

template<class Tf1, class Tf2>
void CCompare<Tf1, Tf2>::doIt (unsigned nIterations)
{
	// Update the filter x,X representation
	f1.update (); f2.update ();
	z = ztrue = uh.h(truth.x);
	dumpCompare();

	for (unsigned i = 0; i < nIterations; i++ ) {
		truth.predict ();		// Predict truth model
		f1.predict (f);			// Predict filters
		f2.predict (f);

		// Update the filter
		f1.update (); f2.update ();

		f1_xpred = f1.x(); f2_xpred = f2.x();
		f1_Xpred = f1.X(); f2_Xpred = f2.X();

		// Observation, true and Randomize based on an uncorrelated noise model
		ztrue = uh.h(truth.x);
		if (NOISE_MODEL) {
			z[0] = Random.normal (ztrue[0], sqrt(uh.Zv[0]) );
			z[1] = Random.normal (ztrue[1], sqrt(uh.Zv[1]) );
		}
		else
			z = ztrue;

		// Observe using model linearised about filter state estimate
		if (Z_CORRELATION == 0)
		{
			uh.state(f1.x()); f1.observe (uh, z);
			uh.state(f2.x()); f2.observe (uh, z);
		}
		else
		{
			ch.state(f1.x()); f1.observe (ch, z);
			ch.state(f2.x()); f2.observe (ch, z);
		}

		// Update the filter
		f1.update (); f2.update ();

		dumpCompare();
		// DEBUG char c;std::cin>>c;
	}
}

int main()
{
	// Other things I might want to test
	extern void other_tests();
	other_tests();

	// Use a know sequence for comparisons between systems
	Random.seed();
	Random2.seed();

	// Setup the test filters
		// Cartesian start position (in meters)
	Vec x_init (NX);
	x_init.clear();
 	x_init[0] = INIT_XY[0];
	x_init[1] = INIT_XY[1];
		// Initial state covariance, correlated
	SymMatrix X_init (NX, NX);
	FM::identity (X_init);
	X_init(0,0) = sqr(INIT_X_NOISE);
	X_init(1,1) = sqr(INIT_Y_NOISE);
	X_init(1,0) = X_init(0,1) = INIT_X_NOISE*INIT_Y_NOISE*INIT_XY_NOISE_CORRELATION;
	// Additional state correlation is useful for testing
		X_init(2,2) = sqr(INIT_2_NOISE);
	X_init(2,0) = X_init(0,2) = INIT_X_NOISE*INIT_2_NOISE*INIT_2_NOISE_CORRELATION;
	X_init(2,1) = X_init(1,2) = INIT_Y_NOISE*INIT_2_NOISE*INIT_2_NOISE_CORRELATION;

	// Initialise and do the comparison
	std::cout << "udfilter, ufilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<UD_scheme>, Filter<Unscented_scheme> > test1(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "cfilter, ifilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<Covariance_scheme>, Filter<Information_linrz_scheme> > test2(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "irfilter, ilfilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<Information_root_scheme>, Filter<Information_scheme> > test3(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "sfilter, itfilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<SIR_kalman_scheme>, Filter<Iterated_covariance_scheme> > test4(x_init, X_init, 4);
	std::cout << std::endl;

	return 0;
}


#include <iostream>
#include <string>
#include <vector>
#include <fstream>

#include <boost/timer.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sophus/se3.h>

/* parameters */
const int boarder = 20;         // boundary
const int width = 640;
const int height = 480;

const double fx = 481.2f;
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;

const int ncc_window_size = 2;  // NCC window width half size
const int ncc_area = (2*ncc_window_size + 1) * (2*ncc_window_size + 1);     // NCC window area size

const double min_cov = 0.1f;    // convergence condition: minimum variance
const double max_cov = 10.f;    // divergence  condition: maximum variance

/* functions declaration */
bool readDatasetFiles(
    const std::string& path,
    std::vector<std::string>& color_image_files,
    std::vector<Sophus::SE3>& poses
);

bool update(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Sophus::SE3& T_c_r,
    cv::Mat& depth,
    cv::Mat& depth_cov
);

bool epipolarSearch(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Sophus::SE3& T_c_r,
    const Eigen::Vector2d& pt_ref,
    const double& depth_mu,
    const double& depth_cov,
    Eigen::Vector2d& pt_curr
);

bool updateDepthFilter(
    const Eigen::Vector2d& pt_ref,
    const Eigen::Vector2d& pt_curr,
    const Sophus::SE3& T_c_r,
    cv::Mat& depth,
    cv::Mat& depth_cov
);

double computeNCC(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& pt_ref,
    const Eigen::Vector2d& pt_curr
);

inline double getBilinearInterpolatedValue(const cv::Mat& image, const Eigen::Vector2d& pt)
{
    uchar* d = & image.data[int(pt.y()) * image.step + int(pt.x())];

    double xx = pt.x() - floor(pt.x());
    double yy = pt.y() - floor(pt.y());

    return ( (1.f-xx) * (1.f-yy) * double(d[0]) +
                xx    * (1.f-yy) * double(d[1]) +
             (1.f-xx) *    yy    * double(d[image.step]) +
                xx    *    yy    * double(d[image.step + 1]) ) / 255.0;
}

/* some tools functions */
void plotDepth(const cv::Mat& depth);

inline Eigen::Vector3d px2cam(const Eigen::Vector2d px)
{
    return Eigen::Vector3d(
        (px.x() - cx) / fx,
        (px.y() - cy) / fy,
        1.0
    );
}

inline Eigen::Vector2d cam2px(const Eigen::Vector3d p_cam)
{
    return Eigen::Vector2d(
        p_cam.x() * fx / p_cam.z() + cx,
        p_cam.y() * fy / p_cam.z() + cy
    );
}

inline bool inside(const Eigen::Vector2d& pt)
{
    return (pt.x() >= boarder && pt.y() >= boarder
        &&  pt.x() + boarder <= width && pt.y() + boarder <= height);
}

void showEpipolarMatch(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& px_ref,
    const Eigen::Vector2d& px_curr
);

void showEpipolarLine(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& px_ref,
    const Eigen::Vector2d& px_mincurr,
    const Eigen::Vector2d& px_maxcurr
);

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: dense_mapping_monocular <path_to_test_dataset>" << std::endl;
        exit(EXIT_FAILURE);
    }

    // read the data form the dataset
    std::vector<std::string> color_image_files;
    std::vector<Sophus::SE3> poses_T_w_c;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_T_w_c);
    if (ret == false) {
        std::cout << "Reading files failed" << std::endl;
        exit(EXIT_FAILURE);
    }

    std::cout << "read total " << color_image_files.size() << " files." << std::endl;

    // first image
    cv::Mat ref = cv::imread(color_image_files[0], 0);      // garg-scale image
    Sophus::SE3 pose_ref_Twc = poses_T_w_c[0];
    double init_depth = 3.0;
    double init_cov2 = 3.0;
    cv::Mat depth(height, width, CV_64F, init_depth);
    cv::Mat depth_cov(height, width, CV_64F, init_cov2);

    for (int index = 1; index < color_image_files.size(); index ++) {
        std::cout << "*** loop " << index << " ***" << std::endl;
        cv::Mat curr = cv::imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        Sophus::SE3 pose_curr_Twc = poses_T_w_c[index];
        Sophus::SE3 pose_curr_ref = pose_curr_Twc.inverse() * pose_ref_Twc;
        update(ref, curr, pose_curr_ref, depth, depth_cov);
        plotDepth(depth);
        cv::imshow("image", curr);
        cv::waitKey(1);
    }

    std::cout << "estimation returens, saving depth map ..." << std::endl;
    cv::imwrite("depth.png", depth);
    std::cout << "done." << std::endl;

    return 0u;
}

bool readDatasetFiles(const std::string& path, std::vector<std::string>& color_image_files, std::vector<Sophus::SE3>& poses)
{
    std::ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while(!fin.eof()) {
        /* the form of dataset : image's name, tx, ty, tz, qx, qy, qz, qw. NOTE: the transformation matrix is camera to world */
        std::string image_name;
        fin >> image_name;

        double data[7];
        for (double& d : data) fin >> d;

        color_image_files.push_back(path + std::string("/images/") + image_name);
        poses.push_back(
            Sophus::SE3(Sophus::Quaterniond(data[6], data[3], data[4], data[5]),
                        Sophus::Vector3d(data[0], data[1], data[2]))
        );

        if (!fin.good()) break;
    }

    return true;
}

/* update the global depth map */
bool update(const cv::Mat& ref, const cv::Mat& curr, const Sophus::SE3& T_c_r, cv::Mat& depth, cv::Mat& depth_cov)
{
#pragma omp parallel for
    for (int x = boarder; x < width - boarder; x++) {
#pragma omp parallel for
        for (int y = boarder; y < height - boarder; y++) {
            // Traversing every pixel
            if (depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov)     // the depth was convergence / divergence
                continue;

            // match the pixel in the epipolar between the reference and the current
            Eigen::Vector2d pt_curr;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_c_r,
                Eigen::Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov.ptr<double>(y)[x]),
                pt_curr
            );

            if (ret == false)   // match false
                continue;

            // 取消该注释以显示匹配
//             showEpipolarMatch(ref, curr, Eigen::Vector2d(x, y), pt_curr);

            // update the depth map
            updateDepthFilter(Eigen::Vector2d(x, y), pt_curr, T_c_r, depth, depth_cov);
        }
    }
    return true;
}

bool epipolarSearch(const cv::Mat& ref, const cv::Mat& curr, const Sophus::SE3& T_c_r, const Eigen::Vector2d& pt_ref, const double& depth_mu, const double& depth_cov, Eigen::Vector2d& pt_curr)
{
    Eigen::Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Eigen::Vector3d p_ref = f_ref*depth_mu;

    Eigen::Vector2d px_mean_curr = cam2px(T_c_r * p_ref);           // 按深度均值投影的像素
    double d_min = depth_mu - 3*depth_cov, d_max = depth_mu + 3*depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Eigen::Vector2d px_min_curr = cam2px(T_c_r * (f_ref * d_min));  // 按最小深度投影的像素
    Eigen::Vector2d px_max_curr = cam2px(T_c_r * (f_ref * d_max));  // 按最大深度投影的像素

    Eigen::Vector2d epipolar_line = px_max_curr - px_min_curr;      // 极线（线段形式）
    Eigen::Vector2d epipolar_direction = epipolar_line;             // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();                // 极线线段的半长度
    if (half_length > 100) half_length = 100;                       // 我们不希望搜索太多东西

    // if the epipolar line length <= 1.5 pixel, we regard that the mean point is the matching point
    if (half_length <= 1.5) {
        pt_curr = px_mean_curr;
        if (!inside(pt_curr))
            return false;
        return true;
    }

    // 取消此句注释以显示极线（线段）
//     showEpipolarLine(ref, curr, pt_ref, px_min_curr, px_max_curr);

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Eigen::Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7){      // l += sqrt(2)
        Eigen::Vector2d px_curr = px_mean_curr + l * epipolar_direction;    // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = computeNCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)       // 只相信 NCC 很高的匹配
        return false;

    pt_curr = best_px_curr;

    return true;
}

double computeNCC(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& pt_ref, const Eigen::Vector2d& pt_curr)
{
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    std::vector<double> values_ref, values_curr;    // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x ++) {
        for (int y = -ncc_window_size; y <= ncc_window_size; y ++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref.y()))[int(x + pt_ref.x())]) / 255.0;
            mean_ref += value_ref;

//             double value_curr = double(curr.ptr<uchar>(int(y + pt_curr.y()))[int(x + pt_curr.x())]) / 255.0;
            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Eigen::Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }
    }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // compute Zero mean NCC
    double numerator = 0, denominator1 = 0, denominator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        denominator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        denominator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(denominator1 * denominator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(const Eigen::Vector2d& pt_ref, const Eigen::Vector2d& pt_curr, const Sophus::SE3& T_c_r, cv::Mat& depth, cv::Mat& depth_cov)
{
    // 用三角化计算深度
    Sophus::SE3 T_r_c = T_c_r.inverse();
    Eigen::Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Eigen::Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    /**
     * function:
     * d_ref * f_ref = d_cur * (R_r_c * f_cur) + t_r_c
     * => [f_ref^T f_ref, -f_ref^T f_cur] [d_ref] = [f_ref^T t]
     *    [f_cur^T f_ref, -f_cur^T f_cur] [d_cur] = [f_cur^T t]
     * 二阶方程用克莱默法则求解并解之
     */
    Eigen::Vector3d t = T_r_c.translation();
    Eigen::Vector3d f2 = T_r_c.rotation_matrix() * f_curr;
    Eigen::Vector2d b = Eigen::Vector2d(t.dot(f_ref), t.dot(f2));

    double A[4];
    A[0] = f_ref.dot(f_ref);
    A[2] = f_ref.dot(f2);
    A[1] = -A[2];
    A[3] = -f2.dot(f2);

    double d = A[0]*A[3] - A[1]*A[2];
    Eigen::Vector2d lambda_vec = Eigen::Vector2d(
        A[3]*b(0, 0) - A[1]*b(1, 0),
        A[0]*b(1, 0) - A[2]*b(0, 0)) / d;

    Eigen::Vector3d xm = lambda_vec(0, 0) * f_ref;
    Eigen::Vector3d xn = lambda_vec(1, 0) * f2 + t;
    Eigen::Vector3d d_esti = (xm + xn) / 2.0;       // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();        // 深度值

    // 计算不确定性（以一个像素为误差）
    Eigen::Vector3d p = f_ref*depth_estimation;
    Eigen::Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(a.dot(-t) / a_norm / t_norm);
    double beta_prime = beta + atan(1.0 / fx / 2.0) * 2.0;
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())];
    double sigma2 = depth_cov.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())];

    double mu_fuse = (d_cov2*mu + sigma2*depth_estimation) / (d_cov2 + sigma2);
    double sigma2_fuse = (d_cov2*sigma2) / (d_cov2 + sigma2);

    depth.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())] = mu_fuse;
    depth_cov.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())] = sigma2_fuse;

    return true;
}

void plotDepth(const cv::Mat& depth)
{
    cv::imshow("depth", depth * 0.4);
    cv::waitKey(1);
}

void showEpipolarMatch(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& px_ref, const Eigen::Vector2d& px_curr)
{
    cv::Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref.x(), px_ref.y()), 5, cv::Scalar(0, 0, 250), 2);
    cv::circle(curr_show, cv::Point2f(px_curr.x(), px_curr.y()), 5, cv::Scalar(0, 0, 250), 2);

    cv::imshow("ref", ref_show);
    cv::imshow("curr", curr_show);
    cv::waitKey(1);
}

void showEpipolarLine(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& px_ref, const Eigen::Vector2d& px_mincurr, const Eigen::Vector2d& px_maxcurr)
{
    cv::Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref.x(), px_ref.y()), 5, cv::Scalar(0, 250, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_mincurr.x(), px_mincurr.y()), 5, cv::Scalar(0, 250, 2), 2);
    cv::circle(curr_show, cv::Point2f(px_maxcurr.x(), px_maxcurr.y()), 5, cv::Scalar(0, 250, 2), 2);
    cv::line(curr_show, cv::Point2f(px_mincurr.x(), px_mincurr.y()), cv::Point2f(px_maxcurr.x(), px_maxcurr.y()), cv::Scalar(0, 250, 0), 1);

    cv::imshow("ref", ref_show);
    cv::imshow("curr", curr_show);
    cv::waitKey(1);
}