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

// @sect3{Include files}

// 同样，前几个include文件已经知道了，所以我们不会对它们进行评论。

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

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.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_tools.h>

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

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

// 现在，这些是多级方法所需的包括。第一个声明了如何处理多网格方法每个层次上的Dirichlet边界条件。对于自由度的实际描述，我们不需要任何新的包含文件，因为DoFHandler已经实现了所有必要的方法。我们只需要将自由度分配给更多的层次。

// 其余的包含文件涉及到作为线性算子（求解器或预处理器）的多重网格的力学问题。

#include <deal.II/multigrid/mg_constrained_dofs.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

// 我们将使用 MeshWorker::mesh_loop 来对单元格进行循环，所以在这里包括它。

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

// 这就是C++。

#include <iostream>
#include <fstream>

using namespace dealii;

namespace Step16
{
// @sect3{The Scratch and Copy objects}

// 我们使用 MeshWorker::mesh_loop() 来组装我们的矩阵。为此，我们需要一个ScratchData对象来存储每个单元的临时数据（这只是FEValues对象）和一个CopyData对象，它将包含每个单元装配的输出。关于scratch和copy对象的用法的更多细节，请参见WorkStream命名空间。

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

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

    FEValues<dim> fe_values;
  };

  struct CopyData
  {
    unsigned int                         level;
    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;

    template <class Iterator>
    void reinit(const Iterator &cell, unsigned int dofs_per_cell)
    {
      cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
      cell_rhs.reinit(dofs_per_cell);

      local_dof_indices.resize(dofs_per_cell);
      cell->get_active_or_mg_dof_indices(local_dof_indices);
      level = cell->level();
    }
  };
// @sect3{The LaplaceProblem class template}

// 这个主类与  step-6  中的同一类相似。就成员函数而言，唯一增加的是。

// --  assemble_multigrid 的函数，该函数组装了对应于中间层离散运算符的矩阵。

// -  cell_worker 函数，它将我们的PDE集合在一个单元上。

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

  private:
    template <class Iterator>
    void cell_worker(const Iterator &  cell,
                     ScratchData<dim> &scratch_data,
                     CopyData &        copy_data);

    void setup_system();
    void assemble_system();
    void assemble_multigrid();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

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

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    AffineConstraints<double> constraints;

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

    const unsigned int degree;

// 以下成员是多网格方法的基本数据结构。前四个表示稀疏模式和多级层次结构中各个层次的矩阵，非常类似于上面的全局网格的对象。

// 然后，我们有两个新的矩阵，只需要在自适应网格上进行局部平滑的多网格方法。它们在细化区域的内部和细化边缘之间传递数据，在 @ref mg_paper "多网格论文 "中详细介绍过。

// 最后一个对象存储了每个层次上的边界指数信息和位于两个不同细化层次之间的细化边缘上的指数信息。因此，它的作用与AffineConstraints相似，但在每个层次上。

    MGLevelObject<SparsityPattern> mg_sparsity_patterns;
    MGLevelObject<SparsityPattern> mg_interface_sparsity_patterns;

    MGLevelObject<SparseMatrix<double>> mg_matrices;
    MGLevelObject<SparseMatrix<double>> mg_interface_matrices;
    MGConstrainedDoFs                   mg_constrained_dofs;
  };
// @sect3{The LaplaceProblem class implementation}

// 关于三角形的构造函数只有一个简短的评论：按照惯例，deal.II中所有自适应精化的三角形在单元格之间的面的变化不会超过一个级别。然而，对于我们的多网格算法，我们需要一个更严格的保证，即网格在连接两个单元的顶点上的变化也不超过细化级别。换句话说，我们必须防止出现以下情况。

//  @image html limit_level_difference_at_vertices.png ""

// 这可以通过向三角化类的构造函数传递 Triangulation::limit_level_difference_at_vertices 标志来实现。

  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem(const unsigned int degree)
    : triangulation(Triangulation<dim>::limit_level_difference_at_vertices)
    , fe(degree)
    , dof_handler(triangulation)
    , degree(degree)
  {}

//  @sect4{LaplaceProblem::setup_system}

// 除了只是在DoFHandler中分配自由度之外，我们在每一层都做同样的事情。然后，我们按照之前的程序，在叶子网格上设置系统。

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

    std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (by level: ";
    for (unsigned int level = 0; level < triangulation.n_levels(); ++level)
      std::cout << dof_handler.n_dofs(level)
                << (level == triangulation.n_levels() - 1 ? ")" : ", ");
    std::cout << std::endl;

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

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

    std::set<types::boundary_id> dirichlet_boundary_ids = {0};
    Functions::ZeroFunction<dim> homogeneous_dirichlet_bc;
    const std::map<types::boundary_id, const Function<dim> *>
      dirichlet_boundary_functions = {
        {types::boundary_id(0), &homogeneous_dirichlet_bc}};
    VectorTools::interpolate_boundary_values(dof_handler,
                                             dirichlet_boundary_functions,
                                             constraints);
    constraints.close();

    {
      DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints);
      sparsity_pattern.copy_from(dsp);
    }
    system_matrix.reinit(sparsity_pattern);

// 多网格约束必须被初始化。他们需要知道在哪里规定了Dirichlet边界条件。

    mg_constrained_dofs.clear();
    mg_constrained_dofs.initialize(dof_handler);
    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler,
                                                       dirichlet_boundary_ids);

// 现在是关于多网格数据结构的事情。首先，我们调整多级对象的大小，以容纳每一级的矩阵和稀疏模式。粗略的级别是零（现在是强制性的，但在未来的修订中可能会改变）。注意，这些函数在这里采取的是一个完整的、包容的范围（而不是一个起始索引和大小），所以最细的级别是 n_levels-1  。我们首先要调整容纳SparseMatrix类的容器的大小，因为它们必须在调整大小时释放它们的SparsityPattern才能被销毁。

    const unsigned int n_levels = triangulation.n_levels();

    mg_interface_matrices.resize(0, n_levels - 1);
    mg_matrices.resize(0, n_levels - 1);
    mg_sparsity_patterns.resize(0, n_levels - 1);
    mg_interface_sparsity_patterns.resize(0, n_levels - 1);

// 现在，我们必须在每个级别上提供一个矩阵。为此，我们首先使用 MGTools::make_sparsity_pattern 函数在每个层次上生成一个初步的压缩稀疏模式（关于这个主题的更多信息，请参见 @ref Sparsity 模块），然后将其复制到我们真正想要的那一个。下一步是用拟合的稀疏度模式初始化接口矩阵。

// 值得指出的是，界面矩阵只包含位于较粗和较细的网格之间的自由度的条目。因此，它们甚至比我们的多网格层次结构中的各个层次的矩阵还要稀疏。因此，我们使用一个专门为此目的而建立的函数来生成它。

    for (unsigned int level = 0; level < n_levels; ++level)
      {
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_sparsity_pattern(dof_handler, dsp, level);

          mg_sparsity_patterns[level].copy_from(dsp);
          mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        }
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_interface_sparsity_pattern(dof_handler,
                                                   mg_constrained_dofs,
                                                   dsp,
                                                   level);
          mg_interface_sparsity_patterns[level].copy_from(dsp);
          mg_interface_matrices[level].reinit(
            mg_interface_sparsity_patterns[level]);
        }
      }
  }
// @sect4{LaplaceProblem::cell_worker}

// cell_worker函数用于在给定的单元上组装矩阵和右手边。这个函数用于活动单元生成system_matrix，并在每个层次上建立层次矩阵。

// 注意，当从assemble_multigrid()调用时，我们也会组装一个右手边，尽管它没有被使用。

  template <int dim>
  template <class Iterator>
  void LaplaceProblem<dim>::cell_worker(const Iterator &  cell,
                                        ScratchData<dim> &scratch_data,
                                        CopyData &        copy_data)
  {
    FEValues<dim> &fe_values = scratch_data.fe_values;
    fe_values.reinit(cell);

    const unsigned int dofs_per_cell = fe_values.get_fe().n_dofs_per_cell();
    const unsigned int n_q_points    = fe_values.get_quadrature().size();

    copy_data.reinit(cell, dofs_per_cell);

    const std::vector<double> &JxW = fe_values.get_JxW_values();

    for (unsigned int q = 0; q < n_q_points; ++q)
      {
        const double coefficient =
          (fe_values.get_quadrature_points()[q][0] < 0.0) ? 1.0 : 0.1;

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                copy_data.cell_matrix(i, j) +=
                  coefficient *
                  (fe_values.shape_grad(i, q) * fe_values.shape_grad(j, q)) *
                  JxW[q];
              }
            copy_data.cell_rhs(i) += 1.0 * fe_values.shape_value(i, q) * JxW[q];
          }
      }
  }

//  @sect4{LaplaceProblem::assemble_system}

// 下面的函数将线性系统集合在网格的活动单元上。为此，我们向Mesh_loop()函数传递两个lambda函数。cell_worker函数重定向到同名的类成员函数，而copyer是这个函数特有的，它使用约束条件将本地矩阵和向量复制到相应的全局矩阵。

  template <int dim>
  void LaplaceProblem<dim>::assemble_system()
  {
    MappingQ1<dim> mapping;

    auto cell_worker =
      [&](const typename DoFHandler<dim>::active_cell_iterator &cell,
          ScratchData<dim> &                                    scratch_data,
          CopyData &                                            copy_data) {
        this->cell_worker(cell, scratch_data, copy_data);
      };

    auto copier = [&](const CopyData &cd) {
      this->constraints.distribute_local_to_global(cd.cell_matrix,
                                                   cd.cell_rhs,
                                                   cd.local_dof_indices,
                                                   system_matrix,
                                                   system_rhs);
    };

    const unsigned int n_gauss_points = degree + 1;

    ScratchData<dim> scratch_data(mapping,
                                  fe,
                                  n_gauss_points,
                                  update_values | update_gradients |
                                    update_JxW_values |
                                    update_quadrature_points);

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          CopyData(),
                          MeshWorker::assemble_own_cells);
  }
// @sect4{LaplaceProblem::assemble_multigrid}

// 下一个函数是建立矩阵，定义每一层网格上的多网格方法。集成的核心与上面的相同，但是下面的循环将遍历所有已存在的单元，而不仅仅是活动的单元，并且必须将结果输入正确的层矩阵。幸运的是，MeshWorker对我们隐藏了大部分的内容，因此这个函数和之前的函数的区别只在于汇编器的设置和循环中的不同迭代器。

// 我们为每个层次生成一个AffineConstraints对象，其中包含边界和界面道夫作为约束条目。然后，相应的对象被用来生成层次矩阵。

  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid()
  {
    MappingQ1<dim>     mapping;
    const unsigned int n_levels = triangulation.n_levels();

    std::vector<AffineConstraints<double>> boundary_constraints(n_levels);
    for (unsigned int level = 0; level < n_levels; ++level)
      {
        IndexSet dofset;
        DoFTools::extract_locally_relevant_level_dofs(dof_handler,
                                                      level,
                                                      dofset);
        boundary_constraints[level].reinit(dofset);
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_refinement_edge_indices(level));
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_boundary_indices(level));
        boundary_constraints[level].close();
      }

    auto cell_worker =
      [&](const typename DoFHandler<dim>::level_cell_iterator &cell,
          ScratchData<dim> &                                   scratch_data,
          CopyData &                                           copy_data) {
        this->cell_worker(cell, scratch_data, copy_data);
      };

    auto copier = [&](const CopyData &cd) {
      boundary_constraints[cd.level].distribute_local_to_global(
        cd.cell_matrix, cd.local_dof_indices, mg_matrices[cd.level]);

      const unsigned int dofs_per_cell = cd.local_dof_indices.size();

// 接口条目在填充mg_matrices[cd.level]时被上面的boundary_constraints对象所忽略。相反，我们手动将这些条目复制到当前级别的界面矩阵中。

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
          if (mg_constrained_dofs.is_interface_matrix_entry(
                cd.level, cd.local_dof_indices[i], cd.local_dof_indices[j]))
            {
              mg_interface_matrices[cd.level].add(cd.local_dof_indices[i],
                                                  cd.local_dof_indices[j],
                                                  cd.cell_matrix(i, j));
            }
    };

    const unsigned int n_gauss_points = degree + 1;

    ScratchData<dim> scratch_data(mapping,
                                  fe,
                                  n_gauss_points,
                                  update_values | update_gradients |
                                    update_JxW_values |
                                    update_quadrature_points);

    MeshWorker::mesh_loop(dof_handler.begin_mg(),
                          dof_handler.end_mg(),
                          cell_worker,
                          copier,
                          scratch_data,
                          CopyData(),
                          MeshWorker::assemble_own_cells);
  }

//  @sect4{LaplaceProblem::solve}

// 这是另外一个在支持多栅求解器（或者说，事实上，我们使用多栅方法的前提条件）方面有明显不同的函数。

// 让我们从建立多层次方法的两个组成部分开始：层次间的转移运算器和最粗层次上的求解器。在有限元方法中，转移算子来自所涉及的有限元函数空间，通常可以用独立于所考虑问题的通用方式计算。在这种情况下，我们可以使用MGTransferPrebuilt类，给定最终线性系统的约束和MGConstrainedDoFs对象，该对象知道每个层次的边界条件和不同细化层次之间接口的自由度，可以从具有层次自由度的DoFHandler对象中建立这些转移操作的矩阵。

// 下面几行的第二部分是关于粗略网格求解器的。由于我们的粗网格确实非常粗，我们决定采用直接求解器（最粗层次矩阵的Householder分解），即使其实现不是特别复杂。如果我们的粗网格比这里的5个单元多得多，那么这里显然需要更合适的东西。

  template <int dim>
  void LaplaceProblem<dim>::solve()
  {
    MGTransferPrebuilt<Vector<double>> mg_transfer(mg_constrained_dofs);
    mg_transfer.build(dof_handler);

    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from(mg_matrices[0]);
    MGCoarseGridHouseholder<double, Vector<double>> coarse_grid_solver;
    coarse_grid_solver.initialize(coarse_matrix);

// 多级求解器或预处理器的下一个组成部分是，我们需要在每一级上设置平滑器。这方面常见的选择是使用松弛方法的应用（如SOR、Jacobi或Richardson方法）或求解器方法的少量迭代（如CG或GMRES）。 mg::SmootherRelaxation 和MGSmootherPrecondition类为这两种平滑器提供支持。这里，我们选择应用单一的SOR迭代。为此，我们定义一个适当的别名，然后设置一个平滑器对象。

// 最后一步是用我们的水平矩阵初始化平滑器对象，并设置一些平滑参数。 initialize() 函数可以有选择地接受额外的参数，这些参数将被传递给每一级的平滑器对象。在目前SOR平滑器的情况下，这可能包括一个松弛参数。然而，我们在这里将这些参数保留为默认值。对 set_steps() 的调用表明我们将在每个级别上使用两个前平滑步骤和两个后平滑步骤；为了在不同级别上使用可变数量的平滑器步骤，可以在对 mg_smoother 对象的构造函数调用中设置更多选项。

// 最后一步的结果是我们使用SOR方法作为平滑器的事实

// --这不是对称的

// 但我们在下面使用共轭梯度迭代（需要对称的预处理），我们需要让多级预处理确保我们得到一个对称的算子，即使是非对称的平滑器。

    using Smoother = PreconditionSOR<SparseMatrix<double>>;
    mg::SmootherRelaxation<Smoother, Vector<double>> mg_smoother;
    mg_smoother.initialize(mg_matrices);
    mg_smoother.set_steps(2);
    mg_smoother.set_symmetric(true);

// 下一个准备步骤是，我们必须将我们的水平和接口矩阵包裹在一个具有所需乘法函数的对象中。我们将为从粗到细的接口对象创建两个对象，反之亦然；多网格算法将在后面的操作中使用转置运算器，允许我们用已经建立的矩阵初始化该运算器的上下版本。

    mg::Matrix<Vector<double>> mg_matrix(mg_matrices);
    mg::Matrix<Vector<double>> mg_interface_up(mg_interface_matrices);
    mg::Matrix<Vector<double>> mg_interface_down(mg_interface_matrices);

// 现在，我们准备设置V型循环算子和多级预处理程序。

    Multigrid<Vector<double>> mg(
      mg_matrix, coarse_grid_solver, mg_transfer, mg_smoother, mg_smoother);
    mg.set_edge_matrices(mg_interface_down, mg_interface_up);

    PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double>>>
      preconditioner(dof_handler, mg, mg_transfer);

// 有了这一切，我们终于可以用通常的方法来解决这个线性系统了。

    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> solver(solver_control);

    solution = 0;

    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    std::cout << "   Number of CG iterations: " << solver_control.last_step()
              << "\n"
              << std::endl;
    constraints.distribute(solution);
  }

//  @sect4{Postprocessing}

// 以下两个函数在计算出解决方案后对其进行后处理。特别是，第一个函数在每个周期开始时细化网格，第二个函数在每个周期结束时输出结果。这些函数与  step-6  中的函数几乎没有变化。

  template <int dim>
  void LaplaceProblem<dim>::refine_grid()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 2),
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.03);
    triangulation.execute_coarsening_and_refinement();
  }

  template <int dim>
  void LaplaceProblem<dim>::output_results(const unsigned int cycle) const
  {
    DataOut<dim> data_out;

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

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{LaplaceProblem::run}

// 和上面的几个函数一样，这几乎是对  step-6  中相应函数的复制。唯一的区别是对 assemble_multigrid 的调用，它负责形成我们在多网格方法中需要的每一层的矩阵。

  template <int dim>
  void LaplaceProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 8; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_ball(triangulation);
            triangulation.refine_global(2);
          }
        else
          refine_grid();

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

        setup_system();

        assemble_system();
        assemble_multigrid();

        solve();
        output_results(cycle);
      }
  }
} // namespace Step16
// @sect3{The main() function}

// 这又是与 step-6 中相同的函数。

int main()
{
  try
    {
      using namespace Step16;

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

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

  return 0;
}


#include <boost/bind.hpp>

#include <ql/time/date.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/integrals/segmentintegral.hpp>

#include <calibrator/models/shortrate/twofactormodels/generalg2.hpp>

namespace HJCALIBRATOR
{
	GeneralizedG2::GeneralizedG2( boost::shared_ptr<Gaussian2FactorDynamics> dynamics,
								  Real integralSignificance,
								  boost::shared_ptr<Integrator> integrator )
		: TwoFactorModel( 5 )
		, AffineModel()
		, TermStructureConsistentModel( dynamics->termStructure() )
		, a_( arguments_[0] ), sigma_( arguments_[1] )
		, b_( arguments_[2] ), eta_( arguments_[3] )
		, rho_( arguments_[4] )
		, integralSignificance_( integralSignificance )
		, integrator_( integrator )
		, dynamics_( dynamics )
	{
		a_ = dynamics->a(0);
		b_ = dynamics->a(1);
		sigma_ = dynamics->sigma(0);
		eta_ = dynamics->sigma(1);
		rho_ = dynamics->rho( 0, 1 );

		generateArguments();

		registerWith( dynamics->termStructure() );
	}

	void GeneralizedG2::generateArguments()
	{
		dynamics_->a( a_, 0 );
		dynamics_->sigma( sigma_, 0 );
		dynamics_->a( b_, 1 );
		dynamics_->sigma( eta_, 1 );
		dynamics_->rho( rho_, 1, 0 );
	}

	Real GeneralizedG2::A( Time t, Time T ) const
	{
		return dynamics_->A( t, T );
	}

	Real GeneralizedG2::discountBond( Time now,
									  Time maturity,
									  Array factors ) const
	{
		Real x = factors[0];
		Real y = factors[1];

		Real Bx = dynamics_->B( 0, now, maturity );
		Real By = dynamics_->B( 1, now, maturity );

		return A( now, maturity ) * exp( - Bx * x - By * y );
	}

	Real GeneralizedG2::discountBondOption( Option::Type type,
											Real strike,
											Time maturity,
											Time bondMaturity ) const
	{
		auto dynamics = dynamics_;

		Real Bx = dynamics->B( 0, maturity, bondMaturity );
		Real By = dynamics->B( 1, maturity, bondMaturity );
		Real rho = dynamics->rho( 0, 1 )(0.0);

		Real Vpratio = 0;

		auto integrand00 = [maturity, dynamics]( Time u )
		{
			Real sigma = dynamics->sigma( 0 )(u);
			Real E = dynamics->E( 0, u, maturity );

			return sigma * sigma / E / E;
		};

		Vpratio += Bx * By * integrator_->operator()( integrand00, 0, maturity );

		auto integrand11 = [maturity, dynamics]( Time u )
		{
			Real sigma = dynamics->sigma( 1 )(u);
			Real E = dynamics->E( 1, u, maturity );

			return sigma * sigma / E / E;
		};

		Vpratio += Bx * By * integrator_->operator()( integrand11, 0, maturity );

		auto integrand01 = [maturity, dynamics]( Time u )
		{
			return dynamics->sigma( 0 )(u) * dynamics->sigma( 1 )(u)
				/ dynamics->E( 0, u, maturity ) / dynamics->E( 1, u, maturity );
		};

		Vpratio += 2 * Bx * Bx * rho * integrator_->operator()( integrand01, 0, maturity );

		Real stdDev = sqrt( std::max( Vpratio, 0.0 ) );

		Real f = termStructure()->discount( bondMaturity );
		Real k = termStructure()->discount( maturity )*strike;

		return blackFormula( type, k, f, stdDev );
	}

	Real GeneralizedG2::discountBondOption( Option::Type type,
											Real strike,
											Time maturity,
											Time bondStart,
											Time bondMaturity ) const
	{
		return discountBondOption( type, strike, bondStart, bondMaturity );
	}

	// Brigo Ch. 4.2
	Real GeneralizedG2::swaption( const Swaption::arguments& arg, Real strike ) const
	{
		Date settlement = termStructure()->referenceDate();
		DayCounter dayCounter = termStructure()->dayCounter();
		Time T = dayCounter.yearFraction( settlement,
											  arg.floatingResetDates[0] );
		Real w = (arg.type == VanillaSwap::Payer ? 1 : -1);

		std::vector<Time> t;
		for ( auto fixedPayDate : arg.fixedPayDates )
		{
			t.push_back( dayCounter.yearFraction( settlement,
													fixedPayDate ) );
		}
		Size N_timestep = t.size();

		Array cA( N_timestep );
		Array Bx( N_timestep );
		Array By( N_timestep );

		for ( Size i = 0; i < N_timestep; i++ )
		{
			Time tau_i = i == 0 ? t[i] - T : t[i] - t[i - 1];
			Real c = i == N_timestep - 1 ? 1 + strike * tau_i : strike * tau_i;
			cA[i] = c * dynamics_->A( T, t[i] );
			Bx[i] = dynamics_->B( 0, T, t[i] );
			By[i] = dynamics_->B( 1, T, t[i]);
		}

		Real mu_x = dynamics_->meanTforward( 0, T, 0, T );
		Real mu_y = dynamics_->meanTforward( 1, T, 0, T );
		Real sigma_x = sqrt(dynamics_->variance( 0, 0, 0, T ));
		Real sigma_y = sqrt(dynamics_->variance( 1, 1, 0, T ));
		Real rho = dynamics_->rho( 0, 1 )(0.0);
		Real var = dynamics_->variance( 0, 1, 0, T );
		Real rho_xy = rho * var / sigma_x / sigma_y;
		Real rhosqrt = sqrt( 1 - rho_xy * rho_xy );

		auto integrand = [&, N_timestep, w, mu_x, mu_y, sigma_x, sigma_y, rho_xy, rhosqrt]( Real x )
		{
			Real dev = (x - mu_x) / sigma_x;

			Array lambda( N_timestep );
			Array kappa( N_timestep );
			for ( Size i = 0; i < N_timestep; i++ )
			{
				lambda[i] = cA[i] * exp( -Bx[i] * x );
				kappa[i] = -By[i] * (mu_y - 0.5 * rhosqrt * rhosqrt * sigma_y * sigma_y * By[i]
											+ rho_xy * sigma_y * (x - mu_x) / sigma_x);
			}

			auto hyperplane = [N_timestep, &lambda, &By]( Real y )
			{
				Real value = 1.;
				for ( Size i = 0; i < N_timestep; i++ )
				{
					Real val = lambda[i] * exp( -By[i] * y );
					value -= val;
				}

				return value;
			};

			Brent solver;
			solver.setMaxEvaluations( 1000 );
			Real ybar = solver.solve( hyperplane, 1e-6, 0.00, -100.0, 100.0 );

			Real h1 = (ybar - mu_y) / (sigma_y * rhosqrt)
				- rho_xy * (x - mu_x) / (sigma_x * rhosqrt);

			CumulativeNormalDistribution Phi;
			Real val = Phi( -w * h1 );
			for ( Size i = 0; i < N_timestep; i++ )
			{
				Real h2 = h1 + By[i] * sigma_y * rhosqrt;

				val -= lambda[i] * exp( kappa[i] ) * Phi( -w * h2 );
			}

			return exp( -0.5*dev*dev ) * val;
		};

		Real N = arg.nominal;
		Real P0T = termStructure()->discount( T );
		Real upper = mu_x + integralSignificance_ *sigma_x;
		Real lower = mu_x - integralSignificance_ *sigma_x;

		Real val = N * w * P0T * integrator_->operator()( integrand, lower, upper ) / sqrt( 2. * M_PI ) / sigma_x;
		return val;
	}
}


/*
 * NormalVectorsFilter.hpp
 *
 *  Created on: May 05, 2015
 *      Author: Peter Fankhauser, Martin Wermelinger
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <filters/filter_base.hpp>
#include <grid_map_core/grid_map_core.hpp>

#include <Eigen/Core>
#include <string>

namespace grid_map {

/*!
 * Compute the normal vectors of a layer in a map.
 */
template <typename T>
class NormalVectorsFilter : public filters::FilterBase<T> {
 public:
  /*!
   * Constructor
   */
  NormalVectorsFilter();

  /*!
   * Destructor.
   */
  ~NormalVectorsFilter() override;

  /*!
   * Configures the filter from parameters on the Parameter Server.
   * This comprehend in order:
   *    1) algorithm, used for choosing between area and raster algorithm (default=area)
   *    2)If algorithm is not raster estimationRadius_ is read an a basic check on its sign is made.
   *      If something is wrong it switches to raster algorithm.
   *    3) parallelization_enabled, used for choosing between serial and parallel algorithm (default=false)
   *    4) thread_count, used to set the number of threads to be used in parallelization is enabled (default=auto)
   * The parallelization_enabled and algorithm parameters allow to choose between the 4 different methods { AreaSerial, AreaParallel,
   * RasterSerial, RasterParallel }
   *    4) normal_vector_positive_axis, used to define the upward positive direction for the normals
   *    5) input_layer, defines in which layer of the grid map lie the information needed (usually elevation or elevation_filtered)
   *    6) output_layers_prefix, defines the prefix for the new 3 layers (x,y,z) that will define the normal vectors
   * Those parameters have to be written in a .yaml file that will be processed as a sequence of filters.
   * An example can be seen in grid_map_demos/config/normal_filter_comparison.yaml.
   */
  bool configure() override;

  /*!
   * Compute the normal vectors of a layer in a map and
   * saves it as additional grid map layer.
   * @param mapIn grid map containing the layer for which the normal vectors are computed for.
   * @param mapOut grid map containing mapIn and the new layers for the normal vectors.
   */
  bool update(const T& mapIn, T& mapOut) override;

 private:
  /*!
   * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function.
   * This function makes use of the area method and is the serial version of such normal vector computation using a
   * simple for cycle to iterate over the whole map.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function.
   * This function makes use of the area method and is the parallel version of such normal vector computation using
   * a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using points within
   * a circle of specified radius.
   *
   * The eigen decomposition of the covariance matrix (3x3) of all data points is used to establish the normal direction.
   * Four cases can be identified when the eigenvalues are ordered in ascending order:
   *    1) The data is in a cloud -> all eigenvalues are non-zero
   *    2) The data is on a plane -> The first eigenvalue is zero
   *    3) The data is on a line -> The first two eigenvalues are zero.
   *    4) The data is in one point -> All eigenvalues are zero
   *
   * Only case 1 & 2 provide enough information the establish a normal direction.
   * The degenerate cases (3 or 4) are identified by checking if the second eigenvalue is zero.
   *
   * The numerical threshold (1e-8) for the eigenvalue being zero is given by the accuracy of the decomposition, as reported by Eigen:
   * https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
   *
   * Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   * @param index: Index of point in the grid map for which this function calculates the normal vector.
   */
  void areaSingleNormalComputation(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix,
                                          const grid_map::Index& index);
  /*!
   * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function.
   * This function makes use of the raster method and is the serial version of such normal vector computation using a
   * simple for cycle to iterate over the whole map.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function.
   * This function makes use of the raster method and is the parallel version of such normal vector computation using
   * a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using neighboring points
   * around the point for which the normal vector is being calculated.
   *
   * The neighboring cells are used to linearly approximate the first derivative in both directions.
   *
   *   |T|   ^
   * |L|C|R| | x Axis
   *   |B|   |
   * <-------|
   * Y Axis
   *
   * Those values are then used to reconstruct the normal vector in the point of interest.
   * This algorithm doesn't make use of the central cell if all the neighboring point heights are present.
   * However, thanks to that, it can accommodate missing point heights, up to one in each direction, by using
   * exactly the central height value.
   * In this case, the resulting approximation, will suffer a loss of quality depending on the local surface shape.
   * This implementation skips the outermost values in order to avoid checks on the validity of height values
   * and therefore have a faster algorithm.
   *
   * Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
   *
   * Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   * @param dataMap: Matrix containing the input layer of the grid map in question.
   * @param index: Index of point in the grid map for which this function calculates the normal vector.
   */
  void rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix, const grid_map::Matrix& dataMap,
                                            const grid_map::Index& index);

  enum class Method { AreaSerial, AreaParallel, RasterSerial, RasterParallel };

  Method method_;

  //! Radius of submap for normal vector estimation.
  double estimationRadius_;

  //! Parameter that specifies whether to parallelize or not.
  bool parallelizationEnabled_;

  //! Parameter that specifies the number of thread used.
  int threadCount_;

  //! Normal vector positive axis.
  Eigen::Vector3d normalVectorPositiveAxis_;

  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayersPrefix_;

  //! Grid Map Resolution.
  double gridMapResolution_;
};

}  // namespace grid_map


#include "rice/Class.hpp"
#include "rice/String.hpp"
#include "rice/Constructor.hpp"
#include "rice/Enum.hpp"

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

using namespace Rice;

typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign>     Vector3d;
typedef Eigen::Matrix<double, 4, 4, Eigen::DontAlign>     Matrix4d;
typedef Eigen::Quaternion<double, Eigen::DontAlign>    Quaterniond;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign>
                                                       MatrixXd;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign>
                                                       VectorXd;
typedef Eigen::Transform< double, 3, Eigen::Isometry > Isometry3d;
typedef Eigen::Transform< double, 3, Eigen::Affine > Affine3d;
typedef Eigen::AngleAxis<double> AngleAxisd;

/*
 * Document-class: Eigen::Vector3
 *
 * A 3-vector holding floating-point numbers
 *
 * @!method initialize(x = 0, y = 0, z = 0)
 *   Creates a new vector
 *   @param [Numeric] x
 *   @param [Numeric] y
 *   @param [Numeric] z
 * @!method norm
 *   The vector's norm
 *   @return [Numeric]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns a normalized self
 *   @return [Vector3]
 * @!method [](index)
 *   Returns an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @return [Numeric]
 * @!method x
 *   Returns X
 *   @return [Numeric]
 * @!method y
 *   Returns Y
 *   @return [Numeric]
 * @!method z
 *   Returns Z
 *   @return [Numeric]
 * @!method x=(value)
 *   Sets X
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method y=(value)
 *   Sets Y
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method z=(value)
 *   Sets Z
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method []=(index, value)
 *   Sets an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method +(v)
 *    Sum
 *    @param [Vector3] v
 *    @return [Vector3] the sum
 * @!method -(v)
 *    Subtracts
 *    @param [Vector3] v
 *    @return [Vector3] the subtraction
 * @!method *(scalar)
 *    Multiplies by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [Vector3] the result
 * @!method /(scalar)
 *    Divides by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [Vector3] the result
 * @!method -@()
 *    Negation
 *    @return [Vector3] the result
 * @!method cross(v)
 *    Cross product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method dot(v)
 *    Dot product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two vectors are within threshold of each other, elementwise
 *    @param [Vector3]
 *    @return [Boolean]
 */

struct Vector3
{
    Vector3d* v;

    Vector3(double x, double y, double z)
        : v(new Vector3d(x, y, z)) {}
    Vector3(Vector3d const& _v)
        : v(new Vector3d(_v)) {}
    ~Vector3()
    { delete v; }

    double x() const { return v->x(); }
    double y() const { return v->y(); }
    double z() const { return v->z(); }
    void setX(double value) { v->x() = value; }
    void setY(double value) { v->y() = value; }
    void setZ(double value) { v->z() = value; }

    double norm() const { return v->norm(); }
    Vector3* normalize() const { return new Vector3(v->normalized()); }
    void normalizeBang() const { v->normalize(); }

    double get(int i) const { return (*v)[i]; }
    void set(int i, double value) { (*v)[i] = value; }

    Vector3* operator + (Vector3 const& other) const
    { return new Vector3(*v + *other.v); }
    Vector3* operator - (Vector3 const& other) const
    { return new Vector3(*v - *other.v); }

    Vector3* operator / (double scalar) const
    { return new Vector3(*v / scalar); }

    Vector3* negate() const
    { return new Vector3(-*v); }
    Vector3* scale(double value) const
    { return new Vector3(*v * value); }
    double dot(Vector3 const& other) const
    { return this->v->dot(*other.v); }
    Vector3* cross(Vector3 const& other) const
    { return new Vector3(this->v->cross(*other.v)); }
    bool operator ==(Vector3 const& other) const
    { return (*this->v) == (*other.v); }
    bool isApprox(Vector3 const& other, double tolerance)
    { return v->isApprox(*other.v, tolerance); }
};

/*
 * Document-class: Eigen::VectorX
 *
 * A variable-length vector holding floating-point numbers
 *
 * @!method initialize(size = 0)
 *   Creates a new vector
 *   @param [Numeric] size
 * @!method resize(new_size)
 *   Changes the vector's size
 *   @param [Integer] new_size
 *   @return [Numeric]
 * @!method norm
 *   The vector's norm
 *   @return [Numeric]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns a normalized self
 *   @return [VectorX]
 * @!method size
 *   Returns the vector's size
 *   @return [Integer]
 * @!method [](index)
 *   Returns an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @return [Numeric]
 * @!method []=(index, value)
 *   Sets an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method +(v)
 *    Sum
 *    @param [VectorX] v
 *    @return [VectorX] the sum
 * @!method -(v)
 *    Subtracts
 *    @param [VectorX] v
 *    @return [VectorX] the subtraction
 * @!method *(scalar)
 *    Multiplies by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [VectorX] the result
 * @!method /(scalar)
 *    Divides by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [VectorX] the result
 * @!method -@()
 *    Negation
 *    @return [VectorX] the result
 * @!method dot(v)
 *    Dot product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two vectors are within threshold of each other, elementwise
 *    @param [VectorX]
 *    @return [Boolean]
 */
struct VectorX {

    VectorXd* v;

    VectorX()
        : v(new VectorXd()) {}
    VectorX(VectorX const& v)
        : v(new VectorXd(*v.v)) {}
    VectorX(int n)
        : v(new VectorXd(n)) {}
    VectorX(VectorXd const& _v)
        : v(new VectorXd(_v)) {}
    ~VectorX()
    { delete v; }

    void resize(int n) { v->resize(n); }
    void conservativeResize(int n) { v->conservativeResize(n); }

    double norm() const { return v->norm(); }
    VectorX* normalize() const { return new VectorX(v->normalized()); }
    void normalizeBang() const { v->normalize(); }

    unsigned int size() { return v->size(); }

    double get(int i) const { return (*v)[i]; }
    void set(int i, double value) { (*v)[i] = value; }

    VectorX* operator + (VectorX const& other) const
    { return new VectorX(*v + *other.v); }
    VectorX* operator - (VectorX const& other) const
    { return new VectorX(*v - *other.v); }

    VectorX* operator / (double scalar) const
    { return new VectorX(*v / scalar); }

    VectorX* negate() const
    { return new VectorX(-*v); }

    VectorX* scale(double value) const
    { return new VectorX(*v * value); }

    double dot(VectorX const& other) const
    { return this->v->dot(*other.v); }

    bool operator ==(VectorX const& other) const
    { return (*this->v) == (*other.v); }

    bool isApprox(VectorX const& other, double tolerance)
    { return v->isApprox(*other.v, tolerance); }

};

/*
 * Document-class: Eigen::Matrix4
 *
 * A 4x4 matrix holding floating-point numbers
 *
 * @!method rows
 *    @return [Integer] the number of rows
 * @!method cols
 *    @return [Numeric] the number of columns
 * @!method size
 *    @return [Numeric] the number of elements
 * @!method [](row, col)
 *    Accesses an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @return [Numeric] the required element
 * @!method []=(row, col, value)
 *    Sets an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @param [Numeric] value the new value
 *    @return [Numeric] the value
 * @!method +(m)
 *    Sums two matrices
 *    @param [Matrix4] m the matrix to add
 *    @return [Matrix4] the sum
 * @!method -(m)
 *    Subtracts two matrices
 *    @param [Matrix4] m the matrix to subtract to self
 *    @return [Matrix4] the subtraction
 * @!method *(v)
 *    Multiplies by a scalar
 *    @param [Numeric] v the scalar
 *    @return [Matrix4] the result
 * @!method /(v)
 *    Divides this matrix by a scalar
 *    @param [Numeric] v the scalar
 *    @return [Matrix4] the result
 * @!method -@(v)
 *    Returns this matrix' negation
 *    @return [Matrix4] the result
 * @!method T
 *    Returns the transposed matrix
 *    @return [Matrix4]
 * @!method norm
 *    Returns the matrix' norm
 *    @return [Numeric]
 * @!method dotM(m)
 *    Matrix multiplication
 *    @param [Matrix4]
 *    @return [Numeric]
 * @!method approx?(m, threshold = dummy_precision)
 *    Verifies that two matrices are within threshold of each other, elementwise
 *    @param [Matrix4]
 *    @return [Boolean]
 */
struct Matrix4
{
    Matrix4d* mx;

    Matrix4() : mx(new Matrix4d()) {}

    Matrix4(Matrix4d const& _mx)
        : mx(new Matrix4d(_mx)) {}

    ~Matrix4()
    { delete mx; }

    double norm() const { return mx->norm(); }

    int rows() const { return mx->rows(); }
    int cols() const { return mx->cols(); }
    int size() const { return mx->size(); }

    double get(int i, int j ) const { return (*mx)(i,j); }
    void set(int i, int j, double value) { (*mx)(i,j) = value; }

    Matrix4* transpose() const
    { return new Matrix4(mx->transpose()); }

    Matrix4* operator + (Matrix4 const& other) const
    { return new Matrix4(*mx + *other.mx); }

    Matrix4* operator - (Matrix4 const& other) const
    { return new Matrix4(*mx - *other.mx); }

    Matrix4* operator / (double scalar) const
    { return new Matrix4(*mx / scalar); }

    Matrix4* negate() const
    { return new Matrix4(-*mx); }

    Matrix4* scale(double value) const
    { return new Matrix4(*mx * value); }

    Matrix4* dotM (Matrix4 const& other) const
    { return new Matrix4(*mx * (*other.mx)); }

    bool operator ==(Matrix4 const& other) const
    { return (*this->mx) == (*other.mx); }

    bool isApprox(Matrix4 const& other, double tolerance)
    { return mx->isApprox(*other.mx, tolerance); }
};

/*
 * Document-class: Eigen::JacobiSVD
 *
 * Linear problem solver
 *
 * This is not constructed directly. Use {Eigen::MatrixX#jacobiSvd} instead.
 *
 * @!method solve(vector)
 *   Solves the linear problem for a given vector
 *   @param [VectorX] vector
 *   @return [VectorX] the result
 */
struct JacobiSVD {
    typedef Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject> EigenT;
    EigenT* j;

    JacobiSVD( EigenT const& j )
    : j(new EigenT(j)) {}
    JacobiSVD( JacobiSVD const& j )
    : j(new EigenT(*j.j)) {}
    ~JacobiSVD()
    { delete j; }

    VectorX* solve(VectorX* y)
    { return new VectorX(j->solve(*y->v)); }
};

/*
 * Document-class: Eigen::MatrixX
 *
 * A variable-size matrix holding floating-point numbers
 *
 * @!method resize(rows, cols)
 *    Resizes the matrix
 *    @param [Integer] rows the new number of rows
 *    @param [Integer] cols the new number of columns
 * @!method rows
 *    @return [Integer] the number of rows
 * @!method cols
 *    @return [Numeric] the number of columns
 * @!method size
 *    @return [Numeric] the number of elements
 * @!method [](row, col)
 *    Accesses an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @return [Numeric] the required element
 * @!method []=(row, col, value)
 *    Sets an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @param [Numeric] value the new value
 *    @return [Numeric] the value
 * @!method setRow(row, vector)
 *    Sets a whole matrix row
 *    @param [Integer] row the row index
 *    @param [VectorX] vector the row values
 * @!method setColumn(column, vector)
 *    Sets a whole matrix column
 *    @param [Integer] row the column index
 *    @param [VectorX] vector the column values
 * @!method +(m)
 *    Sums two matrices
 *    @param [MatrixX] m the matrix to add
 *    @return [MatrixX] the sum
 * @!method -(m)
 *    Subtracts two matrices
 *    @param [MatrixX] m the matrix to subtract to self
 *    @return [MatrixX] the subtraction
 * @!method *(v)
 *    Multiplies by a scalar
 *    @param [Numeric] v the scalar
 *    @return [MatrixX] the result
 * @!method /(v)
 *    Divides this matrix by a scalar
 *    @param [Numeric] v the scalar
 *    @return [MatrixX] the result
 * @!method -@(v)
 *    Returns this matrix' negation
 *    @return [MatrixX] the result
 * @!method T
 *    Returns the transposed matrix
 *    @return [MatrixX]
 * @!method norm
 *    Returns the matrix' norm
 *    @return [Numeric]
 * @!method dotV(m)
 *    Matrix/vector multiplication
 *    @param [VectorX]
 *    @return [Numeric]
 * @!method dotM(m)
 *    Matrix multiplication
 *    @param [Matrix4]
 *    @return [Numeric]
 * @!method approx?(m, threshold = dummy_precision)
 *    Verifies that two matrices are within threshold of each other, elementwise
 *    @param [Matrix4]
 *    @return [Boolean]
 * @!method jacobiSvd(flags = 0)
 *    Returns a SVD solver to find V from W in self.dotV(V) = W
 *    @param [Integer] flags solver flags, as OR-ed values of Eigen::ComputeFullU,
 *      Eigen::ComputeThinU and Eigen::ComputeThinV. See Eigen documentation
 *    @return [JacobiSVD]
 */
struct MatrixX {

    MatrixXd* m;

    MatrixX() : m(new MatrixXd()) {}
    MatrixX(const MatrixX& m) : m(new MatrixXd(*m.m)) {}
    MatrixX(int rows, int cols) : m(new MatrixXd(rows,cols)) {}
    MatrixX(const MatrixXd& _m) : m(new MatrixXd(_m)) {}
    ~MatrixX() { delete m; }

    void resize(int rows, int cols) { m->resize(rows,cols); }
    void conservativeResize(int rows, int cols) { m->conservativeResize(rows,cols); }

    double norm() const { return m->norm(); }

    unsigned int rows() const { return m->rows(); }
    unsigned int cols() const { return m->cols(); }
    unsigned int size() const { return m->size(); }

    double get(int i, int j ) const { return (*m)(i,j); }
    void set(int i, int j, double value) { (*m)(i,j) = value; }

    VectorX* getRow(int i) const { return new VectorX(m->row(i)); }
    void setRow(int i, const VectorX& v) { m->row(i) = *(v.v); }

    VectorX* getColumn(int j) const { return new VectorX(m->col(j)); }
    void setColumn(int j, const VectorX& v) { m->col(j) = *(v.v); }

    MatrixX* transpose() const
    { return new MatrixX(m->transpose()); }

    MatrixX* operator + (MatrixX const& other) const
    { return new MatrixX(*m + *other.m); }

    MatrixX* operator - (MatrixX const& other) const
    { return new MatrixX(*m - *other.m); }

    MatrixX* operator / (double scalar) const
    { return new MatrixX(*m / scalar); }

    MatrixX* negate() const
    { return new MatrixX(-*m); }

    MatrixX* scale (double scalar) const
    { return new MatrixX(*m * scalar); }

    VectorX* dotV (VectorX const& other) const
    { return new VectorX(*m * *other.v); }

    MatrixX* dotM (MatrixX const& other) const
    { return new MatrixX(*m * (*other.m)); }

    JacobiSVD* jacobiSvd(int flags = 0) const
    { return new JacobiSVD(m->jacobiSvd(flags)); }

    bool operator ==(MatrixX const& other) const
    { return (*this->m) == (*other.m); }

    bool isApprox(MatrixX const& other, double tolerance)
    { return m->isApprox(*other.m, tolerance); }
};

/*
 * Document-class: Eigen::Quaternion
 *
 * A floating-point valued quaternion
 *
 * @!method w
 *   The real part of the quaternion
 *   @return [Numeric]
 * @!method x
 *   The first element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method y
 *   The second element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method z
 *   The third element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method w=(value)
 *   Sets the real part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method x=(value)
 *   Sets the first element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method y=(value)
 *   Sets the second element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method z=(value)
 *   Sets the third element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method norm
 *   The norm
 *   @return [Numeric]
 * @!method concatenate(q)
 *   Quaternion multiplication
 *   @param [Quaternion] q
 *   @return [Quaternion] self * q
 * @!method inverse
 *   The quaternion inverse
 *   @return [Quaternion]
 * @!method transform(v)
 *   Transform a vector-3 by this quaternion
 *   @param [Vector3] v
 *   @return [Vector3]
 * @!method matrix
 *   The rotation matrix equivalent to this unit quaternion
 *   @return [MatrixX]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns the self normalized
 *   @return [Quaternion]
 * @!method approx?(q, threshold = dummy_precision)
 *   Verifies that two quaternions are within threshold of each other, elementwise
 *   @param [Quaternion]
 *   @return [Boolean]
 * @!method to_euler
 *   Converts this quaternion into Euler-Bryant angles
 *   @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
 *     pitch and .z yaw
 * @!method from_euler(v)
 *   Initializes from euler angles
 *   @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
 *     pitch and .z yaw
 *   @return [void]
 * @!method from_angle_axis(angle_axis)
 *   Initializes from an angle-axis representation
 *   @param [AngleAxis] an angle-axis representation of a rotation
 *   @return [void]
 * @!method from_matrix(matrix)
 *   Initializes from a rotation matrix
 *   @param [MatrixX]
 *   @return [void]
 */
struct Quaternion
{
    Quaterniond* q;
    Quaternion(double w, double x, double y, double z)
        : q(new Quaterniond(w, x, y, z)) { }
    Quaternion(Quaternion const& q)
        : q(new Quaterniond(*q.q)) { }
    Quaternion(Quaterniond const& _q)
        : q(new Quaterniond(_q)) {}

    ~Quaternion()
    { delete q; }

    double w() const { return q->w(); }
    double x() const { return q->x(); }
    double y() const { return q->y(); }
    double z() const { return q->z(); }
    void setW(double value) { q->w() = value; }
    void setX(double value) { q->x() = value; }
    void setY(double value) { q->y() = value; }
    void setZ(double value) { q->z() = value; }

    double norm() const { return q->norm(); }

    bool operator ==(Quaternion const& other) const
    { return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w(); }

    Quaternion* concatenate(Quaternion const& other) const
    { return new Quaternion((*q) * (*other.q)); }
    Vector3* transform(Vector3 const& v) const
    { return new Vector3((*q) * (*v.v)); }
    Quaternion* inverse() const
    { return new Quaternion(q->inverse()); }
    void normalizeBang()
    { q->normalize(); }
    Quaternion* normalize() const
    {
        Quaterniond q = *this->q;
        q.normalize();
        return new Quaternion(q);
    }
    MatrixX* matrix() const
    {
        return new MatrixX(q->matrix());
    }

    void fromAngleAxis(double angle, Vector3 const& axis)
    {
	*(this->q) =
            Eigen::AngleAxisd(angle, *axis.v);
    }

    void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
    {
        *(this->q) =
            Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
            Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
            Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
    }

    void fromMatrix(MatrixX const& matrix)
    {
	*(this->q) =
            Quaterniond(Eigen::Matrix3d(*matrix.m));
    }

    bool isApprox(Quaternion const& other, double tolerance)
    {
        return q->isApprox(*other.q, tolerance);
    }

    Vector3* toEuler()
    {
        const Eigen::Matrix3d m = q->toRotationMatrix();
        double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
        double y = atan2(-m.coeff(2,0), i);
        double x=0,z=0;
        if (i > Eigen::NumTraits<double>::dummy_precision()){
            x = ::atan2(m.coeff(1,0), m.coeff(0,0));
            z = ::atan2(m.coeff(2,1), m.coeff(2,2));
        }else{
            z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
        }
        return new Vector3(x,y,z);
    }
};

/*
 * Document-class: Eigen::AngleAxis
 *
 * A rotation represented by an axis and angle
 *
 * @!method angle
 *   The angle (in radians)
 *   @return [Numeric]
 * @!method axis
 *   The rotation axis
 *   @return [Vector3]
 * @!method concatenate(aa)
 *   Combine this rotation with another rotation
 *   @param [AngleAxis] aa
 *   @return [AngleAxis]
 * @!method inverse
 *   The inverse rotation
 *   @return [AngleAxis]
 * @!method transform(v)
 *   Apply this rotation on a 3-vector
 *   @param [Vector3] v
 *   @return [Vector3]
 * @!method matrix
 *   The rotation matrix equivalent to this unit quaternion
 *   @return [MatrixX]
 * @!method approx?(aa, threshold = dummy_precision)
 *    Verifies that two angle-axis are within threshold of each other, elementwise
 *    @param [AngleAxis]
 *    @return [Boolean]
 * @!method to_euler
 *    Converts this quaternion into Euler-Bryant angles
 *    @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
 *      pitch and .z yaw
 * @!method from_euler(v)
 *    Initializes from euler angles
 *    @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
 *      pitch and .z yaw
 * @!method from_quaternion(q)
 *    Initializes from a quaternion
 *    @param [Quaternion] q
 * @!method from_matrix(matrix)
 *    Initializes from a rotation matrix
 *    @param [MatrixX]
 */
struct AngleAxis
{
    AngleAxisd* aa;
    AngleAxis(double angle, Vector3 const& axis)
        : aa(new AngleAxisd(angle, Eigen::Vector3d(*axis.v))){}
    AngleAxis(AngleAxis const& aa)
        : aa(new AngleAxisd(*aa.aa)) { }
    AngleAxis(AngleAxisd const& _aa)
        : aa(new AngleAxisd(_aa)) {}

    ~AngleAxis()
    { delete aa; }

    bool operator ==(AngleAxis const& other) const
    { return angle() == other.angle() && axis() == other.axis(); }

    double angle() const { return aa->angle(); }
    Vector3* axis() const { return new Vector3(aa->axis()); }

    AngleAxis* concatenate(AngleAxis const& other) const
    { return new AngleAxis(static_cast<AngleAxisd>((*aa) * (*other.aa))); }

    Vector3* transform(Vector3 const& v) const
    { return new Vector3((*aa) * (*v.v)); }

    AngleAxis* inverse() const
    { return new AngleAxis(aa->inverse()); }

    MatrixX* matrix() const
    {
        return new MatrixX(aa->matrix());
    }

    void fromQuaternion(Quaternion const& q)
    {
        *(this->aa) = Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z());
    }

    void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
    {
        *(this->aa) =
            Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
            Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
            Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
    }

    void fromMatrix(MatrixX const& matrix)
    {
	    *(this->aa) =
            AngleAxisd(Eigen::Matrix3d(*matrix.m));
    }

    bool isApprox(AngleAxis const& other, double tolerance)
    {
        return aa->isApprox(*other.aa, tolerance);
    }

    Vector3* toEuler()
    {
        const Eigen::Matrix3d m = aa->toRotationMatrix();
        double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
        double y = atan2(-m.coeff(2,0), i);
        double x=0,z=0;
        if (i > Eigen::NumTraits<double>::dummy_precision()){
            x = ::atan2(m.coeff(1,0), m.coeff(0,0));
            z = ::atan2(m.coeff(2,1), m.coeff(2,2));
        }else{
            z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
        }
        return new Vector3(x,y,z);
    }
};

#include <iostream>

/*
 * Document-class: Eigen::Isometry3
 *
 * An isometry
 *
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two isometries are within threshold of each other, elementwise
 *    @param [Isometry3]
 *    @return [Boolean]
 * @!method inverse
 *    The inverse transformation
 *    @return [Isometry3]
 * @!method translation
 *    The translation part of this transformation
 *    @return [Vector3]
 * @!method rotation
 *    The rotation part of this transformation
 *    @return [Quaternion]
 * @!method concatenate(is)
 *    Concatenate self and another transformation
 *    @param [Isometry3] is
 *    @return [Isometry3]
 * @!method translate(v)
 *    Add a new translation after the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method rotate(q)
 *    Add a new rotation after the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method pretranslate(v)
 *    Add a new translation before the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method prerotate(q)
 *    Add a new rotation before the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method matrix
 *    The transformation matrix equivalent to self
 *    @return [MatrixX]
 */
struct Isometry3
{
    Isometry3d *t;

    Isometry3() : t(new Isometry3d()) { t->setIdentity(); }
    Isometry3(const Isometry3& _m) : t(new Isometry3d(*_m.t)) {}
    Isometry3(const Isometry3d& _m) : t(new Isometry3d(_m)) {}
    ~Isometry3() { delete t; }

    Isometry3* inverse() const
    { return new Isometry3( t->inverse() ); }

    Vector3* translation() const
    { return new Vector3( t->translation() ); }

    Quaternion* rotation() const
    { return new Quaternion( Eigen::Quaterniond(t->linear()) ); }

    Isometry3* concatenate(Isometry3 const& other) const
    { return new Isometry3( *t * *other.t ); }

    Vector3* transform(Vector3 const& other) const
    { return new Vector3( *t * *other.v ); }

    MatrixX* matrix() const
    { return new MatrixX( t->matrix() ); }

    void translate( Vector3 const& other ) const
    { t->translate( *other.v ); }

    void pretranslate( Vector3 const& other ) const
    { t->pretranslate( *other.v ); }

    void rotate( Quaternion const& other ) const
    { t->rotate( *other.q ); }

    void prerotate( Quaternion const& other ) const
    { t->prerotate( *other.q ); }

    bool operator ==(Isometry3 const& other) const
    { return (*this->t).matrix() == (*other.t).matrix(); }

    bool isApprox(Isometry3 const& other, double tolerance)
    { return t->isApprox(*other.t, tolerance); }
};

/*
 * Document-class: Eigen::Affine3
 *
 * An affine transformation
 *
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two transformations are within threshold of each other, elementwise
 *    @param [Isometry3]
 *    @return [Boolean]
 * @!method inverse
 *    The inverse transformation
 *    @return [Affine3]
 * @!method translation
 *    The translation part of this transformation
 *    @return [Vector3]
 * @!method rotation
 *    The rotation part of this transformation
 *    @return [Quaternion]
 * @!method concatenate(is)
 *    Concatenate self and another transformation
 *    @param [Affine3] is
 *    @return [Affine3]
 * @!method translate(v)
 *    Add a new translation after the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method rotate(q)
 *    Add a new rotation after the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method pretranslate(v)
 *    Add a new translation before the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method prerotate(q)
 *    Add a new rotation before the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method matrix
 *    The transformation matrix equivalent to self
 *    @return [MatrixX]
 */
struct Affine3
{
    Affine3d *t;

    Affine3() : t(new Affine3d()) { t->setIdentity(); }
    Affine3(const Affine3& _m) : t(new Affine3d(*_m.t)) {}
    Affine3(const Affine3d& _m) : t(new Affine3d(_m)) {}
    ~Affine3() { delete t; }

    Affine3* inverse() const
    { return new Affine3( t->inverse() ); }

    Vector3* translation() const
    { return new Vector3( t->translation() ); }

    Quaternion* rotation() const
    { return new Quaternion( Eigen::Quaterniond(t->linear()) ); }

    Affine3* concatenate(Affine3 const& other) const
    { return new Affine3( *t * *other.t ); }

    Vector3* transform(Vector3 const& other) const
    { return new Vector3( *t * *other.v ); }

    MatrixX* matrix() const
    { return new MatrixX( t->matrix() ); }

    void translate( Vector3 const& other ) const
    { t->translate( *other.v ); }

    void pretranslate( Vector3 const& other ) const
    { t->pretranslate( *other.v ); }

    void rotate( Quaternion const& other ) const
    { t->rotate( *other.q ); }

    void prerotate( Quaternion const& other ) const
    { t->prerotate( *other.q ); }

    bool operator ==(Affine3 const& other) const
    { return (*this->t).matrix() == (*other.t).matrix(); }

    bool isApprox(Affine3 const& other, double tolerance)
    { return t->isApprox(*other.t, tolerance); }
};

extern "C" void Init_eigen()
{
     Rice::Module rb_mEigen = define_module("Eigen");

     Data_Type<Vector3> rb_Vector3 = define_class_under<Vector3>(rb_mEigen, "Vector3")
       .define_constructor(Constructor<Vector3,double,double,double>(),
               (Arg("x") = static_cast<double>(0),
               Arg("y") = static_cast<double>(0),
               Arg("z") = static_cast<double>(0)))
       .define_method("__equal__",  &Vector3::operator ==)
       .define_method("norm",  &Vector3::norm)
       .define_method("normalize!",  &Vector3::normalizeBang)
       .define_method("normalize",  &Vector3::normalize)
       .define_method("[]",  &Vector3::get)
       .define_method("[]=",  &Vector3::set)
       .define_method("x",  &Vector3::x)
       .define_method("y",  &Vector3::y)
       .define_method("z",  &Vector3::z)
       .define_method("x=", &Vector3::setX)
       .define_method("y=", &Vector3::setY)
       .define_method("z=", &Vector3::setZ)
       .define_method("+",  &Vector3::operator +)
       .define_method("-",  &Vector3::operator -)
       .define_method("/",  &Vector3::operator /)
       .define_method("-@", &Vector3::negate)
       .define_method("*",  &Vector3::scale)
       .define_method("cross", &Vector3::cross)
       .define_method("dot",  &Vector3::dot)
       .define_method("approx?", &Vector3::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Quaternion> rb_Quaternion = define_class_under<Quaternion>(rb_mEigen, "Quaternion")
       .define_constructor(Constructor<Quaternion,double,double,double,double>())
       .define_method("__equal__", &Quaternion::operator ==)
       .define_method("w",  &Quaternion::w)
       .define_method("x",  &Quaternion::x)
       .define_method("y",  &Quaternion::y)
       .define_method("z",  &Quaternion::z)
       .define_method("w=", &Quaternion::setW)
       .define_method("x=", &Quaternion::setX)
       .define_method("y=", &Quaternion::setY)
       .define_method("z=", &Quaternion::setZ)
       .define_method("norm", &Quaternion::norm)
       .define_method("concatenate", &Quaternion::concatenate)
       .define_method("inverse", &Quaternion::inverse)
       .define_method("transform", &Quaternion::transform)
       .define_method("matrix", &Quaternion::matrix)
       .define_method("normalize!", &Quaternion::normalizeBang)
       .define_method("normalize", &Quaternion::normalize)
       .define_method("approx?", &Quaternion::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("to_euler", &Quaternion::toEuler)
       .define_method("from_euler", &Quaternion::fromEuler)
       .define_method("from_angle_axis", &Quaternion::fromAngleAxis)
       .define_method("from_matrix", &Quaternion::fromMatrix);

     Data_Type<AngleAxis> rb_AngleAxis = define_class_under<AngleAxis>(rb_mEigen, "AngleAxis")
       .define_constructor(Constructor<AngleAxis,double,Vector3 const&>())
       .define_method("__equal__", &AngleAxis::operator ==)
       .define_method("angle",  &AngleAxis::angle)
       .define_method("axis",  &AngleAxis::axis)
       .define_method("concatenate", &AngleAxis::concatenate)
       .define_method("inverse", &AngleAxis::inverse)
       .define_method("transform", &AngleAxis::transform)
       .define_method("matrix", &AngleAxis::matrix)
       .define_method("approx?", &AngleAxis::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("to_euler", &AngleAxis::toEuler)
       .define_method("from_euler", &AngleAxis::fromEuler)
       .define_method("from_quaternion", &AngleAxis::fromQuaternion)
       .define_method("from_matrix", &AngleAxis::fromMatrix);

     Data_Type<VectorX> rb_VectorX = define_class_under<VectorX>(rb_mEigen, "VectorX")
       .define_constructor(Constructor<VectorX,int>(),
               (Arg("rows") = static_cast<int>(0)))
       .define_method("resize", &VectorX::resize)
       .define_method("__equal__",  &VectorX::operator ==)
       .define_method("norm",  &VectorX::norm)
       .define_method("normalize!",  &VectorX::normalizeBang)
       .define_method("normalize",  &VectorX::normalize)
       .define_method("size", &VectorX::size)
       .define_method("[]",  &VectorX::get)
       .define_method("[]=",  &VectorX::set)
       .define_method("+",  &VectorX::operator +)
       .define_method("-",  &VectorX::operator -)
       .define_method("/",  &VectorX::operator /)
       .define_method("-@", &VectorX::negate)
       .define_method("*",  &VectorX::scale)
       .define_method("dot",  &VectorX::dot)
       .define_method("approx?", &VectorX::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Matrix4> rb_Matrix4 = define_class_under<Matrix4>(rb_mEigen, "Matrix4")
       .define_constructor(Constructor<Matrix4>())
       .define_method("__equal__",  &Matrix4::operator ==)
       .define_method("T", &Matrix4::transpose)
       .define_method("norm",  &Matrix4::norm)
       .define_method("rows", &Matrix4::rows)
       .define_method("cols", &Matrix4::cols)
       .define_method("size", &Matrix4::size)
       .define_method("[]",  &Matrix4::get)
       .define_method("[]=",  &Matrix4::set)
       .define_method("+",  &Matrix4::operator +)
       .define_method("-",  &Matrix4::operator -)
       .define_method("/",  &Matrix4::operator /)
       .define_method("-@", &Matrix4::negate)
       .define_method("*",  &Matrix4::scale)
       .define_method("dotM",  &Matrix4::dotM)
       .define_method("approx?", &Matrix4::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     rb_mEigen.const_set("ComputeFullU", INT2FIX(Eigen::ComputeFullU));
     rb_mEigen.const_set("ComputeThinU", INT2FIX(Eigen::ComputeThinU));
     rb_mEigen.const_set("ComputeThinV", INT2FIX(Eigen::ComputeThinV));

     Data_Type<JacobiSVD> rb_JacobiSVD = define_class_under<JacobiSVD>(rb_mEigen, "JacobiSVD")
        .define_method("solve", &JacobiSVD::solve);

     Data_Type<MatrixX> rb_MatrixX = define_class_under<MatrixX>(rb_mEigen, "MatrixX")
       .define_constructor(Constructor<MatrixX,int,int>(),
               (Arg("rows") = static_cast<int>(0),
                Arg("cols") = static_cast<int>(0)))
       .define_method("resize", &MatrixX::resize)
       .define_method("__equal__",  &MatrixX::operator ==)
       .define_method("T", &MatrixX::transpose)
       .define_method("norm",  &MatrixX::norm)
       .define_method("rows", &MatrixX::rows)
       .define_method("cols", &MatrixX::cols)
       .define_method("size", &MatrixX::size)
       .define_method("[]",  &MatrixX::get)
       .define_method("[]=",  &MatrixX::set)
       .define_method("row", &MatrixX::getRow)
       .define_method("setRow", &MatrixX::setRow)
       .define_method("col", &MatrixX::getColumn)
       .define_method("setCol", &MatrixX::setColumn)
       .define_method("+",  &MatrixX::operator +)
       .define_method("-",  &MatrixX::operator -)
       .define_method("/",  &MatrixX::operator /)
       .define_method("-@", &MatrixX::negate)
       .define_method("*",  &MatrixX::scale)
       .define_method("dotV",  &MatrixX::dotV)
       .define_method("dotM",  &MatrixX::dotM)
       .define_method("jacobiSvd", &MatrixX::jacobiSvd, (Arg("flags") = 0))
       .define_method("approx?", &MatrixX::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Isometry3> rb_Isometry3 = define_class_under<Isometry3>(rb_mEigen, "Isometry3")
       .define_constructor(Constructor<Isometry3>())
       .define_method("__equal__",  &Isometry3::operator ==)
       .define_method("approx?", &Isometry3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("inverse", &Isometry3::inverse)
       .define_method("translation", &Isometry3::translation)
       .define_method("rotation", &Isometry3::rotation)
       .define_method("concatenate", &Isometry3::concatenate)
       .define_method("transform", &Isometry3::transform)
       .define_method("matrix", &Isometry3::matrix)
       .define_method("translate", &Isometry3::translate)
       .define_method("pretranslate", &Isometry3::pretranslate)
       .define_method("rotate", &Isometry3::rotate)
       .define_method("prerotate", &Isometry3::prerotate);

     Data_Type<Affine3> rb_Affine3 = define_class_under<Affine3>(rb_mEigen, "Affine3")
       .define_constructor(Constructor<Affine3>())
       .define_method("__equal__",  &Affine3::operator ==)
       .define_method("approx?", &Affine3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("inverse", &Affine3::inverse)
       .define_method("translation", &Affine3::translation)
       .define_method("rotation", &Affine3::rotation)
       .define_method("concatenate", &Affine3::concatenate)
       .define_method("transform", &Affine3::transform)
       .define_method("matrix", &Affine3::matrix)
       .define_method("translate", &Affine3::translate)
       .define_method("pretranslate", &Affine3::pretranslate)
       .define_method("rotate", &Affine3::rotate)
       .define_method("prerotate", &Affine3::prerotate);
}


//
//  ComputeHausdorffDistance.cpp
//  Elasticity
//
//  Created by Wim van Rees on 12/28/16.
//  Copyright © 2016 Wim van Rees. All rights reserved.
//

#include "ComputeHausdorffDistance.hpp"

#include <igl/hausdorff.h>
#include <Eigen/Geometry>
#include "WriteVTK.hpp"

Real ComputeHausdorffDistance::compute(const Eigen::Ref<const Eigen::MatrixXd> vertices_A, const Eigen::Ref<const Eigen::MatrixXd> vertices_B, const Eigen::Ref<const Eigen::MatrixXi> faces, const Real rescale) const
{
    // compute the least-squares transformation between two point sets
    //https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60

    const Eigen::MatrixXd trafo = Eigen::umeyama(vertices_A.transpose(), vertices_B.transpose(), rescale);
    const Eigen::Matrix3d rotmat = trafo.block<3,3>(0,0);
    const Eigen::Vector3d transv = trafo.block<3,1>(0,3);
    // now we have the rotation and scaling matrices to transform vertices_A into vertices_B

    // apply the transform
    const int nVertices = vertices_A.rows();
    Eigen::MatrixXd vertices_A_trafo(nVertices,3);
    for(int i=0;i<nVertices;++i)
    {
        const Eigen::Vector3d vertA = vertices_A.row(i);
        const Eigen::Vector3d vertA_trafo = rotmat * vertA + transv;
        vertices_A_trafo.row(i) = vertA_trafo;
    }

    if(bDump)
    {
        WriteVTK writerA(vertices_A, faces);
        writerA.write("haussdorf_A");

        WriteVTK writerB(vertices_B, faces);
        writerB.write("haussdorf_B");

        WriteVTK writerA_trafo(vertices_A_trafo, faces);
        writerA_trafo.write("haussdorf_A_trafo");
    }

    // compute the distance
    Real retval;
//    igl::hausdorff<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXd, Eigen::MatrixXi, Real>(vertices_A_trafo, faces, vertices_B, faces, retval);

    // IGL does not handle Eigen::Ref well -- need to define a reference explicitly
    const Eigen::MatrixXd & ref_vertices_B = vertices_B;
    const Eigen::MatrixXi & ref_faces = faces;
    igl::hausdorff(vertices_A_trafo, ref_faces, ref_vertices_B, ref_faces, retval);

    return retval;
}


#ifndef __POLYNOMIAL_H__
#define __POLYNOMIAL_H__

#include <iostream>
#include <vector>
#include <math.h>
#include <NTL/ZZ_p.h>

#include "src/utils/math.hpp"

// TODO : REFACTOR -> use recursive polynomials
// TODO : Templatize the class

typedef std::vector<unsigned int> monomial;

class MultiVariatePolynomial
{
public:
    MultiVariatePolynomial(unsigned int arity, unsigned int degree, unsigned int order) : arity(arity), degree(degree), order(order), maxNbElements(binomialCoefficient(degree + arity, degree)), coefficients(maxNbElements)
    {
        NTL::ZZ p((long)order);
        NTL::ZZ_p::init(p);
    }
    MultiVariatePolynomial(unsigned int arity, unsigned int degree, unsigned int order, std::vector<NTL::ZZ_p> values) : MultiVariatePolynomial(arity, degree, order)
    {
        if (values.size() != this->getMaxNbElements())
            throw std::invalid_argument("Incorrect number of values provided");
        coefficients = values;
    }
    MultiVariatePolynomial() = delete;

    unsigned int getArity() const
    {
        return this->arity;
    };
    void setArity(unsigned int arity)
    {
        this->arity = arity;
    };
    unsigned int getDegree() const
    {
        return this->degree;
    };
    void setDegree(unsigned int degree)
    {
        this->degree = degree;
    };
    unsigned int getOrder() const
    {
        return this->order;
    };
    void setOrder(unsigned int order)
    {
        this->order = order;
    };

    unsigned int getMaxNbElements() const
    {
        return this->maxNbElements;
    };

    void setElement(std::vector<unsigned int>& monomial, NTL::ZZ_p value);

    void setElement(unsigned int index, NTL::ZZ_p value);

    NTL::ZZ_p getElement(std::vector<unsigned int>& monomial) const;

    NTL::ZZ_p getElement(unsigned int index) const;

    void print() const;

    std::string to_string() const;

    MultiVariatePolynomial add(const MultiVariatePolynomial& other) const;
    MultiVariatePolynomial operator+(const MultiVariatePolynomial& other) const;

    MultiVariatePolynomial sub(const MultiVariatePolynomial& other) const;
    MultiVariatePolynomial operator-(const MultiVariatePolynomial& other) const;

    NTL::ZZ_p evaluate(std::vector<NTL::ZZ_p>& point) const;

    static MultiVariatePolynomial interpolate(std::vector<std::vector<NTL::ZZ_p>>& known_values, int degree, int arity, int order);

private:
    unsigned int arity;
    unsigned int degree;
    unsigned int order;
    unsigned int maxNbElements;
    std::vector<NTL::ZZ_p> coefficients;
};

#endif // __POLYNOMIAL_H__


/*
 * fclayer.cpp
 *
 * Feed-forward:
 *    z(l) = a(l-1) * w + b // where * (vector, matrix) multiplication
 *    a(l) = activation(z(l))
 *
 * Back propagation:
 *    delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
 *    dC/db(l) = delta(l)
 *    dC/dw(l) = a(l-1) * delta(l)
 *    gradient(C, a(l - 1)) = transp(w) * delta(l)
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "fclayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

////////////////////////////////////////////////////////////////////////////////////////////////
//
// FullyConnectedLayer_Context implementation
//
class FullyConnectedLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class FullyConnectedLayer;

public:
  FullyConnectedLayer_Context(const MatrixSize & output_size,
                              const MatrixSize & batch_size) :
    Base(output_size, batch_size)
  {
    YANN_CHECK_GT(output_size, 0);
    YANN_CHECK_GT(batch_size, 0);
    _zz.resize(get_batch_size(), get_output_size());
  }

  FullyConnectedLayer_Context(const RefVectorBatch & output) :
    Base(output)
  {
    YANN_CHECK_GT(yann::get_batch_size(output), 0);
    YANN_CHECK_GT(yann::get_batch_item_size(output), 0);

    _zz.resize(get_batch_size(), get_output_size());
  }

protected:
  VectorBatch              _zz;
}; // class FullyConnectedLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// FullyConnectedLayer_TrainingContext implementation
//
class FullyConnectedLayer_TrainingContext :
    public FullyConnectedLayer_Context
{
  typedef FullyConnectedLayer_Context Base;

  typedef vector<pair<MatrixSize, Value>> SamplingSortingVector;
  typedef vector<MatrixSize> SamplingCounterVector;

  friend class FullyConnectedLayer;

public:
  FullyConnectedLayer_TrainingContext(
      const MatrixSize & input_size,
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const unique_ptr<Layer::Updater> & updater,
      bool is_sampled) :
    Base(output_size, batch_size),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(input_size, 0);

    _delta_ww.resize(input_size, get_output_size());
    _delta_bb.resize(get_output_size());
    _delta.resizeLike(_zz);

    _sigma_derivative_zz.resizeLike(_zz);
    _collapse_vector = Vector::Ones(get_batch_size());

    _ww_updater->init(input_size, get_output_size());
    _bb_updater->init(1, get_output_size()); // RowMajor

    if(is_sampled) {
      _sampling_gradient_out_sorting = SamplingSortingVector(get_output_size());
      _sampling_counter = SamplingCounterVector(get_output_size(), 0);
    }
  }

  FullyConnectedLayer_TrainingContext(
      const MatrixSize & input_size,
      const RefVectorBatch & output,
      const unique_ptr<Layer::Updater> & updater,
      bool is_sampled) :
    Base(output),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(input_size, 0);

    _delta_ww.resize(input_size, get_output_size());
    _delta_bb.resize(get_output_size());
    _delta.resizeLike(_zz);

    _sigma_derivative_zz.resizeLike(_zz);
    _collapse_vector = Vector::Ones(get_batch_size());

    _ww_updater->init(input_size, get_output_size());
    _bb_updater->init(1, get_output_size()); // RowMajor

    if(is_sampled) {
      _sampling_gradient_out_sorting = SamplingSortingVector(get_output_size());
      _sampling_counter = SamplingCounterVector(get_output_size(), 0);
    }
  }

  // Layer::Context overwrites
  virtual void start_epoch()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::start_epoch();

    _ww_updater->start_epoch();
    _bb_updater->start_epoch();
  }

  virtual void reset_state()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::reset_state();

    _delta_ww.setZero();
    _delta_bb.setZero();

    _ww_updater->reset();
    _bb_updater->reset();

    if(_sampling_counter) {
      fill(_sampling_counter->begin(), _sampling_counter->end(), 0);
    }
  }

private:
  Matrix _delta_ww;
  Vector _delta_bb;
  VectorBatch _delta;

  VectorBatch _sigma_derivative_zz;
  Vector _collapse_vector;

  unique_ptr<Layer::Updater> _ww_updater;
  unique_ptr<Layer::Updater> _bb_updater;

  // sampling support
  optional<SamplingSortingVector> _sampling_gradient_out_sorting;
  optional<SamplingCounterVector> _sampling_counter;
}; // class FullyConnectedLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::FullyConnectedLayer implementation
//
yann::FullyConnectedLayer::FullyConnectedLayer(const MatrixSize & input_size,
                                               const MatrixSize & output_size) :
    _ww(input_size, output_size),
    _bb(output_size),
    _fixed_bias(false),
    _sampling_rate(2.0), // bigger than 1.0
    _activation_function(new SigmoidFunction())
{
}

yann::FullyConnectedLayer::~FullyConnectedLayer()
{
}

void yann::FullyConnectedLayer::set_activation_function(
    const unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK(activation_function);
  _activation_function = activation_function->copy();
}

void yann::FullyConnectedLayer::set_values(const Matrix & ww, const Vector & bb)
{
  YANN_CHECK(is_same_size(ww, _ww));
  YANN_CHECK(is_same_size(bb, _bb));
  _ww = ww;
  _bb = bb;
}

void yann::FullyConnectedLayer::set_fixed_bias(const Value & val)
{
  _bb.setConstant(val);
  _fixed_bias = true;
}

void yann::FullyConnectedLayer::set_sampling_rate(const double & sampling_rate)
{
  YANN_CHECK_GE(sampling_rate, 0.0);
  YANN_CHECK_LE(sampling_rate, 1.0);
  _sampling_rate = sampling_rate;
}

bool yann::FullyConnectedLayer::is_sampled() const
{
  return 0 < _sampling_rate && _sampling_rate < 1.0;
}

// Layer overwrites
bool yann::FullyConnectedLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_activation_function) {
    return false;
  }
  if(_ww.cols() != _bb.size()) {
    return false;
  }
  return true;
}

std::string yann::FullyConnectedLayer::get_name() const
{
  return "FullyConnectedLayer";
}

string yann::FullyConnectedLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << " activation: " << _activation_function->get_info()
      ;
  return oss.str();
}

bool yann::FullyConnectedLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const FullyConnectedLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  // TOOD: add deep compare
  if(_activation_function->get_info() != the_other->_activation_function->get_info()) {
    return false;
  }
  if(_fixed_bias != the_other->_fixed_bias) {
    return false;
  }
  if(!_ww.isApprox(the_other->_ww, tolerance)) {
    return false;
  }
  if(!_bb.isApprox(the_other->_bb, tolerance)) {
    return false;
  }
  return true;
}

MatrixSize yann::FullyConnectedLayer::get_input_size() const
{
  return _ww.rows();
}

MatrixSize yann::FullyConnectedLayer::get_output_size() const
{
  return _ww.cols();
}

unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<FullyConnectedLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<FullyConnectedLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_training_context(
    const MatrixSize & batch_size, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<FullyConnectedLayer_TrainingContext>(
      get_input_size(), get_output_size(), batch_size, updater, is_sampled());
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_training_context(
    const RefVectorBatch & output, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<FullyConnectedLayer_TrainingContext>(
      get_input_size(), output, updater, is_sampled());
}

template<typename InputType>
void yann::FullyConnectedLayer::feedforward_internal(
    const InputType & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_EQ(get_batch_size(input), ctx->get_batch_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  // z(l) = a(l-1)*w + b
  ctx->_zz.noalias() = MatrixFunctions<InputType>::product(input, _ww);
  plus_batches(ctx->_zz, _bb);

  // a(l) = activation(z(l))
  YANN_CHECK(is_same_size(ctx->_zz, ctx->get_output()));
  _activation_function->f(ctx->_zz, ctx->get_output(), mode);
}

void yann::FullyConnectedLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

void yann::FullyConnectedLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

template<typename InputType>
void yann::FullyConnectedLayer::backprop_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // just to make it easier to read
  const auto & zz = ctx->_zz;
  auto & sigma_derivative_zz = ctx->_sigma_derivative_zz;
  auto & delta = ctx->_delta;
  const auto & collapse_vector = ctx->_collapse_vector;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;
  const auto batch_size = get_batch_size(input);

  // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
  YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
  YANN_CHECK(is_same_size(zz, gradient_output));
  _activation_function->derivative(zz, sigma_derivative_zz);
  delta.array() = gradient_output.array() * sigma_derivative_zz.array();

  // update deltas
  // dC/db(l) = delta(l)
  // dC/dw(l) = a(l-1) * delta(l)
  YANN_CHECK_EQ(collapse_vector.size(), get_batch_size(delta));
  YANN_CHECK_EQ(delta_bb.size(), get_output_size());
  YANN_CHECK_EQ(batch_size,  get_batch_size(gradient_output));
  YANN_CHECK(is_same_size(delta_ww, _ww));
  YANN_CHECK(is_same_size(delta_bb, _bb));
  delta_ww.noalias() += MatrixFunctions<InputType>::product(input.transpose(), delta);
  if(!_fixed_bias) {
    delta_bb.noalias() += MatrixFunctions<InputType>::product(collapse_vector, delta);
  }

  // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
  // gradient(C, a(l - 1)) = transp(w) * delta(l)
  if(gradient_input) {
    (*gradient_input).noalias() = MatrixFunctions<InputType>::product(delta, _ww.transpose());
  }
}

template<typename InputType>
void yann::FullyConnectedLayer::backprop_with_sampling_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_sampling_gradient_out_sorting);
  YANN_CHECK(ctx->_sampling_counter);
  YANN_CHECK(is_valid());
  YANN_CHECK(is_sampled());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // one row at a time
  const auto batch_size = get_batch_size(input);
  const auto output_size = get_output_size();
  const MatrixSize sampled_num = get_output_size() * _sampling_rate;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;
  YANN_SLOW_CHECK_EQ(delta_bb.size(), get_output_size());
  auto & sampling_gradient_out_sorting = *(ctx->_sampling_gradient_out_sorting);
  auto & sampling_counter = *(ctx->_sampling_counter);

  YANN_SLOW_CHECK(is_same_size(delta_ww, _ww));
  YANN_SLOW_CHECK(is_same_size(delta_bb, _bb));

  if(gradient_input) {
    gradient_input->setZero();
  }

  for(MatrixSize batch_pos = 0; batch_pos < batch_size; ++batch_pos) {
    // just to make it easier to read
    const auto in = get_batch(input, batch_pos);
    const auto gradient_out = get_batch(gradient_output, batch_pos);
    const auto zz = get_batch(ctx->_zz, batch_pos);
    const auto sigma_derivative_zz = get_batch(ctx->_sigma_derivative_zz, batch_pos);

    YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
    YANN_CHECK(is_same_size(zz, gradient_out));

    // prepare sampling
    for(MatrixSize ii = 0, out_size = output_size; ii < out_size; ++ii) {
      sampling_gradient_out_sorting[ii].first = ii;
      sampling_gradient_out_sorting[ii].second = std::abs(gradient_out(ii));
    }
    sort(sampling_gradient_out_sorting.begin(), sampling_gradient_out_sorting.end(),
        [](const auto & left, const auto & right) -> bool
        {
          return left.second > right.second; // sort in descending order
        }
    );
    // TODO: we don't need to calculate it for all values
    _activation_function->derivative(zz, sigma_derivative_zz);

    for(MatrixSize ii = 0; ii < sampled_num; ++ii) {
      const auto out_pos = sampling_gradient_out_sorting[ii].first;
      YANN_SLOW_CHECK_GE(out_pos, 0);
      YANN_SLOW_CHECK_LT(out_pos, output_size);
      ++sampling_counter[out_pos];

      // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
      const auto delta = gradient_out(out_pos) * sigma_derivative_zz(out_pos);

      // update deltas
      // dC/db(l) = delta(l)
      // dC/dw(l) = a(l-1) * delta(l)
      delta_ww.col(out_pos) += in * delta;
      if(!_fixed_bias) {
        delta_bb(out_pos) += delta;
      }
      // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
      // // gradient(C, a(l - 1)) = transp(w) * delta(l)
      if(gradient_input) {
        auto gradient_in = get_batch(*gradient_input, batch_pos);
        gradient_in.noalias() += delta * _ww.col(out_pos);
      }
    }
  }
}

void yann::FullyConnectedLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  if(is_sampled()) {
    backprop_with_sampling_internal(gradient_output, input, gradient_input, context);
  } else {
    backprop_internal(gradient_output, input, gradient_input, context);
  }
}

void yann::FullyConnectedLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  // <RefConstSparseMatrix> is required for MatrixFunctions<>::product
  if(is_sampled()) {
    backprop_with_sampling_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
  } else {
    backprop_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
  }
}

void yann::FullyConnectedLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    _ww.setZero();
    if(!_fixed_bias) {
      _bb.setZero();
    }
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      gen01->generate(_ww);
      if(!_fixed_bias) {
        gen01->generate(_bb);
      }
    }
    break;
  }
}

void yann::FullyConnectedLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_ww_updater);
  YANN_CHECK(ctx->_bb_updater);
  YANN_CHECK(is_same_size(_ww, ctx->_delta_ww));
  YANN_CHECK(is_same_size(_bb, ctx->_delta_bb));

  if(!is_sampled()) {
    ctx->_ww_updater->update(ctx->_delta_ww, tests_num, _ww);
    if(!_fixed_bias) {
      ctx->_bb_updater->update(ctx->_delta_bb, tests_num, _bb);
    }
  } else {
    const auto & sampling_counter = *(ctx->_sampling_counter);
    const auto & delta_ww = ctx->_delta_ww;
    const auto & delta_bb = ctx->_delta_bb;
    const auto rows = delta_ww.rows();

    YANN_CHECK_EQ((MatrixSize)sampling_counter.size(), delta_ww.cols());
    for(MatrixSize ii = 0; ii < delta_ww.cols(); ++ii) {
      if(sampling_counter[ii] <= 0) {
        continue;
      }
      // can't use delta_ww.col(ii) and _ww.col(ii) here
      ctx->_ww_updater->update(
          delta_ww.block(0, ii, rows, 1),
          sampling_counter[ii],
          _ww.block(0, ii, rows, 1));
      if(!_fixed_bias) {
        ctx->_bb_updater->update(delta_bb(ii), sampling_counter[ii], _bb(ii));
      }
    }
  }
}

// the format is (w:<weights>,b:<biases>)
void yann::FullyConnectedLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');
  read_object(is, "w", _ww);
  read_char(is, ',');
  read_object(is, "b", _bb);
  read_char(is, ')');
}

// the format is (w:<weights>,b:<biases>)
void yann::FullyConnectedLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  write_object(os, "w", _ww);
  os << ",";
  write_object(os, "b", _bb);
  os << ")";
}