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

// @sect3{Include files}

// 我们首先包括所有必要的deal.II头文件和一些C++相关的文件。它们中的每一个都已经在以前的教程程序中讨论过了，所以我们在这里就不做详细介绍了。

#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/point.h>
#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/base/parallel.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/conditional_ostream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/sparse_ilu.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/affine_constraints.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_in.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

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

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

#include <fstream>
#include <cmath>
#include <iostream>

// 最后这和以前的所有程序一样。

namespace Step35
{
  using namespace dealii;
// @sect3{Run time parameters}

// 由于我们的方法有几个可以微调的参数，我们把它们放到一个外部文件中，这样就可以在运行时确定它们。

// 这尤其包括辅助变量  $\phi$  的方程表述，为此我们声明一个  enum  。接下来，我们声明一个类，它将读取和存储我们的程序运行所需的所有参数。

  namespace RunTimeParameters
  {
    enum class Method
    {
      standard,
      rotational
    };

    class Data_Storage
    {
    public:
      Data_Storage();

      void read_data(const std::string &filename);

      Method form;

      double dt;
      double initial_time;
      double final_time;

      double Reynolds;

      unsigned int n_global_refines;

      unsigned int pressure_degree;

      unsigned int vel_max_iterations;
      unsigned int vel_Krylov_size;
      unsigned int vel_off_diagonals;
      unsigned int vel_update_prec;
      double       vel_eps;
      double       vel_diag_strength;

      bool         verbose;
      unsigned int output_interval;

    protected:
      ParameterHandler prm;
    };

// 在这个类的构造函数中，我们声明所有的参数。这方面的细节已经在其他地方讨论过了，例如在  step-29  。

    Data_Storage::Data_Storage()
      : form(Method::rotational)
      , dt(5e-4)
      , initial_time(0.)
      , final_time(1.)
      , Reynolds(1.)
      , n_global_refines(0)
      , pressure_degree(1)
      , vel_max_iterations(1000)
      , vel_Krylov_size(30)
      , vel_off_diagonals(60)
      , vel_update_prec(15)
      , vel_eps(1e-12)
      , vel_diag_strength(0.01)
      , verbose(true)
      , output_interval(15)
    {
      prm.declare_entry("Method_Form",
                        "rotational",
                        Patterns::Selection("rotational|standard"),
                        " Used to select the type of method that we are going "
                        "to use. ");
      prm.enter_subsection("Physical data");
      {
        prm.declare_entry("initial_time",
                          "0.",
                          Patterns::Double(0.),
                          " The initial time of the simulation. ");
        prm.declare_entry("final_time",
                          "1.",
                          Patterns::Double(0.),
                          " The final time of the simulation. ");
        prm.declare_entry("Reynolds",
                          "1.",
                          Patterns::Double(0.),
                          " The Reynolds number. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Time step data");
      {
        prm.declare_entry("dt",
                          "5e-4",
                          Patterns::Double(0.),
                          " The time step size. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Space discretization");
      {
        prm.declare_entry("n_of_refines",
                          "0",
                          Patterns::Integer(0, 15),
                          " The number of global refines we do on the mesh. ");
        prm.declare_entry("pressure_fe_degree",
                          "1",
                          Patterns::Integer(1, 5),
                          " The polynomial degree for the pressure space. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Data solve velocity");
      {
        prm.declare_entry(
          "max_iterations",
          "1000",
          Patterns::Integer(1, 1000),
          " The maximal number of iterations GMRES must make. ");
        prm.declare_entry("eps",
                          "1e-12",
                          Patterns::Double(0.),
                          " The stopping criterion. ");
        prm.declare_entry("Krylov_size",
                          "30",
                          Patterns::Integer(1),
                          " The size of the Krylov subspace to be used. ");
        prm.declare_entry("off_diagonals",
                          "60",
                          Patterns::Integer(0),
                          " The number of off-diagonal elements ILU must "
                          "compute. ");
        prm.declare_entry("diag_strength",
                          "0.01",
                          Patterns::Double(0.),
                          " Diagonal strengthening coefficient. ");
        prm.declare_entry("update_prec",
                          "15",
                          Patterns::Integer(1),
                          " This number indicates how often we need to "
                          "update the preconditioner");
      }
      prm.leave_subsection();

      prm.declare_entry("verbose",
                        "true",
                        Patterns::Bool(),
                        " This indicates whether the output of the solution "
                        "process should be verbose. ");

      prm.declare_entry("output_interval",
                        "1",
                        Patterns::Integer(1),
                        " This indicates between how many time steps we print "
                        "the solution. ");
    }

    void Data_Storage::read_data(const std::string &filename)
    {
      std::ifstream file(filename);
      AssertThrow(file, ExcFileNotOpen(filename));

      prm.parse_input(file);

      if (prm.get("Method_Form") == std::string("rotational"))
        form = Method::rotational;
      else
        form = Method::standard;

      prm.enter_subsection("Physical data");
      {
        initial_time = prm.get_double("initial_time");
        final_time   = prm.get_double("final_time");
        Reynolds     = prm.get_double("Reynolds");
      }
      prm.leave_subsection();

      prm.enter_subsection("Time step data");
      {
        dt = prm.get_double("dt");
      }
      prm.leave_subsection();

      prm.enter_subsection("Space discretization");
      {
        n_global_refines = prm.get_integer("n_of_refines");
        pressure_degree  = prm.get_integer("pressure_fe_degree");
      }
      prm.leave_subsection();

      prm.enter_subsection("Data solve velocity");
      {
        vel_max_iterations = prm.get_integer("max_iterations");
        vel_eps            = prm.get_double("eps");
        vel_Krylov_size    = prm.get_integer("Krylov_size");
        vel_off_diagonals  = prm.get_integer("off_diagonals");
        vel_diag_strength  = prm.get_double("diag_strength");
        vel_update_prec    = prm.get_integer("update_prec");
      }
      prm.leave_subsection();

      verbose = prm.get_bool("verbose");

      output_interval = prm.get_integer("output_interval");
    }
  } // namespace RunTimeParameters

//  @sect3{Equation data}

// 在下一个命名空间中，我们声明初始和边界条件。

  namespace EquationData
  {

// 由于我们选择了一个完全解耦的公式，我们将不利用deal.II处理矢量值问题的能力。然而，我们确实希望为方程数据使用一个独立于维度的接口。为了做到这一点，我们的函数应该能够知道我们目前在哪个空间分量上工作，而且我们应该能够有一个通用的接口来做到这一点。下面的类是在这个方向上的一个尝试。

    template <int dim>
    class MultiComponentFunction : public Function<dim>
    {
    public:
      MultiComponentFunction(const double initial_time = 0.);
      void set_component(const unsigned int d);

    protected:
      unsigned int comp;
    };

    template <int dim>
    MultiComponentFunction<dim>::MultiComponentFunction(
      const double initial_time)
      : Function<dim>(1, initial_time)
      , comp(0)
    {}

    template <int dim>
    void MultiComponentFunction<dim>::set_component(const unsigned int d)
    {
      Assert(d < dim, ExcIndexRange(d, 0, dim));
      comp = d;
    }

// 有了这个类的定义，我们声明描述速度和压力的边界条件的类。

    template <int dim>
    class Velocity : public MultiComponentFunction<dim>
    {
    public:
      Velocity(const double initial_time = 0.0);

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

      virtual void value_list(const std::vector<Point<dim>> &points,
                              std::vector<double> &          values,
                              const unsigned int component = 0) const override;
    };

    template <int dim>
    Velocity<dim>::Velocity(const double initial_time)
      : MultiComponentFunction<dim>(initial_time)
    {}

    template <int dim>
    void Velocity<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<double> &          values,
                                   const unsigned int) const
    {
      const unsigned int n_points = points.size();
      Assert(values.size() == n_points,
             ExcDimensionMismatch(values.size(), n_points));
      for (unsigned int i = 0; i < n_points; ++i)
        values[i] = Velocity<dim>::value(points[i]);
    }

    template <int dim>
    double Velocity<dim>::value(const Point<dim> &p, const unsigned int) const
    {
      if (this->comp == 0)
        {
          const double Um = 1.5;
          const double H  = 4.1;
          return 4. * Um * p(1) * (H - p(1)) / (H * H);
        }
      else
        return 0.;
    }

    template <int dim>
    class Pressure : public Function<dim>
    {
    public:
      Pressure(const double initial_time = 0.0);

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

      virtual void value_list(const std::vector<Point<dim>> &points,
                              std::vector<double> &          values,
                              const unsigned int component = 0) const override;
    };

    template <int dim>
    Pressure<dim>::Pressure(const double initial_time)
      : Function<dim>(1, initial_time)
    {}

    template <int dim>
    double Pressure<dim>::value(const Point<dim> & p,
                                const unsigned int component) const
    {
      (void)component;
      AssertIndexRange(component, 1);
      return 25. - p(0);
    }

    template <int dim>
    void Pressure<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<double> &          values,
                                   const unsigned int component) const
    {
      (void)component;
      AssertIndexRange(component, 1);
      const unsigned int n_points = points.size();
      Assert(values.size() == n_points,
             ExcDimensionMismatch(values.size(), n_points));
      for (unsigned int i = 0; i < n_points; ++i)
        values[i] = Pressure<dim>::value(points[i]);
    }
  } // namespace EquationData

//  @sect3{The NavierStokesProjection class}

// 现在是程序的主类。它实现了纳维-斯托克斯方程的各种版本的投影方法。考虑到介绍中给出的实现细节，所有方法和成员变量的名称应该是不言自明的。

  template <int dim>
  class NavierStokesProjection
  {
  public:
    NavierStokesProjection(const RunTimeParameters::Data_Storage &data);

    void run(const bool verbose = false, const unsigned int n_plots = 10);

  protected:
    RunTimeParameters::Method type;

    const unsigned int deg;
    const double       dt;
    const double       t_0;
    const double       T;
    const double       Re;

    EquationData::Velocity<dim>               vel_exact;
    std::map<types::global_dof_index, double> boundary_values;
    std::vector<types::boundary_id>           boundary_ids;

    Triangulation<dim> triangulation;

    FE_Q<dim> fe_velocity;
    FE_Q<dim> fe_pressure;

    DoFHandler<dim> dof_handler_velocity;
    DoFHandler<dim> dof_handler_pressure;

    QGauss<dim> quadrature_pressure;
    QGauss<dim> quadrature_velocity;

    SparsityPattern sparsity_pattern_velocity;
    SparsityPattern sparsity_pattern_pressure;
    SparsityPattern sparsity_pattern_pres_vel;

    SparseMatrix<double> vel_Laplace_plus_Mass;
    SparseMatrix<double> vel_it_matrix[dim];
    SparseMatrix<double> vel_Mass;
    SparseMatrix<double> vel_Laplace;
    SparseMatrix<double> vel_Advection;
    SparseMatrix<double> pres_Laplace;
    SparseMatrix<double> pres_Mass;
    SparseMatrix<double> pres_Diff[dim];
    SparseMatrix<double> pres_iterative;

    Vector<double> pres_n;
    Vector<double> pres_n_minus_1;
    Vector<double> phi_n;
    Vector<double> phi_n_minus_1;
    Vector<double> u_n[dim];
    Vector<double> u_n_minus_1[dim];
    Vector<double> u_star[dim];
    Vector<double> force[dim];
    Vector<double> v_tmp;
    Vector<double> pres_tmp;
    Vector<double> rot_u;

    SparseILU<double>   prec_velocity[dim];
    SparseILU<double>   prec_pres_Laplace;
    SparseDirectUMFPACK prec_mass;
    SparseDirectUMFPACK prec_vel_mass;

    DeclException2(ExcInvalidTimeStep,
                   double,
                   double,
                   << " The time step " << arg1 << " is out of range."
                   << std::endl
                   << " The permitted range is (0," << arg2 << "]");

    void create_triangulation_and_dofs(const unsigned int n_refines);

    void initialize();

    void interpolate_velocity();

    void diffusion_step(const bool reinit_prec);

    void projection_step(const bool reinit_prec);

    void update_pressure(const bool reinit_prec);

  private:
    unsigned int vel_max_its;
    unsigned int vel_Krylov_size;
    unsigned int vel_off_diagonals;
    unsigned int vel_update_prec;
    double       vel_eps;
    double       vel_diag_strength;

    void initialize_velocity_matrices();

    void initialize_pressure_matrices();

// 接下来的几个结构和函数是用来做各种并行的事情。它们遵循 @ref threads 中规定的方案，使用WorkStream类。正如那里所解释的，这需要我们为每个装配器声明两个结构，一个是每个任务的数据，一个是scratch数据结构。然后，这些结构被移交给组装本地贡献的函数，并将这些本地贡献复制到全局对象上。

// 这个程序的一个特殊之处在于，我们并不是只有一个DoFHandler对象来代表速度和压力，而是为这两种变量使用单独的DoFHandler对象。当我们想把涉及这两个变量的条款，如速度的发散和压力的梯度，乘以各自的测试函数时，我们要为这种优化付费。在这样做的时候，我们不能只使用一个FEValues对象，而是需要两个，而且需要用单元格迭代器来初始化它们，这些单元格迭代器指向三角形中的同一个单元格，但不同的DoFHandlers。

// 为了在实践中做到这一点，我们声明一个 "同步 "迭代器--一个内部由几个（在我们的例子中是两个）迭代器组成的对象，每次同步迭代器向前移动一步，内部存储的每个迭代器也向前移动一步，从而始终保持同步。碰巧的是，有一个deal.II类可以促进这种事情。这里重要的是要知道，建立在同一个三角形上的两个DoFHandler对象将以相同的顺序走过三角形的单元。

    using IteratorTuple =
      std::tuple<typename DoFHandler<dim>::active_cell_iterator,
                 typename DoFHandler<dim>::active_cell_iterator>;

    using IteratorPair = SynchronousIterators<IteratorTuple>;

    void initialize_gradient_operator();

    struct InitGradPerTaskData
    {
      unsigned int                         d;
      unsigned int                         vel_dpc;
      unsigned int                         pres_dpc;
      FullMatrix<double>                   local_grad;
      std::vector<types::global_dof_index> vel_local_dof_indices;
      std::vector<types::global_dof_index> pres_local_dof_indices;

      InitGradPerTaskData(const unsigned int dd,
                          const unsigned int vdpc,
                          const unsigned int pdpc)
        : d(dd)
        , vel_dpc(vdpc)
        , pres_dpc(pdpc)
        , local_grad(vdpc, pdpc)
        , vel_local_dof_indices(vdpc)
        , pres_local_dof_indices(pdpc)
      {}
    };

    struct InitGradScratchData
    {
      unsigned int  nqp;
      FEValues<dim> fe_val_vel;
      FEValues<dim> fe_val_pres;
      InitGradScratchData(const FE_Q<dim> &  fe_v,
                          const FE_Q<dim> &  fe_p,
                          const QGauss<dim> &quad,
                          const UpdateFlags  flags_v,
                          const UpdateFlags  flags_p)
        : nqp(quad.size())
        , fe_val_vel(fe_v, quad, flags_v)
        , fe_val_pres(fe_p, quad, flags_p)
      {}
      InitGradScratchData(const InitGradScratchData &data)
        : nqp(data.nqp)
        , fe_val_vel(data.fe_val_vel.get_fe(),
                     data.fe_val_vel.get_quadrature(),
                     data.fe_val_vel.get_update_flags())
        , fe_val_pres(data.fe_val_pres.get_fe(),
                      data.fe_val_pres.get_quadrature(),
                      data.fe_val_pres.get_update_flags())
      {}
    };

    void assemble_one_cell_of_gradient(const IteratorPair & SI,
                                       InitGradScratchData &scratch,
                                       InitGradPerTaskData &data);

    void copy_gradient_local_to_global(const InitGradPerTaskData &data);

// 同样的一般布局也适用于以下实现平流项组装的类和函数。

    void assemble_advection_term();

    struct AdvectionPerTaskData
    {
      FullMatrix<double>                   local_advection;
      std::vector<types::global_dof_index> local_dof_indices;
      AdvectionPerTaskData(const unsigned int dpc)
        : local_advection(dpc, dpc)
        , local_dof_indices(dpc)
      {}
    };

    struct AdvectionScratchData
    {
      unsigned int                nqp;
      unsigned int                dpc;
      std::vector<Point<dim>>     u_star_local;
      std::vector<Tensor<1, dim>> grad_u_star;
      std::vector<double>         u_star_tmp;
      FEValues<dim>               fe_val;
      AdvectionScratchData(const FE_Q<dim> &  fe,
                           const QGauss<dim> &quad,
                           const UpdateFlags  flags)
        : nqp(quad.size())
        , dpc(fe.n_dofs_per_cell())
        , u_star_local(nqp)
        , grad_u_star(nqp)
        , u_star_tmp(nqp)
        , fe_val(fe, quad, flags)
      {}

      AdvectionScratchData(const AdvectionScratchData &data)
        : nqp(data.nqp)
        , dpc(data.dpc)
        , u_star_local(nqp)
        , grad_u_star(nqp)
        , u_star_tmp(nqp)
        , fe_val(data.fe_val.get_fe(),
                 data.fe_val.get_quadrature(),
                 data.fe_val.get_update_flags())
      {}
    };

    void assemble_one_cell_of_advection(
      const typename DoFHandler<dim>::active_cell_iterator &cell,
      AdvectionScratchData &                                scratch,
      AdvectionPerTaskData &                                data);

    void copy_advection_local_to_global(const AdvectionPerTaskData &data);

// 最后几个函数实现了扩散解以及输出的后处理，包括计算速度的曲线。

    void diffusion_component_solve(const unsigned int d);

    void output_results(const unsigned int step);

    void assemble_vorticity(const bool reinit_prec);
  };

//  @sect4{ NavierStokesProjection::NavierStokesProjection }

// 在构造函数中，我们只是从作为参数传递的 Data_Storage 对象中读取所有数据，验证我们读取的数据是否合理，最后，创建三角形并加载初始数据。

  template <int dim>
  NavierStokesProjection<dim>::NavierStokesProjection(
    const RunTimeParameters::Data_Storage &data)
    : type(data.form)
    , deg(data.pressure_degree)
    , dt(data.dt)
    , t_0(data.initial_time)
    , T(data.final_time)
    , Re(data.Reynolds)
    , vel_exact(data.initial_time)
    , fe_velocity(deg + 1)
    , fe_pressure(deg)
    , dof_handler_velocity(triangulation)
    , dof_handler_pressure(triangulation)
    , quadrature_pressure(deg + 1)
    , quadrature_velocity(deg + 2)
    , vel_max_its(data.vel_max_iterations)
    , vel_Krylov_size(data.vel_Krylov_size)
    , vel_off_diagonals(data.vel_off_diagonals)
    , vel_update_prec(data.vel_update_prec)
    , vel_eps(data.vel_eps)
    , vel_diag_strength(data.vel_diag_strength)
  {
    if (deg < 1)
      std::cout
        << " WARNING: The chosen pair of finite element spaces is not stable."
        << std::endl
        << " The obtained results will be nonsense" << std::endl;

    AssertThrow(!((dt <= 0.) || (dt > .5 * T)), ExcInvalidTimeStep(dt, .5 * T));

    create_triangulation_and_dofs(data.n_global_refines);
    initialize();
  }
// @sect4{NavierStokesProjection::create_triangulation_and_dofs}

// 创建三角形的方法，并将其细化到所需的次数。在创建三角形之后，它创建了与网格相关的数据，即分配自由度和重新编号，并初始化我们将使用的矩阵和向量。

  template <int dim>
  void NavierStokesProjection<dim>::create_triangulation_and_dofs(
    const unsigned int n_refines)
  {
    GridIn<dim> grid_in;
    grid_in.attach_triangulation(triangulation);

    {
      std::string   filename = "nsbench2.inp";
      std::ifstream file(filename);
      Assert(file, ExcFileNotOpen(filename.c_str()));
      grid_in.read_ucd(file);
    }

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

    boundary_ids = triangulation.get_boundary_ids();

    DoFRenumbering::boost::Cuthill_McKee(dof_handler_velocity);
    dof_handler_pressure.distribute_dofs(fe_pressure);
    DoFRenumbering::boost::Cuthill_McKee(dof_handler_pressure);

    initialize_velocity_matrices();
    initialize_pressure_matrices();
    initialize_gradient_operator();

    pres_n.reinit(dof_handler_pressure.n_dofs());
    pres_n_minus_1.reinit(dof_handler_pressure.n_dofs());
    phi_n.reinit(dof_handler_pressure.n_dofs());
    phi_n_minus_1.reinit(dof_handler_pressure.n_dofs());
    pres_tmp.reinit(dof_handler_pressure.n_dofs());
    for (unsigned int d = 0; d < dim; ++d)
      {
        u_n[d].reinit(dof_handler_velocity.n_dofs());
        u_n_minus_1[d].reinit(dof_handler_velocity.n_dofs());
        u_star[d].reinit(dof_handler_velocity.n_dofs());
        force[d].reinit(dof_handler_velocity.n_dofs());
      }
    v_tmp.reinit(dof_handler_velocity.n_dofs());
    rot_u.reinit(dof_handler_velocity.n_dofs());

    std::cout << "dim (X_h) = " << (dof_handler_velocity.n_dofs() * dim) //
              << std::endl                                               //
              << "dim (M_h) = " << dof_handler_pressure.n_dofs()         //
              << std::endl                                               //
              << "Re        = " << Re << std::endl                       //
              << std::endl;
  }
// @sect4{ NavierStokesProjection::initialize }

// 该方法创建常数矩阵并加载初始数据。

  template <int dim>
  void NavierStokesProjection<dim>::initialize()
  {
    vel_Laplace_plus_Mass = 0.;
    vel_Laplace_plus_Mass.add(1. / Re, vel_Laplace);
    vel_Laplace_plus_Mass.add(1.5 / dt, vel_Mass);

    EquationData::Pressure<dim> pres(t_0);
    VectorTools::interpolate(dof_handler_pressure, pres, pres_n_minus_1);
    pres.advance_time(dt);
    VectorTools::interpolate(dof_handler_pressure, pres, pres_n);
    phi_n         = 0.;
    phi_n_minus_1 = 0.;
    for (unsigned int d = 0; d < dim; ++d)
      {
        vel_exact.set_time(t_0);
        vel_exact.set_component(d);
        VectorTools::interpolate(dof_handler_velocity,
                                 vel_exact,
                                 u_n_minus_1[d]);
        vel_exact.advance_time(dt);
        VectorTools::interpolate(dof_handler_velocity, vel_exact, u_n[d]);
      }
  }
// @sect4{ NavierStokesProjection::initialize_*_matrices }

// 在这组方法中，我们初始化了稀疏模式、约束条件（如果有的话）并组装了不依赖于时间步长的矩阵  dt  。请注意，对于拉普拉斯矩阵和质量矩阵，我们可以使用库中的函数来做这件事。因为这个函数的昂贵操作--创建两个矩阵--是完全独立的，我们原则上可以把它们标记为可以使用 Threads::new_task 函数进行%并行工作的任务。我们不会在这里这样做，因为这些函数在内部已经被并行化了，特别是由于当前的函数在每个程序运行中只被调用一次，所以在每个时间步长中不会产生费用。然而，必要的修改将是非常直接的。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_velocity_matrices()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_velocity.n_dofs(),
                                 dof_handler_velocity.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_velocity, dsp);
      sparsity_pattern_velocity.copy_from(dsp);
    }
    vel_Laplace_plus_Mass.reinit(sparsity_pattern_velocity);
    for (unsigned int d = 0; d < dim; ++d)
      vel_it_matrix[d].reinit(sparsity_pattern_velocity);
    vel_Mass.reinit(sparsity_pattern_velocity);
    vel_Laplace.reinit(sparsity_pattern_velocity);
    vel_Advection.reinit(sparsity_pattern_velocity);

    MatrixCreator::create_mass_matrix(dof_handler_velocity,
                                      quadrature_velocity,
                                      vel_Mass);
    MatrixCreator::create_laplace_matrix(dof_handler_velocity,
                                         quadrature_velocity,
                                         vel_Laplace);
  }

//作用于压力空间的矩阵的初始化与作用于速度空间的矩阵相似。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_pressure_matrices()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_pressure.n_dofs(),
                                 dof_handler_pressure.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_pressure, dsp);
      sparsity_pattern_pressure.copy_from(dsp);
    }

    pres_Laplace.reinit(sparsity_pattern_pressure);
    pres_iterative.reinit(sparsity_pattern_pressure);
    pres_Mass.reinit(sparsity_pattern_pressure);

    MatrixCreator::create_laplace_matrix(dof_handler_pressure,
                                         quadrature_pressure,
                                         pres_Laplace);
    MatrixCreator::create_mass_matrix(dof_handler_pressure,
                                      quadrature_pressure,
                                      pres_Mass);
  }

// 对于梯度算子，我们从初始化稀疏模式和压缩它开始。这里需要注意的是，梯度算子从压力空间作用到速度空间，所以我们必须处理两个不同的有限元空间。为了保持循环的同步，我们使用之前定义的别名，即 PairedIterators and IteratorPair  。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_gradient_operator()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_velocity.n_dofs(),
                                 dof_handler_pressure.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_velocity,
                                      dof_handler_pressure,
                                      dsp);
      sparsity_pattern_pres_vel.copy_from(dsp);
    }

    InitGradPerTaskData per_task_data(0,
                                      fe_velocity.n_dofs_per_cell(),
                                      fe_pressure.n_dofs_per_cell());
    InitGradScratchData scratch_data(fe_velocity,
                                     fe_pressure,
                                     quadrature_velocity,
                                     update_gradients | update_JxW_values,
                                     update_values);

    for (unsigned int d = 0; d < dim; ++d)
      {
        pres_Diff[d].reinit(sparsity_pattern_pres_vel);
        per_task_data.d = d;
        WorkStream::run(
          IteratorPair(IteratorTuple(dof_handler_velocity.begin_active(),
                                     dof_handler_pressure.begin_active())),
          IteratorPair(IteratorTuple(dof_handler_velocity.end(),
                                     dof_handler_pressure.end())),
          *this,
          &NavierStokesProjection<dim>::assemble_one_cell_of_gradient,
          &NavierStokesProjection<dim>::copy_gradient_local_to_global,
          scratch_data,
          per_task_data);
      }
  }

  template <int dim>
  void NavierStokesProjection<dim>::assemble_one_cell_of_gradient(
    const IteratorPair & SI,
    InitGradScratchData &scratch,
    InitGradPerTaskData &data)
  {
    scratch.fe_val_vel.reinit(std::get<0>(*SI));
    scratch.fe_val_pres.reinit(std::get<1>(*SI));

    std::get<0>(*SI)->get_dof_indices(data.vel_local_dof_indices);
    std::get<1>(*SI)->get_dof_indices(data.pres_local_dof_indices);

    data.local_grad = 0.;
    for (unsigned int q = 0; q < scratch.nqp; ++q)
      {
        for (unsigned int i = 0; i < data.vel_dpc; ++i)
          for (unsigned int j = 0; j < data.pres_dpc; ++j)
            data.local_grad(i, j) +=
              -scratch.fe_val_vel.JxW(q) *
              scratch.fe_val_vel.shape_grad(i, q)[data.d] *
              scratch.fe_val_pres.shape_value(j, q);
      }
  }

  template <int dim>
  void NavierStokesProjection<dim>::copy_gradient_local_to_global(
    const InitGradPerTaskData &data)
  {
    for (unsigned int i = 0; i < data.vel_dpc; ++i)
      for (unsigned int j = 0; j < data.pres_dpc; ++j)
        pres_Diff[data.d].add(data.vel_local_dof_indices[i],
                              data.pres_local_dof_indices[j],
                              data.local_grad(i, j));
  }
// @sect4{ NavierStokesProjection::run }

// 这是时间行进函数，从 t_0 开始，使用时间步长 dt 的投影法在时间上前进，直到 T  。

// 它的第二个参数 verbose 表示该函数是否应该输出它在任何特定时刻正在做什么的信息：例如，它将说明我们是否正在进行扩散、投影子步骤；更新前置条件器等等。我们没有使用像
// @code
//    if (verbose) std::cout << "something";
//  @endcode
//  那样的代码来实现这种输出，而是使用ConditionalOStream类来为我们做这个。该类接受一个输出流和一个条件，该条件表明你传递给它的东西是否应该被传递到给定的输出流，或者应该被忽略。这样，上面的代码就变成了
//  @code
//    verbose_cout << "something";
//  @endcode，并且在任何情况下都会做正确的事情。

  template <int dim>
  void NavierStokesProjection<dim>::run(const bool         verbose,
                                        const unsigned int output_interval)
  {
    ConditionalOStream verbose_cout(std::cout, verbose);

    const auto n_steps = static_cast<unsigned int>((T - t_0) / dt);
    vel_exact.set_time(2. * dt);
    output_results(1);
    for (unsigned int n = 2; n <= n_steps; ++n)
      {
        if (n % output_interval == 0)
          {
            verbose_cout << "Plotting Solution" << std::endl;
            output_results(n);
          }
        std::cout << "Step = " << n << " Time = " << (n * dt) << std::endl;
        verbose_cout << "  Interpolating the velocity " << std::endl;

        interpolate_velocity();
        verbose_cout << "  Diffusion Step" << std::endl;
        if (n % vel_update_prec == 0)
          verbose_cout << "    With reinitialization of the preconditioner"
                       << std::endl;
        diffusion_step((n % vel_update_prec == 0) || (n == 2));
        verbose_cout << "  Projection Step" << std::endl;
        projection_step((n == 2));
        verbose_cout << "  Updating the Pressure" << std::endl;
        update_pressure((n == 2));
        vel_exact.advance_time(dt);
      }
    output_results(n_steps);
  }

  template <int dim>
  void NavierStokesProjection<dim>::interpolate_velocity()
  {
    for (unsigned int d = 0; d < dim; ++d)
      {
        u_star[d].equ(2., u_n[d]);
        u_star[d] -= u_n_minus_1[d];
      }
  }
// @sect4{NavierStokesProjection::diffusion_step}

// 扩散步骤的实现。请注意，昂贵的操作是函数末尾的扩散解，我们必须为每个速度分量做一次。为了加快进度，我们允许以%并行方式进行，使用 Threads::new_task 函数，确保 dim 的求解都得到处理，并被安排到可用的处理器上：如果你的机器有一个以上的处理器核心，并且这个程序的其他部分目前没有使用资源，那么扩散求解将以%并行方式运行。另一方面，如果你的系统只有一个处理器核心，那么以%并行方式运行将是低效的（因为它导致了，例如，缓存拥堵），事情将被顺序地执行。

  template <int dim>
  void NavierStokesProjection<dim>::diffusion_step(const bool reinit_prec)
  {
    pres_tmp.equ(-1., pres_n);
    pres_tmp.add(-4. / 3., phi_n, 1. / 3., phi_n_minus_1);

    assemble_advection_term();

    for (unsigned int d = 0; d < dim; ++d)
      {
        force[d] = 0.;
        v_tmp.equ(2. / dt, u_n[d]);
        v_tmp.add(-.5 / dt, u_n_minus_1[d]);
        vel_Mass.vmult_add(force[d], v_tmp);

        pres_Diff[d].vmult_add(force[d], pres_tmp);
        u_n_minus_1[d] = u_n[d];

        vel_it_matrix[d].copy_from(vel_Laplace_plus_Mass);
        vel_it_matrix[d].add(1., vel_Advection);

        vel_exact.set_component(d);
        boundary_values.clear();
        for (const auto &boundary_id : boundary_ids)
          {
            switch (boundary_id)
              {
                case 1:
                  VectorTools::interpolate_boundary_values(
                    dof_handler_velocity,
                    boundary_id,
                    Functions::ZeroFunction<dim>(),
                    boundary_values);
                  break;
                case 2:
                  VectorTools::interpolate_boundary_values(dof_handler_velocity,
                                                           boundary_id,
                                                           vel_exact,
                                                           boundary_values);
                  break;
                case 3:
                  if (d != 0)
                    VectorTools::interpolate_boundary_values(
                      dof_handler_velocity,
                      boundary_id,
                      Functions::ZeroFunction<dim>(),
                      boundary_values);
                  break;
                case 4:
                  VectorTools::interpolate_boundary_values(
                    dof_handler_velocity,
                    boundary_id,
                    Functions::ZeroFunction<dim>(),
                    boundary_values);
                  break;
                default:
                  Assert(false, ExcNotImplemented());
              }
          }
        MatrixTools::apply_boundary_values(boundary_values,
                                           vel_it_matrix[d],
                                           u_n[d],
                                           force[d]);
      }

    Threads::TaskGroup<void> tasks;
    for (unsigned int d = 0; d < dim; ++d)
      {
        if (reinit_prec)
          prec_velocity[d].initialize(vel_it_matrix[d],
                                      SparseILU<double>::AdditionalData(
                                        vel_diag_strength, vel_off_diagonals));
        tasks += Threads::new_task(
          &NavierStokesProjection<dim>::diffusion_component_solve, *this, d);
      }
    tasks.join_all();
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::diffusion_component_solve(const unsigned int d)
  {
    SolverControl solver_control(vel_max_its, vel_eps * force[d].l2_norm());
    SolverGMRES<Vector<double>> gmres(
      solver_control,
      SolverGMRES<Vector<double>>::AdditionalData(vel_Krylov_size));
    gmres.solve(vel_it_matrix[d], u_n[d], force[d], prec_velocity[d]);
  }
// @sect4{ NavierStokesProjection::assemble_advection_term }

// 下面的几个函数是关于集合平流项的，平流项是扩散步骤的系统矩阵的一部分，在每个时间步骤中都会发生变化。如上所述，我们将使用WorkStream类和文件模块 @ref threads 中描述的其他设施，在所有单元上平行运行装配循环。

  template <int dim>
  void NavierStokesProjection<dim>::assemble_advection_term()
  {
    vel_Advection = 0.;
    AdvectionPerTaskData data(fe_velocity.n_dofs_per_cell());
    AdvectionScratchData scratch(fe_velocity,
                                 quadrature_velocity,
                                 update_values | update_JxW_values |
                                   update_gradients);
    WorkStream::run(
      dof_handler_velocity.begin_active(),
      dof_handler_velocity.end(),
      *this,
      &NavierStokesProjection<dim>::assemble_one_cell_of_advection,
      &NavierStokesProjection<dim>::copy_advection_local_to_global,
      scratch,
      data);
  }

  template <int dim>
  void NavierStokesProjection<dim>::assemble_one_cell_of_advection(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    AdvectionScratchData &                                scratch,
    AdvectionPerTaskData &                                data)
  {
    scratch.fe_val.reinit(cell);
    cell->get_dof_indices(data.local_dof_indices);
    for (unsigned int d = 0; d < dim; ++d)
      {
        scratch.fe_val.get_function_values(u_star[d], scratch.u_star_tmp);
        for (unsigned int q = 0; q < scratch.nqp; ++q)
          scratch.u_star_local[q](d) = scratch.u_star_tmp[q];
      }

    for (unsigned int d = 0; d < dim; ++d)
      {
        scratch.fe_val.get_function_gradients(u_star[d], scratch.grad_u_star);
        for (unsigned int q = 0; q < scratch.nqp; ++q)
          {
            if (d == 0)
              scratch.u_star_tmp[q] = 0.;
            scratch.u_star_tmp[q] += scratch.grad_u_star[q][d];
          }
      }

    data.local_advection = 0.;
    for (unsigned int q = 0; q < scratch.nqp; ++q)
      for (unsigned int i = 0; i < scratch.dpc; ++i)
        for (unsigned int j = 0; j < scratch.dpc; ++j)
          data.local_advection(i, j) += (scratch.u_star_local[q] *            //
                                           scratch.fe_val.shape_grad(j, q) *  //
                                           scratch.fe_val.shape_value(i, q)   //
                                         +                                    //
                                         0.5 *                                //
                                           scratch.u_star_tmp[q] *            //
                                           scratch.fe_val.shape_value(i, q) * //
                                           scratch.fe_val.shape_value(j, q))  //
                                        * scratch.fe_val.JxW(q);
  }

  template <int dim>
  void NavierStokesProjection<dim>::copy_advection_local_to_global(
    const AdvectionPerTaskData &data)
  {
    for (unsigned int i = 0; i < fe_velocity.n_dofs_per_cell(); ++i)
      for (unsigned int j = 0; j < fe_velocity.n_dofs_per_cell(); ++j)
        vel_Advection.add(data.local_dof_indices[i],
                          data.local_dof_indices[j],
                          data.local_advection(i, j));
  }

//  @sect4{NavierStokesProjection::projection_step}

// 这实现了投影的步骤。

  template <int dim>
  void NavierStokesProjection<dim>::projection_step(const bool reinit_prec)
  {
    pres_iterative.copy_from(pres_Laplace);

    pres_tmp = 0.;
    for (unsigned d = 0; d < dim; ++d)
      pres_Diff[d].Tvmult_add(pres_tmp, u_n[d]);

    phi_n_minus_1 = phi_n;

    static std::map<types::global_dof_index, double> bval;
    if (reinit_prec)
      VectorTools::interpolate_boundary_values(dof_handler_pressure,
                                               3,
                                               Functions::ZeroFunction<dim>(),
                                               bval);

    MatrixTools::apply_boundary_values(bval, pres_iterative, phi_n, pres_tmp);

    if (reinit_prec)
      prec_pres_Laplace.initialize(pres_iterative,
                                   SparseILU<double>::AdditionalData(
                                     vel_diag_strength, vel_off_diagonals));

    SolverControl solvercontrol(vel_max_its, vel_eps * pres_tmp.l2_norm());
    SolverCG<Vector<double>> cg(solvercontrol);
    cg.solve(pres_iterative, phi_n, pres_tmp, prec_pres_Laplace);

    phi_n *= 1.5 / dt;
  }
// @sect4{ NavierStokesProjection::update_pressure }

// 这是投影法的压力更新步骤。它实现了该方法的标准表述，即
//  @f[ p^{n+1} = p^n +
//  \phi^{n+1},
//  @f]
//  或旋转形式，即
//  @f[ p^{n+1} = p^n +
//  \phi^{n+1} - \frac{1}{Re} \nabla\cdot u^{n+1}.
//  @f]

  template <int dim>
  void NavierStokesProjection<dim>::update_pressure(const bool reinit_prec)
  {
    pres_n_minus_1 = pres_n;
    switch (type)
      {
        case RunTimeParameters::Method::standard:
          pres_n += phi_n;
          break;
        case RunTimeParameters::Method::rotational:
          if (reinit_prec)
            prec_mass.initialize(pres_Mass);
          pres_n = pres_tmp;
          prec_mass.solve(pres_n);
          pres_n.sadd(1. / Re, 1., pres_n_minus_1);
          pres_n += phi_n;
          break;
        default:
          Assert(false, ExcNotImplemented());
      };
  }
// @sect4{ NavierStokesProjection::output_results }

// 该方法绘制了当前的解决方案。主要的困难是，我们想创建一个单一的输出文件，其中包含所有的速度分量、压力以及流动的涡度的数据。另一方面，速度和压力存在于不同的DoFHandler对象中，因此不能用一个DataOut对象写入同一个文件。因此，我们必须更努力地把各种数据放到一个DoFHandler对象中，然后用它来驱动图形输出。

// 我们不会在这里详细说明这个过程，而是参考  step-32  ，那里使用了一个类似的程序（并有记录），为所有变量创建一个联合的 DoFHandler 对象。

// 我们还注意到，我们在这里将涡度作为一个单独的函数中的标量来计算，使用 $L^2$ 量的投影 $\text{curl} u$ 到用于速度成分的有限元空间。但原则上，我们也可以从速度中计算出一个点状量，并通过 step-29 和 step-33 中讨论的DataPostprocessor机制实现。

  template <int dim>
  void NavierStokesProjection<dim>::output_results(const unsigned int step)
  {
    assemble_vorticity((step == 1));
    const FESystem<dim> joint_fe(
      fe_velocity, dim, fe_pressure, 1, fe_velocity, 1);
    DoFHandler<dim> joint_dof_handler(triangulation);
    joint_dof_handler.distribute_dofs(joint_fe);
    Assert(joint_dof_handler.n_dofs() ==
             ((dim + 1) * dof_handler_velocity.n_dofs() +
              dof_handler_pressure.n_dofs()),
           ExcInternalError());
    Vector<double> joint_solution(joint_dof_handler.n_dofs());
    std::vector<types::global_dof_index> loc_joint_dof_indices(
      joint_fe.n_dofs_per_cell()),
      loc_vel_dof_indices(fe_velocity.n_dofs_per_cell()),
      loc_pres_dof_indices(fe_pressure.n_dofs_per_cell());
    typename DoFHandler<dim>::active_cell_iterator
      joint_cell = joint_dof_handler.begin_active(),
      joint_endc = joint_dof_handler.end(),
      vel_cell   = dof_handler_velocity.begin_active(),
      pres_cell  = dof_handler_pressure.begin_active();
    for (; joint_cell != joint_endc; ++joint_cell, ++vel_cell, ++pres_cell)
      {
        joint_cell->get_dof_indices(loc_joint_dof_indices);
        vel_cell->get_dof_indices(loc_vel_dof_indices);
        pres_cell->get_dof_indices(loc_pres_dof_indices);
        for (unsigned int i = 0; i < joint_fe.n_dofs_per_cell(); ++i)
          switch (joint_fe.system_to_base_index(i).first.first)
            {
              case 0:
                Assert(joint_fe.system_to_base_index(i).first.second < dim,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) =
                  u_n[joint_fe.system_to_base_index(i).first.second](
                    loc_vel_dof_indices[joint_fe.system_to_base_index(i)
                                          .second]);
                break;
              case 1:
                Assert(joint_fe.system_to_base_index(i).first.second == 0,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) =
                  pres_n(loc_pres_dof_indices[joint_fe.system_to_base_index(i)
                                                .second]);
                break;
              case 2:
                Assert(joint_fe.system_to_base_index(i).first.second == 0,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) = rot_u(
                  loc_vel_dof_indices[joint_fe.system_to_base_index(i).second]);
                break;
              default:
                Assert(false, ExcInternalError());
            }
      }
    std::vector<std::string> joint_solution_names(dim, "v");
    joint_solution_names.emplace_back("p");
    joint_solution_names.emplace_back("rot_u");
    DataOut<dim> data_out;
    data_out.attach_dof_handler(joint_dof_handler);
    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      component_interpretation(
        dim + 2, DataComponentInterpretation::component_is_part_of_vector);
    component_interpretation[dim] =
      DataComponentInterpretation::component_is_scalar;
    component_interpretation[dim + 1] =
      DataComponentInterpretation::component_is_scalar;
    data_out.add_data_vector(joint_solution,
                             joint_solution_names,
                             DataOut<dim>::type_dof_data,
                             component_interpretation);
    data_out.build_patches(deg + 1);
    std::ofstream output("solution-" + Utilities::int_to_string(step, 5) +
                         ".vtk");
    data_out.write_vtk(output);
  }

// 下面是一个辅助函数，通过将 $\text{curl} u$ 项投影到用于速度分量的有限元空间来计算涡度。这个函数只有在我们生成图形输出时才会被调用，所以不是很频繁，因此我们没有像对待其他装配函数那样，麻烦地使用WorkStream概念来并行化它。不过，如果需要的话，这应该不会太复杂。此外，我们在这里的实现只适用于2D，所以如果不是这种情况，我们就放弃了。

  template <int dim>
  void NavierStokesProjection<dim>::assemble_vorticity(const bool reinit_prec)
  {
    Assert(dim == 2, ExcNotImplemented());
    if (reinit_prec)
      prec_vel_mass.initialize(vel_Mass);

    FEValues<dim>      fe_val_vel(fe_velocity,
                             quadrature_velocity,
                             update_gradients | update_JxW_values |
                               update_values);
    const unsigned int dpc = fe_velocity.n_dofs_per_cell(),
                       nqp = quadrature_velocity.size();
    std::vector<types::global_dof_index> ldi(dpc);
    Vector<double>                       loc_rot(dpc);

    std::vector<Tensor<1, dim>> grad_u1(nqp), grad_u2(nqp);
    rot_u = 0.;

    for (const auto &cell : dof_handler_velocity.active_cell_iterators())
      {
        fe_val_vel.reinit(cell);
        cell->get_dof_indices(ldi);
        fe_val_vel.get_function_gradients(u_n[0], grad_u1);
        fe_val_vel.get_function_gradients(u_n[1], grad_u2);
        loc_rot = 0.;
        for (unsigned int q = 0; q < nqp; ++q)
          for (unsigned int i = 0; i < dpc; ++i)
            loc_rot(i) += (grad_u2[q][0] - grad_u1[q][1]) * //
                          fe_val_vel.shape_value(i, q) *    //
                          fe_val_vel.JxW(q);

        for (unsigned int i = 0; i < dpc; ++i)
          rot_u(ldi[i]) += loc_rot(i);
      }

    prec_vel_mass.solve(rot_u);
  }
} // namespace Step35
// @sect3{ The main function }

// 主函数看起来和其他所有的教程程序非常相似，所以这里没有什么可评论的。

int main()
{
  try
    {
      using namespace Step35;

      RunTimeParameters::Data_Storage data;
      data.read_data("parameter-file.prm");

      deallog.depth_console(data.verbose ? 2 : 0);

      NavierStokesProjection<2> test(data);
      test.run(data.verbose, data.output_interval);
    }
  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;
    }
  std::cout << "----------------------------------------------------"
            << std::endl
            << "Apparently everything went fine!" << std::endl
            << "Don't forget to brush your teeth :-)" << std::endl
            << std::endl;
  return 0;
}


#pragma once
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <Eigen/Dense>
#include <memory>
#include <vector>
#include <iostream>
#include <cmath>

namespace py = pybind11;

namespace dummyml{

using EigenMatrix = Eigen::Matrix<
    double,
    Eigen::Dynamic,
    Eigen::Dynamic,
    Eigen::RowMajor
>;
using EigenVector = Eigen::VectorXd;

class mean_variance
{
private:
    std::vector<double> _mean;
    std::vector<double> _variance;
public:
    mean_variance() = default;
    mean_variance(size_t size): _mean(size), _variance(size){}
    inline size_t size() const {
        return _mean.size();
    }
    inline const    double& mean    (size_t index) const {
        return _mean.at(index);
    }
    inline          double& mean    (size_t index) {
        return _mean.at(index);
    }
    inline const    double& variance(size_t index) const {
        return _variance.at(index);
    }
    inline          double& variance(size_t index) {
        return _variance.at(index);
    }
    inline static double normalDistribution(double m, double v, double x, bool ignore_const = false) {
        return exp(-pow(x-m,2.0) / (ignore_const ? v : 2.0*v)) / sqrt(ignore_const ? v : v*M_PI_2);
    }
    inline static double logNormalDistribution(double m, double v, double x, bool ignore_const = false) {
        return -pow(x-m,2.0) / (ignore_const ? v : 2.0*v) - 0.5*log(ignore_const ? v : v*M_PI_2);
    }
    double* mean_data(){
        return _mean.data();
    }
    double* variance_data(){
        return _variance.data();
    }
};

class kernel{
public:
    enum type{
        NoneKernel = 0,
        LinearKernel,
        RadialBasisFunctionKernel
    } _T;
    kernel() = default;
    virtual double operator()(const double &x1, const double &x2) = 0;
    virtual double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2) = 0;
};

class linear_kernel: public kernel{
public:
    linear_kernel(): kernel(){
        _T = LinearKernel;
    }
    double operator()(const double &x1, const double &x2){
        return x1*x2;
    }
    double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2){
        return x1.dot(x2);
    }
};

class radial_basis_function_kernel: public kernel{
private:
    double gamma;
public:
    radial_basis_function_kernel(double g = 0.1): kernel(), gamma(g){
        _T = RadialBasisFunctionKernel;
    }
    void set_gamma(double g){
        gamma = g;
    }
    double operator()(const double &x1, const double &x2){
        return exp(-gamma*(x1-x2)*(x1-x2));
    }
    double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2){
        return exp(-gamma*((x1-x2).squaredNorm()));
    }
};

std::unique_ptr<kernel> get_kernel(kernel::type);

std::vector<double> softmax(const std::vector<double>&);

template<typename To, typename From>
To dummy_cast(From ptr){
    return static_cast<To>(static_cast<void*>(ptr));
}

} // namespace dummyml

void export_utils(py::module_ &m);


#include <boost/math/distributions/hypergeometric.hpp>
#include <algorithm> // for min and max
#include <Rcpp.h>

using namespace Rcpp;
using namespace boost::math;
using namespace std;

double fisher_exact_test(double a, double b, double c, double d) {
  double N = a + b + c + d;
  double r = a + c;
  double n = c + d;
  double max_for_k = min(r, n);
  double min_for_k = (double)max(0.0, double(r + n - N));
  hypergeometric_distribution<> hgd(r, n, N);
  double cutoff = pdf(hgd, c);
  double tmp_p = 0.0;
  for(int k = min_for_k; k < max_for_k+1; k++) {
    double p = pdf(hgd, k);
    if(p <= cutoff) {
      tmp_p += p;
    }
  }
  return tmp_p;
}

// [[Rcpp::export]]
SEXP fisherExactTest(SEXP a, SEXP b, SEXP c, SEXP d) {

  Rcpp::IntegerVector a_(a), b_(b), c_(c), d_(d);
  Rcpp::NumericVector pval(a);
  int n = a_.size();

  for(int i = 0; i < n; i++) {
    pval(i) = fisher_exact_test(a_(i), b_(i), c_(i), d_(i));
  }

  return wrap(pval);
}


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

#include "../config.h"
#include <fstream>
#include <iomanip>
#include <map>

#include <boost/logic/tribool.hpp>

#include "total_unimodularity.hpp"
#include "matroid_decomposition.hpp"
#include "unimodularity.hpp"
#include "smith_normal_form.hpp"

template <typename Set, typename Element>
bool contains(const Set& set, const Element& element)
{
  return set.find(element) != set.end();
}

void print_matroid_graph(const unimod::matroid_graph& graph, const std::string& indent = "")
{
  std::cout << boost::num_vertices(graph) << " nodes and " << boost::num_edges(graph) << " edges:";

  typedef boost::graph_traits <unimod::matroid_graph> traits;
  traits::vertex_iterator vertex_iter, vertex_end;
  traits::out_edge_iterator edge_iter, edge_end;

  for (boost::tie(vertex_iter, vertex_end) = boost::vertices(graph); vertex_iter != vertex_end; ++vertex_iter)
  {
    std::cout << '\n' << indent << *vertex_iter << ':';
    for (boost::tie(edge_iter, edge_end) = boost::out_edges(*vertex_iter, graph); edge_iter != edge_end; ++edge_iter)
    {
      int matroid_element = boost::get(unimod::edge_matroid_element, graph, *edge_iter);
      std::cout << ' ' << boost::target(*edge_iter, graph) << " (" << (matroid_element < 0 ? "row " : "column ") << matroid_element << ") ";
    }
  }
  std::cout << '\n';
}

void print_decomposition(const unimod::decomposed_matroid* decomposition, std::string indent = "")
{
  if (decomposition->is_leaf())
  {
    unimod::decomposed_matroid_leaf* leaf = (unimod::decomposed_matroid_leaf*) (decomposition);

    if (leaf->is_R10())
    {
      std::cout << indent << "R10:";
      for (unimod::matroid_element_set::const_iterator iter = leaf->elements().begin(); iter != leaf->elements().end(); ++iter)
        std::cout << " " << *iter;
      std::cout << "\n";
    }
    else if (leaf->is_graphic() && leaf->is_cographic())
    {
      std::cout << indent << "planar binary matroid.\n";
      std::cout << indent << "graph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->graph(), indent + "  ");
      std::cout << indent << "}\n" << indent << "cograph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->cograph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else if (leaf->is_graphic())
    {
      std::cout << indent << "graphic binary matroid.\n";
      std::cout << indent << "graph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->graph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else if (leaf->is_cographic())
    {
      std::cout << indent << "cographic binary matroid.\n";
      std::cout << indent << "cograph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->cograph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else
    {
      std::cout << indent << "irregular matroid.\n";
    }
  }
  else
  {
    unimod::decomposed_matroid_separator* separator = (unimod::decomposed_matroid_separator*) (decomposition);

    if (separator->separation_type() == unimod::decomposed_matroid_separator::ONE_SEPARATION)
    {
      std::cout << indent << "1-separation:\n";

    }
    else if (separator->separation_type() == unimod::decomposed_matroid_separator::TWO_SEPARATION)
    {
      std::cout << indent << "2-separation:\n";
    }
    else if (separator->separation_type() == unimod::decomposed_matroid_separator::THREE_SEPARATION)
    {
      std::cout << indent << "3-separation:\n";
    }
    else
    {
      std::cout << indent << "invalid separation:\n";
    }
    std::cout << indent << "{\n";
    print_decomposition(separator->first(), indent + "  ");
    print_decomposition(separator->second(), indent + "  ");
    std::cout << indent << "}\n";
  }
}

void print_violator(const unimod::integer_matrix& matrix, const unimod::submatrix_indices& violator)
{
  typedef boost::numeric::ublas::matrix_indirect <const unimod::integer_matrix, unimod::submatrix_indices::indirect_array_type> indirect_matrix_t;

  const indirect_matrix_t indirect_matrix(matrix, violator.rows, violator.columns);

  for (size_t row = 0; row < indirect_matrix.size1(); ++row)
  {
    for (size_t column = 0; column < indirect_matrix.size2(); ++column)
    {
      std::cout << std::setw(4) << indirect_matrix(row, column);
    }
    std::cout << '\n';
  }
  std::cout << "\nRow indices in range [0," << (matrix.size1() - 1) << "]:\n";
  for (size_t row = 0; row < violator.rows.size(); ++row)
    std::cout << (row == 0 ? "" : " ") << violator.rows[row];
  std::cout << "\n\nColumn indices in range [0," << (matrix.size2() - 1) << "]:\n";
  for (size_t column = 0; column < violator.columns.size(); ++column)
    std::cout << (column == 0 ? "" : " ") << violator.columns[column] << ' ';
  std::cout << std::endl;
}

void print_result(std::ostream& stream, const std::string& name, boost::logic::tribool result)
{
  stream << std::setw(20) << name << ": ";

  if (result)
    stream << "yes\n";
  else if (!result)
    stream << "no\n";
  else
    stream << "not determined\n";
}

bool test_total_unimodularity(unimod::integer_matrix& matrix, bool show_certificates, unimod::log_level level)
{
  bool result;

  if (show_certificates)
  {
    unimod::submatrix_indices violator;
    unimod::decomposed_matroid* decomposition;

    result = unimod::is_totally_unimodular(matrix, decomposition, violator, level);

    if (result)
    {
      std::cout << "\nThe matrix is totally unimodular due to the following decomposition:\n" << std::endl;

      print_decomposition(decomposition);
    }
    else
    {
      int det = unimod::submatrix_determinant(matrix, violator);
      assert (violator.rows.size() == violator.columns.size());
      std::cout << "\nThe matrix is not totally unimodular due to the following " << violator.rows.size() << " x " << violator.columns.size()
          << " submatrix with determinant " << det << "." << std::endl;
      print_violator(matrix, violator);
    }
    delete decomposition;
  }
  else
  {
    result = unimod::is_totally_unimodular(matrix, level);
    std::cout << "The matrix is " << (result ? "" : "not ") << "totally unimodular." << std::endl;
  }

  return result;
}

int run(const std::string& file_name, const std::set <char>& tests, bool show_certificates, unimod::log_level level)
{
  /// Open the file

  std::ifstream file(file_name.c_str());
  if (!file.good())
  {
    std::cout << "Error: cannot open file \"" << file_name << "\"." << std::endl;
    return EXIT_FAILURE;
  }

  /// Read height and width

  size_t height, width;
  file >> height >> width;
  if (!file.good())
  {
    std::cout << "Error: cannot read matrix size from input file." << std::endl;
    return EXIT_FAILURE;
  }

  /// Read matrix entries

  unimod::integer_matrix matrix(height, width);
  for (size_t row = 0; row < height; ++row)
  {
    for (size_t column = 0; column < width; ++column)
    {
      if (!file.good())
      {
        std::cout << "Error: cannot read matrix data." << std::endl;
      }
      int value;
      file >> value;
      matrix(row, column) = value;
    }
  }

  file.close();
  std::cout << "Read a " << matrix.size1() << " x " << matrix.size2() << " matrix.\n" << std::endl;

  std::map <char, boost::logic::tribool> results;
  for (size_t i = 0; i < 5; ++i)
    results["tUuMm"[i]] = boost::logic::indeterminate;
  size_t rank = 0;
  bool know_rank = false;
  unsigned int k = 0;

  if (contains(tests, 't'))
  {
    /// Let's test for total unimodularity.

    results['t'] = test_total_unimodularity(matrix, show_certificates, level);
    k = 1;
  }

  if (results['t'])
  {
    for (size_t i = 0; i < 4; ++i)
    {
      results["uUmM"[i]] = true;
    }
  }
  else
  {
    if (contains(tests, 'm') || contains(tests, 'M'))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing matrix for k-modularity... " << std::flush;
      results['m'] = unimod::is_k_modular(matrix, rank, k, unimod::LOG_PROGRESSIVE);
      std::cout << "The matrix is " << (results['m'] ? "" : "not ") << "k-modular.\n" << std::flush;

      results['u'] = (results['m'] && k == 1);
      if (results['m'])
        std::cout << "The matrix is " << (results['u'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['m'])
        results['M'] = false;
      if (!results['u'])
      {
        results['U'] = false;
        results['t'] = false;
      }
      know_rank = true;
    }
    else if (contains(tests, 'u') || contains(tests, 'U'))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing matrix for unimodularity... " << std::flush;
      results['u'] = unimod::is_unimodular(matrix, rank, unimod::LOG_QUIET);
      std::cout << "The matrix is " << (results['u'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (results['u'])
        results['m'] = true;
      if (!results['u'])
      {
        results['U'] = false;
        results['t'] = false;
      }
      know_rank = true;
    }
    if (contains(tests, 'M') && boost::logic::indeterminate(results['M']))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing transpose of matrix for k-modularity... " << std::flush;
      unimod::matrix_transposed <unimod::integer_matrix> transposed(matrix);
      results['M'] = unimod::is_k_modular(transposed, rank, k, unimod::LOG_QUIET);
      std::cout << "The transpose is " << (results['M'] ? "" : "not ") << "k-modular.\n" << std::flush;

      results['U'] = (results['M'] && k == 1);
      if (results['M'])
        std::cout << "The transpose is " << (results['U'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['U'])
        results['t'] = false;
      know_rank = true;
    }
    else if (contains(tests, 'U') && boost::logic::indeterminate(results['U']))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing transpose of matrix for unimodularity... " << std::flush;
      unimod::matrix_transposed <unimod::integer_matrix> transposed(matrix);
      results['U'] = unimod::is_unimodular(transposed, rank, unimod::LOG_QUIET);
      std::cout << "The transpose is " << (results['U'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['U'])
        results['t'] = false;
      know_rank = true;
    }
  }

  /// Print a summary

  if (know_rank)
    std::cout << "\nSummary of rank " << rank << " matrix:\n\n";
  else
    std::cout << "\nSummary:\n\n";

  print_result(std::cout, "Totally unimodular", results['t']);
  print_result(std::cout, "Strongly unimodular", results['U']);
  print_result(std::cout, "Unimodular", results['u']);
  print_result(std::cout, "Strongly k-modular", results['M']);
  print_result(std::cout, "k-modular", results['m']);
  if (results['m'])
    std::cout << "                  k = " << k << "\n";
  if (know_rank)
    print_result(std::cout, "Dantzig property", results['m'] && rank == matrix.size1());

  std::cout << std::flush;

  return EXIT_SUCCESS;
}

bool extract_option(char c, std::set <char>& tests, bool& certs, unimod::log_level& level, bool& help)
{
  if (c == 't' || c == 'u' || c == 'm' || c == 'U' || c == 'M')
    tests.insert(c);
  else if (c == 'a')
  {
    tests.insert('t');
    tests.insert('u');
    tests.insert('U');
    tests.insert('m');
    tests.insert('M');
  }
  else if (c == 'h')
    help = true;
  else if (c == 'c')
    certs = true;
  else if (c == 'q')
    level = unimod::LOG_QUIET;
  else if (c == 'p')
    level = unimod::LOG_PROGRESSIVE;
  else if (c == 'v')
    level = unimod::LOG_VERBOSE;
  else
    return false;

  return true;
}

int main(int argc, char **argv)
{
  /// Possible parameters
  std::string matrix_file_name = "";
  bool certs = false;
  unimod::log_level level = unimod::LOG_PROGRESSIVE;
  bool help = false;
  std::set <char> tests;

  bool options_done = false;
  for (int a = 1; a < argc; ++a)
  {
    const std::string current = argv[a];
    if (!options_done)
    {
      if (current == std::string("--"))
      {
        options_done = true;
        continue;
      }
      else if (current != "" && current[0] == '-')
      {
        for (size_t i = 1; i < current.size(); ++i)
        {
          if (!extract_option(current[i], tests, certs, level, help))
          {
            std::cerr << "Unknown option: -" << current[i] << "\nSee " << argv[0] << " -h for usage." << std::endl;
            return EXIT_FAILURE;
          }
        }
        continue;
      }
    }

    if (matrix_file_name != "")
    {
      std::cerr << "Matrix file was given twice!\nSee " << argv[0] << " -h for usage." << std::endl;
      return EXIT_FAILURE;
    }
    matrix_file_name = current;
  }

  if (tests.empty())
  {
    tests.insert('t');
    tests.insert('u');
    tests.insert('U');
    tests.insert('m');
    tests.insert('M');
  }

  if (help)
  {
    std::cerr << "Usage: " << argv[0] << " [OPTIONS] [--] MATRIX_FILE\n";
    std::cerr << "Options:\n";
    std::cerr << " -h Shows a help message.\n";
    std::cerr << " -a Test for everything possible (default).\n";
    std::cerr << " -t Test for total unimodularity.\n";
    std::cerr << " -U Test for strong unimodularity.\n";
    std::cerr << " -u Test for unimodularity.\n";
    std::cerr << " -M Test for strong k-modularity.\n";
    std::cerr << " -m Test for k-modularity.\n";
    std::cerr << " -c Prints certificates: Try to find certificates for the results.\n";
    std::cerr << " -p Progressive logging (default).\n";
    std::cerr << " -v Verbose logging.\n";
    std::cerr << " -q No logging at all.\n";
    std::cerr << std::flush;
    return EXIT_SUCCESS;
  }

  if (matrix_file_name == "")
  {
    std::cerr << "No matrix file was given!\nSee " << argv[0] << " -h for usage." << std::endl;
    return EXIT_FAILURE;
  }

  return run(matrix_file_name, tests, certs, level);
}


#include "nonThermalLosses.h"
#include <fparameters/parameters.h>
#include <fmath/physics.h>
#include <gsl/gsl_math.h>
#include <boost/property_tree/ptree.hpp>
#include <gsl/gsl_sf_bessel.h>

double adiabaticLosses(double E, double z, double vel_lat, double gamma)  //en [erg/s]
{
	static const double openingAngle = GlobalConfig.get<double>("openingAngle");

	double jetRadius = z*openingAngle;

	return gamma*2.0*(vel_lat*E / (3.0*jetRadius));
	//en el sist lab es sin Gamma
	//termina quedando return 2.0*cLight*E / (3.0*z);
}

double BohmDiffusionCoeff(double E, double B)
{
	double larmorR = E/(electronCharge*B);
	return 1.0/3.0 * larmorR * cLight;
}

double diffusionTimeParallel(double E, double height, double B)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double diffCoeff = zeda * BohmDiffusionCoeff(E,B);
	return height*height/diffCoeff;
}

double diffusionTimePerpendicular(double E, double height, double B)
{
	double larmorR = E/(electronCharge*B);
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double meanFreePath = larmorR/(3.0*zeda) * pow(height/larmorR,q-1.0);
	double diffCoeff = zeda * BohmDiffusionCoeff(E,B) / (1.0 + P2(meanFreePath/larmorR));
	return height*height/diffCoeff;
}

double diffCoeff_p(double E, Particle& p, double height, double B, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double g = E / (p.mass*cLight2);
	double kMin = 1.0/height;
	double vA = B / sqrt(4.0*pi*rho);
	double rL = E/(electronCharge*B);
	return zeda * gsl_pow_2(p.mass*cLight)* (cLight*kMin) *gsl_pow_2(vA/cLight) * pow(rL*kMin,q-2) * g*g;
}

double diffCoeff_g(double g, Particle& p, double height, double B, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double kMin = 1.0/height;
	double vA = B / sqrt(4.0*pi*rho);
	double rL = g*p.mass*cLight2/(electronCharge*B);
	return zeda * (cLight*kMin) *gsl_pow_2(vA/cLight) * pow(rL*kMin,q-2) * g*g;
}

double diffCoeff_r(double g, Particle& p, double height, double B)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double kMin = 1.0/height;
	double rL = g*p.mass*cLight2/(electronCharge*B);
	return 1.0/9.0 * cLight / zeda * rL * pow(kMin*rL,1-q);
}

double diffLength(double g, Particle& p, double r, double height, double B, double vR)
{
	return diffCoeff_r(g,p,height,B)/abs(vR);
}

double diffusionTimeTurbulence(double E, double height, Particle& p, double B)   //en [s]
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double larmorRadius = E/(electronCharge*B);
	double tEscape = height/cLight;
	return 9.0*zeda*tEscape*pow(larmorRadius/height, q-2.0);
}

double accelerationTimeSDA(double E, Particle& p, double B, double height, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double larmorRadius = E/(electronCharge*B);
	double tEscape = height/cLight;
	double alfvenVel = B/sqrt(4.0*pi*rho);
	return (1.0/zeda) / P2(alfvenVel/cLight) * tEscape * pow(larmorRadius/height,2.0-q);
}

double accelerationRateSDA(double E, Particle& p, double B, double height, double rho)
{
	double gamma = E / (p.mass*cLight2);
	return diffCoeff_g(gamma,p,height,B,rho) / (gamma*gamma);
}

double relaxTime_e(double E, double temp, double dens) {

	double gamma = E / electronRestEnergy;
	double lnLambda = 20.0;
	double theta = boltzmann*temp / electronRestEnergy;

	if (gamma > 2.0 && theta > 0.3) {
		double k1 = gsl_sf_bessel_K1(1.0/theta);
		double k2 = gsl_sf_bessel_Kn(2, 1.0/theta);
		double factor = abs(k1/k2 - 1.0/gamma);
		return 2.0/3.0 * gamma / (dens*thomson*cLight*(lnLambda+9.0/16.0-0.5*log(2.0))) /
				factor;
	} else
		return 4.0*sqrt(pi)*pow(theta, 1.5) / (dens*thomson*cLight*lnLambda);
}

double relaxTime_p(double E, double temp, double dens) {

	double gamma = E / (protonMass*cLight2);
	double lnLambda = 20.0;
	double theta = boltzmann*temp / (protonMass*cLight2);
	double b1 = 70e6*EV_TO_ERG / (protonMass*cLight2);
	double b2 = 500e6*EV_TO_ERG / (protonMass*cLight2);
	if ( gamma-1.0 > b1 && gamma-1.0 < b2 ) {
		double beta = sqrt(1.0-1.0/(gamma*gamma));
		double sigma_h = 2.3e-26;
		return 4.0 * gamma*gamma * beta / (gamma*gamma - 1.0) / (dens*sigma_h*cLight);
	} else
		return 4.0*sqrt(pi)*pow(theta, 1.5) * P2(protonMass/electronMass) / (dens*thomson*cLight*lnLambda);
}

double accelerationRate(double E, double B) //en [s]^-1
{
	double accEff = GlobalConfig.get<double>("nonThermal.injection.PL.accEfficiency");
	return accEff*cLight*electronCharge*B/E;
}

double escapeRate(double size, double vel) //en [1/s]
{
	//static const double Gamma = GlobalConfig.get<double>("Gamma");

	return vel /size;  //ver si necesito algun gamma para escribirlo en el FF
}


#include <svo/direct/elder_zucker.h>
#ifdef __SSSE3__
#include <tmmintrin.h>
#endif
#include <boost/math/special_functions/erf.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace svo {
namespace elder_zucker {

void detectEdges(
    const std::vector<cv::Mat>& img_pyr,
    const double sigma,
    cv::Mat& edge_map,
    cv::Mat& level_map)
{
//  printf("detect edges\n");
  const float pi = 3.14159265358979323846264;
  const float sn = sigma;
  const float alpha_p = 2e-7;
  const int n_levels = img_pyr.size()-1;

  // STEP-1: Use local scale control to realiably estimate the intensity
  // gradient at each image point.
  std::vector<cv::Mat> img_pyr_smoothed(n_levels);
  std::vector<cv::Mat> angle_pyr(n_levels);
  for(int L=0; L<n_levels; ++L)
  {
    // smooth image pyramid
    cv::GaussianBlur(img_pyr[L], img_pyr_smoothed[L], cv::Size(3,3), 0);

    // compute image first derivative
    const int delta=0;
    cv::Mat dx, dy;
    cv::Scharr(img_pyr_smoothed[L], dx, CV_16S, 1, 0, 1, delta, cv::BORDER_DEFAULT );
    cv::Scharr(img_pyr_smoothed[L], dy, CV_16S, 0, 1, 1, delta, cv::BORDER_DEFAULT );

    // compute critical threshold 1
    const float scale = L+1;
    const float s1 = sn * (1.0 / (2.0*sqrt(2.0*pi)*scale*scale));
    const float c1 = s1 * sqrt(-2.0*log(alpha_p));
//    printf("c1 = %f\n", c1);

    // compute angle and magnitude in angle direction
    const int n_rows = img_pyr_smoothed[L].rows;
    const int n_cols = img_pyr_smoothed[L].cols;
    angle_pyr[L] = cv::Mat(dx.size(), CV_32F);
    for(int y=0; y<n_rows; ++y)
    {
      int16_t* p_dx = dx.ptr<int16_t>(y);
      int16_t* p_dy = dy.ptr<int16_t>(y);
      float* p_a = angle_pyr[L].ptr<float>(y);
      for(int x=0; x<n_cols; ++x)
      {
        p_a[x] = std::atan2(p_dy[x], p_dx[x]);
        float mag = std::cos(p_a[x])*p_dx[x]+std::sin(p_a[x])*p_dy[x];
        if(std::abs(mag) < c1)
          p_a[x]=0;
      }
    }
  }

  // find minimum level
  cv::Mat angle(img_pyr_smoothed[0].size(), CV_32FC1, cv::Scalar(0));
  for(int y=0; y<img_pyr_smoothed[0].rows; ++y)
  {
    float* p_a = angle.ptr<float>(y);
    uint8_t* p_lev = level_map.ptr<uint8_t>(y);
    for(int x=0; x<img_pyr_smoothed[0].cols; ++x)
    {
      for(int L=0; L<n_levels; ++L)
      {
        const float a = angle_pyr[L].at<float>(y/(1<<L), x/(1<<L));
        if(a != 0.0) // TODO: what if angle is actually 0.0?
        {
          p_a[x] = a;
          break;
        }
      }
    }
  }

  std::vector<cv::Mat> lap_of_gau_pyr(img_pyr_smoothed.size());
  for(int L=0; L<n_levels; ++L)
  {
    // compute image second derivative
    cv::Mat dxdx1, dydy1, dxdy1;
    getCovarEntries(img_pyr_smoothed[L], dxdx1, dydy1, dxdy1);

    // smooth
    cv::Mat dxdx, dydy, dxdy;
    filterGauss3by316S(dxdx1, dxdx);
    filterGauss3by316S(dydy1, dydy);
    filterGauss3by316S(dxdy1, dxdy);

    // compute critical threshold 2
    const float scale = L+1;
    const float s2 = sn / (4.0 * sqrt(pi/3.0)*scale*scale*scale);
    const float c2 = sqrt(2.0) * s2 * (boost::math::erf_inv(1-alpha_p));

    // compute laplacian of gaussians
    const int n_rows = img_pyr_smoothed[L].rows;
    const int n_cols = img_pyr_smoothed[L].cols;
    lap_of_gau_pyr[L] = cv::Mat(img_pyr_smoothed[L].size(), CV_32F);
    for(int y=0; y<n_rows; ++y)
    {
      int16_t* p_dxdx = dxdx.ptr<int16_t>(y);
      int16_t* p_dxdy = dxdy.ptr<int16_t>(y);
      int16_t* p_dydy = dydy.ptr<int16_t>(y);
      float* p_a = angle.ptr<float>(y);
      float* p_l = lap_of_gau_pyr[L].ptr<float>(y);
      for(int x=0; x<n_cols; ++x)
      {
        const float ca = cos(p_a[x]);
        const float sa = sin(p_a[x]);
        p_l[x] = (ca*ca*p_dxdx[x])+(sa*sa*p_dydy[x])-(2*ca*sa*p_dxdy[x]);
        if(fabs(p_l[x]) < c2)
          p_l[x]=0;
      }
    }
  }

  // find minimum level
  edge_map = cv::Mat(img_pyr_smoothed[0].size(), CV_32FC1, cv::Scalar(0));
  level_map = cv::Mat(img_pyr_smoothed[0].size(), CV_8UC1, cv::Scalar(0));
  for(int y=0; y<img_pyr_smoothed[0].rows; ++y)
  {
    float* p_e = edge_map.ptr<float>(y);
    uint8_t* p_lev = level_map.ptr<uint8_t>(y);
    for(int x=0; x<img_pyr_smoothed[0].cols; ++x)
    {
      for(int L=0; L<n_levels; ++L)
      {
        const float e = lap_of_gau_pyr[L].at<float>(y/(1<<L), x/(1<<L));
        if(e != 0.0)
        {
          p_e[x] = e;
          p_lev[x] = L;
          break;
        }
      }
    }
  }
}

void getCovarEntries(
    const cv::Mat& src,
    cv::Mat& dxdx,
    cv::Mat& dydy,
    cv::Mat& dxdy)
{
#ifdef __SSSE3__
  cv::Mat kernel=cv::Mat::zeros(3,3,CV_8S);
  kernel.at<char>(0,0)=3*8;
  kernel.at<char>(1,0)=10*8;
  kernel.at<char>(2,0)=3*8;
  kernel.at<char>(0,2)=-3*8;
  kernel.at<char>(1,2)=-10*8;
  kernel.at<char>(2,2)=-3*8;

  const unsigned int X=3; // kernel size
  const unsigned int Y=3; // kernel size
  const unsigned int cx=1;
  const unsigned int cy=1;

  // dest will be 16 bit
  dxdx=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  dydy=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  dxdy=cv::Mat::zeros(src.rows,src.cols,CV_16S);

  const unsigned int maxJ=((src.cols-2)/16)*16;
  const unsigned int maxI=src.rows-2;
  const unsigned int stride=src.cols;

  __m128i mask_hi = _mm_set_epi8(0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF);
  __m128i mask_lo = _mm_set_epi8(0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00);

  for(unsigned int i=0; i<maxI; ++i)
  {
    bool end=false;
    for(unsigned int j=0; j<maxJ; )
    {
      //__m128i result = _mm_set_epi16 ( -127,-127,-127,-127,-127,-127,-127,-127,-127,-127);
      __m128i result_hi_dx = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_lo_dx = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_hi_dy = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_lo_dy = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);

      // enter convolution with kernel
      for(unsigned int x=0;x<X;++x)
      {
        //if(dx&&x==1)continue; // jump, 0 kernel
        for(unsigned int y=0;y<Y;++y)
        {
          //if(!dx&&y==1)continue; // jump, 0 kernel
          const char m_dx=kernel.at<char>(y,x);
          const char m_dy=kernel.at<char>(x,y);
          __m128i mult_dx = _mm_set_epi16(m_dx,m_dx,m_dx,m_dx,m_dx,m_dx,m_dx,m_dx);
          __m128i mult_dy = _mm_set_epi16(m_dy,m_dy,m_dy,m_dy,m_dy,m_dy,m_dy,m_dy);
          uchar* p=(src.data+(stride*(i+y))+x+j);
          __m128i i0 = _mm_loadu_si128 ((__m128i*)p);
          __m128i i0_hi=_mm_and_si128(i0,mask_hi);
          __m128i i0_lo=_mm_srli_si128(_mm_and_si128(i0,mask_lo),1);

          if(m_dx!=0)
          {
            __m128i i_hi_dx = _mm_mullo_epi16 (i0_hi, mult_dx);
            __m128i i_lo_dx = _mm_mullo_epi16 (i0_lo, mult_dx);
            result_hi_dx=_mm_add_epi16(result_hi_dx,i_hi_dx);
            result_lo_dx=_mm_add_epi16(result_lo_dx,i_lo_dx);
          }

          if(m_dy!=0)
          {
            __m128i i_hi_dy = _mm_mullo_epi16 (i0_hi, mult_dy);
            __m128i i_lo_dy = _mm_mullo_epi16 (i0_lo, mult_dy);
            result_hi_dy=_mm_add_epi16(result_hi_dy,i_hi_dy);
            result_lo_dy=_mm_add_epi16(result_lo_dy,i_lo_dy);
          }
        }
      }

      // calculate covariance entries - remove precision (ends up being 4 bit), then remove 4 more bits
      __m128i i_hi_dx_dx = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dx, result_hi_dx),4);
      __m128i i_hi_dy_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dy, result_hi_dy),4);
      __m128i i_hi_dx_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dy, result_hi_dx),4);
      __m128i i_lo_dx_dx = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dx, result_lo_dx),4);
      __m128i i_lo_dy_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dy, result_lo_dy),4);
      __m128i i_lo_dx_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dy, result_lo_dx),4);

      // store
      uchar* p_lo_dxdx=(dxdx.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dxdx=(dxdx.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dxdx,_mm_unpackhi_epi16 (i_hi_dx_dx, i_lo_dx_dx));
      _mm_storeu_si128 ((__m128i*)p_lo_dxdx,_mm_unpacklo_epi16 (i_hi_dx_dx, i_lo_dx_dx));
      uchar* p_lo_dydy=(dydy.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dydy=(dydy.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dydy,_mm_unpackhi_epi16 (i_hi_dy_dy, i_lo_dy_dy));
      _mm_storeu_si128 ((__m128i*)p_lo_dydy,_mm_unpacklo_epi16 (i_hi_dy_dy, i_lo_dy_dy));
      uchar* p_lo_dxdy=(dxdy.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dxdy=(dxdy.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dxdy,_mm_unpackhi_epi16 (i_hi_dx_dy, i_lo_dx_dy));
      _mm_storeu_si128 ((__m128i*)p_lo_dxdy,_mm_unpacklo_epi16 (i_hi_dx_dy, i_lo_dx_dy));

      // take care about end
      j+=16;
      if(j>=maxJ&&!end)
      {
        j=stride-2-16;
        end=true;
      }
    }
  }
#endif
}

void filterGauss3by316S(
    const cv::Mat& src,
    cv::Mat& dst)
{
#ifdef __SSSE3__
  // sanity check
  const unsigned int X=3;
  const unsigned int Y=3;
  assert(X%2!=0);
  assert(Y%2!=0);
  int cx=X/2;
  int cy=Y/2;

  // dest will be 16 bit
  dst=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  const unsigned int maxJ=((src.cols-2)/8)*8;
  const unsigned int maxI=src.rows-2;
  const unsigned int stride=src.cols;

  for(unsigned int i=0; i<maxI; ++i)
  {
    bool end=false;
    for(unsigned int j=0; j<maxJ; )
    {
      // enter convolution with kernel. do the multiplication with 2/4 at the same time
      __m128i i00 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i,j));
      __m128i i10 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j)),1);
      __m128i i20 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j));
      __m128i i01 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i,j+1)),1);
      __m128i i11 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j+1)),2);
      __m128i i21 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j+1)),1);
      __m128i i02 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i,j+2));
      __m128i i12 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j+2)),1);
      __m128i i22 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j+2));
      __m128i result = i11;

      // add up
      result=_mm_add_epi16(result,i00);
      result=_mm_add_epi16(result,i20);
      result=_mm_add_epi16(result,i02);
      result=_mm_add_epi16(result,i22);

      result=_mm_add_epi16(result,i10);
      result=_mm_add_epi16(result,i01);
      result=_mm_add_epi16(result,i12);
      result=_mm_add_epi16(result,i21);

      // store
      //uchar* p_r=(dst.data+(2*stride*(i+cy)))+2*cx+2*j;
      _mm_storeu_si128 ((__m128i*)&dst.at<short>(i+cy,j+cx),result);

      // take care about end
      j+=8;
      if(j>=maxJ&&!end)
      {
              j=stride-2-8;
              end=true;
      }
    }
  }
#endif
}

} // namespace elder_zucker
} // namespace svo


#include <state_estimation/filters/ekf_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void EKFVS::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Update the state and covariance. For the covariance the update will happen on the state
    // subsets then be converted back to the full dimensionality.
    filter_state_.x = system_model_->g();

    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd G_subset = getSubset(system_model_->G(), system_model_->activeStates());
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    const Eigen::MatrixXd cov_prime_subset = G_subset * cov_subset * G_subset.transpose() +
                                             P_subset * system_model_->Rp() * P_subset.transpose() +
                                             V_subset * Rc_subset * V_subset.transpose();

    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF predicition update:" << std::endl
              << "g=" << printMatrix(system_model_->g()) << std::endl
              << "G=" << std::endl
              << printMatrix(system_model_->G()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void EKFVS::myCorrect(const Eigen::VectorXd& z,
                      measurement_models::NonlinearMeasurementModel* model, double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Get our sub matrices/vectors
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd H_subset =
        getSubset(model->H(), model->activeMeasurements(), system_model_->activeStates());
    const Eigen::MatrixXd meas_cov_subset =
        getSubset(model->covariance(), model->activeMeasurements());

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_H_T = cov_subset * H_subset.transpose();
    const Eigen::MatrixXd K = cov_H_T * (H_subset * cov_H_T + meas_cov_subset).inverse();

    // Update the state
    const Eigen::VectorXd dz_full = model->subtractVectors(z, model->h());
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Update the covariance
    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(cov_subset.rows(), cov_subset.rows());
    const Eigen::MatrixXd cov_prime = (I - K * H_subset) * cov_subset;

    convertSubsetToFull(cov_prime, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF measurement update:" << std::endl
              << "h=" << printMatrix(model->h()) << std::endl
              << "H=" << std::endl
              << printMatrix(model->H()) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "Innovation=" << printMatrix(dx_full) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


// g++ -std=c++11 mandel2.cpp -o mandel2 -O3 -lpng

#include <iostream>
#include <array>
#include <complex>
#include <utility>
#include <tuple>
#include <sstream>

#define png_infopp_NULL (png_infopp)NULL
#define int_p_NULL (int*)NULL

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

#include <boost/format.hpp>

class FrAdapter;

class Fr
{
public:
	friend class FrAdapter;

	using fl_t = double;
	using cp_t = std::complex<fl_t>;

	Fr() = default;
	Fr(const Fr&) = default;
	~Fr() = default;

	std::pair<fl_t, fl_t> step() const {
		return std::make_pair(
			((xmax_ - xmin_) / res_x_)
			, ((ymax_ - ymin_) / res_y_)
			);
	}

	char project_iter(unsigned i) const {
		return ccc_[i * ccc_.size() / static_cast<fl_t>(max_)];
	}

	unsigned iterate(const cp_t& c) const {
		cp_t cc = c;
		unsigned i = 0;
		while(i < max_) {
			if (abs(cc) > 4.0)
				return i;
			cc = pow(cc, this->pow_) + c;
			++i;
		}
		return max_ - 1;
	}

	fl_t iterate_d(const cp_t& c) const {
		return this->iterate(c) / static_cast<fl_t>(max_);
	}

	void render(std::ostream& o = std::cout) const {
		fl_t fx, fy;
		std::tie(fx, fy) = this->step();
		for(unsigned ry = 0; ry < res_y_; ++ry) {
			fl_t y = ymax_ - ry * fy;
			for(unsigned rx = 0; rx < res_x_; ++rx) {
				unsigned i = this->iterate(cp_t(xmin_ + rx * fx, y));
				o << this->project_iter(i);
			}
			o << std::endl;
		}
	}

	void fit_range_to_res(fl_t fd = 0.5) {
		//std::pair<fl_t, fl_t> old = std::make_pair(res_x_, res_y_);
		fl_t fr = static_cast<fl_t>(res_x_) / static_cast<fl_t>(res_y_) * fd;
		//std::cout << "fr " << fr << std::endl;
		auto lx = (xmax_ - xmin_);
		auto ly = (ymax_ - ymin_);
		fl_t fw = lx / ly;
		//std::cout << "fw " << fw << std::endl;
		if (fr > fw) {
			// stretch x
			auto lxn = ly * fr;
			auto lxd = lxn - lx;
			xmin_ -= lxd / 2.0;
			xmax_ += lxd / 2.0;
		} else if (fr < fw) {
			// stretch y
			// fr = 5/4 = 1.25
			// fw = 3/2 = 1.5
			// fwn= 3/
			auto lyn = lx * 1 / fr;
			auto lyd = lyn - ly;
			ymin_ -= lyd / 2.0;
			ymax_ += lyd / 2.0;
		}
	}

	void set_res(unsigned rx, unsigned ry) {
		std::tie(this->res_x_, this->res_y_) = std::make_tuple(rx, ry);
	}

	void set_pow(fl_t p) {
		this->pow_ = p;
	}

	void set_max(unsigned m) {
		this->max_ = m;
	}

	void set_window(const std::tuple<fl_t, fl_t, fl_t, fl_t>& w) {
		std::tie(xmin_, ymin_, xmax_, ymax_) = w;
		if (xmin_ > xmax_)
			std::swap(xmin_, xmax_);
		if (ymin_ > ymax_)
			std::swap(ymin_, ymax_);
	}

private:
	fl_t xmin_{-2.0}, ymin_{-1.2}, xmax_{0.8}, ymax_{1.2};
	fl_t pow_{2.0};
	unsigned max_ {256};
	unsigned res_x_ {1000}, res_y_{500};
	std::array<char, 14> ccc_ {{'-', '.', ',', ':', ';', '+', '*', '=', 'o', 'O', '0', '#', 'M', ' '}};
};

class FrAdapter {
public:
	typedef boost::gil::point2<ptrdiff_t>   point_t;

    typedef FrAdapter           const_t;
    typedef boost::gil::gray8_pixel_t       value_type;
    typedef value_type          reference;
    typedef value_type          const_reference;
    typedef point_t             argument_type;
    typedef reference           result_type;
    BOOST_STATIC_CONSTANT(bool, is_mutable=false);

    FrAdapter(const point_t& siz = point_t(200,76), Fr::fl_t pow = 2.0) {
    	fr_.set_res(siz.x, siz.y);
    	fr_.fit_range_to_res(1.0);
    	fr_.set_pow(pow);
    }

    Fr::cp_t map_to_complex(const point_t& p) const {
    	return Fr::cp_t(((p.x / static_cast<Fr::fl_t>(fr_.res_x_)) * (fr_.xmax_ - fr_.xmin_)) + fr_.xmin_
    			      , ((p.y / static_cast<Fr::fl_t>(fr_.res_y_)) * (fr_.ymax_ - fr_.ymin_)) + fr_.ymin_
    		);
    }
    result_type operator()(const point_t& p) const {
    	auto c = this->map_to_complex(p);
    	//std::cerr << fr_.xmax_ - fr_.xmin_ << " ";
    	//std::cerr << "(" << p.x << "; " << p.y << ") -> " << c << std::endl; //<< c.real() << ", " << c.imag() << ")" << std::endl;
    	auto i = fr_.iterate_d(c);
    	return value_type((boost::gil::bits8)(pow(i,this->pow_)*255));
    }

    Fr& fr() {return fr_;};

    void set_pow(Fr::fl_t p) {
    	this->pow_ = p;
    }

private:
	Fr fr_;
	Fr::fl_t pow_{0.5};
};

int main(int argc, char const *argv[])
{
	//Fr f;
	//std::cout << f.step().first << " " << f.step().second << std::endl;
	//f.render();
	//f.set_res(200, 76);
	//f.set_res(400, 150);
	//f.set_res(800, 300);
	//f.set_res(4000, 1600);
	//f.render();
	//f.fit_range_to_res();
	//std::cout << f.step().first << " " << f.step().second << std::endl;
	//f.render();
	typedef boost::gil::virtual_2d_locator<FrAdapter,false> locator_t;
	typedef boost::gil::image_view<locator_t> my_virt_view_t;
	FrAdapter::point_t res(1000,1000);

	for(double d = 2.0; d < 2.1; d += 0.025) {
		for(double p = 0.5; p < 0.6; p += 0.1) {
			std::ostringstream oss;
			oss << boost::format{"p_%3.2f_%2.1f.png"} % d % p;
			std::string filename = oss.str();
			std::cout << filename << std::endl;
			FrAdapter fra(res, d);
			//fra.fr().set_window(std::make_tuple(-2.0, -1.5, 1.0, 1.5));
			fra.fr().set_window(std::make_tuple(-0.0, -0.9, 0.4, -0.4));
			fra.fr().fit_range_to_res(1.0);
			fra.set_pow(p);
			my_virt_view_t mandel(res, locator_t(FrAdapter::point_t(0, 0), FrAdapter::point_t(1, 1), fra));

			//boost::gil::gray8s_image_t img(res);
			boost::gil::png_write_view(filename, mandel);

		}
	}

	return 0;
}


#include <TSR.h>
#include <Eigen/Geometry>
#include <ompl/util/RandomNumbers.h>
#include <vector>

using namespace or_ompl;

TSR::TSR() : _initialized(false) {

}

TSR::TSR(const Eigen::Affine3d &T0_w, const Eigen::Affine3d &Tw_e, const Eigen::Matrix<double, 6, 2> &Bw) :
	_T0_w(T0_w), _Tw_e(Tw_e), _Bw(Bw), _initialized(true) {

	_T0_w_inv = _T0_w.inverse();
	_Tw_e_inv = _Tw_e.inverse();

}

bool TSR::deserialize(std::stringstream &ss) {

	// TODO: Do we need this stuff?
	int manipind_ignored;
    ss >> manipind_ignored;

	std::string relativebodyname_ignored;
    ss >> relativebodyname_ignored;

    if( relativebodyname_ignored != "NULL" )
    {
		std::string relativelinkname_ignored;
        ss >> relativelinkname_ignored;
    }

	// Read in the T0_w matrix
	double tmp;
	for(unsigned int c=0; c < 3; c++){
		for(unsigned int r=0; r < 3; r++){
			ss >> tmp;
			_T0_w.matrix()(r,c) = tmp;
		}
	}

	for(unsigned int idx=0; idx < 3; idx++){
		ss >> tmp;
		_T0_w.translation()(idx) = tmp;
	}

	// Read in the Tw_e matrix
	for(unsigned int c=0; c < 3; c++){
		for(unsigned int r=0; r < 3; r++){
			ss >> tmp;
			_Tw_e.matrix()(r,c) = tmp;
		}
	}

	for(unsigned int idx=0; idx < 3; idx++){
		ss >> tmp;
		_Tw_e.translation()(idx) = tmp;
	}

	// Read in the Bw matrix
	for(unsigned int r=0; r < 6; r++){
		for(unsigned int c=0; c < 2; c++){
			ss >> tmp;
			_Bw(r,c) = tmp;
		}
	}

	_T0_w_inv = _T0_w.inverse();
	_Tw_e_inv = _Tw_e.inverse();

	_initialized = true;

    return _initialized;
}

Eigen::Matrix<double, 6, 1> TSR::distance(const Eigen::Affine3d &ee_pose) const {
	Eigen::Matrix<double, 6, 1> dist = Eigen::Matrix<double, 6, 1>::Zero();

	// First compute the pose of the w frame in world coordinates, given the ee_pose
	Eigen::Affine3d w_in_world = ee_pose * _Tw_e_inv;

	// Next compute the pose of the w frame relative to its original pose (as specified by T0_w)
	Eigen::Affine3d w_offset = _T0_w_inv * w_in_world;

	// Now compute the elements of the distance matrix
	dist(0,0) = w_offset.translation()(0);
	dist(1,0) = w_offset.translation()(1);
	dist(2,0) = w_offset.translation()(2);
	dist(3,0) = atan2(w_offset.rotation()(2,1), w_offset.rotation()(2,2));
	dist(4,0) = -asin(w_offset.rotation()(2,0));
	dist(5,0) = atan2(w_offset.rotation()(1,0), w_offset.rotation()(0,0));

	return dist;
}

Eigen::Matrix<double, 6, 1> TSR::displacement(const Eigen::Affine3d &ee_pose) const {

	Eigen::Matrix<double, 6, 1> dist = distance(ee_pose);
	Eigen::Matrix<double, 6, 1> disp = Eigen::Matrix<double, 6, 1>::Zero();

	for(unsigned int idx=0; idx < 6; idx++){

		if(dist(idx,0) < _Bw(idx,0)){
			disp(idx,0) = dist(idx,0) - _Bw(idx,0);
		}else if(dist(idx,0) > _Bw(idx,1)){
			disp(idx,0) = dist(idx,0) - _Bw(idx,1);
		}
	}

	return disp;
}

Eigen::Affine3d TSR::sampleDisplacementTransform(void) const {

	// First sample uniformly betwee each of the bounds of Bw
	std::vector<double> d_sample(6);

	ompl::RNG rng;
	for(unsigned int idx=0; idx < d_sample.size(); idx++){
		if(_Bw(idx,1) > _Bw(idx,0)){
			d_sample[idx] = rng.uniformReal(_Bw(idx,0), _Bw(idx,1));
		}
	}

	Eigen::Affine3d return_tf;
	return_tf.translation() << d_sample[0], d_sample[1], d_sample[2];

	// Convert to a transform matrix
	double roll = d_sample[3];
	double pitch = d_sample[4];
	double yaw = d_sample[5];

	double A = cos(yaw);
	double B = sin(yaw);
	double C = cos(pitch);
	double D = sin(pitch);
	double E = cos(roll);
	double F = sin(roll);
	return_tf.linear() << A*C, A*D*F - B*E, B*F + A*D*E,
		B*C, A*E + B*D*F, B*D*E - A*F,
		-D, C*F, C*E;

	return return_tf;
}

Eigen::Affine3d TSR::sample() const {

	Eigen::Affine3d tf = sampleDisplacementTransform();

	return _T0_w * tf * _Tw_e;
}