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

// @sect3{Include files}

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

#include <deal.II/base/convergence_table.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.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/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/data_out_stack.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/solution_transfer.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>
#include <iostream>

// 然后照例将这个程序的所有内容放入一个命名空间，并将deal.II命名空间导入到我们将要工作的命名空间中。我们还定义了一个标识符，以便在 MMS 被定义时可以运行MMS代码。否则，该程序就会解决原来的问题。

namespace BlackScholesSolver
{
  using namespace dealii;

#define MMS
// @sect3{Solution Class}

// 在使用MMS进行测试时，这部分为已知的解决方案创建一个类。这里我们使用 $v(\tau,S) = -\tau^2 -S^2 + 6$ 作为解决方案。我们需要包括求解方程和梯度，以便进行H1半规范计算。

  template <int dim>
  class Solution : public Function<dim>
  {
  public:
    Solution(const double maturity_time);

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

    virtual Tensor<1, dim>
    gradient(const Point<dim> & p,
             const unsigned int component = 0) const override;

  private:
    const double maturity_time;
  };

  template <int dim>
  Solution<dim>::Solution(const double maturity_time)
    : maturity_time(maturity_time)
  {
    Assert(dim == 1, ExcNotImplemented());
  }

  template <int dim>
  double Solution<dim>::value(const Point<dim> & p,
                              const unsigned int component) const
  {
    return -Utilities::fixed_power<2, double>(p(component)) -
           Utilities::fixed_power<2, double>(this->get_time()) + 6;
  }

  template <int dim>
  Tensor<1, dim> Solution<dim>::gradient(const Point<dim> & p,
                                         const unsigned int component) const
  {
    return Point<dim>(-2 * p(component));
  }

//  @sect3{Equation Data}

// 在下面的类和函数中，我们实现了定义这个问题的右手边和边界值，为此我们需要函数对象。右手边的选择是在介绍的最后讨论的。

// 首先，我们处理初始条件。

  template <int dim>
  class InitialConditions : public Function<dim>
  {
  public:
    InitialConditions(const double strike_price);

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

  private:
    const double strike_price;
  };

  template <int dim>
  InitialConditions<dim>::InitialConditions(const double strike_price)
    : strike_price(strike_price)
  {}

  template <int dim>
  double InitialConditions<dim>::value(const Point<dim> & p,
                                       const unsigned int component) const
  {
#ifdef MMS
    return -Utilities::fixed_power<2, double>(p(component)) + 6;
#else
    return std::max(p(component) - strike_price, 0.);
#endif
  }

// 接下来，我们处理左边的边界条件。

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

  template <int dim>
  double LeftBoundaryValues<dim>::value(const Point<dim> &,
                                        const unsigned int /*component*/) const
  {
#ifdef MMS
    return -Utilities::fixed_power<2, double>(this->get_time()) + 6;
#else
    return 0.;
#endif
  }

// 然后，我们处理右边的边界条件。

  template <int dim>
  class RightBoundaryValues : public Function<dim>
  {
  public:
    RightBoundaryValues(const double strike_price, const double interest_rate);

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

  private:
    const double strike_price;
    const double interest_rate;
  };

  template <int dim>
  RightBoundaryValues<dim>::RightBoundaryValues(const double strike_price,
                                                const double interest_rate)
    : strike_price(strike_price)
    , interest_rate(interest_rate)
  {}

  template <int dim>
  double RightBoundaryValues<dim>::value(const Point<dim> & p,
                                         const unsigned int component) const
  {
#ifdef MMS
    return -Utilities::fixed_power<2, double>(p(component)) -
           Utilities::fixed_power<2, double>(this->get_time()) + 6;
#else
    return (p(component) - strike_price) *
           exp((-interest_rate) * (this->get_time()));
#endif
  }

// 最后，我们处理右边的问题。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide(const double asset_volatility, const double interest_rate);

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

  private:
    const double asset_volatility;
    const double interest_rate;
  };

  template <int dim>
  RightHandSide<dim>::RightHandSide(const double asset_volatility,
                                    const double interest_rate)
    : asset_volatility(asset_volatility)
    , interest_rate(interest_rate)
  {}

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
#ifdef MMS
    return 2 * (this->get_time()) -
           Utilities::fixed_power<2, double>(asset_volatility * p(component)) -
           2 * interest_rate * Utilities::fixed_power<2, double>(p(component)) -
           interest_rate *
             (-Utilities::fixed_power<2, double>(p(component)) -
              Utilities::fixed_power<2, double>(this->get_time()) + 6);
#else
    (void)p;
    (void)component;
    return 0.0;
#endif
  }

//  @sect3{The BlackScholes Class}

// 下一块是这个程序的主类的声明。这与 Step-26 的教程非常相似，只是做了一些修改。必须添加新的矩阵来计算A和B矩阵，以及介绍中提到的 $V_{diff}$ 向量。我们还定义了问题中使用的参数。

// -  maximum_stock_price  ：空间域的强加上限。这是允许的最大股票价格。

// -  maturity_time  ：时间域的上限。这是期权到期的时间。

// -  asset_volatility  ：股票价格的波动率。

// -  interest_rate  : 无风险利率。

// -  strike_price  ：买方在到期时可以选择购买股票的约定价格。

// 本程序与 step-26 之间的一些细微差别是创建了 a_matrix and the b_matrix  ，这在介绍中已经说明。然后，我们还需要存储当前时间、时间步长和当前时间步长的数字。接下来，我们将把输出存储到一个 DataOutStack 的变量中，因为我们将把每个时间的解分层在上面，以创建解流形。然后，我们有一个变量来存储当前的周期和我们在计算解决方案时将运行的周期数。循环是给定一个网格的一个完整的解决方案计算。我们在每个周期之间细化一次网格，以展示我们程序的收敛特性。最后，我们将收敛数据存储到一个收敛表中。

// 就成员函数而言，我们有一个函数可以计算每个周期的收敛信息，称为  process_solution  。这就像在  step-7  中所做的那样。

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

    void run();

  private:
    void setup_system();
    void solve_time_step();
    void refine_grid();
    void process_solution();
    void add_results_for_output();
    void write_convergence_table();

    const double maximum_stock_price;
    const double maturity_time;
    const double asset_volatility;
    const double interest_rate;
    const double strike_price;

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

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> a_matrix;
    SparseMatrix<double> b_matrix;
    SparseMatrix<double> system_matrix;

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

    double time;
    double time_step;

    const double       theta;
    const unsigned int n_cycles;
    const unsigned int n_time_steps;

    DataOutStack<dim>        data_out_stack;
    std::vector<std::string> solution_names;

    ConvergenceTable convergence_table;
  };

// @sect3{The BlackScholes Implementation}

// 现在，我们进入主类的实现阶段。我们将为问题中使用的各种参数设置数值。选择这些是因为它们是这些参数的相当正常的值。尽管股票价格在现实中没有上限（事实上是无限的），但我们规定了一个上限，即行权价格的两倍。两倍于行权价的选择有些武断，但它足够大，可以看到解决方案的有趣部分。

  template <int dim>
  BlackScholes<dim>::BlackScholes()
    : maximum_stock_price(1.)
    , maturity_time(1.)
    , asset_volatility(.2)
    , interest_rate(0.05)
    , strike_price(0.5)
    , fe(1)
    , dof_handler(triangulation)
    , time(0.0)
    , theta(0.5)
    , n_cycles(4)
    , n_time_steps(5000)
  {
    Assert(dim == 1, ExcNotImplemented());
  }
// @sect4{BlackScholes::setup_system}

// 下一个函数设置了DoFHandler对象，计算了约束条件，并将线性代数对象设置为正确的大小。我们还在这里通过调用库中的一个函数来计算质量矩阵。接下来我们将计算其他三个矩阵，因为这些矩阵需要 "手工 "计算。

// 注意，时间步长在这里被初始化，因为计算时间步长需要成熟的时间。

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

    time_step = maturity_time / n_time_steps;

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    constraints.close();
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler,
                                    dsp,
                                    constraints,
                                    /*keep_constrained_dofs =  */ true);
    sparsity_pattern.copy_from(dsp);

    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    a_matrix.reinit(sparsity_pattern);
    b_matrix.reinit(sparsity_pattern);
    system_matrix.reinit(sparsity_pattern);

    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);

// 下面是创建非恒定系数的拉普拉斯矩阵的代码。这与介绍中的矩阵D相对应。这个非恒定系数在 current_coefficient 变量中表示。

    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    QGauss<dim>        quadrature_formula(fe.degree + 1);
    FEValues<dim>      fe_values(fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0.;
        fe_values.reinit(cell);
        for (const unsigned int q_index : fe_values.quadrature_point_indices())
          {
            const double current_coefficient =
              fe_values.quadrature_point(q_index).square();
            for (const unsigned int i : fe_values.dof_indices())
              {
                for (const unsigned int j : fe_values.dof_indices())
                  cell_matrix(i, j) +=
                    (current_coefficient *              // (x_q)^2
                     fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                     fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                     fe_values.JxW(q_index));           // dx
              }
          }
        cell->get_dof_indices(local_dof_indices);
        for (const unsigned int i : fe_values.dof_indices())
          {
            for (const unsigned int j : fe_values.dof_indices())
              laplace_matrix.add(local_dof_indices[i],
                                 local_dof_indices[j],
                                 cell_matrix(i, j));
          }
      }

// 现在我们将创建A矩阵。下面是创建矩阵A的代码，在介绍中已经讨论过。非恒定系数再次用 current_coefficient 这个变量表示。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0.;
        fe_values.reinit(cell);
        for (const unsigned int q_index : fe_values.quadrature_point_indices())
          {
            const Tensor<1, dim> current_coefficient =
              fe_values.quadrature_point(q_index);
            for (const unsigned int i : fe_values.dof_indices())
              {
                for (const unsigned int j : fe_values.dof_indices())
                  {
                    cell_matrix(i, j) +=
                      (current_coefficient *               // x_q
                       fe_values.shape_grad(i, q_index) *  // grad phi_i(x_q)
                       fe_values.shape_value(j, q_index) * // phi_j(x_q)
                       fe_values.JxW(q_index));            // dx
                  }
              }
          }
        cell->get_dof_indices(local_dof_indices);
        for (const unsigned int i : fe_values.dof_indices())
          {
            for (const unsigned int j : fe_values.dof_indices())
              a_matrix.add(local_dof_indices[i],
                           local_dof_indices[j],
                           cell_matrix(i, j));
          }
      }

// 最后我们将创建矩阵B。下面是创建矩阵B的代码，在介绍中已经讨论过。非恒定系数再次用 current_coefficient 这个变量表示。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0.;
        fe_values.reinit(cell);
        for (const unsigned int q_index : fe_values.quadrature_point_indices())
          {
            const Tensor<1, dim> current_coefficient =
              fe_values.quadrature_point(q_index);
            for (const unsigned int i : fe_values.dof_indices())
              {
                for (const unsigned int j : fe_values.dof_indices())
                  cell_matrix(i, j) +=
                    (current_coefficient *               // x_q
                     fe_values.shape_value(i, q_index) * // phi_i(x_q)
                     fe_values.shape_grad(j, q_index) *  // grad phi_j(x_q)
                     fe_values.JxW(q_index));            // dx
              }
          }
        cell->get_dof_indices(local_dof_indices);
        for (const unsigned int i : fe_values.dof_indices())
          {
            for (const unsigned int j : fe_values.dof_indices())
              b_matrix.add(local_dof_indices[i],
                           local_dof_indices[j],
                           cell_matrix(i, j));
          }
      }

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
// @sect4{BlackScholes::solve_time_step}

// 下一个函数是解决单个时间步长的实际线性系统的函数。这里唯一有趣的是，我们建立的矩阵是对称正定的，所以我们可以使用共轭梯度法。

  template <int dim>
  void BlackScholes<dim>::solve_time_step()
  {
    SolverControl                          solver_control(1000, 1e-12);
    SolverCG<Vector<double>>               cg(solver_control);
    PreconditionSSOR<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.0);
    cg.solve(system_matrix, solution, system_rhs, preconditioner);
    constraints.distribute(solution);
  }
// @sect4{BlackScholes::add_results_for_output}

// 这是简单地将解决方案的碎片拼接起来的功能。为此，我们在每个时间段创建一个新的层，然后添加该时间段的解决方案向量。然后，该函数使用'build_patches'将其与旧的解决方案缝合在一起。

  template <int dim>
  void BlackScholes<dim>::add_results_for_output()
  {
    data_out_stack.new_parameter_value(time, time_step);
    data_out_stack.attach_dof_handler(dof_handler);
    data_out_stack.add_data_vector(solution, solution_names);
    data_out_stack.build_patches(2);
    data_out_stack.finish_parameter_value();
  }
// @sect4{BlackScholes::refine_grid}

// 对于我们所做的全局细化来说，有一个函数是有些不必要的。之所以有这个函数，是为了允许以后有可能进行适应性细化。

  template <int dim>
  void BlackScholes<dim>::refine_grid()
  {
    triangulation.refine_global(1);
  }
// @sect4{BlackScholes::process_solution}

// 这就是我们计算收敛和误差数据的地方，以评估程序的有效性。在这里，我们计算 $L^2$  、 $H^1$  和 $L^{\infty}$ 的准则。

  template <int dim>
  void BlackScholes<dim>::process_solution()
  {
    Solution<dim> sol(maturity_time);
    sol.set_time(time);
    Vector<float> difference_per_cell(triangulation.n_active_cells());
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      sol,
                                      difference_per_cell,
                                      QGauss<dim>(fe.degree + 1),
                                      VectorTools::L2_norm);
    const double L2_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::L2_norm);
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      sol,
                                      difference_per_cell,
                                      QGauss<dim>(fe.degree + 1),
                                      VectorTools::H1_seminorm);
    const double H1_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::H1_seminorm);
    const QTrapezoid<1>  q_trapezoid;
    const QIterated<dim> q_iterated(q_trapezoid, fe.degree * 2 + 1);
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      sol,
                                      difference_per_cell,
                                      q_iterated,
                                      VectorTools::Linfty_norm);
    const double Linfty_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::Linfty_norm);
    const unsigned int n_active_cells = triangulation.n_active_cells();
    const unsigned int n_dofs         = dof_handler.n_dofs();
    convergence_table.add_value("cells", n_active_cells);
    convergence_table.add_value("dofs", n_dofs);
    convergence_table.add_value("L2", L2_error);
    convergence_table.add_value("H1", H1_error);
    convergence_table.add_value("Linfty", Linfty_error);
  }
// @sect4{BlackScholes::write_convergence_table }

// 接下来的部分是建立收敛和误差表。通过这个，我们需要设置如何输出在  BlackScholes::process_solution  期间计算的数据。首先，我们将创建标题并正确设置单元格。在这期间，我们还将规定结果的精度。然后，我们将根据  $L^2$  、  $H^1$  和  $L^{\infty}$  规范把计算出来的误差写到控制台和错误的LaTeX文件中。

  template <int dim>
  void BlackScholes<dim>::write_convergence_table()
  {
    convergence_table.set_precision("L2", 3);
    convergence_table.set_precision("H1", 3);
    convergence_table.set_precision("Linfty", 3);
    convergence_table.set_scientific("L2", true);
    convergence_table.set_scientific("H1", true);
    convergence_table.set_scientific("Linfty", true);
    convergence_table.set_tex_caption("cells", "\\# cells");
    convergence_table.set_tex_caption("dofs", "\\# dofs");
    convergence_table.set_tex_caption("L2", "@f$L^2@f$-error");
    convergence_table.set_tex_caption("H1", "@f$H^1@f$-error");
    convergence_table.set_tex_caption("Linfty", "@f$L^\\infty@f$-error");
    convergence_table.set_tex_format("cells", "r");
    convergence_table.set_tex_format("dofs", "r");
    std::cout << std::endl;
    convergence_table.write_text(std::cout);
    std::string error_filename = "error";
    error_filename += "-global";
    error_filename += ".tex";
    std::ofstream error_table_file(error_filename);
    convergence_table.write_tex(error_table_file);

// 接下来，我们将制作收敛表。我们将再次把它写到控制台和收敛LaTeX文件中。

    convergence_table.add_column_to_supercolumn("cells", "n cells");
    std::vector<std::string> new_order;
    new_order.emplace_back("n cells");
    new_order.emplace_back("H1");
    new_order.emplace_back("L2");
    convergence_table.set_column_order(new_order);
    convergence_table.evaluate_convergence_rates(
      "L2", ConvergenceTable::reduction_rate);
    convergence_table.evaluate_convergence_rates(
      "L2", ConvergenceTable::reduction_rate_log2);
    convergence_table.evaluate_convergence_rates(
      "H1", ConvergenceTable::reduction_rate);
    convergence_table.evaluate_convergence_rates(
      "H1", ConvergenceTable::reduction_rate_log2);
    std::cout << std::endl;
    convergence_table.write_text(std::cout);
    std::string conv_filename = "convergence";
    conv_filename += "-global";
    switch (fe.degree)
      {
        case 1:
          conv_filename += "-q1";
          break;
        case 2:
          conv_filename += "-q2";
          break;
        default:
          Assert(false, ExcNotImplemented());
      }
    conv_filename += ".tex";
    std::ofstream table_file(conv_filename);
    convergence_table.write_tex(table_file);
  }
// @sect4{BlackScholes::run}

// 现在我们进入了程序的主要驱动部分。在这里我们要做的是在时间步数中循环往复，并在每次计算解向量的工作。在这里的顶部，我们设置初始细化值，然后创建一个网格。然后我们对这个网格进行一次细化。接下来，我们设置了data_out_stack对象来存储我们的解决方案。最后，我们启动一个for循环来循环处理这些循环。这让我们为每一个连续的网格细化重新计算出一个解决方案。在每次迭代开始时，我们需要重新设置时间和时间步长。我们引入一个if语句来完成这个任务，因为我们不想在第一次迭代时就这样做。

  template <int dim>
  void BlackScholes<dim>::run()
  {
    GridGenerator::hyper_cube(triangulation, 0.0, maximum_stock_price, true);
    triangulation.refine_global(0);

    solution_names.emplace_back("u");
    data_out_stack.declare_data_vector(solution_names,
                                       DataOutStack<dim>::dof_vector);

    Vector<double> vmult_result;
    Vector<double> forcing_terms;

    for (unsigned int cycle = 0; cycle < n_cycles; cycle++)
      {
        if (cycle != 0)
          {
            refine_grid();
            time = 0.0;
          }

        setup_system();

        std::cout << std::endl
                  << "===========================================" << std::endl
                  << "Cycle " << cycle << ':' << std::endl
                  << "Number of active cells: "
                  << triangulation.n_active_cells() << std::endl
                  << "Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl
                  << std::endl;

        VectorTools::interpolate(dof_handler,
                                 InitialConditions<dim>(strike_price),
                                 solution);

        if (cycle == (n_cycles - 1))
          {
            add_results_for_output();
          }

// 接下来，我们运行主循环，该循环一直运行到超过成熟时间。我们首先计算方程的右侧，这在介绍中有所描述。回顾一下，它包含术语 $\left[-\frac{1}{4}k_n\sigma^2\mathbf{D}-k_nr\mathbf{M}+k_n\sigma^2 \mathbf{B}-k_nr\mathbf{A}+\mathbf{M}\right]V^{n-1}$  。我们把这些项放到变量system_rhs中，借助于一个临时向量。

        vmult_result.reinit(dof_handler.n_dofs());
        forcing_terms.reinit(dof_handler.n_dofs());
        for (unsigned int timestep_number = 0; timestep_number < n_time_steps;
             ++timestep_number)
          {
            time += time_step;

            if (timestep_number % 1000 == 0)
              std::cout << "Time step " << timestep_number << " at t=" << time
                        << std::endl;

            mass_matrix.vmult(system_rhs, solution);

            laplace_matrix.vmult(vmult_result, solution);
            system_rhs.add(
              (-1) * (1 - theta) * time_step *
                Utilities::fixed_power<2, double>(asset_volatility) * 0.5,
              vmult_result);
            mass_matrix.vmult(vmult_result, solution);

            system_rhs.add((-1) * (1 - theta) * time_step * interest_rate * 2,
                           vmult_result);

            a_matrix.vmult(vmult_result, solution);
            system_rhs.add((-1) * time_step * interest_rate, vmult_result);

            b_matrix.vmult(vmult_result, solution);
            system_rhs.add(
              (-1) * Utilities::fixed_power<2, double>(asset_volatility) *
                time_step * 1,
              vmult_result);

// 第二块是计算源项的贡献。这与术语  $-k_n\left[\frac{1}{2}F^{n-1} +\frac{1}{2}F^n\right]$  相对应。下面的代码调用  VectorTools::create_right_hand_side  来计算向量  $F$  ，在这里我们在评估之前设置了右侧（源）函数的时间。这一切的结果最终都在forcing_terms变量中。

            RightHandSide<dim> rhs_function(asset_volatility, interest_rate);
            rhs_function.set_time(time);
            VectorTools::create_right_hand_side(dof_handler,
                                                QGauss<dim>(fe.degree + 1),
                                                rhs_function,
                                                forcing_terms);
            forcing_terms *= time_step * theta;
            system_rhs -= forcing_terms;

            rhs_function.set_time(time - time_step);
            VectorTools::create_right_hand_side(dof_handler,
                                                QGauss<dim>(fe.degree + 1),
                                                rhs_function,
                                                forcing_terms);
            forcing_terms *= time_step * (1 - theta);
            system_rhs -= forcing_terms;

// 接下来，我们将强迫项添加到来自时间步长的强迫项中，同时建立矩阵 $\left[\mathbf{M}+ \frac{1}{4}k_n\sigma^2\mathbf{D}+k_nr\mathbf{M}\right]$ ，我们必须在每个时间步长中进行反转。这些操作的最后一块是消除线性系统中悬挂的节点约束自由度。

            system_matrix.copy_from(mass_matrix);
            system_matrix.add(
              (theta)*time_step *
                Utilities::fixed_power<2, double>(asset_volatility) * 0.5,
              laplace_matrix);
            system_matrix.add((time_step)*interest_rate * theta * (1 + 1),
                              mass_matrix);

            constraints.condense(system_matrix, system_rhs);

// 在解决这个问题之前，我们还需要做一个操作：边界值。为此，我们创建一个边界值对象，将适当的时间设置为当前时间步长的时间，并像以前多次那样对其进行评估。其结果也被用来在线性系统中设置正确的边界值。

            {
              RightBoundaryValues<dim> right_boundary_function(strike_price,
                                                               interest_rate);
              LeftBoundaryValues<dim>  left_boundary_function;
              right_boundary_function.set_time(time);
              left_boundary_function.set_time(time);
              std::map<types::global_dof_index, double> boundary_values;
              VectorTools::interpolate_boundary_values(dof_handler,
                                                       0,
                                                       left_boundary_function,
                                                       boundary_values);
              VectorTools::interpolate_boundary_values(dof_handler,
                                                       1,
                                                       right_boundary_function,
                                                       boundary_values);
              MatrixTools::apply_boundary_values(boundary_values,
                                                 system_matrix,
                                                 solution,
                                                 system_rhs);
            }

// 解决了这个问题，我们要做的就是求解系统，生成最后一个周期的图形数据，并创建收敛表数据。

            solve_time_step();

            if (cycle == (n_cycles - 1))
              {
                add_results_for_output();
              }
          }
#ifdef MMS
        process_solution();
#endif
      }

    const std::string filename = "solution.vtk";
    std::ofstream     output(filename);
    data_out_stack.write_vtk(output);

#ifdef MMS
    write_convergence_table();
#endif
  }

} // namespace BlackScholesSolver
// @sect3{The main Function}

// 走到这一步，这个程序的主函数又没有什么好讨论的了：看起来自 step-6 以来的所有此类函数。

int main()
{
  try
    {
      using namespace BlackScholesSolver;

      BlackScholes<1> black_scholes_solver;
      black_scholes_solver.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2013 Gauthier Brun <brun.gauthier@gmail.com>
// Copyright (C) 2013 Nicolas Carre <nicolas.carre@ensimag.fr>
// Copyright (C) 2013 Jean Ceccato <jean.ceccato@ensimag.fr>
// Copyright (C) 2013 Pierre Zoppitelli <pierre.zoppitelli@ensimag.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/

// discard stack allocation as that too bypasses malloc
#define EIGEN_STACK_ALLOCATION_LIMIT 0
#define EIGEN_RUNTIME_NO_MALLOC

#include "main.h"
#include <Eigen/SVD>
#include <iostream>
#include <Eigen/LU>

#define SVD_DEFAULT(M) BDCSVD<M>
#define SVD_FOR_MIN_NORM(M) BDCSVD<M>
#include "svd_common.h"

// Check all variants of JacobiSVD
template<typename MatrixType>
void bdcsvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
{
  MatrixType m = a;
  if(pickrandom)
    svd_fill_random(m);

  CALL_SUBTEST(( svd_test_all_computation_options<BDCSVD<MatrixType> >(m, false)  ));
}

template<typename MatrixType>
void bdcsvd_method()
{
  enum { Size = MatrixType::RowsAtCompileTime };
  typedef typename MatrixType::RealScalar RealScalar;
  typedef Matrix<RealScalar, Size, 1> RealVecType;
  MatrixType m = MatrixType::Identity();
  VERIFY_IS_APPROX(m.bdcSvd().singularValues(), RealVecType::Ones());
  VERIFY_RAISES_ASSERT(m.bdcSvd().matrixU());
  VERIFY_RAISES_ASSERT(m.bdcSvd().matrixV());
  VERIFY_IS_APPROX(m.bdcSvd(ComputeFullU|ComputeFullV).solve(m), m);
}

// compare the Singular values returned with Jacobi and Bdc
template<typename MatrixType>
void compare_bdc_jacobi(const MatrixType& a = MatrixType(), unsigned int computationOptions = 0)
{
  MatrixType m = MatrixType::Random(a.rows(), a.cols());
  BDCSVD<MatrixType> bdc_svd(m);
  JacobiSVD<MatrixType> jacobi_svd(m);
  VERIFY_IS_APPROX(bdc_svd.singularValues(), jacobi_svd.singularValues());
  if(computationOptions & ComputeFullU) VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU());
  if(computationOptions & ComputeThinU) VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU());
  if(computationOptions & ComputeFullV) VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV());
  if(computationOptions & ComputeThinV) VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV());
}

void test_bdcsvd()
{
  CALL_SUBTEST_3(( svd_verify_assert<BDCSVD<Matrix3f>  >(Matrix3f()) ));
  CALL_SUBTEST_4(( svd_verify_assert<BDCSVD<Matrix4d>  >(Matrix4d()) ));
  CALL_SUBTEST_7(( svd_verify_assert<BDCSVD<MatrixXf>  >(MatrixXf(10,12)) ));
  CALL_SUBTEST_8(( svd_verify_assert<BDCSVD<MatrixXcd> >(MatrixXcd(7,5)) ));

  CALL_SUBTEST_101(( svd_all_trivial_2x2(bdcsvd<Matrix2cd>) ));
  CALL_SUBTEST_102(( svd_all_trivial_2x2(bdcsvd<Matrix2d>) ));

  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_3(( bdcsvd<Matrix3f>() ));
    CALL_SUBTEST_4(( bdcsvd<Matrix4d>() ));
    CALL_SUBTEST_5(( bdcsvd<Matrix<float,3,5> >() ));

    int r = internal::random<int>(1, EIGEN_TEST_MAX_SIZE/2),
        c = internal::random<int>(1, EIGEN_TEST_MAX_SIZE/2);

    TEST_SET_BUT_UNUSED_VARIABLE(r)
    TEST_SET_BUT_UNUSED_VARIABLE(c)

    CALL_SUBTEST_6((  bdcsvd(Matrix<double,Dynamic,2>(r,2)) ));
    CALL_SUBTEST_7((  bdcsvd(MatrixXf(r,c)) ));
    CALL_SUBTEST_7((  compare_bdc_jacobi(MatrixXf(r,c)) ));
    CALL_SUBTEST_10(( bdcsvd(MatrixXd(r,c)) ));
    CALL_SUBTEST_10(( compare_bdc_jacobi(MatrixXd(r,c)) ));
    CALL_SUBTEST_8((  bdcsvd(MatrixXcd(r,c)) ));
    CALL_SUBTEST_8((  compare_bdc_jacobi(MatrixXcd(r,c)) ));

    // Test on inf/nan matrix
    CALL_SUBTEST_7(  (svd_inf_nan<BDCSVD<MatrixXf>, MatrixXf>()) );
    CALL_SUBTEST_10( (svd_inf_nan<BDCSVD<MatrixXd>, MatrixXd>()) );
  }

  // test matrixbase method
  CALL_SUBTEST_1(( bdcsvd_method<Matrix2cd>() ));
  CALL_SUBTEST_3(( bdcsvd_method<Matrix3f>() ));

  // Test problem size constructors
  CALL_SUBTEST_7( BDCSVD<MatrixXf>(10,10) );

  // Check that preallocation avoids subsequent mallocs
  // Disbaled because not supported by BDCSVD
  // CALL_SUBTEST_9( svd_preallocate<void>() );

  CALL_SUBTEST_2( svd_underoverflow<void>() );
}


#define BOOST_TEST_MODULE "test_excluded_volume_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/external/ExcludedVolumeWallPotential.hpp>

BOOST_AUTO_TEST_CASE(ExcludedVolumeWallPotential_double)
{
    using real_type = double;
    constexpr static std::size_t N = 10000;
    constexpr static real_type h   = 1e-6;
    constexpr static real_type tol = 1e-5;

    const real_type epsilon = 1.0;
    const real_type radius  = 1.0;
    const std::vector<std::pair<std::size_t, real_type>> radii{{0, radius}};

    mjolnir::ExcludedVolumeWallPotential<real_type> exvw(epsilon, 2.0, radii);

    const real_type cutoff_length = exvw.max_cutoff_length();
    const real_type z_max         = cutoff_length;
    const real_type z_min         = radius * 0.5;
    const real_type dz            = (z_max - z_min) / N;

    real_type z = z_min;
    for(std::size_t i = 0; i < N; ++i)
    {
        const real_type pot1 = exvw.potential(0, z + h);
        const real_type pot2 = exvw.potential(0, z - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = exvw.derivative(0, z);

        if(std::abs(deri) > tol)
        {
            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
        }
        else
        {
            BOOST_TEST(deri == 0.0, boost::test_tools::tolerance(tol));
        }
        z += dz;
    }
}


#include <iostream>
#include <fstream>
#include <stack>
#include <queue>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/assignment.hpp>

namespace stackqueueAlgorithms
{
      using namespace std;

      /*  print a stack in from bottom to top */
      template <typename elementType>
      void printStack(stack<elementType> &input)
      {
            stack<elementType> cache;
            while (!input.empty())
            {
                  cache.push(input.top());
                  input.pop();
            }
            while (!cache.empty())
            {
                  cout << "|" << cache.top();
                  input.push(cache.top());
                  cache.pop();
            }
            cout << endl;
      };

      /* 3.2
            a stack class with O(1) return time for the minimum element
      */
      template <typename elementType>
      class minStack
      {
            private:
                  stack< pair<elementType,int> > mins;
                  stack<elementType> elements;
            public:
                  bool empty() { return elements.empty(); };

                  int size() { return elements.size(); };

                  elementType top() { return elements.top(); };

                  void push(elementType newElement)
                  {
                        if (mins.empty() || newElement < (mins.top()).first )
                        { mins.push(make_pair(newElement,1)); }
                        else
                        {
                              if (newElement == (mins.top()).first )
                              { (mins.top()).second++; }
                        }
                        elements.push(newElement);
                  };

                  void pop()
                  {
                        if (elements.top() == (mins.top()).first)
                        { (mins.top()).second--; }
                        if ((mins.top()).second==0) {mins.pop();}
                        elements.pop();
                  };

                  elementType min() { return (mins.top()).first; };

                  void print() { printStack(elements); };
      };

      /*3.3
            a conventional stack, but as a stack of "piles"
            which are sub-stacks that can also be poped.
      */
      template <typename elementType>
      class pileStack
      {
            private:
                  stack< stack<elementType> > superStack;
                  int pileSize;
                  int endPileSize;
            public:
                  pileStack(int pileSize)
                  {
                        this->pileSize = pileSize;
                        stack<elementType> s;
                        superStack.push(s);
                        endPileSize = 0;
                  };

                  int getPileSize() { return pileSize; };

                  bool empty() { return superStack.empty(); };

                  int size() { return (superStack.size()-1)*pileSize + endPileSize; };

                  elementType top() { return superStack.top().top(); };

                  void push(elementType newElement)
                  {
                        if (endPileSize<pileSize)
                        {
                              superStack.top().push(newElement);
                              endPileSize++;
                        }
                        else
                        {
                              stack<elementType> s;
                              superStack.push(s);
                              superStack.top().push(newElement);
                              endPileSize = 1;
                        }
                  };

                  void pop()
                  {
                        superStack.top().pop();
                        endPileSize--;
                        if (superStack.top().empty())
                        {
                              superStack.pop();
                              endPileSize = pileSize;
                        }
                  };

                  void popPile()
                  {
                        superStack.pop();
                        endPileSize = pileSize;
                  };

                  void print()
                  {
                        stack< stack<elementType> > cache;
                        while (!superStack.empty())
                        {
                              cache.push(superStack.top());
                              superStack.pop();
                        }

                        while (!cache.empty())
                        {
                              printStack(cache.top());
                              superStack.push(cache.top());
                              cache.pop();
                        }
                        cout << "--- end --- " << endl;
                  };
      };

      /*3.5
            a queue implemented using two stacks
       */
      template <typename elementType>
      class dsQueue
      {
            private:
                  stack<elementType> ls,rs;

                  void move(bool print)
                  {
                        stack<elementType> *fs,*es;
                        if (ls.empty()) { es = &ls; fs = &rs; }
                        else { es = &rs; fs = &ls; }

                        while (!fs->empty())
                        {
                              elementType e = fs->top();
                              es->push(e); fs->pop();
                              if (print) { cout << e << "<"; }
                        }
                        if (print) { cout << endl; }
                  };

                  bool L() {return rs.empty() && !ls.empty();}
                  bool R() {return ls.empty() && !rs.empty();}

            public:
                  bool empty() { return ls.empty() && rs.empty() ;};

                  int size() {return ls.size() + rs.size(); };

                  void push(elementType newElement)
                  {
                        if (this->R()) { this->move(false); }
                        ls.push(newElement);
                  };

                  void pop()
                  {
                        if (this->L()) { this->move(false); }
                        if (!this->empty()) { rs.pop(); }
                  };

                  elementType front()
                  {
                        if (this->L()) { this->move(false); }
                        return rs.top();
                  };

                  elementType back()
                  {
                        if (this->R()) { this->move(false); }
                        return ls.top();
                  };

                  void print()
                  {
                        if (this->L()) { this->move(false); }
                        this->move(true);
                  };
      };

      /* 3.6
            an insertion-sort type sorting method ( O(N^2) )
            for a stack using another stack of the same size
      */
      template <typename elementType>
      void slowSort(stack<elementType> &s)
      {
            stack<elementType> cache;
            stack<elementType> *si,*sf,*temp;
            int iter = 0;
            int totalLength;

            while (!s.empty()) { cache.push(s.top()); s.pop(); iter++; }
            totalLength = iter;

            si = &cache; sf = &s;
            while (iter!=0)
            {
                  si->push( filterMove(iter,(totalLength-iter)%2!=0,si,sf) );
                  temp = si; si = sf; sf = temp;
                  iter--;
            }

           while (!cache.empty()) { s.push(cache.top()); cache.pop(); }
      }

      // move N elements from si to sf, while taking out the
      // min (max) if "min" = true (false)
      template <typename elementType>
      elementType filterMove(int N, bool min, stack<elementType> *si, stack<elementType> *sf)
      {
            elementType e,t;
            e = si->top(); si->pop();
            for (int j=0;j<N-1;j++)
            {
                  t = si->top();
                  if ( (min && t<e) || (!min && t>e) ) { sf->push(e); e=t; }
                  else { sf->push(t); }
                  si->pop();
            }
            return e;
      };

      /*
            print the last K lines of a file in a single run
            all lines returned if there're fewer than K lines
      */
      void printLastKLines(int K, char* filename)
      {
            ifstream file(filename);
            string line;
            queue<string> q;
            int count = 0;

            if (file.is_open())
            {
                  while (getline(file,line))
                  {
                        q.push(line);
                        count++;
                        if (count>K) { q.pop(); }
                  }
                  file.close();
            }

            while(!q.empty())
            {
                  cout << q.front() << endl;
                  q.pop();
            }
            cout << "total line count: " << count << endl;
      };
};

int main()
{
      using namespace stackqueueAlgorithms;
      using namespace boost::numeric::ublas;

      matrix<int> input(12,1);
      input <<= 1,0,1,2,2,1,3,3,3,0,10,11;

      // testing minStack
      /*
      minStack<int> s;
      for (int j=0;j<input.size1();j++)
      { s.push(input(j,0)); }

      while (!s.empty())
      {
            cout << "current stack: ";
            s.print();
            cout << "mininum is " << s.min() << endl;
            s.pop();
      }
      */

      // testing pileStack
      /*
      pileStack<int> pS = pileStack<int>(4);
      for (int j=0;j<input.size1();j++)
      { pS.push(input(j,0)); }
      pS.print();
      for (int j=0;j<3;j++)
      { pS.popPile(); pS.print(); }
      */

      // testing dsQueue
      /*
      dsQueue<int> dsq;
      for (int j=0;j<input.size1();j++)
      { dsq.push(input(j,0)); }
      while (!dsq.empty())
      {
            cout << "size= " << dsq.size() << endl;
            dsq.print();
            dsq.pop();
      }
      */

      // testing slowSort
      /*
      stack<int> s;
      for (int j=0;j<input.size1();j++)
      { s.push(input(j,0)); }
      printStack(s);
      slowSort(s);
      printStack(s);
      */

      // testing printLastKLines()
      char filename[] = "data.txt";
      printLastKLines(4,filename);

      return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <chrono>
#include <ctime>
#include <iostream>
#include <cmath>
#include <cfloat>

#include <posit/posit>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include "defines.hpp"
#include "utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

cpp_dec_float_100 decimal_accuracy(cpp_dec_float_100 exact, cpp_dec_float_100 computed) {
        if (boost::math::isnan(exact) || boost::math::isnan(computed) ||
            (boost::math::sign(exact) != boost::math::sign(computed))) {
                return std::numeric_limits<cpp_dec_float_100>::quiet_NaN();
        } else if (exact == computed) {
                return std::numeric_limits<cpp_dec_float_100>::infinity();
        } else if ((exact == std::numeric_limits<cpp_dec_float_100>::infinity() &&
                    computed != std::numeric_limits<cpp_dec_float_100>::infinity()) ||
                   (exact != std::numeric_limits<cpp_dec_float_100>::infinity() &&
                    computed == std::numeric_limits<cpp_dec_float_100>::infinity()) || (exact == 0 && computed != 0) ||
                   (exact != 0 && computed == 0)) {
                return -std::numeric_limits<cpp_dec_float_100>::infinity();
        } else {
                return -log10(abs(log10(computed / exact)));
        }
}

void writeBenchmark(DebugValues<posit<NBITS, ES> > &hw_debug_values, DebugValues<posit<NBITS, ES> > &sw_debug_values, DebugValues<float> &float_debug_values,
                    DebugValues<cpp_dec_float_100> &dec_debug_values, std::string filename) {
        ofstream outfile(filename, ios::out);

        auto dec_values = dec_debug_values.items;
        auto hw_values = hw_debug_values.items;
        auto sw_values = sw_debug_values.items;
        auto float_values = float_debug_values.items;

        outfile << "name,dE_f,dE_sw,dE_hw,log(abs(dE_f)),log(abs(dE_sw)),log(abs(dE_hw)),E,E_f,E_sw,E_hw,da_F,da_SW,da_HW" << endl;
        for (int i = 0; i < dec_values.size(); i++) {
                cpp_dec_float_100 E, E_f, E_p, E_hw, E_sw, dE_f, dE_p, dE_hw, dE_sw;
                cpp_dec_float_100 da_F, da_HW, da_SW; // decimal accuracies

                string name = dec_values[i].name;
                E = dec_values[i].value;

                auto E_f_entry = std::find_if(float_values.begin(), float_values.end(), find_entry(name));
                E_f = E_f_entry->value;

                auto E_hw_entry = std::find_if(hw_values.begin(), hw_values.end(), find_entry(name));
                E_hw = E_hw_entry->value;

                auto E_sw_entry = std::find_if(sw_values.begin(), sw_values.end(), find_entry(name));
                E_sw = E_sw_entry->value;

                if (name != E_f_entry->name || name != E_sw_entry->name || name != E_hw_entry->name) {
                        cout << "Error: mismatching names! Could not find name '" << E_f_entry->name << endl;
                }

                cout << "Decimal accuracy..." << endl;
                da_F = decimal_accuracy(E, E_f);
                da_HW = decimal_accuracy(E, E_hw);
                da_SW = decimal_accuracy(E, E_sw);

                if (E == 0) {
                        dE_f = 0;
                        dE_sw = 0;
                        dE_hw = 0;
                } else {
                        dE_f = (E_f - E) / E;
                        dE_sw = (E_sw - E) / E;
                        dE_hw = (E_hw - E) / E;
                }

                cout << "Writing out..." << endl;

                // Relative error values
                outfile << name << ",";
                outfile << setprecision(100) << fixed << dE_f << "," << dE_sw << "," << dE_hw << "," << flush;
                outfile << setprecision(100) << fixed << log10(abs(dE_f)) << "," << log10(abs(dE_sw)) << "," << log10(abs(dE_hw)) << "," << flush;
                outfile << setprecision(100) << fixed << E << "," << E_f << "," << E_sw << "," << E_hw << "," << flush;
                outfile << setprecision(100) << fixed << da_F << "," << da_SW << "," << da_HW << endl << flush;
        }
        cout << "Closing file..." << endl;
        outfile.close();
}


/**
 * @file
 *
 * @brief Test the dlib library for fitting.
 *
 * @author Hendrix Demers <hendrix.demers@mail.mcgill.ca>
 * @since 1.0
 */

// Copyright 2016 Hendrix Demers
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.

// C system headers
// C++ system header
#include <iostream>
#include <vector>
// Library headers
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
// Precompiled header
#pragma hdrstop
// Current declaration header file of this implementation file.
#include <dlib/optimization.h>
// Project headers
// Project private headers

// Global and constant variables/functions.

using namespace std;
using namespace dlib;

typedef matrix<double,2,1> input_vector;
typedef matrix<double,3,1> parameter_vector;

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
        const input_vector& input,
        const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

// ----------------------------------------------------------------------------------------

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
        const std::pair<input_vector, double>& data,
        const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

// ----------------------------------------------------------------------------------------

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
        const std::pair<input_vector, double>& data,
        const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

BOOST_AUTO_TEST_SUITE(test_dlib)

/**
 * @brief Test if this testcase file is included in the testsuite and run.
 */
BOOST_AUTO_TEST_CASE(test_is_working)
{
    //BOOST_FAIL( "Nothing to test" );
    BOOST_CHECK(true);
}

    /**
     * @brief Test
     */
    BOOST_AUTO_TEST_CASE(test_least_square_example)
    {
        const double tolerance = 0.001;

        try
        {
            // randomly pick a set of parameters to use in this example
            const parameter_vector params = 10*randm(3,1);

            // Now let's generate a bunch of input/output pairs according to our model.
            std::vector<std::pair<input_vector, double> > data_samples;
            input_vector input;
            for (int i = 0; i < 1000; ++i)
            {
                input = 10*randm(2,1);
                const double output = model(input, params);

                // save the pair
                data_samples.push_back(make_pair(input, output));
            }

            // Before we do anything, let's make sure that our derivative function defined above matches
            // the approximate derivative computed using central differences (via derivative()).
            // If this value is big then it means we probably typed the derivative function incorrectly.
            BOOST_CHECK_SMALL(length(residual_derivative(data_samples[0], params) -
                                                  derivative(residual)(data_samples[0], params) ), 1.0e-05);

            // Now let's use the solve_least_squares_lm() routine to figure out what the
            // parameters are based on just the data_samples.
            parameter_vector x;

            x = 1;
            // Use the Levenberg-Marquardt method to determine the parameters which
            // minimize the sum of all squared residuals.
            solve_least_squares_lm(objective_delta_stop_strategy(1e-7),
                                   residual,
                                   residual_derivative,
                                   data_samples,
                                   x);

            // Now x contains the solution.  If everything worked it will be equal to params.
            BOOST_CHECK_CLOSE(params(0, 0), x(0, 0), tolerance);
            BOOST_CHECK_CLOSE(params(1, 0), x(1, 0), tolerance);
            BOOST_CHECK_CLOSE(params(2, 0), x(2, 0), tolerance);
            BOOST_CHECK_SMALL(length(x - params), 3.0e-15);

            x = 1;
            // Use the Levenberg-Marquardt method to determine the parameters which
            // minimize the sum of all squared residuals.
            solve_least_squares_lm(objective_delta_stop_strategy(1e-7),
                                   residual,
                                   derivative(residual),
                                   data_samples,
                                   x);

            // Now x contains the solution.  If everything worked it will be equal to params.
            BOOST_CHECK_CLOSE(params(0, 0), x(0, 0), tolerance);
            BOOST_CHECK_CLOSE(params(1, 0), x(1, 0), tolerance);
            BOOST_CHECK_CLOSE(params(2, 0), x(2, 0), tolerance);
            BOOST_CHECK_SMALL(length(x - params), 5.0e-15);

            x = 1;
            // If we didn't create the residual_derivative function then we could
            // have used this method which numerically approximates the derivatives for you.
            solve_least_squares_lm(objective_delta_stop_strategy(1e-7),
                                   residual,
                                   derivative(residual),
                                   data_samples,
                                   x);

            // Now x contains the solution.  If everything worked it will be equal to params.
            BOOST_CHECK_CLOSE(params(0, 0), x(0, 0), tolerance);
            BOOST_CHECK_CLOSE(params(1, 0), x(1, 0), tolerance);
            BOOST_CHECK_CLOSE(params(2, 0), x(2, 0), tolerance);
            BOOST_CHECK_SMALL(length(x - params), 1.0e-14);

            x = 1;
            // This version of the solver uses a method which is appropriate for problems
            // where the residuals don't go to zero at the solution.  So in these cases
            // it may provide a better answer.
            solve_least_squares(objective_delta_stop_strategy(1e-7),
                                residual,
                                residual_derivative,
                                data_samples,
                                x);

            // Now x contains the solution.  If everything worked it will be equal to params.
            BOOST_CHECK_CLOSE(params(0, 0), x(0, 0), tolerance);
            BOOST_CHECK_CLOSE(params(1, 0), x(1, 0), tolerance);
            BOOST_CHECK_CLOSE(params(2, 0), x(2, 0), tolerance);
            BOOST_CHECK_SMALL(length(x - params), 1.0e-14);
        }
        catch (std::exception& e)
        {
            cout << e.what() << endl;
        }

        //BOOST_FAIL( "Nothing to test" );
        BOOST_CHECK(true);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int a, k; cin >> a >> k;
    if (k == 0) cout << (cpp_int)(2000000000000) - a << endl;
    else {
        cpp_int sum = a, cnt = 0;
        while ((cpp_int)(2000000000000) > sum) {
            cnt++;
            sum += 1 + (k * sum);
        }
        cout << cnt << endl;
    }
}


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <boost/cstdint.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/gray.hpp>
#include <boost/gil/rgb.hpp>
#include <boost/gil/rgba.hpp>

namespace svgpp
{

namespace gil_detail
{

/// red * 0.2125 + green * 0.7154 + blue * 0.0721

// The default implementation of to_luminance uses float0..1 as the intermediate channel type
template <typename RedChannel, typename GreenChannel, typename BlueChannel, typename GrayChannelValue>
struct rgb_to_luminance_fn
{
  GrayChannelValue operator()(const RedChannel& red, const GreenChannel& green, const BlueChannel& blue) const
  {
    using namespace boost::gil;
    return channel_convert<GrayChannelValue>(
      channel_convert<float32_t>(red  )*0.2125f +
      channel_convert<float32_t>(green)*0.7154f +
      channel_convert<float32_t>(blue )*0.0721f);
  }
};

// performance specialization for unsigned char
template <typename GrayChannelValue>
struct rgb_to_luminance_fn<boost::uint8_t, boost::uint8_t, boost::uint8_t, GrayChannelValue>
{
  GrayChannelValue operator()(boost::uint8_t red, boost::uint8_t green, boost::uint8_t blue) const
  {
    return boost::gil::channel_convert<GrayChannelValue>(boost::uint8_t(
       (boost::uint32_t(red)   * boost::uint32_t(0.2125 * (1 << 14))
      + boost::uint32_t(green) * boost::uint32_t(0.7154 * (1 << 14))
      + boost::uint32_t(blue)  * boost::uint32_t(0.0721 * (1 << 14))) >> 14));
  }
};

template <typename GrayChannel, typename RedChannel, typename GreenChannel, typename BlueChannel>
inline typename boost::gil::channel_traits<GrayChannel>::value_type rgb_to_luminance(
  const RedChannel& red, const GreenChannel& green, const BlueChannel& blue)
{
  return rgb_to_luminance_fn<RedChannel,GreenChannel,BlueChannel,
                              typename boost::gil::channel_traits<GrayChannel>::value_type>()(red,green,blue);
}

}   // namespace gil_detail

namespace gil_utility
{

template<class DestChannel = boost::gil::gray_color_t>
struct rgb_to_luminance_color_converter
{
  template <typename P1, typename P2>
  void operator()(const P1& src, P2& dst) const
  {
    using namespace boost::gil;
    get_color(dst, DestChannel()) =
      gil_detail::rgb_to_luminance<typename color_element_type<P2, DestChannel>::type>(
          get_color(src, red_t()),
          get_color(src, green_t()),
          get_color(src, blue_t())
      );
  }
};

template<class DestChannel = boost::gil::gray_color_t>
struct rgba_to_mask_color_converter
{
  template <typename P1, typename P2>
  void operator()(const P1& src, P2& dst) const
  {
    using namespace boost::gil;
    get_color(dst, DestChannel()) =
      boost::gil::channel_convert<typename color_element_type<P2, DestChannel>::type>(
        channel_multiply(
          gil_detail::rgb_to_luminance<typename color_element_type<P1, alpha_t>::type>(
              get_color(src, red_t()),
              get_color(src, green_t()),
              get_color(src, blue_t())
          ),
          get_color(src, alpha_t())
        )
      );
  }
};

}}


/**
 * \file
 *
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <boost/math/special_functions/pow.hpp>
#include <cmath>

#include "MaterialLib/MPL/Properties/Exponential.h"

namespace MaterialPropertyLib
{
Exponential::Exponential(std::string name,
                         PropertyDataType const& property_reference_value,
                         ExponentData const& v)
    : exponent_data_(v)
{
    name_ = std::move(name);
    auto const f = std::get<double>(exponent_data_.factor);
    auto const v0 = std::get<double>(exponent_data_.reference_condition);
    value_ = std::get<double>(property_reference_value) * std::exp(-f * v0);
}

PropertyDataType Exponential::value(
    VariableArray const& variable_array,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    auto const f = std::get<double>(exponent_data_.factor);
    auto const v =
        std::get<double>(variable_array[static_cast<int>(exponent_data_.type)]);

    return std::get<double>(value_) * std::exp(f * v);
}

PropertyDataType Exponential::dValue(
    VariableArray const& variable_array, Variable const primary_variable,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    if (exponent_data_.type != primary_variable)
    {
        return 0.;
    }

    auto const f = std::get<double>(exponent_data_.factor);
    auto const v =
        std::get<double>(variable_array[static_cast<int>(exponent_data_.type)]);

    return std::get<double>(value_) * f * std::exp(f * v);
}

PropertyDataType Exponential::d2Value(
    VariableArray const& variable_array, Variable const pv1, Variable const pv2,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    if (exponent_data_.type != pv1 && exponent_data_.type != pv2)
    {
        return 0.;
    }

    auto const f = std::get<double>(exponent_data_.factor);
    auto const v =
        std::get<double>(variable_array[static_cast<int>(exponent_data_.type)]);

    return std::get<double>(value_) * f * f * std::exp(f * v);
}

}  // namespace MaterialPropertyLib


// Copyright (c) 2020, Ryohei Sasaki
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//  * Neither the name of {copyright_holder} nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#ifndef IMU_ESTIMATOR__EKF_HPP_
#define IMU_ESTIMATOR__EKF_HPP_

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

class EKFEstimator
{
public:
  EKFEstimator()
  : Cov_(Eigen::Matrix<double, 7, 7>::Identity() * 1.0),
    Q_(Eigen::Matrix3d::Identity() * 0.01),
    R_(Eigen::Matrix3d::Identity() * 0.033),
    gravity_vec_(Eigen::Vector3d(0.0, 0.0, 9.8067)),
    tau_gyro_bias_{1.0}
  {
    /* x =[qw qx qy qz gyro_bias_x gyro_bias_y gyro_bias_z] */
    x_ << 1, 0, 0, 0, 0, 0, 0;
  }

  void predictionUpdate(
    Eigen::Matrix<double, 7, 1> & predeted_x,
    Eigen::Matrix<double, 7, 7> & predeted_Cov,
    const Eigen::Vector3d & gyro)
  {
    Eigen::Vector4d quat = x_.head<4>();
    Eigen::Vector3d gyro_bias = x_.tail<3>();

    /* gyro2omega */
    Eigen::Vector3d w = gyro - gyro_bias;
    Eigen::Matrix4d omega;
    omega <<
      0, -w[0], -w[1], -w[2],
      w[0], 0, w[2], -w[1],
      w[1], -w[2], 0, w[0],
      w[2], w[1], -w[0], 0;

    /* predeted_x = f(x, w) */
    Eigen::Vector4d predeted_quat;
    predeted_quat = quat + dt_ / 2 * omega * quat;
    predeted_x.head<4>() = predeted_quat.normalized();
    predeted_x.tail<3>() = gyro_bias;

    /* predeted_Cov = F Cov_ Ft + L Q_ Lt */
    /* F */
    Eigen::Matrix<double, 7, 7> F;
    F = Eigen::Matrix<double, 7, 7>::Identity();
    F.block<4, 4>(0, 0) += dt_ / 2 * omega;
    F.block<4, 3>(0, 4) <<
      +quat[1], +quat[2], +quat[3],
      -quat[0], +quat[3], -quat[2],
      -quat[3], -quat[0], +quat[1],
      +quat[2], -quat[1], -quat[0];
    F.block<4, 3>(0, 4) *= dt_ / 2;
    F.block<3, 3>(3, 3) -= Eigen::Matrix<double, 3, 3>::Identity() * dt_ / tau_gyro_bias_;
    /* L */
    Eigen::Matrix<double, 7, 3> L;
    L <<
      -quat[1], -quat[2], -quat[3],
      +quat[0], -quat[3], +quat[2],
      +quat[3], +quat[0], -quat[1],
      -quat[2], +quat[1], +quat[0],
      0, 0, 0,
      0, 0, 0,
      0, 0, 0;
    L *= dt_ / 2;
    predeted_Cov = F * Cov_ * F.transpose() + L * Q_ * L.transpose();
  }

  void observationUpdate(
    const Eigen::Matrix<double, 7, 1> & predeted_x,
    const Eigen::Matrix<double, 7, 7> & predeted_Cov,
    const Eigen::Vector3d & z)
  {
    Eigen::Vector4d predeted_quat = predeted_x.head<4>();

    /* y = z - h(x) */
    Eigen::Vector3d acc;
    Eigen::Quaternion<double> quat_tmp(predeted_quat[0], predeted_quat[1],
      predeted_quat[2], predeted_quat[3]);
    acc = quat_tmp.conjugate()._transformVector(gravity_vec_);
    Eigen::Vector3d y = z - acc;

    /* H */
    Eigen::Matrix<double, 3, 7> H;
    double qw = predeted_quat[0], qx = predeted_quat[1],
      qy = predeted_quat[2], qz = predeted_quat[3];
    H <<
      -qy, +qz, -qw, +qx, 0, 0, 0,
      +qx, +qw, +qz, +qy, 0, 0, 0,
      +qw, -qx, -qy, +qz, 0, 0, 0;
    H *= 2 * gravity_vec_[2];

    /* x_ */
    Eigen::Matrix<double, 7, 3> K = predeted_Cov * H.transpose() *
      (H * predeted_Cov * H.transpose() + R_).inverse();
    Eigen::Matrix<double, 7, 1> x_tmp = predeted_x + K * y;
    x_.head<4>() = x_tmp.head<4>().normalized();
    x_.tail<3>() = x_tmp.tail<3>();

    /* Cov_ */
    Cov_ = (Eigen::Matrix<double, 7, 7>::Identity() - K * H) * predeted_Cov;
  }

  void filterOneStep(
    Eigen::Quaternion<double> & quat,
    const double dt,
    const Eigen::Vector3d & acc,
    const Eigen::Vector3d & gyro)
  {
    setdt(dt);

    Eigen::Matrix<double, 7, 1> predeted_x;
    Eigen::Matrix<double, 7, 7> predeted_Cov;
    predictionUpdate(predeted_x, predeted_Cov, gyro);

    observationUpdate(predeted_x, predeted_Cov, acc);

    Eigen::Quaternion<double> quat_tmp(x_[0], x_[1], x_[2], x_[3]);
    quat = quat_tmp;
  }

  void setdt(const double dt)
  {
    dt_ = dt;
  }

  void setProcessNoize(const double process_noize)
  {
    Q_ = Eigen::Matrix3d::Identity() * process_noize;
  }

  void setObservationNoize(const double observation_noize)
  {
    R_ = Eigen::Matrix3d::Identity() * observation_noize;
  }

private:
  Eigen::Matrix<double, 7, 1> x_;
  Eigen::Matrix<double, 7, 7> Cov_;
  double dt_;

  Eigen::Matrix3d Q_, R_;
  Eigen::Vector3d gravity_vec_;
  double tau_gyro_bias_;
};

#endif  // IMU_ESTIMATOR__EKF_HPP_


#pragma once

#include <Eigen/Dense>
#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include <iostream>

const double PI = boost::math::constants::pi<double>();

template <typename FT2, typename FT3>
double
overlap3_inner(const FT2 &ft2,
               const FT3 &ft3,
               int k1,
               int k2,
               double txp,
               double txpp,
               double tx,
               double typ,
               double typp,
               double ty)
{
  // k' <=> ft2
  // k'' <=> ft3
  int Np = ft2.rows();
  int Mp = ft2.cols();
  int Npp = ft3.rows();
  int Mpp = ft3.cols();

  BOOST_VERIFY(Np % 2 == 0);
  BOOST_VERIFY(Mp % 2 == 0);
  BOOST_VERIFY(Npp % 2 == 0);
  BOOST_VERIFY(Mpp % 2 == 0);

  int np = Np / 2;
  int mp = Mp / 2;
  int npp = Npp / 2;
  int mpp = Mpp / 2;

  double vsum = 0;

  for (int k1p = std::max(-npp + 1 - k1, -np); k1p <= std::min(npp - k1, np - 1); ++k1p) {
    int i1p = k1p + np;
    for (typename FT2::InnerIterator itp(ft2, i1p); itp; ++itp) {
      int i2p = itp.col();
      int k2p = i2p - mp;
      int k1pp = -k1 - k1p;
      int i1pp = k1pp + npp;
      int k2pp = -k2 - k2p;
      int i2pp = k2pp + mpp;

      if (k2pp >= -mpp + 1 && k2pp <= mpp - 1)
        vsum += itp.value() * ft3.coeff(i1pp, i2pp) *
                std::cos(2 * PI *
                         (k2p * typ + k2pp * typp + k2 * ty + k1p * txp + k1pp * txpp + k1 * tx));
    }
  }
  // std::cout << "hits: (" << k1 << ", " << k2 << "): " << hits << "\n";
  return vsum;
}

/**
 *
 *
 * @param ft1
 * @param ft2
 * @param ft3
 * @param tp    \f$t' \in [0,1] \f$
 * @param tpp   \f$t'' \in [0,1] \f$
 * @param t     \f$t \in [0,1] \f$
 *
 * @return
 */
template <typename FT1, typename FT2, typename FT3>
double
overlap3(const FT1 &ft1,
         const FT2 &ft2,
         const FT3 &ft3,
         const Eigen::Vector2d &tp = {0, 0},
         const Eigen::Vector2d &tpp = {0, 0},
         const Eigen::Vector2d &t = {0, 0})
{
  int N = ft1.rows();
  int n = N / 2;
  int M = ft1.cols();
  int m = M / 2;

  BOOST_VERIFY(N % 2 == 0);
  BOOST_VERIFY(M % 2 == 0);

  double txp = tp[0];
  double typ = tp[1];
  double txpp = tpp[0];
  double typp = tpp[1];
  double tx = t[0];
  double ty = t[1];
  double vsum = 0;
  for (int i = 0; i < N; ++i) {
    int k1 = i - n;
    if (k1 >= 0)
      for (typename FT1::InnerIterator it(ft1, i); it; ++it) {
        int j = it.col();
        int k2 = j - m;
        double f1 = it.value();
        if (k1 == 0)
          vsum += f1 * overlap3_inner(ft2, ft3, k1, k2, txp, txpp, tx, typ, typp, ty);
        else
          vsum += 2 * f1 * overlap3_inner(ft2, ft3, k1, k2, txp, txpp, tx, typ, typp, ty);
      }
    else
      continue;
  }
  return vsum;
}

// center ft1 on (0, 0) frequency at (ky, kx) in ft2 and compute cwise prod and
// sum
template <typename FT>
double
overlap_simple_inner(const FT &ft1,
                     const FT &ft2,
                     int ky,  // row frequency
                     int kx,  // col frequency
                     const Eigen::Vector2d &t12,
                     const Eigen::Vector2d &t23)
{
  // std::cout << "overlap_simple_inner (ky, kx): " << ky << " " << kx << "\n";
  int N1 = ft1.rows();
  int M1 = ft1.cols();

  int N2 = ft2.rows();
  int M2 = ft2.cols();

  static_assert(FT::IsRowMajor);

  // assume n, m even!
  assert(N1 % 2 == 0);
  assert(M1 % 2 == 0);
  assert(N2 % 2 == 0);
  assert(M2 % 2 == 0);

  int n1 = N1 / 2;
  int m1 = M1 / 2;
  int n2 = N2 / 2;
  int m2 = M2 / 2;

  // freq. range of ft1
  int y1h_min = -n1;
  int y1h_max = n1 - 1;
  int x1h_min = -m1;
  int x1h_max = m1 - 2;

  // freq. range of ft2
  int y2h_min = -n2;
  int y2h_max = n2 - 1;
  int x2h_min = -m2;
  int x2h_max = m2 - 1;

  // compute the minimum frequency range of ft1 that has to be considered
  int y1h_beg = std::max(-n1, 1 - n2 + ky);
  int y1h_end = std::min(n1 - 1, n2 + ky);
  int x1h_beg = std::max(-m1, 1 - m2 + kx);
  int x1h_end = std::min(m1 - 1, m2 + kx);

  // define function which maps frequencies (yhat, xhat) to array indices (i,
  // j).
  auto to_i1 = [n1](int y1h) { return y1h + n1; };
  auto to_j1 = [m1](int x1h) { return x1h + m1; };
  auto to_i2 = [n2](int y2h) { return y2h + n2; };
  auto to_j2 = [m2](int x2h) { return x2h + m2; };
  // and their inverses:
  auto to_y1 = [n1](int i1) { return i1 - n1; };
  auto to_x1 = [m1](int j1) { return j1 - m1; };
  auto to_y2 = [n2](int i2) { return i2 - n2; };
  auto to_x2 = [m2](int j2) { return j2 - m2; };

  // std::cout << "y in [" << y1h_beg << ", " << y1h_end << "]"
  //           << std::endl
  //           << "x in [" << x1h_beg << ", " << x1h_end << "]"
  //           << std::endl;

  const Eigen::Vector2d xh = {ky, kx};

  typedef typename FT::InnerIterator it_t;
  typedef typename FT::StorageIndex StorageIndex;
  double vsum = 0;

  const StorageIndex *ft2_cols = ft2.innerIndexPtr();
  const StorageIndex *ft2_outer = ft2.outerIndexPtr();
  const double *ft2_values = ft2.valuePtr();

  // iterate over rows in ft1
  for (int y1h = y1h_beg; y1h <= y1h_end; ++y1h) {
    // obtain row of ft1
    it_t ity1(ft1, to_i1(y1h));
    int x1h = to_x1(ity1.col());

    // std::cout << "x1h (first nnz): " << x1h << "\t";
    // std::cout << "xbegin: " << x1h_beg << "\n";

    // search forward in ft1
    while (to_x1(ity1.col()) < x1h_beg) {
      // std::cout << " nnz... " << to_x1(ity1.col()) << "\n";
      ++ity1;
    }
    if (!bool(ity1)) {
      continue;  // skip, there are no entries in ft1(y1h, :) to consider
    }

    int y2h = ky - y1h;  // corresp. row freq. in ft2
    // get raw Eigen pointers to current row in ft2
    int ft2_col = to_i2(y2h);
    int ft2_row_begin = ft2_outer[ft2_col];
    int ft2_row_end = ft2_outer[ft2_col + 1];

    // get last nonzero position in current row of ft2
    int ft2_rindex = ft2_row_end - 1;
    // iterate over columns in ft1
    for (; bool(ity1) && to_x1(ity1) <= x1h_end; ++ity1) {
      x1h = to_x1(ity1.col());
      // search backward in ft2
      // Note: to_x2(ft2_cols[ft2_rindex]) => x2h
      while (to_x2(ft2_cols[ft2_rindex]) > kx - x1h && ft2_rindex >= ft2_row_begin) --ft2_rindex;
      int x2h = to_x2(ft2_cols[ft2_rindex]);
      if (x2h == kx - x1h) {
        // matching position found, update
        double ft2_value = ft2_values[ft2_rindex];
        const Eigen::Vector2d xhp = {y1h, x1h};
        vsum += std::cos(2 * PI * t12.dot(xhp) + 2 * PI * t23.dot(xh)) * ity1.value() * ft2_value;
      } else if (x2h < kx - x1h_end) {
        // x2h hast left the range of ft1,
        // done with current row, continue with next row (y1h).
        break;
      }
    }
  }
  return vsum;
}

template <typename FT>
double
overlap3_simple(const FT &ft1,
                const FT &ft2,
                const FT &ft3,
                const Eigen::Vector2d &t12 = {0, 0},
                const Eigen::Vector2d &t23 = {0, 0})
{
  int N = ft1.rows();
  int M = ft1.cols();

  assert(N % 2 == 0);
  assert(M % 2 == 0);

  int n = N / 2;
  int m = M / 2;
  double vsum = 0;

  for (int i = n; i < N; ++i) {
    int k1 = i - n;
    // std::cout << "overlap3_simple::k1 " << k1 << "\n";

    if (k1 >= 0)
      for (typename FT::InnerIterator it(ft1, i); it; ++it) {
        int j = it.col();
        int k2 = j - m;
        double f1 = it.value();

        if (k1 == 0)
          vsum += f1 * overlap_simple_inner(ft2, ft3, k1, k2, t12, t23);
        else
          vsum += 2 * f1 * overlap_simple_inner(ft2, ft3, k1, k2, t12, t23);
      }
    else
      continue;
  }
  return vsum;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2013 - 2017 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------

 *
 * Author: Wolfgang Bangerth, Texas A&M University, 2013
 */

// The program starts with the usual include files, all of which you should
// have seen before by now:
#include <deal.II/base/utilities.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/solution_transfer.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/lac/sparsity_tools.h>
// MPI STUFF
#include <deal.II/base/mpi.h>
#include <deal.II/distributed/shared_tria.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>

#include <deal.II/grid/grid_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

#include <fstream>
#include <iostream>

namespace DistributedHE
{
	using namespace dealii;

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

		private:
			void setup_system();
			double solve_time_step();
			void create_grid();
			void output_results() const;
			void assemble_system();
			void make_dirichlet_boundary_conditions();

			MPI_Comm mpi_communicator;
			const unsigned int n_mpi_processes;
			const unsigned int this_mpi_process;

			ConditionalOStream pcout;

			parallel::shared::Triangulation<dim> triangulation;
			FE_Q<dim>            fe;
			DoFHandler<dim>      dof_handler;

			ConstraintMatrix     constraints;

			PETScWrappers::MPI::SparseMatrix system_matrix;

			Vector<double>                   solution;
			PETScWrappers::MPI::Vector       system_rhs;

			IndexSet locally_owned_dofs;
			IndexSet locally_relevant_dofs;
			std::vector<types::global_dof_index> local_dofs_per_process;
			unsigned int n_local_cells;

			double               time = 0;
			double               time_step = 60;
			unsigned int         timestep_number = 0;
			const double         theta = 0.5;
		}
		;

	template <int dim>
		void HeatEquation<dim>::increment_time()
		{
			timestep_number++;
			time += time_step;
		}

	template<int dim>
		void HeatEquation<dim>::create_grid()
		{
			const double size = 0.1;   	// meter
			GridGenerator::hyper_cube(triangulation, 0, size, true);
			triangulation.refine_global(5);
		}

	template<int dim>
		HeatEquation<dim>::HeatEquation()
			: mpi_communicator(MPI_COMM_WORLD)
			, n_mpi_processes(Utilities::MPI::n_mpi_processes(mpi_communicator))
			, this_mpi_process(Utilities::MPI::this_mpi_process(mpi_communicator))
			, pcout(std::cout, (this_mpi_process == 0))
			, triangulation(MPI_COMM_WORLD)
			, fe(1)
			, dof_handler(triangulation)
			, n_local_cells (numbers::invalid_unsigned_int)
		{

		}

	template<int dim>
		void HeatEquation<dim>::setup_system()
		{
			dof_handler.distribute_dofs(fe);
			locally_owned_dofs = dof_handler.locally_owned_dofs();
			DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
			n_local_cells  = GridTools::count_cells_with_subdomain_association(triangulation, triangulation.locally_owned_subdomain());
			local_dofs_per_process = dof_handler.n_locally_owned_dofs_per_processor();

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

			DynamicSparsityPattern dsp(locally_relevant_dofs);
			DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, /*keep_constrained_dofs = */ false);
			SparsityTools::distribute_sparsity_pattern(dsp,
				local_dofs_per_process,
				mpi_communicator,
				locally_relevant_dofs);

			system_matrix.reinit(locally_owned_dofs, locally_owned_dofs, dsp, mpi_communicator);

			if (timestep_number == 0)
			{
				solution.reinit(dof_handler.n_dofs());
				solution = 800;
			}

			system_rhs.reinit(locally_owned_dofs, mpi_communicator);
		}

	template<int dim>
		void HeatEquation<dim>::output_results() const
		{
			std::string filename = "solution-" + Utilities::int_to_string(timestep_number, 4)
			                       + "." + Utilities::int_to_string(this_mpi_process, 3)
			                       + ".vtu";

			std::ofstream output(filename.c_str());
			DataOut<dim> data_out;
			data_out.attach_dof_handler(dof_handler);
			data_out.add_data_vector(solution, "Temperature");
			std::vector<unsigned int> partition_int(triangulation.n_active_cells());
			GridTools::get_subdomain_association(triangulation, partition_int);
			const Vector<double> partitioning(partition_int.begin(), partition_int.end());
			data_out.add_data_vector(partitioning, "Partitioning");
			data_out.build_patches();
			data_out.write_vtu(output);

			if (this_mpi_process == 0)
			{
				std::vector<std::string> filenames;
				for (unsigned int i = 0; i < n_mpi_processes; ++i)
					filenames.push_back("solution-" + Utilities::int_to_string(timestep_number, 4)
					                     + "." + Utilities::int_to_string(i, 3)
					                     + ".vtu");

				const std::string pvtu_master_filename = ("solution-" +
				                        Utilities::int_to_string(timestep_number, 4) +
				                        ".pvtu");
				std::ofstream pvtu_master(pvtu_master_filename.c_str());
				data_out.write_pvtu_record(pvtu_master, filenames);
				static std::vector<std::pair<double, std::string> > times_and_names;
				times_and_names.push_back(std::pair<double, std::string> (time, pvtu_master_filename));
				std::ofstream pvd_output("solution.pvd");
				DataOutBase::write_pvd_record(pvd_output, times_and_names);
			}

		}

	template<int dim>
		void HeatEquation<dim>::assemble_system()
		{
			double temp_env_top = 20;
			double convection_top = 0;
			double temp_env_right = 20;
			double convection_right = 0;

			QGauss<dim> quadrature_formula(2);
			QGauss<dim - 1> face_quadrature_formula(3);

			QTrapez<3> quadrature;

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

			const unsigned int dofs_per_cell = fe.dofs_per_cell;

			const unsigned int cell_quadrature_points = quadrature_formula.size();
			const unsigned int face_q_pts = face_quadrature_formula.size();

			// Auxiliary matrices
			FullMatrix<double> cell_h(dofs_per_cell, dofs_per_cell);
			FullMatrix<double> cell_hq(dofs_per_cell, dofs_per_cell);
			FullMatrix<double> cell_c(dofs_per_cell, dofs_per_cell);
			Vector<double> cell_p(dofs_per_cell);

			// Resulting matrices
			FullMatrix<double> cell_a(dofs_per_cell, dofs_per_cell);
			Vector<double> cell_rhs(dofs_per_cell);

			typename DoFHandler<dim>::active_cell_iterator cell = dof_handler.begin_active(), endc = dof_handler.end();

			const double cond = 90;
			const double cp = 490;
			const double ro = 7820;
			double alpha = 0;
			double tenv = 0;

			for (; cell != endc; ++cell) {
				if (cell->is_locally_owned())
				{

					cell_fe_values.reinit(cell);
					fe_values.reinit(cell);

					cell_h = 0;
					cell_c = 0;
					cell_p = 0;
					cell_hq = 0;

					// Reinit matrices by 0
					cell_a = 0;
					cell_rhs = 0;

					for (unsigned int i = 0; i < dofs_per_cell; ++i) {
						for (unsigned int j = 0; j < dofs_per_cell; ++j) {
							for (unsigned int q_point = 0; q_point < cell_quadrature_points; ++q_point) {

								double dn_dx = cell_fe_values.shape_grad(i, q_point)[0];
								double dk_dx = cell_fe_values.shape_grad(j, q_point)[0];

								double dn_dy = cell_fe_values.shape_grad(i, q_point)[1];
								double dk_dy = cell_fe_values.shape_grad(j, q_point)[1];

								double n = cell_fe_values.shape_value(i, q_point);
								double k = cell_fe_values.shape_value(j, q_point);

								double jdet = cell_fe_values.JxW(q_point);

								cell_h(i, j) += cond * jdet * (dn_dx * dk_dx + dn_dy * dk_dy);
								cell_c(i, j) += cp * ro * n * k * jdet;
							}
						}
					}

					// Apply Neumann boundary condition
					for(unsigned int face = 0 ; face < GeometryInfo<dim>::faces_per_cell ; ++face) {
						if (cell->face(face)->at_boundary() && (cell->face(face)->boundary_id() == 2 || cell->face(face)->boundary_id() == 3)) {

							if (cell->face(face)->boundary_id() == 2) {
								alpha = convection_top;
								tenv = temp_env_top;
							}
							else if (cell->face(face)->boundary_id() == 3) {
								alpha = convection_right;
								tenv = temp_env_right;
							}

							//Point<dim> debugpt = cell->face(face)->center();
							face_fe_values.reinit(cell, face);

							for (unsigned int q = 0; q < face_q_pts; ++q) {
								double jdet = face_fe_values.JxW(q);
								for (unsigned int ii = 0; ii < dofs_per_cell; ++ii) {
									double n = face_fe_values.shape_value(ii, q);
									cell_p(ii) += alpha * tenv * n * jdet;

									for (unsigned int jj = 0; jj < dofs_per_cell; jj++) {
										double k = face_fe_values.shape_value(jj, q);
										cell_hq(ii, jj) += alpha * (n * k) * jdet;

									}

								}
							}
						}
					}

					// Integration over time
					// values from previous time step

					std::vector<double> local_values(dofs_per_cell);
					fe_values.get_function_values(solution, local_values);

					//	std::vector<double> local_values(dofs_per_cell);
					//	cell_fe_values.get_function_values(solution, local_values);
					//
					//	Vector<double> local_values(dofs_per_cell);
					//	cell->get_dof_values(solution, local_values);

										for(unsigned int i = 0 ; i < dofs_per_cell ; ++i) {
						for (unsigned int j = 0; j < dofs_per_cell; ++j) {
							// Galerkin
							double t0 = local_values[j];
//							cell_a(i, j) += 2.0 * cell_h(i,j) + (3.0/time_step) * cell_c(i,j) + cell_hq(i,j);
//							cell_rhs(i) += (-cell_h(i,j) + (3.0/time_step) * cell_c(i,j)) * t0;

							// Crank-Nicolson
							//cell_a(i, j) += cell_h(i, j) + 2.0*cell_c(i, j) / time_step + cell_hq(i, j);
							//cell_rhs(i) += (-cell_h(i, j) + 2.0*cell_c(i, j) / time_step) * t0;

							// Euler
							cell_a(i, j) += cell_h(i, j) + cell_hq(i, j) + cell_c(i, j) / time_step;
							cell_rhs(i) += (cell_c(i, j) / time_step) * t0;
						}

						// Galerkin
//						cell_rhs(i) += 3.0 * cell_p(i);

						// Crank-Nicolson
						//cell_rhs(i) += 2.0 * cell_p(i);

						//Euler
						cell_rhs(i) += cell_p(i);
					}

					// Aggregation system of equation
					 std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
					cell->get_dof_indices(local_dof_indices);
					constraints.distribute_local_to_global(cell_a, cell_rhs, local_dof_indices, system_matrix, system_rhs);
				}
			}

			system_matrix.compress(VectorOperation::add);
			system_rhs.compress(VectorOperation::add);

		}

	template <int dim>
	void HeatEquation<dim>::make_dirichlet_boundary_conditions()
	{
		VectorTools::interpolate_boundary_values(dof_handler, 2, ConstantFunction<dim>(20), constraints);
		VectorTools::interpolate_boundary_values(dof_handler, 3, ConstantFunction<dim>(20), constraints);
	}

	template<int dim>
		double HeatEquation<dim>::solve_time_step()
		{
			PETScWrappers::MPI::Vector distributed_solution(locally_owned_dofs, mpi_communicator);
			distributed_solution = solution;

			SolverControl solver_control(dof_handler.n_dofs(), 1e-8*system_rhs.l2_norm());
			PETScWrappers::SolverCG cg(solver_control, mpi_communicator);
			PETScWrappers::PreconditionBlockJacobi preconditioner(system_matrix);
			cg.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
			constraints.distribute(distributed_solution);
			solution = distributed_solution;

			return solver_control.last_step();
		}

	template<int dim>
		void HeatEquation<dim>::run()
		{
			for (unsigned int time = 0; time < 10; ++time)
			{
				pcout << "Time " << time << ':' << std::endl;
				if (time == 0)
				{
					create_grid();
				}

				pcout << "   Number of active cells:       "
				      << triangulation.n_active_cells()
				      << std::endl;
				setup_system();
				pcout << "   Number of degrees of freedom: "
				      << dof_handler.n_dofs()
				      << " (by partition:";
				for (unsigned int p = 0; p < n_mpi_processes; ++p)
					pcout << (p == 0 ? ' ' : '+')
					      << (DoFTools::
					          count_dofs_with_subdomain_association(dof_handler, p));

				pcout << ")" << std::endl;
				pcout << "   Assembling." << std::endl;
				assemble_system();
				pcout << "   Solving." << std::endl;
				const unsigned int n_iterations = solve_time_step();
				pcout << "   Solver converged in " << n_iterations
				      << " iterations." << std::endl;
				output_results();
				increment_time();
			}
		}
}

int main(int argc, char **argv)
{
	try
	{
		using namespace dealii;
		using namespace DistributedHE;

		Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
		HeatEquation<3> heat_equation_solver;
		heat_equation_solver.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;
}