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

// @sect3{Include files}

// 本程序中使用的大部分include文件都是 step-6 和类似程序中众所周知的。

#include <deal.II/base/quadrature_lib.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_refinement.h>

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

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

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

// 新的只有以下三个。第一个声明了DiscreteTime类，它帮助我们在时间相关的模拟中跟踪时间。后面两个提供了所有的粒子功能，即记录位于网格上的粒子的方法（ Particles::ParticleHandler 类）和为可视化目的输出这些粒子的位置及其属性的能力（ Particles::DataOut  类）。

#include <deal.II/base/discrete_time.h>
#include <deal.II/particles/particle_handler.h>
#include <deal.II/particles/data_out.h>

#include <fstream>

using namespace dealii;
// @sect3{Global definitions}

// 按照惯例，我们把所有与程序细节相对应的东西都放到一个自己的命名空间中。在顶部，我们定义了一些常量，我们宁愿使用符号名称而不是硬编码的数字。

// 具体来说，我们为几何学的各个部分定义了 @ref GlossBoundaryIndicator "边界指标 "的数字，以及电子的物理属性和我们在这里使用的其他具体设置。

// 对于边界指标，让我们从某个随机值101开始列举。这里的原则是要使用*不常见的数字。如果之前有`GridGenerator'函数设置的预定义边界指标，它们很可能是从0开始的小整数，但不是在这个相当随机的范围内。使用下面这样的数字可以避免冲突的可能性，同时也减少了在程序中直接拼出这些数字的诱惑（因为你可能永远不会记得哪个是哪个，而如果它们从0开始，你可能会受到诱惑）。

namespace Step19
{
  namespace BoundaryIds
  {
    constexpr types::boundary_id open          = 101;
    constexpr types::boundary_id cathode       = 102;
    constexpr types::boundary_id focus_element = 103;
    constexpr types::boundary_id anode         = 104;
  } // namespace BoundaryIds

  namespace Constants
  {
    constexpr double electron_mass   = 9.1093837015e-31;
    constexpr double electron_charge = 1.602176634e-19;

    constexpr double V0 = 1;

    constexpr double E_threshold = 0.05;

    constexpr double electrons_per_particle = 3e15;
  } // namespace Constants
// @sect3{The main class}

// 然后，下面是这个程序的主类。从根本上说，它的结构与 step-6 和其他许多教程程序相同。这包括大部分的成员函数（其余部分的目的可能从它们的名字中不难看出），以及超出 step-6 的少量成员变量，所有这些都与处理粒子有关。

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

    void run();

  private:
    void make_grid();
    void setup_system();
    void assemble_system();
    void solve_field();
    void refine_grid();

    void create_particles();
    void move_particles();
    void track_lost_particle(
      const typename Particles::ParticleIterator<dim> &        particle,
      const typename Triangulation<dim>::active_cell_iterator &cell);

    void update_timestep_size();
    void output_results() const;

    Triangulation<dim>        triangulation;
    MappingQGeneric<dim>      mapping;
    FE_Q<dim>                 fe;
    DoFHandler<dim>           dof_handler;
    AffineConstraints<double> constraints;

    SparseMatrix<double> system_matrix;
    SparsityPattern      sparsity_pattern;

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

    Particles::ParticleHandler<dim> particle_handler;
    types::particle_index           next_unused_particle_id;
    types::particle_index           n_recently_lost_particles;
    types::particle_index           n_total_lost_particles;
    types::particle_index           n_particles_lost_through_anode;

    DiscreteTime time;
  };

//  @sect3{The CathodeRaySimulator class implementation}
// @sect4{The CathodeRaySimulator constructor}

// 那么，让我们开始执行。构造函数所做的实际上只是对顶部的所有成员变量进行简单的初始化。唯一值得一提的是`particle_handler'，它被交给了一个指向粒子所在的三角形的引用（目前当然还是空的，但是粒子处理程序存储了这个引用，一旦粒子被添加，就会使用它--这发生在三角形被构建之后）。它得到的另一个信息是每个粒子需要存储多少 "属性"。在这里，我们需要每个粒子记住的是它当前的速度，也就是一个带有`dim`分量的矢量。然而，每个粒子还有其他的内在属性， Particles::ParticleHandler 类会自动并始终确保这些属性是可用的；特别是，这些属性是粒子的当前位置、它所在的单元格、它在该单元格中的参考位置，以及粒子的ID。

// 唯一感兴趣的其他变量是 "时间"，一个DiscreteTime类型的对象。它记录了我们在一个随时间变化的模拟中的当前时间，并以开始时间（零）和结束时间（ $10^{-4}$ ）初始化。我们以后将在`update_timestep_size()`中设置时间步长。

// 构造函数的主体由我们在介绍中已经讨论过的一段代码组成。也就是说，我们要确保每次有粒子离开域时，`track_lost_particle()`函数都会被`particle_handler`对象调用。

  template <int dim>
  CathodeRaySimulator<dim>::CathodeRaySimulator()
    : mapping(1)
    , fe(2)
    , dof_handler(triangulation)
    , particle_handler(triangulation, mapping, /*n_properties=*/dim)
    , next_unused_particle_id(0)
    , n_recently_lost_particles(0)
    , n_total_lost_particles(0)
    , n_particles_lost_through_anode(0)
    , time(0, 1e-4)
  {
    particle_handler.signals.particle_lost.connect(
      [this](const typename Particles::ParticleIterator<dim> &        particle,
             const typename Triangulation<dim>::active_cell_iterator &cell) {
        this->track_lost_particle(particle, cell);
      });
  }

//  @sect4{The CathodeRaySimulator::make_grid function}

// 下一个函数是负责生成我们要解决的网格。回顾一下域的样子。
//

//
//
  我们把这个几何体细分为 $4\times 2$ 个单元的网格，看起来像这样。
//    @code
//    *---*---*---*---*
//    \   |   |   |   |
//     *--*---*---*---*
//    /   |   |   |   |
//    *---*---*---*---*
//  @endcode
//  这样做的方法是首先定义 $15=5\times 3$ 顶点的位置--在这里，我们说它们在整数点上，左边的中间点向右移动了`delta=0.5`的值。

// 在下文中，我们必须说明哪些顶点共同组成了8个单元。下面的代码就完全等同于我们在 step-14 中的做法。

  template <int dim>
  void CathodeRaySimulator<dim>::make_grid()
  {
    static_assert(dim == 2,
                  "This function is currently only implemented for 2d.");

    const double       delta = 0.5;
    const unsigned int nx    = 5;
    const unsigned int ny    = 3;

    const std::vector<Point<dim>> vertices //
      = {{0, 0},
         {1, 0},
         {2, 0},
         {3, 0},
         {4, 0},
         {delta, 1},
         {1, 1},
         {2, 1},
         {3, 1},
         {4, 1},
         {0, 2},
         {1, 2},
         {2, 2},
         {3, 2},
         {4, 2}};
    AssertDimension(vertices.size(), nx * ny);

    const std::vector<unsigned int> cell_vertices[(nx - 1) * (ny - 1)] = {
      {0, 1, nx + 0, nx + 1},
      {1, 2, nx + 1, nx + 2},
      {2, 3, nx + 2, nx + 3},
      {3, 4, nx + 3, nx + 4},

      {5, nx + 1, 2 * nx + 0, 2 * nx + 1},
      {nx + 1, nx + 2, 2 * nx + 1, 2 * nx + 2},
      {nx + 2, nx + 3, 2 * nx + 2, 2 * nx + 3},
      {nx + 3, nx + 4, 2 * nx + 3, 2 * nx + 4}};

// 有了这些数组，我们可以转向稍高的高层数据结构。我们创建一个CellData对象的向量，为每个要创建的单元存储相关的顶点以及 @ref GlossMaterialId "材料ID"（我们在这里将其简单地设置为0，因为我们在程序中不使用它）。

// 然后，这些信息将被传递给 Triangulation::create_triangulation() 函数，并对网格进行两次全局细化。

    std::vector<CellData<dim>> cells((nx - 1) * (ny - 1), CellData<dim>());
    for (unsigned int i = 0; i < cells.size(); ++i)
      {
        cells[i].vertices    = cell_vertices[i];
        cells[i].material_id = 0;
      }

    triangulation.create_triangulation(
      vertices,
      cells,
      SubCellData()); // No boundary information

    triangulation.refine_global(2);

// 该函数的其余部分循环所有的单元格和它们的面，如果一个面在边界上，则决定哪个边界指标应该应用于它。如果你将代码与上面的几何图形相比较，各种条件应该是有意义的。

// 一旦完成了这一步，我们再全局地细化一下网格。

    for (auto &cell : triangulation.active_cell_iterators())
      for (auto &face : cell->face_iterators())
        if (face->at_boundary())
          {
            if ((face->center()[0] > 0) && (face->center()[0] < 0.5) &&
                (face->center()[1] > 0) && (face->center()[1] < 2))
              face->set_boundary_id(BoundaryIds::cathode);
            else if ((face->center()[0] > 0) && (face->center()[0] < 2))
              face->set_boundary_id(BoundaryIds::focus_element);
            else if ((face->center()[0] > 4 - 1e-12) &&
                     ((face->center()[1] > 1.5) || (face->center()[1] < 0.5)))
              face->set_boundary_id(BoundaryIds::anode);
            else
              face->set_boundary_id(BoundaryIds::open);
          }

    triangulation.refine_global(1);
  }
// @sect4{The CathodeRaySimulator::setup_system function}

// 本程序中的下一个函数是处理与解决偏微分方程有关的各种对象的设置。它本质上是对 step-6 中相应函数的复制，不需要进一步讨论。

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

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

    DoFTools::make_hanging_node_constraints(dof_handler, constraints);

    VectorTools::interpolate_boundary_values(dof_handler,
                                             BoundaryIds::cathode,
                                             Functions::ConstantFunction<dim>(
                                               -Constants::V0),
                                             constraints);
    VectorTools::interpolate_boundary_values(dof_handler,
                                             BoundaryIds::focus_element,
                                             Functions::ConstantFunction<dim>(
                                               -Constants::V0),
                                             constraints);
    VectorTools::interpolate_boundary_values(dof_handler,
                                             BoundaryIds::anode,
                                             Functions::ConstantFunction<dim>(
                                               +Constants::V0),
                                             constraints);
    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler,
                                    dsp,
                                    constraints,
                                    /*keep_constrained_dofs =  */ false);

    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
  }
// @sect4{The CathodeRaySimulator::assemble_system function}

// 计算矩阵项的函数实质上还是复制了  step-6  中的相应函数。

  template <int dim>
  void CathodeRaySimulator<dim>::assemble_system()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const QGauss<dim> quadrature_formula(fe.degree + 1);

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

    const unsigned int dofs_per_cell = fe.dofs_per_cell;

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0;
        cell_rhs    = 0;

        fe_values.reinit(cell);

        for (const unsigned int q_index : fe_values.quadrature_point_indices())
          for (const unsigned int i : fe_values.dof_indices())
            {
              for (const unsigned int j : fe_values.dof_indices())
                cell_matrix(i, j) +=
                  (fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                   fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                   fe_values.JxW(q_index));           // dx
            }

// 这个函数唯一有趣的部分是它是如何形成线性系统的右手边的。回顾一下，PDE的右边是
// @f[
//    \sum_p (N e)\delta(\mathbf x-\mathbf x_p),
//  @f]
//  ，在这里我们用 $p$ 来索引粒子，以避免与形状函数 $\varphi_i$ 混淆； $\mathbf x_p$ 是第 $p$ 个粒子的位置。

// 当与测试函数 $\varphi_i$ 相乘并在域上积分时，会得到一个右手边的向量
// @f{align*}{
//    F_i &= \int_\Omega \varphi_i (\mathbf x)\left[
//                 \sum_p (N e)\delta(\mathbf x-\mathbf x_p) \right] dx
//    \\  &=  \sum_p (N e) \varphi_i(\mathbf x_p).
//  @f}
//  注意最后一行不再包含一个积分，因此也没有出现 $dx$ ，这需要在我们的代码中出现`JxW`符号。
//
// 对于一个给定的单元 $K$ ，这个单元对右边的贡献是
//  @f{align*}{
//    F_i^K &= \sum_{p, \mathbf x_p\in K} (N e) \varphi_i(\mathbf x_p),
//  @f}，
//  也就是说，我们只需要担心那些实际位于当前单元 $K$ 上的粒子。

// 在实践中，我们在这里所做的是以下几点。如果当前单元格上有任何粒子，那么我们首先获得一个迭代器范围，指向该单元格的第一个粒子以及该单元格上最后一个粒子之后的粒子（或结束迭代器）--即C++函数中常见的半开放范围。现在知道了粒子的列表，我们查询它们的参考位置（相对于参考单元），评估这些参考位置的形状函数，并根据上面的公式计算力（没有任何  FEValues::JxW).  ）。
// @note  值得指出的是，调用 Particles::ParticleHandler::particles_in_cell() 和 Particles::ParticleHandler::n_particles_in_cell() 函数在有大量粒子的问题上不是很有效。但是它说明了写这个算法的最简单的方法，所以我们愿意为了说明问题而暂时承担这个代价。  我们在下面的"possibilities for extensions" section中更详细地讨论了这个问题，并在 step-70 中使用了一个更好的方法，例如：。

        if (particle_handler.n_particles_in_cell(cell) > 0)
          for (const auto &particle : particle_handler.particles_in_cell(cell))
            {
              const Point<dim> &reference_location =
                particle.get_reference_location();
              for (const unsigned int i : fe_values.dof_indices())
                cell_rhs(i) +=
                  (fe.shape_value(i, reference_location) * // phi_i(x_p)
                   (-Constants::electrons_per_particle *   // N
                    Constants::electron_charge));          // e
            }

// 最后，我们可以把这个单元格的贡献复制到全局矩阵和右边的向量中。

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(
          cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
      }
  }
// @sect4{CathodeRaySimulator::solve}

// 解决线性系统的函数又与 step-6 中的完全一样。

  template <int dim>
  void CathodeRaySimulator<dim>::solve_field()
  {
    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> solver(solver_control);

    PreconditionSSOR<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    solver.solve(system_matrix, solution, system_rhs, preconditioner);

    constraints.distribute(solution);
  }
// @sect4{CathodeRaySimulator::refine_grid}

// 最后一个与场相关的函数是细化网格的函数。我们将在第一个时间步骤中多次调用它，以获得一个能很好地适应解的结构的网格，特别是解决解中由于重心角和边界条件类型变化的地方而产生的各种奇异现象。你可能想再参考一下 step-6 以了解更多的细节。

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

    KellyErrorEstimator<dim>::estimate(dof_handler,
                                       QGauss<dim - 1>(fe.degree + 1),
                                       {},
                                       solution,
                                       estimated_error_per_cell);

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.1,
                                                    0.03);

    triangulation.execute_coarsening_and_refinement();
  }
// @sect4{CathodeRaySimulator::create_particles}

// 现在让我们来看看处理粒子的函数。第一个是关于粒子的创建。正如介绍中提到的，如果电场 $\mathbf E=\nabla V$ 超过某个阈值，即如果 $|\mathbf E| \ge E_\text{threshold}$ ，并且如果电场进一步指向域内（即如果 $\mathbf E \cdot \mathbf n < 0$ ），我们希望在阴极的各点创建一个粒子。正如有限元方法中常见的那样，我们在特定的评估点评估场（及其导数）；通常，这些是 "正交点"，因此我们创建了一个 "正交公式"，我们将用它来指定我们要评估解决方案的点。在这里，我们将简单地采用QMidpoint，意味着我们将只在面的中点检查阈值条件。然后我们用它来初始化一个FEFaceValues类型的对象来评估这些点的解。

// 然后，所有这些将被用于所有单元格、它们的面，特别是那些位于边界的面，而且是边界的阴极部分的循环中。

  template <int dim>
  void CathodeRaySimulator<dim>::create_particles()
  {
    FEFaceValues<dim> fe_face_values(fe,
                                     QMidpoint<dim - 1>(),
                                     update_quadrature_points |
                                       update_gradients |
                                       update_normal_vectors);

    std::vector<Tensor<1, dim>> solution_gradients(
      fe_face_values.n_quadrature_points);

    for (const auto &cell : dof_handler.active_cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->at_boundary() &&
            (face->boundary_id() == BoundaryIds::cathode))
          {
            fe_face_values.reinit(cell, face);

// 所以我们已经找到了阴极上的一个面。接下来，我们让FEFaceValues对象计算每个 "正交 "点的解的梯度，并通过 @ref vector_valued "矢量值问题 "文件模块中讨论的方法，以张量变量的形式从梯度中提取电场向量。

            const FEValuesExtractors::Scalar electric_potential(0);
            fe_face_values[electric_potential].get_function_gradients(
              solution, solution_gradients);
            for (const unsigned int q_point :
                 fe_face_values.quadrature_point_indices())
              {
                const Tensor<1, dim> E = solution_gradients[q_point];

// 只有当电场强度超过阈值时，电子才能逃离阴极，而且关键是，如果电场指向*域内，电子才能逃离阴极。      一旦我们检查了这一点，我们就在这个位置创建一个新的 Particles::Particle 对象，并将其插入到 Particles::ParticleHandler 对象中，并设置一个唯一的ID。            这里唯一不明显的是，我们还将这个粒子与我们当前所在的单元格的参考坐标中的位置联系起来。这样做是因为我们将在下游函数中计算诸如粒子位置的电场等量（例如，在每个时间步长中更新其位置时计算作用于它的力）。在任意坐标上评估有限元场是一个相当昂贵的操作，因为形状函数实际上只定义在参考单元上，所以当要求一个任意点的电场时，我们首先要确定这个点的参考坐标是什么。为了避免反复操作，我们一次性地确定这些坐标，然后将这些参考坐标直接存储在粒子上。

                if ((E * fe_face_values.normal_vector(q_point) < 0) &&
                    (E.norm() > Constants::E_threshold))
                  {
                    const Point<dim> &location =
                      fe_face_values.quadrature_point(q_point);

                    Particles::Particle<dim> new_particle;
                    new_particle.set_location(location);
                    new_particle.set_reference_location(
                      mapping.transform_real_to_unit_cell(cell, location));
                    new_particle.set_id(next_unused_particle_id);
                    particle_handler.insert_particle(new_particle, cell);

                    ++next_unused_particle_id;
                  }
              }
          }

// 在所有这些插入结束时，我们让`particle_handler`更新它所存储的粒子的一些内部统计数据。

    particle_handler.update_cached_numbers();
  }
// @sect4{CathodeRaySimulator::move_particles}

// 第二个与粒子有关的函数是在每个时间步骤中移动粒子的函数。要做到这一点，我们必须在所有的单元格、每个单元格中的粒子上循环，并评估每个粒子位置的电场。

// 这里使用的方法在概念上与`assemble_system()`函数中使用的相同。我们在所有单元中循环，找到位于那里的粒子（同样要注意这里用来寻找这些粒子的算法的低效率），并使用FEPointEvaluation对象来评估这些位置的梯度。

  template <int dim>
  void CathodeRaySimulator<dim>::move_particles()
  {
    const double dt = time.get_next_step_size();

    Vector<double>            solution_values(fe.n_dofs_per_cell());
    FEPointEvaluation<1, dim> evaluator(mapping, fe, update_gradients);

    for (const auto &cell : dof_handler.active_cell_iterators())
      if (particle_handler.n_particles_in_cell(cell) > 0)
        {
          const typename Particles::ParticleHandler<
            dim>::particle_iterator_range particles_in_cell =
            particle_handler.particles_in_cell(cell);

          std::vector<Point<dim>> particle_positions;
          for (const auto &particle : particles_in_cell)
            particle_positions.push_back(particle.get_reference_location());

          cell->get_dof_values(solution, solution_values);

// 然后，我们可以向FEPointEvaluation对象询问这些位置的解决方案的梯度（即电场 $\mathbf E$ ），并在各个粒子上循环。

          evaluator.reinit(cell, particle_positions);
          evaluator.evaluate(make_array_view(solution_values),
                             EvaluationFlags::gradients);

          {
            typename Particles::ParticleHandler<dim>::particle_iterator
              particle = particles_in_cell.begin();
            for (unsigned int particle_index = 0;
                 particle != particles_in_cell.end();
                 ++particle, ++particle_index)
              {
                const Tensor<1, dim> &E =
                  evaluator.get_gradient(particle_index);

// 现在我们已经得到了其中一个粒子位置的电场，我们首先用它来更新速度，然后更新位置。为此，我们首先从粒子的属性中获取旧的速度，计算加速度，更新速度，并将这个新的速度再次存储在粒子的属性中。回顾一下，这对应于介绍中所讨论的以下一组更新方程中的第一个。
      //  @f{align*}{
      //      \frac{{\mathbf v}_i^{(n)}
      //            -{\mathbf v}_i^{(n-1)}}{\Delta t}
      //      &= \frac{e\nabla V^{(n)}}{m}
      //   \\ \frac{{\mathbf x}_i^{(n)}-{\mathbf x}_i^{(n-1)}}
      //           {\Delta t} &= {\mathbf v}_i^{(n)}.
      //  @f}

                const Tensor<1, dim> old_velocity(particle->get_properties());

                const Tensor<1, dim> acceleration =
                  Constants::electron_charge / Constants::electron_mass * E;

                const Tensor<1, dim> new_velocity =
                  old_velocity + acceleration * dt;

                particle->set_properties(make_array_view(new_velocity));

// 有了新的速度，我们也就可以更新粒子的位置，并告诉粒子这个位置。

                const Point<dim> new_location =
                  particle->get_location() + dt * new_velocity;
                particle->set_location(new_location);
              }
          }
        }

// 在更新了所有粒子的位置和属性（即速度）之后，我们需要确保`particle_handler`再次知道它们在哪个单元中，以及它们在参考单元坐标系中的位置。下面的函数就是这样做的。(它还确保在并行计算中，如果粒子从一个处理器拥有的子域移动到另一个处理器拥有的子域，那么粒子会从一个处理器移动到另一个处理器。)

    particle_handler.sort_particles_into_subdomains_and_cells();
  }
// @sect4{CathodeRaySimulator::track_lost_particle}

// 最后一个与粒子相关的函数是当一个粒子从模拟中丢失时被调用的函数。这通常发生在它离开域的时候。如果发生这种情况，这个函数会同时调用单元（我们可以询问它的新位置）和它之前所在的单元。然后，该函数不断跟踪更新这个时间步骤中丢失的粒子数，丢失的粒子总数，然后估计该粒子是否通过阳极中间的孔离开。我们这样做，首先检查它最后所在的单元是否有一个 $x$ 坐标在右边边界的左边（位于 $x=4$ ），而粒子现在的位置在右边边界的右边。如果是这样的话，我们就计算出它的运动方向矢量，这个方向矢量被归一化了，所以方向矢量的 $x$ 分量等于 $1$  。有了这个方向矢量，我们可以计算出它与直线 $x=4$ 的相交位置。如果这个相交点在 $0.5$ 和 $1.5$ 之间，那么我们就声称粒子从孔中离开，并增加一个计数器。

  template <int dim>
  void CathodeRaySimulator<dim>::track_lost_particle(
    const typename Particles::ParticleIterator<dim> &        particle,
    const typename Triangulation<dim>::active_cell_iterator &cell)
  {
    ++n_recently_lost_particles;
    ++n_total_lost_particles;

    const Point<dim> current_location              = particle->get_location();
    const Point<dim> approximate_previous_location = cell->center();

    if ((approximate_previous_location[0] < 4) && (current_location[0] > 4))
      {
        const Tensor<1, dim> direction =
          (current_location - approximate_previous_location) /
          (current_location[0] - approximate_previous_location[0]);

        const double right_boundary_intercept =
          approximate_previous_location[1] +
          (4 - approximate_previous_location[0]) * direction[1];
        if ((right_boundary_intercept > 0.5) &&
            (right_boundary_intercept < 1.5))
          ++n_particles_lost_through_anode;
      }
  }

//  @sect4{CathodeRaySimulator::update_timestep_size}

// 正如在介绍中详细讨论的那样，我们需要尊重一个时间步长条件，即颗粒在一个时间步长中不能移动超过一个单元。为了确保这一点，我们首先计算每个单元上所有粒子的最大速度，然后用该速度除以单元大小。然后，我们使用介绍中讨论的安全系数，将下一个时间步长计算为所有单元上这个量的最小值，并使用 DiscreteTime::set_desired_time_step_size() 函数将其设定为所需的时间步长。

  template <int dim>
  void CathodeRaySimulator<dim>::update_timestep_size()
  {
    if (time.get_step_number() > 0)
      {
        double min_cell_size_over_velocity = std::numeric_limits<double>::max();

        for (const auto &cell : dof_handler.active_cell_iterators())
          if (particle_handler.n_particles_in_cell(cell) > 0)
            {
              const double cell_size = cell->minimum_vertex_distance();

              double max_particle_velocity(0.0);

              for (const auto &particle :
                   particle_handler.particles_in_cell(cell))
                {
                  const Tensor<1, dim> velocity(particle.get_properties());
                  max_particle_velocity =
                    std::max(max_particle_velocity, velocity.norm());
                }

              if (max_particle_velocity > 0)
                min_cell_size_over_velocity =
                  std::min(min_cell_size_over_velocity,
                           cell_size / max_particle_velocity);
            }

        constexpr double c_safety = 0.5;
        time.set_desired_next_step_size(c_safety * 0.5 *
                                        min_cell_size_over_velocity);
      }

// 正如在介绍中提到的，我们必须以不同的方式对待第一个时间步长，因为在那里，粒子还没有出现，或者还没有我们计算合理步长所需的相关信息。下面的公式遵循介绍中的讨论。

    else
      {
        const QTrapezoid<dim> vertex_quadrature;
        FEValues<dim> fe_values(fe, vertex_quadrature, update_gradients);

        std::vector<Tensor<1, dim>> field_gradients(vertex_quadrature.size());

        double min_timestep = std::numeric_limits<double>::max();

        for (const auto &cell : dof_handler.active_cell_iterators())
          if (particle_handler.n_particles_in_cell(cell) > 0)
            {
              const double cell_size = cell->minimum_vertex_distance();

              fe_values.reinit(cell);
              fe_values.get_function_gradients(solution, field_gradients);

              double max_E = 0;
              for (const auto q_point : fe_values.quadrature_point_indices())
                max_E = std::max(max_E, field_gradients[q_point].norm());

              if (max_E > 0)
                min_timestep =
                  std::min(min_timestep,
                           std::sqrt(0.5 * cell_size *
                                     Constants::electron_mass /
                                     Constants::electron_charge / max_E));
            }

        time.set_desired_next_step_size(min_timestep);
      }
  }

//  @sect4{The CathodeRaySimulator::output_results() function}

// 实现整个算法的最后一个函数是生成图形输出的函数。在目前的情况下，我们想同时输出电势场以及粒子的位置和速度。但我们也想输出电场，即解决方案的梯度。

// deal.II有一个一般的方法，可以从解决方案中计算出派生量，并输出这些量。在这里，这是电场，但也可以是其他的量--比如说，电场的法线，或者事实上任何其他人们想从解 $V_h(\mathbf x)$ 或其导数中计算的量。这个一般的解决方案使用了DataPostprocessor类，在像这里的情况下，我们想输出一个代表矢量场的量，则使用DataPostprocessorVector类。

// 与其尝试解释这个类是如何工作的，不如让我们简单地参考一下DataPostprocessorVector类的文档，这个案例基本上是一个有据可查的例子。

  template <int dim>
  class ElectricFieldPostprocessor : public DataPostprocessorVector<dim>
  {
  public:
    ElectricFieldPostprocessor()
      : DataPostprocessorVector<dim>("electric_field", update_gradients)
    {}

    virtual void evaluate_scalar_field(
      const DataPostprocessorInputs::Scalar<dim> &input_data,
      std::vector<Vector<double>> &computed_quantities) const override
    {
      AssertDimension(input_data.solution_gradients.size(),
                      computed_quantities.size());

      for (unsigned int p = 0; p < input_data.solution_gradients.size(); ++p)
        {
          AssertDimension(computed_quantities[p].size(), dim);
          for (unsigned int d = 0; d < dim; ++d)
            computed_quantities[p][d] = input_data.solution_gradients[p][d];
        }
    }
  };

// 有了这个，`output_results()`函数就变得相对简单了。我们使用DataOut类，就像我们在以前几乎所有的教程程序中使用的那样，来输出解决方案（"电动势"），我们使用上面定义的后处理程序来输出其梯度（"电场"）。这些都被写入一个VTU格式的文件中，同时将当前时间和时间步长与该文件联系起来。

  template <int dim>
  void CathodeRaySimulator<dim>::output_results() const
  {
    {
      ElectricFieldPostprocessor<dim> electric_field;
      DataOut<dim>                    data_out;
      data_out.attach_dof_handler(dof_handler);
      data_out.add_data_vector(solution, "electric_potential");
      data_out.add_data_vector(solution, electric_field);
      data_out.build_patches();

      data_out.set_flags(
        DataOutBase::VtkFlags(time.get_current_time(), time.get_step_number()));

      std::ofstream output("solution-" +
                           Utilities::int_to_string(time.get_step_number(), 4) +
                           ".vtu");
      data_out.write_vtu(output);
    }

// 输出粒子的位置和属性并不复杂。 Particles::DataOut 类扮演了粒子的DataOut类的角色，我们所要做的就是告诉该类从哪里获取粒子，以及如何解释属性中的`dim`分量--即作为表示速度的单一矢量，而不是作为`dim`标量属性。剩下的就和上面一样了。

    {
      Particles::DataOut<dim, dim> particle_out;
      particle_out.build_patches(
        particle_handler,
        std::vector<std::string>(dim, "velocity"),
        std::vector<DataComponentInterpretation::DataComponentInterpretation>(
          dim, DataComponentInterpretation::component_is_part_of_vector));

      particle_out.set_flags(
        DataOutBase::VtkFlags(time.get_current_time(), time.get_step_number()));

      std::ofstream output("particles-" +
                           Utilities::int_to_string(time.get_step_number(), 4) +
                           ".vtu");
      particle_out.write_vtu(output);
    }
  }
// @sect4{CathodeRaySimulator::run}

// 这个程序的主类的最后一个成员函数是驱动。在顶层，它通过在一连串越来越细的网格上求解问题（尚未创建粒子），对网格进行多次细化。

  template <int dim>
  void CathodeRaySimulator<dim>::run()
  {
    make_grid();

//在前面做几个细化循环

    const unsigned int n_pre_refinement_cycles = 3;
    for (unsigned int refinement_cycle = 0;
         refinement_cycle < n_pre_refinement_cycles;
         ++refinement_cycle)
      {
        setup_system();
        assemble_system();
        solve_field();
        refine_grid();
      }

// 现在进行时间上的循环。这个步骤的顺序紧跟介绍中讨论的算法大纲。正如在DiscreteTime类的文档中详细讨论的那样，虽然我们将场和粒子信息向前移动了一个时间步长，但存储在`time`变量中的时间与这些量的（部分）位置不一致（在DiscreteTime的字典中，这就是 "更新阶段"）。对`time.advance_time()`的调用通过将`time`变量设置为场和粒子已经处于的时间而使一切重新保持一致，一旦我们处于这个 "一致阶段"，我们就可以生成图形输出并将模拟的当前状态的信息写入屏幕。

    setup_system();
    do
      {
        std::cout << "Timestep " << time.get_step_number() + 1 << std::endl;
        std::cout << "  Field degrees of freedom:                 "
                  << dof_handler.n_dofs() << std::endl;

        assemble_system();
        solve_field();

        create_particles();
        std::cout << "  Total number of particles in simulation:  "
                  << particle_handler.n_global_particles() << std::endl;

        n_recently_lost_particles = 0;
        update_timestep_size();
        move_particles();

        time.advance_time();

        output_results();

        std::cout << "  Number of particles lost this time step:  "
                  << n_recently_lost_particles << std::endl;
        if (n_total_lost_particles > 0)
          std::cout << "  Fraction of particles lost through anode: "
                    << 1. * n_particles_lost_through_anode /
                         n_total_lost_particles
                    << std::endl;

        std::cout << std::endl
                  << "  Now at t=" << time.get_current_time()
                  << ", dt=" << time.get_previous_step_size() << '.'
                  << std::endl
                  << std::endl;
      }
    while (time.is_at_end() == false);
  }
} // namespace Step19

//  @sect3{The main function}

// 程序的最后一个函数又是`main()`函数。自 step-6 以来，它在所有的教程程序中都没有变化，因此没有什么新的内容需要讨论。

int main()
{
  try
    {
      Step19::CathodeRaySimulator<2> cathode_ray_simulator_2d;
      cathode_ray_simulator_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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


//---------------------------------------------------------------------------------------------------------------------
//  Vertical Engineering Solutions
//---------------------------------------------------------------------------------------------------------------------
//
//  Copyright 2020 Vertical Engineering Solutions  - All Rights Reserved
//
//  Unauthorized copying of this file, via any medium is strictly prohibited Proprietary and confidential.
//
//  All information contained herein is, and remains the property of Vertical Engineering Solutions.  The
//  intellectual and technical concepts contained herein are proprietary to Vertical Engineering Solutions
//  and its suppliers and may be covered by UE and Foreign Patents, patents in process, and are protected
//  by trade secret or copyright law. Dissemination of this information or reproduction of this material is
//  strictly forbidden unless prior written permission is obtained from Vertical Engineering Solutions.
//
//---------------------------------------------------------------------------------------------------------------------
//
//  Maintainer: pramon@vengineerings.com
//
//---------------------------------------------------------------------------------------------------------------------

#include <mico/robotics/PID.h>
#include <algorithm>
#include <thread>
#include <chrono>
#include <iostream>
#include <cmath>
#include <Eigen/Eigen>

#include <cmath>

namespace mico{

    namespace robotics{
        PID::PID(float _kp, float _ki, float _kd, float _fc, float _minSat, float _maxSat) {
            kp_ = _kp;
            ki_ = _ki;
            kd_ = _kd;
            fc_ = _fc;
            assert(_minSat <= _maxSat);
            minSat_ = _minSat;
            maxSat_ = _maxSat;
            clear();
        }

        /*void PID::setAntiWindup(AntiWindupMethod _antiWindup, std::vector<float> _params){

        }*/

        void PID::clear(){
            lastError_ = 0;
            lastResult_ = 0;
            accumErr_ = 0;
            accumDeriv_ = 0;
        }

        void PID::overrideAccumulative(float _accumulative, bool _isOutputScale){
            if(!std::isnan(_accumulative)){
                if(_isOutputScale){
                    if(ki_ != 0.0f){
                        accumErr_ = _accumulative/ki_;
                    }
                }else{
                    accumErr_ = _accumulative;
                }
            }
        }

        void PID::reference(float _ref, float _time, bool _reset) {
            reference_ = _ref;
            targetReference_ = _ref;
            // if(_time == 0){
            //     reference_ = _ref;
            //     targetReference_ = _ref;
            // }else{
            //     targetReference_ = _ref;
            //     slope_ = (targetReference_ - reference_)/_time;
            // }

            if(_reset){
                clear();
            }
        }

        float PID::update(float _val, float _incT) {

            float dt = _incT; // 666 input arg?
            float err = reference_ - _val;

            float up = kp_ * err;

            accumErr_ += err * dt;
            float ui = ki_ * accumErr_;

            float ud = ((kd_ * err) - accumDeriv_) * fc_ ;
            accumDeriv_ += ud * dt;

            // Compute PID
            float unsaturated =   up + ui + ud;

            lastError_ = err;

            // Saturate signal
            float saturated = std::min(std::max(unsaturated, minSat_), maxSat_);
            lastResult_ = saturated;

            return lastResult_;

        }
    }
}


#pragma once

#include <Eigen/Dense>
#include <utility>
#include <vector>

#include "basis.hpp"
#include "sparse_vector.hpp"

namespace Time {

template <typename I, typename BasisIn, typename BasisOut>
class LinearOperator {
 public:
  // This MatVec applies the operator column-wise.
  SparseVector<BasisOut> MatVec(const SparseVector<BasisIn> &vec) const;
  SparseVector<BasisIn> RMatVec(const SparseVector<BasisOut> &vec) const;

  // This MatVec applies the operator row-wise for the given output indices.
  SparseVector<BasisOut> MatVec(
      const SparseVector<BasisIn> &vec,
      const SparseIndices<BasisOut> &indices_out) const;
  SparseVector<BasisIn> RMatVec(const SparseVector<BasisOut> &vec,
                                const SparseIndices<BasisIn> &indices_in) const;

  // Create functor operators, for convenience.
  SparseVector<BasisOut> operator()(const SparseVector<BasisIn> &vec) const {
    return MatVec(vec);
  }
  SparseVector<BasisOut> operator()(
      const SparseVector<BasisIn> &vec,
      const SparseIndices<BasisOut> &indices_out) const {
    return MatVec(vec, indices_out);
  }

  // Return the range of this operator if you were to apply the given indices.
  SparseIndices<BasisOut> Range(const SparseIndices<BasisIn> &ind) const;

  // Debug function, O(n^2).
  Eigen::MatrixXd ToMatrix(const SparseIndices<BasisIn> &indices_in,
                           const SparseIndices<BasisOut> &indices_out) const;
};

/**
 *  Below are the operations for transformations between single and multi scale.
 */
template <typename Wavelet>
class WaveletToScaling
    : public LinearOperator<WaveletToScaling<Wavelet>, Wavelet,
                            typename FunctionTrait<Wavelet>::Scaling> {
 public:
  static inline const auto &Column(Wavelet *psi_in) {
    return psi_in->single_scale();
  }

  // We do not have an implementation of the transpose.
};

/**
 *  Below are the prolongation/restriction operators for single scale functions.
 */
template <typename Basis>
class Prolongate : public LinearOperator<Prolongate<Basis>, Basis, Basis> {
 public:
  static inline auto Column(Basis *phi_in);
  static inline auto Row(Basis *phi_out);
};

/**
 *   Below are the single scale (levelwise) operators.
 */
template <typename BasisIn, typename BasisOut>
class MassOperator : public LinearOperator<MassOperator<BasisIn, BasisOut>,
                                           BasisIn, BasisOut> {
 public:
  static inline auto Column(BasisIn *phi_in);
  static inline auto Row(BasisOut *phi_out);
};

// Evaluates the functions in zero: <gamma_0 phi, gamma_0 psi> = phi(0)
// psi(0).
template <typename BasisIn, typename BasisOut>
class ZeroEvalOperator
    : public LinearOperator<ZeroEvalOperator<BasisIn, BasisOut>, BasisIn,
                            BasisOut> {
 public:
  static inline auto Column(BasisIn *phi_in);
  static inline auto Row(BasisOut *phi_out);
};

// Transport matrix <phi, d/dt psi>.
template <typename BasisIn, typename BasisOut>
class TransportOperator
    : public LinearOperator<TransportOperator<BasisIn, BasisOut>, BasisIn,
                            BasisOut> {
 public:
  static inline auto Column(BasisIn *phi_in);
  static inline auto Row(BasisOut *phi_out);
};

}  // namespace Time

#include "linear_operator.ipp"


// boost\math\distributions\beta.hpp

// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2006.

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

// http://en.wikipedia.org/wiki/Beta_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda366h.htm
// http://mathworld.wolfram.com/BetaDistribution.html

// The Beta Distribution is a continuous probability distribution.
// The beta distribution is used to model events which are constrained to take place
// within an interval defined by maxima and minima,
// so is used extensively in PERT and other project management systems
// to describe the time to completion.
// The cdf of the beta distribution is used as a convenient way
// of obtaining the sum over a set of binomial outcomes.
// The beta distribution is also used in Bayesian statistics.

#ifndef BOOST_MATH_DIST_BETA_HPP
#define BOOST_MATH_DIST_BETA_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for beta.
#include <boost/math/distributions/complement.hpp> // complements.
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.

#if defined (BOOST_MSVC)
#  pragma warning(push)
#  pragma warning(disable: 4702) // unreachable code
// in domain_error_imp in error_handling
#endif

#include <utility>

namespace boost
{
  namespace math
  {
    namespace beta_detail
    {
      // Common error checking routines for beta distribution functions:
      template <class RealType, class Policy>
      inline bool check_alpha(const char* function, const RealType& alpha, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(alpha) || (alpha <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Alpha argument is %1%, but must be > 0 !", alpha, pol);
          return false;
        }
        return true;
      } // bool check_alpha

      template <class RealType, class Policy>
      inline bool check_beta(const char* function, const RealType& beta, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(beta) || (beta <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Beta argument is %1%, but must be > 0 !", beta, pol);
          return false;
        }
        return true;
      } // bool check_beta

      template <class RealType, class Policy>
      inline bool check_prob(const char* function, const RealType& p, RealType* result, const Policy& pol)
      {
        if((p < 0) || (p > 1) || !(boost::math::isfinite)(p))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Probability argument is %1%, but must be >= 0 and <= 1 !", p, pol);
          return false;
        }
        return true;
      } // bool check_prob

      template <class RealType, class Policy>
      inline bool check_x(const char* function, const RealType& x, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(x) || (x < 0) || (x > 1))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "x argument is %1%, but must be >= 0 and <= 1 !", x, pol);
          return false;
        }
        return true;
      } // bool check_x

      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& alpha, const RealType& beta, RealType* result, const Policy& pol)
      { // Check both alpha and beta.
        return check_alpha(function, alpha, result, pol)
          && check_beta(function, beta, result, pol);
      } // bool check_dist

      template <class RealType, class Policy>
      inline bool check_dist_and_x(const char* function, const RealType& alpha, const RealType& beta, RealType x, RealType* result, const Policy& pol)
      {
        return check_dist(function, alpha, beta, result, pol)
          && beta_detail::check_x(function, x, result, pol);
      } // bool check_dist_and_x

      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, const RealType& alpha, const RealType& beta, RealType p, RealType* result, const Policy& pol)
      {
        return check_dist(function, alpha, beta, result, pol)
          && check_prob(function, p, result, pol);
      } // bool check_dist_and_prob

      template <class RealType, class Policy>
      inline bool check_mean(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(mean) || (mean <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "mean argument is %1%, but must be > 0 !", mean, pol);
          return false;
        }
        return true;
      } // bool check_mean
      template <class RealType, class Policy>
      inline bool check_variance(const char* function, const RealType& variance, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(variance) || (variance <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "variance argument is %1%, but must be > 0 !", variance, pol);
          return false;
        }
        return true;
      } // bool check_variance
    } // namespace beta_detail

    // typedef beta_distribution<double> beta;
    // is deliberately NOT included to avoid a name clash with the beta function.
    // Use beta_distribution<> mybeta(...) to construct type double.

    template <class RealType = double, class Policy = policies::policy<> >
    class beta_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      beta_distribution(RealType alpha = 1, RealType beta = 1) : m_alpha(alpha), m_beta(beta)
      {
        RealType result;
        beta_detail::check_dist(
           "boost::math::beta_distribution<%1%>::beta_distribution",
          m_alpha,
          m_beta,
          &result, Policy());
      } // beta_distribution constructor.
      // Accessor functions:
      RealType alpha() const
      {
        return m_alpha;
      }
      RealType beta() const
      { // .
        return m_beta;
      }

      // Estimation of the alpha & beta parameters.
      // http://en.wikipedia.org/wiki/Beta_distribution
      // gives formulae in section on parameter estimation.
      // Also NIST EDA page 3 & 4 give the same.
      // http://www.itl.nist.gov/div898/handbook/eda/section3/eda366h.htm
      // http://www.epi.ucdavis.edu/diagnostictests/betabuster.html

      static RealType find_alpha(
        RealType mean, // Expected value of mean.
        RealType variance) // Expected value of variance.
      {
        static const char* function = "boost::math::beta_distribution<%1%>::find_alpha";
        RealType result = 0; // of error checks.
        if(false ==
            (
              beta_detail::check_mean(function, mean, &result, Policy())
              && beta_detail::check_variance(function, variance, &result, Policy())
            )
          )
        {
          return result;
        }
        return mean * (( (mean * (1 - mean)) / variance)- 1);
      } // RealType find_alpha

      static RealType find_beta(
        RealType mean, // Expected value of mean.
        RealType variance) // Expected value of variance.
      {
        static const char* function = "boost::math::beta_distribution<%1%>::find_beta";
        RealType result = 0; // of error checks.
        if(false ==
            (
              beta_detail::check_mean(function, mean, &result, Policy())
              &&
              beta_detail::check_variance(function, variance, &result, Policy())
            )
          )
        {
          return result;
        }
        return (1 - mean) * (((mean * (1 - mean)) /variance)-1);
      } //  RealType find_beta

      // Estimate alpha & beta from either alpha or beta, and x and probability.
      // Uses for these parameter estimators are unclear.

      static RealType find_alpha(
        RealType beta, // from beta.
        RealType x, //  x.
        RealType probability) // cdf
      {
        static const char* function = "boost::math::beta_distribution<%1%>::find_alpha";
        RealType result = 0; // of error checks.
        if(false ==
            (
             beta_detail::check_prob(function, probability, &result, Policy())
             &&
             beta_detail::check_beta(function, beta, &result, Policy())
             &&
             beta_detail::check_x(function, x, &result, Policy())
            )
          )
        {
          return result;
        }
        return ibeta_inva(beta, x, probability, Policy());
      } // RealType find_alpha(beta, a, probability)

      static RealType find_beta(
        // ibeta_invb(T b, T x, T p); (alpha, x, cdf,)
        RealType alpha, // alpha.
        RealType x, // probability x.
        RealType probability) // probability cdf.
      {
        static const char* function = "boost::math::beta_distribution<%1%>::find_beta";
        RealType result = 0; // of error checks.
        if(false ==
            (
              beta_detail::check_prob(function, probability, &result, Policy())
              &&
              beta_detail::check_alpha(function, alpha, &result, Policy())
              &&
              beta_detail::check_x(function, x, &result, Policy())
            )
          )
        {
          return result;
        }
        return ibeta_invb(alpha, x, probability, Policy());
      } //  RealType find_beta(alpha, x, probability)

    private:
      RealType m_alpha; // Two parameters of the beta distribution.
      RealType m_beta;
    }; // template <class RealType, class Policy> class beta_distribution

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const beta_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable x.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const beta_distribution<RealType, Policy>&  /* dist */)
    { // Range of supported values for random variable x.
      // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline RealType mean(const beta_distribution<RealType, Policy>& dist)
    { // Mean of beta distribution = np.
      return  dist.alpha() / (dist.alpha() + dist.beta());
    } // mean

    template <class RealType, class Policy>
    inline RealType variance(const beta_distribution<RealType, Policy>& dist)
    { // Variance of beta distribution = np(1-p).
      RealType a = dist.alpha();
      RealType b = dist.beta();
      return  (a * b) / ((a + b ) * (a + b) * (a + b + 1));
    } // variance

    template <class RealType, class Policy>
    inline RealType mode(const beta_distribution<RealType, Policy>& dist)
    {
      static const char* function = "boost::math::mode(beta_distribution<%1%> const&)";

      RealType result;
      if ((dist.alpha() <= 1))
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "mode undefined for alpha = %1%, must be > 1!", dist.alpha(), Policy());
        return result;
      }

      if ((dist.beta() <= 1))
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "mode undefined for beta = %1%, must be > 1!", dist.beta(), Policy());
        return result;
      }
      RealType a = dist.alpha();
      RealType b = dist.beta();
      return (a-1) / (a + b - 2);
    } // mode

    //template <class RealType, class Policy>
    //inline RealType median(const beta_distribution<RealType, Policy>& dist)
    //{ // Median of beta distribution is not defined.
    //  return tools::domain_error<RealType>(function, "Median is not implemented, result is %1%!", std::numeric_limits<RealType>::quiet_NaN());
    //} // median

    //But WILL be provided by the derived accessor as quantile(0.5).

    template <class RealType, class Policy>
    inline RealType skewness(const beta_distribution<RealType, Policy>& dist)
    {
      BOOST_MATH_STD_USING // ADL of std functions.
      RealType a = dist.alpha();
      RealType b = dist.beta();
      return (2 * (b-a) * sqrt(a + b + 1)) / ((a + b + 2) * sqrt(a * b));
    } // skewness

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const beta_distribution<RealType, Policy>& dist)
    {
      RealType a = dist.alpha();
      RealType b = dist.beta();
      RealType a_2 = a * a;
      RealType n = 6 * (a_2 * a - a_2 * (2 * b - 1) + b * b * (b + 1) - 2 * a * b * (b + 2));
      RealType d = a * b * (a + b + 2) * (a + b + 3);
      return  n / d;
    } // kurtosis_excess

    template <class RealType, class Policy>
    inline RealType kurtosis(const beta_distribution<RealType, Policy>& dist)
    {
      return 3 + kurtosis_excess(dist);
    } // kurtosis

    template <class RealType, class Policy>
    inline RealType pdf(const beta_distribution<RealType, Policy>& dist, const RealType& x)
    { // Probability Density/Mass Function.
      BOOST_FPU_EXCEPTION_GUARD

      static const char* function = "boost::math::pdf(beta_distribution<%1%> const&, %1%)";

      BOOST_MATH_STD_USING // for ADL of std functions

      RealType a = dist.alpha();
      RealType b = dist.beta();

      // Argument checks:
      RealType result = 0;
      if(false == beta_detail::check_dist_and_x(
        function,
        a, b, x,
        &result, Policy()))
      {
        return result;
      }
      using boost::math::beta;
      return ibeta_derivative(a, b, x, Policy());
    } // pdf

    template <class RealType, class Policy>
    inline RealType cdf(const beta_distribution<RealType, Policy>& dist, const RealType& x)
    { // Cumulative Distribution Function beta.
      BOOST_MATH_STD_USING // for ADL of std functions

      static const char* function = "boost::math::cdf(beta_distribution<%1%> const&, %1%)";

      RealType a = dist.alpha();
      RealType b = dist.beta();

      // Argument checks:
      RealType result = 0;
      if(false == beta_detail::check_dist_and_x(
        function,
        a, b, x,
        &result, Policy()))
      {
        return result;
      }
      // Special cases:
      if (x == 0)
      {
        return 0;
      }
      else if (x == 1)
      {
        return 1;
      }
      return ibeta(a, b, x, Policy());
    } // beta cdf

    template <class RealType, class Policy>
    inline RealType cdf(const complemented2_type<beta_distribution<RealType, Policy>, RealType>& c)
    { // Complemented Cumulative Distribution Function beta.

      BOOST_MATH_STD_USING // for ADL of std functions

      static const char* function = "boost::math::cdf(beta_distribution<%1%> const&, %1%)";

      RealType const& x = c.param;
      beta_distribution<RealType, Policy> const& dist = c.dist;
      RealType a = dist.alpha();
      RealType b = dist.beta();

      // Argument checks:
      RealType result = 0;
      if(false == beta_detail::check_dist_and_x(
        function,
        a, b, x,
        &result, Policy()))
      {
        return result;
      }
      if (x == 0)
      {
        return 1;
      }
      else if (x == 1)
      {
        return 0;
      }
      // Calculate cdf beta using the incomplete beta function.
      // Use of ibeta here prevents cancellation errors in calculating
      // 1 - x if x is very small, perhaps smaller than machine epsilon.
      return ibetac(a, b, x, Policy());
    } // beta cdf

    template <class RealType, class Policy>
    inline RealType quantile(const beta_distribution<RealType, Policy>& dist, const RealType& p)
    { // Quantile or Percent Point beta function or
      // Inverse Cumulative probability distribution function CDF.
      // Return x (0 <= x <= 1),
      // for a given probability p (0 <= p <= 1).
      // These functions take a probability as an argument
      // and return a value such that the probability that a random variable x
      // will be less than or equal to that value
      // is whatever probability you supplied as an argument.

      static const char* function = "boost::math::quantile(beta_distribution<%1%> const&, %1%)";

      RealType result = 0; // of argument checks:
      RealType a = dist.alpha();
      RealType b = dist.beta();
      if(false == beta_detail::check_dist_and_prob(
        function,
        a, b, p,
        &result, Policy()))
      {
        return result;
      }
      // Special cases:
      if (p == 0)
      {
        return 0;
      }
      if (p == 1)
      {
        return 1;
      }
      return ibeta_inv(a, b, p, static_cast<RealType*>(0), Policy());
    } // quantile

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<beta_distribution<RealType, Policy>, RealType>& c)
    { // Complement Quantile or Percent Point beta function .
      // Return the number of expected x for a given
      // complement of the probability q.

      static const char* function = "boost::math::quantile(beta_distribution<%1%> const&, %1%)";

      //
      // Error checks:
      RealType q = c.param;
      const beta_distribution<RealType, Policy>& dist = c.dist;
      RealType result = 0;
      RealType a = dist.alpha();
      RealType b = dist.beta();
      if(false == beta_detail::check_dist_and_prob(
        function,
        a,
        b,
        q,
        &result, Policy()))
      {
        return result;
      }
      // Special cases:
      if(q == 1)
      {
        return 0;
      }
      if(q == 0)
      {
        return 1;
      }

      return ibetac_inv(a, b, q, static_cast<RealType*>(0), Policy());
    } // Quantile Complement

  } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#if defined (BOOST_MSVC)
# pragma warning(pop)
#endif

#endif // BOOST_MATH_DIST_BETA_HPP


#include "Crypt.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/program_options.hpp>
#include <algorithm>
#include <fstream>
#include <iterator>

namespace mp = boost::multiprecision;
namespace po = boost::program_options;

int main(int argc, char** argv)
{
	po::options_description commandLineOptions
	{
		"Usage: rsa [options]\n"
		"Where options are"
	};
	commandLineOptions.add_options()
		("decrypt,d", po::value<std::string>(), "specifies the input file for decryption")
		("encrypt,e", po::value<std::string>(), "specifies the input file for encryption")
		("help,h", "produce help messages")
		("key,k", po::value<std::string>(), "specifies the key file")
		("output,o", po::value<std::string>(), "specify the output file")
		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, commandLineOptions), vm);
	po::notify(vm);

	// Run decryption
	if(vm.count("decrypt") && vm.count("key") && !vm.count("encrypt"))
	{
		const std::string inFileName = vm["decrypt"].as<std::string>();
		const std::string keyFileName = vm["key"].as<std::string>();
		const std::string outFileName = vm.count("output") ? vm["output"].as<std::string>() : inFileName;

		// Get key parameters
		std::ifstream keyFile{keyFileName};
		mp::cpp_int p, q, e;
		keyFile >> p >> q >> e;
		keyFile.close();

		// Decrypt data
		std::ifstream inFile{inFileName};
		std::ofstream outFile{outFileName};
		std::istream_iterator<mp::cpp_int> inFileFirst{inFile}, inFileLast;
		std::ostream_iterator<char> outFileResult{outFile};
		RSAKey privateKey = makePrivateKey(p, q, e);
		std::transform(inFileFirst, inFileLast, outFileResult, [&privateKey](const auto& cypher) { return (char) decrypt(privateKey, cypher); });
	}
	// Run encryption
	else if(vm.count("encrypt") && vm.count("key") && !vm.count("decrypt"))
	{
		const std::string inFileName = vm["encrypt"].as<std::string>();
		const std::string keyFileName = vm["key"].as<std::string>();
		const std::string outFileName = vm.count("output") ? vm["output"].as<std::string>() : inFileName;

		// Get key parameters
		std::ifstream keyFile{keyFileName};
		mp::cpp_int n, e;
		keyFile >> n >> e;
		keyFile.close();

		// Encrypt data
		std::ifstream inFile{inFileName};
		std::ofstream outFile{outFileName};
		std::istream_iterator<char> inFileFirst{inFile}, inFileLast;
		std::ostream_iterator<mp::cpp_int> outFileResult{outFile, " "};
		RSAKey publicKey = makePublicKey(n, e);
		std::noskipws(inFile);
		std::transform(inFileFirst, inFileLast, outFileResult, [&publicKey](char plain) { return encrypt(publicKey, plain); });
	}
	// Invalid arguments or specified help
	else
		std::cout << commandLineOptions << std::endl;

	return 0;
}


#pragma once

#include <boost/random.hpp>

class Distributions {

private:
    boost::mt19937 rng;

public:

    void set_prng(unsigned int seed) {
        rng = boost::mt19937(seed);
    }

    unsigned int get_random_number() {
        return rng();
    }

    boost::mt19937& get_rng() {
        return rng;
    }

    double inv_scaled_chisq_rng(const double a, const double b) {
        return inv_gamma_rng(0.5 * a, 0.5 * a * b);
    }

    double inv_gamma_rng(const double a, const double b) {
        return 1.0 / rgamma(a, 1.0 / b);
    }

    double rgamma(const double a, const double b) {
        boost::random::gamma_distribution<double> myGamma(a, b);
        boost::random::variate_generator<boost::mt19937&, boost::random::gamma_distribution<> > rand_gamma(rng, myGamma);
        double val = rand_gamma();
        return val;
    }

    double beta_rng(const double a, const double b) {
        //std::cout << "@@ beta_rng " << a << ", " << b << std::endl;
        boost::random::beta_distribution<double> mybeta(a, b);
        boost::random::variate_generator<boost::mt19937&, boost::random::beta_distribution<> > rand_beta(rng, mybeta);
        double val = rand_beta();
        //std::cout << "@@ beta_rng val = " << val << std::endl;
        return val;
    }

    double norm_rng(double mean, double sigma2) {
        //std::cout << "@@ norm_rng on " << mean << ", " << sigma2 << std::endl;
        boost::random::normal_distribution<double> nd(mean, std::sqrt(sigma2));
        boost::random::variate_generator< boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
        return var_nor();
    }

    double unif_rng() {
        boost::random::uniform_real_distribution<double> myU(0,1);
        boost::random::variate_generator<boost::mt19937&, boost::random::uniform_real_distribution<> > real_variate_generator(rng, myU);
        return real_variate_generator();
    }

};


#include <iostream>
#include <fstream>
#include <armadillo>
#include <omp.h>
#include "Hyperphysics.h"

int main() {
    //Setup
    double simWidth =  300;
    double simHeight = 300;
    double simDepth  = 50;
    int nElem    = 3000;
    int step     = 20000;

    Hyperphysics physics(nElem, simWidth, simHeight,simDepth);
    physics.randomPosition();
    physics.randomMasse();
    physics.setElemPosition(0,arma::mat {{simWidth/2,simHeight/2,simDepth/2}});
    physics.setElemMasse(0,1e9);
    //Print information about the simulation
    std::cout << "Simulation READY for " << omp_get_max_threads() << " processors "<<std::endl;
    std::cout << nElem << " initialized" << std::endl;
    char filePath[255];

    //Simulation Loop
    for (int i = 1; i < step ; ++i) {
        snprintf(filePath,255,"data.csv.%i",i);
        physics.step();
	if(i%10==1){
        	std::ofstream outFile(filePath);
		outFile<< "x, y, z, ux, uy, uz, masse, radius" <<std::endl;
        	outFile<< physics ;
        	outFile.close();
	}
        std::cout << "Time: " << i << std::endl;
    }
    return 0;
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <fstream>
#include <inttypes.h>
#include <stdint.h>
#include <dSFMT-calc-jump.hpp>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace NTL;
using namespace std;
using namespace dsfmt;

static void read_file(GF2X& lcmpoly, long line_no, const string& file);

int main(int argc, char *argv[]) {
    if (argc < 2) {
	cout << argv[0] << " filename" << endl;
	return -1;
    }
    string filename = argv[1];
    GF2X poly;
    read_file(poly, 0, filename);

    vec_pair_GF2X_long factors;
    CanZass(factors, poly);
    cout << "degree = " << dec << deg(poly) << endl;
    cout << "=== factors ===" << endl;
    for (int i = 0; i < factors.length(); i++) {
	cout << factors[i].a;
	cout << ":";
	cout << dec << deg(factors[i].a);
	cout << ":";
	cout << factors[i].b << endl;
    }
    cout << "=== factors ===" << endl;
}

static void read_file(GF2X& lcmpoly, long line_no, const string& file)
{
    ifstream ifs(file.c_str());
    string line;
    for (int i = 0; i < line_no; i++) {
	ifs >> line;
	ifs >> line;
    }
    if (ifs) {
	ifs >> line;
	line = "";
	ifs >> line;
    }
    stringtopoly(lcmpoly, line);
}


// log.inl
//
// Copyright (c) 2016 Piotr K. Semenov (piotr.k.semenov at gmail dot com)
// Distributed under the New BSD License. (See accompanying file LICENSE)

/*!
 \file log.inl

 Provides CORDIC for ln function
 \ref see C. Baumann, "A simple and fast look-up table method to compute the
 exp(x) and ln(x) functions", 2004
*/

#ifndef INC_STD_LOG_INL_
#define INC_STD_LOG_INL_

#include <boost/integer/static_min_max.hpp>
#include <boost/integer/static_log2.hpp>

#include <cassert>

namespace libq {
namespace details {
/*!
 \brief
 \note
*/
template<typename T, std::size_t n, std::size_t f, int e, typename op, typename up>  // NOLINT
class log_of
    : public type_promotion_base<
        fixed_point<typename std::make_signed<T>::type, n, f, e, op, up>
        , boost::static_unsigned_max<
                      (f > 0) ? (boost::static_log2<f>::value) : 0,
                      (n > 0) ? (boost::static_log2<n>::value) : 0>::value + 1u
        , 0
        , 0> {
};
}  // namespace details
}  // namespace libq

namespace std {
template<typename T, std::size_t n, std::size_t f, int e, typename op, typename up>  // NOLINT
typename libq::details::log_of<T, n, f, e, op, up>::promoted_type
    log(libq::fixed_point<T, n, f, e, op, up> _val) {
    using Q = libq::fixed_point<T, n, f, e, op, up>;
    using log_type =
        typename libq::details::log_of<T, n, f, e, op, up>::promoted_type;
    using lut = libq::cordic::lut<f, Q>;

    assert(("[std::log] argument is negaitve", _val >= Q(0)));
    if (_val <= Q(0)) {
        throw std::logic_error("[std::log]: argument is negative");
    }

    // one need 1 bit to represent integer part of reals from [1.0, 2.0]
    using work_type = libq::UQ<f + 1u, f, 0, op, up>;

    // reduces argument to interval [1.0, 2.0]
    int power(0);
    Q arg(_val);
    while (arg >= Q(2.0)) {
        libq::lift(arg) >>= 1u;
        power++;
    }
    while (arg < Q(1.0)) {
        libq::lift(arg) <<= 1u;
        power--;
    }

    // one can consider 0 < y = log(2, x) < 1 as x = 2^y
    // so CORDIC rotation is just a multiplication by 2^{1/2^i}:
    // 2^y = 2^{a1/2} * 2^{a2/4} * ... * 2^{ai/2^i}, where ai is from
    // {0, 1}
    static libq::cordic::lut<f, Q> const inv_pow2_lut = lut::inv_pow2();

    work_type result(0);

#ifdef LOOP_UNROLLING
    auto const iteration_body = [&](std::size_t i) {  // NOLINT
#else
    for (std::size_t i = 0; i != f; ++i) {
#endif
        if (work_type(arg * inv_pow2_lut[i]) >= work_type(1.0)) {
            arg = work_type(arg * inv_pow2_lut[i]);

            libq::lift(result) +=
                typename work_type::storage_type(1u) << (f - i - 1u);
        }
    };  // NOLINT
#ifdef LOOP_UNROLLING
    libq::details::unroll(iteration_body, 0u, libq::details::loop_size<f-1>());
#endif

    log_type const r0(log_type(result) + log_type(power));
    log_type const r1(r0 * work_type::CONST_1_LOG2E);

    return r1;
}
}  // namespace std

#endif  // INC_STD_LOG_INL_


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct Limb
{
public:
  int a, b, c;

  Limb(int a, int b, int c) : a(a), b(b), c(c){};
};

class EdgeAdder
{
  Graph &G;

public:
  explicit EdgeAdder(Graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity)
  {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void testcase()
{
  int n, m;
  std::cin >> n >> m;
  assert(n >= 2 && n <= 200 && m >= 0 && m <= 5000);

  std::vector<std::vector<int>> limb_index_by_a_b(n, std::vector<int>(n, -1));
  std::vector<Limb> limbs;
  for (int i = 0; i < m; i++)
  {
    int a, b, c;
    std::cin >> a >> b >> c;
    assert(a >= 0 && a < n && b >= 0 && b < n && a != b && c >= 1 && c <= 1000);

    int &limb_index = limb_index_by_a_b.at(a).at(b);
    if (limb_index == -1)
    {
      limb_index = limbs.size();
      limbs.emplace_back(a, b, 0);
    }

    Limb &limb = limbs.at(limb_index);
    assert(limb.a == a && limb.b == b);
    limb.c += c;
  }

  int next_free_node = 0;
  const auto get_node_for_figure = [next_free_node, n](int i) {
    assert(i >= 0 && i < n);
    return next_free_node + i;
  };
  next_free_node += n;
  const int num_nodes = next_free_node;

  Graph G(num_nodes);
  EdgeAdder adder(G);
  auto c_map = boost::get(boost::edge_capacity, G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  for (const Limb &limb : limbs)
  {
    adder.add_edge(get_node_for_figure(limb.a), get_node_for_figure(limb.b), limb.c);
  }

  int min_cut_cost_ever = std::numeric_limits<int>::max();
  for (int source_figure = 0; source_figure < n; source_figure++)
  {
    const int target_figure = (source_figure + 1) % n;

    const int node_source = get_node_for_figure(source_figure);
    const int node_target = get_node_for_figure(target_figure);

    boost::push_relabel_max_flow(G, node_source, node_target);

    std::vector<bool> visited_by_node(num_nodes, false);
    visited_by_node.at(node_source) = true;
    std::deque<int> queue{node_source};
    while (!queue.empty())
    {
      const int node = queue.front();
      queue.pop_front();
      for (auto it = boost::out_edges(node, G); it.first != it.second; it.first++)
      {
        const int next_node = boost::target(*it.first, G);
        if (rc_map[*it.first] > 0 && !visited_by_node.at(next_node))
        {
          visited_by_node.at(next_node) = true;
          queue.push_back(next_node);
        }
      }
    }

    int min_cut_cost = 0;
    for (int i = 0; i < num_nodes; i++)
    {
      if (!visited_by_node.at(i))
      {
        continue;
      }
      for (auto it = boost::out_edges(i, G); it.first != it.second; it.first++)
      {
        const int j = boost::target(*it.first, G);
        if (!visited_by_node.at(j))
        {
          min_cut_cost += c_map[*it.first];
        }
      }
    }
    assert(min_cut_cost >= 0);
    min_cut_cost_ever = std::min(min_cut_cost_ever, min_cut_cost);
  }

  std::cout << min_cut_cost_ever << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>
#include <CGAL/QP_solution.h>
#include <Eigen/Dense>
#include <iostream>
#include "QPSolver.h"

// choose exact integral type
#ifdef CGAL_USE_GMP
#include <CGAL/Gmpz.h>
typedef CGAL::Gmpz ET;
#else
#include <CGAL/MP_Float.h>
typedef CGAL::MP_Float ET;
#endif

typedef CGAL::Linear_program_from_iterators
<const float**, const float*, CGAL::Comparison_result*, const bool*, const float*, const bool*, const float*, const float*> LpProgram;

typedef CGAL::Quadratic_program_from_iterators
<const float**, const float*, const CGAL::Comparison_result*, const bool*, const float*, const bool*, const float*, const float**, const float*> QpProgram;

typedef CGAL::Quadratic_program_from_iterators
<const int**, const int*, const CGAL::Comparison_result*, const bool*, const int*, const bool*, const int*, const int**, const int*> QpProgram_int;

typedef CGAL::Quadratic_program_solution<ET> Solution;

std::shared_ptr<const int*> Eigen2CgalArray2d(Eigen::MatrixXi const& eigenMat) {
	auto cgalMat = std::shared_ptr<const int*>(new const int* [eigenMat.cols()]);
	//auto cgalMat = std::make_shared<float*>(new float[eigenMat.cols()]);
	for (int i = 0; i < eigenMat.cols(); i++)
	{
		cgalMat.get()[i] = eigenMat.data() + i * eigenMat.rows();
	}
	return cgalMat;
}

std::shared_ptr<const float*> Eigen2CgalArray2d(Eigen::MatrixXf const &eigenMat) {
	auto cgalMat = std::shared_ptr<const float*>(new const float*[eigenMat.cols()]);
	//auto cgalMat = std::make_shared<float*>(new float[eigenMat.cols()]);
	for (int i = 0; i < eigenMat.cols(); i++)
	{
		cgalMat.get()[i] = eigenMat.data() + i * eigenMat.rows();
	}
	return cgalMat;
}

LinearProgram::LinearProgram(
	const Eigen::MatrixXf& A,
	const Eigen::VectorXf& b,
	const Eigen::VectorXf& c,
	const Eigen::VectorXi& equalityConditions,
	const Eigen::VectorXf& lowerbound,
	const Eigen::VectorXf& upperbound,
	float accuracy
):
	A(A),
	b(b),
	c(c),
	equalityConditions(equalityConditions),
	lowerbound(lowerbound),
	upperbound(upperbound),
	accuracy(accuracy)
{}

LinearProgram::Result LinearProgram::solve() {
	auto bound_scale_factor = 0.01f;

	Eigen::MatrixXf A_scaled = A / accuracy * bound_scale_factor;
	Eigen::VectorXf b_scaled = b / accuracy;
	Eigen::VectorXf c_scaled = c / accuracy;
	Eigen::VectorXf lb_scaled = lowerbound / bound_scale_factor;
	Eigen::VectorXf ub_scaled = upperbound / bound_scale_factor;
	auto _A_scaled = Eigen2CgalArray2d(A_scaled);

	CGAL::Comparison_result* r = new CGAL::Comparison_result[A.rows()];
	bool* flb = new bool[A.cols()];
	bool* fub = new bool[A.cols()];

	for (int i = 0; i < A.cols(); i++)
	{
		flb[i] = true;
		fub[i] = true;
	}

	for (int iCond = 0; iCond < A.rows(); iCond++)
	{
		if (equalityConditions[iCond] < 0)
			r[iCond] = CGAL::SMALLER;
		else if (equalityConditions[iCond] > 0)
			r[iCond] = CGAL::LARGER;
		else
			r[iCond] = CGAL::EQUAL;
	}
	LpProgram lp(A.cols(), A.rows(), _A_scaled.get(), b_scaled.data(), r, flb, lb_scaled.data(), fub, ub_scaled.data(), c_scaled.data(), 0.f);
	Solution s = CGAL::solve_linear_program(lp, ET());
	Eigen::VectorXf result(A.cols());
	for (auto itr = s.variable_values_begin(); itr != s.variable_values_end(); itr++)
	{
		int index = std::distance(s.variable_values_begin(), itr);
		result[index] = (*itr).numerator().to_double() / (*itr).denominator().to_double();
	}
	result *= bound_scale_factor;
	delete fub;
	delete flb;
	delete r;
	return LinearProgram::Result{
		s.objective_value().numerator().to_double() / s.objective_value().denominator().to_double() * accuracy,
		result
	};
}

QuadraticProgram::QuadraticProgram(
	const Eigen::MatrixXf& A,
	const Eigen::VectorXf& b,
	const Eigen::MatrixXf& D,
	const Eigen::VectorXf& c,
	const Eigen::VectorXi& equalityConditions,
	const Eigen::VectorXf& lowerbound,
	const Eigen::VectorXf& upperbound,
	float accuracy
) :
	A(A),
	b(b),
	D(D),
	c(c),
	equalityConditions(equalityConditions),
	lowerbound(lowerbound),
	upperbound(upperbound),
	accuracy(accuracy)
{}

QuadraticProgram::Result QuadraticProgram::solve()
{
	constexpr auto bound_scale_factor = 0.01f;
	Eigen::MatrixXf A_scaled = A / accuracy * bound_scale_factor;
	Eigen::VectorXf b_scaled = b / accuracy;
	Eigen::MatrixXf D_scaled = D / accuracy * bound_scale_factor;
	Eigen::VectorXf c_scaled = c / accuracy;
	Eigen::VectorXf lb_scaled = lowerbound / bound_scale_factor;
	Eigen::VectorXf ub_scaled = upperbound / bound_scale_factor;
	auto _A_scaled = Eigen2CgalArray2d(A_scaled);
	auto _D_scaled = Eigen2CgalArray2d(D_scaled);
	CGAL::Comparison_result* r = new CGAL::Comparison_result[A.rows()];
	for (int i_eq = 0; i_eq < A.rows(); i_eq++) {
		if (equalityConditions[i_eq] < 0) {
			r[i_eq] = CGAL::SMALLER;
		}
		else if (equalityConditions[i_eq] > 0)
			r[i_eq] = CGAL::LARGER;
		else
			r[i_eq] = CGAL::EQUAL;
	}
	bool* flb = new bool[A.cols()];
	bool* fub = new bool[A.cols()];
	for (int i = 0; i < A.cols(); i++)
	{
		flb[i] = true;
		fub[i] = true;
	}

	QpProgram qp(A.cols(), A.rows(), _A_scaled.get(), b_scaled.data(), r, flb, lb_scaled.data(), fub, ub_scaled.data(), _D_scaled.get(), c_scaled.data(), 0);
	Solution s = CGAL::solve_quadratic_program(qp, ET());
	Eigen::VectorXf result(A.cols());
	for (auto itr = s.variable_values_begin(); itr != s.variable_values_end(); itr++)
	{
		int index = std::distance(s.variable_values_begin(), itr);
		result[index] = (*itr).numerator().to_double() / (*itr).denominator().to_double();
	}
	result *= bound_scale_factor;
	delete fub;
	delete flb;
	delete r;
	return Result{
		s.objective_value().numerator().to_double() / s.objective_value().denominator().to_double() * accuracy,
		result
	};
}


//property of Alistair child
//alistair@mtoto.org

#define _USE_MATH_DEFINES

#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <typeinfo>
#include <iostream>
#include <cmath>
#include <fstream>
//#include <stdio.h>
#include <algorithm>
#include <complex>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <typeinfo>
#include <string>
//#include <iterator>
using namespace std;

void generic_args(po::options_description &desc,po::positional_options_description &p) {
    desc.add_options()
        ("help,h", "produce help message")
        ("lowfield", po::value<double>()->default_value(0), "lowest field used (default 0)")
        ("highfield", po::value<double>()->default_value(0.15), "highest field used (default 0.15)")
        ("magres", po::value<int>()->default_value(60), "n points per angle (default 100)")
        ("intres", po::value<int>()->default_value(60), "n trapezoid integral! (default 100)")
        ("length", po::value<double>()->default_value(500E-9), "length default (500E-9)")
        ("width", po::value<double>()->default_value(500E-9), "width default (500E-9)")
        ("height", po::value<double>()->default_value(1E-9), "height default (1E-9)")
        ("angleincrement", po::value<double>()->default_value(0.5), "how much to inrement angle by 0-90 (default 1).")
        ("step_height", po::value<double>()->default_value(10), "fraction of max critica current. (default 0.1)")
        ("write_to",   po::value<std::string>()->default_value("tsv"),      "csv or tsv ")
        ("distribution",   po::value<std::string>()->default_value("2d"),      "current density 1d or 2d")
        ("view",   po::value<std::string>()->default_value("critical_current"),      "profile or critical_current ")
        ("step_thickness", po::value<double>()->default_value(0.1), "fraction of length.(default 0.1) ");
    p.add("input", -1);
}

struct Results
{
    vector<vector<double> > critical_current;
    vector<vector<double> > magnetic_flux;
    vector<vector<double> > current_density_profile;
};

struct Options
{
    double lowfield;
    double highfield;
    int N;
    int n;
    double L;
    double w;
    double d;
    double angleincrement;
    double stepheight;
    double stepthickness;
    string write_to;
    string view;
    string distribution;
};

void resolvedfields(const Options &opts, double k, double theta, double field, double *Bx, double *By, double *kx, double *ky)
{

	*Bx = sin((theta * M_PI)/180)*(field);
	*By = cos((theta * M_PI)/180)*(field);
	*kx = k * *By;// + ((4*M_PI)/opts.L)* ((a / (1- (b*exp(-K * (*By - C) ) ) ) ) - (a*0.5));
	*ky = k * *Bx; //+ ((4*M_PI)/opts.w)* ((a / (1- (b*exp(-K * (*Bx - C) ) ) ) ) - (a*0.5));
}

double geometry(const Options &opts, double x, double y, double thickness, double L, double w, double height)
{
    double J0;
    if ((x <= (- L/2 + thickness) || x >= (L/2 - thickness)) && (opts.distribution == "2d"))
    {
        //J0 = height * cos((2 * M_PI * x) / L - M_PI) + height;
        J0 = height;
    }
    else if ((y <= (- w/2 + thickness) || y >= (w/2 - thickness)))
    {
        J0 = height;
        //J0 = height * cos((2 * M_PI * y) / L - M_PI) + height;
    }
    else
    {
        J0 = 0;
        //J0 = height * cos((4 * M_PI * x ) / L) + height;
    }
    return J0;
}

complex<double> current_density(const Options &opts,double Bx,double By,double x, double y, complex<double> Kx, complex<double> Ky, double thickness,double L, double w,double height)
{

    double a =100;
    double b = 0.33341;
    double C = 0.1;
    double K = 0.12089;

    double J0 = geometry(opts, x, y, thickness, L, w, height);
	return 1E7 * J0 * exp( Kx * x - Ky * y);// + ((((4*M_PI)/opts.L)* ((a / (1- (b * exp(-K * (2 - C) ) ) ) ))) - (a*0.5))+ (((4*M_PI)/opts.L)* ((a / (1- (b * exp(-K * (0 - C) ) ) ) ) - (a*0.5))));//complex exponential (Phase relations)
}

double sumdoubleintegral(const Options &opts, double Bx, double By, complex<double> Kx, complex<double> Ky,double lowbound1, double lowbound2 ,int n, double dy, double dx, double thickness,double L, double w,double height)
{
	complex<double> cumbigsum (0,0);//define and initialise a complex cumsum for outer integral

	for(int i=0; i<n ;i++)//set up for loop for outer
	{
		double xi = lowbound1 + i*dx;//define the trapezoid width
		complex<double>  cumsmallsum (0,0);//inner cumsum initialised

		for (int j=0; j<n; j++)//inner loop
		{
			double yi = lowbound2 + dy * j;//define trapeziod width
			complex<double> funvalue = current_density(opts, Bx ,By,xi, yi, Kx, Ky, thickness, L, w, height);//call function to evaluate return complex double
			complex<double> rectanglearea = funvalue * dy;//multiply width by height
			cumsmallsum += rectanglearea;//add to inner cumsum
		}

		complex<double> secondrectanglearea = cumsmallsum*dx;//use total inner cumsum as cross section
		cumbigsum += secondrectanglearea;//add up sliced area to get total
	}

	return abs(cumbigsum);//return the absolute value of complex number
}

void make_profile(const Options &opts, Results &result)
{
    double lowbound1 = -opts.L/2, lowbound2 = -opts.w/2;
	double upbound1 = opts.L/2, upbound2 = opts.w/2;
    double dx = (double)(upbound2-lowbound2)/opts.n;//dx
	double dy = (double)(upbound1-lowbound1)/opts.n;//dy

    for(int xaxis = 0; xaxis<opts.n ; ++xaxis){
        double xi = lowbound1 + xaxis * dx;
        result.current_density_profile.push_back(vector<double>());
        for(int yaxis = 0 ; yaxis<opts.n ; ++yaxis){
            double yi = lowbound2 + dy * yaxis;
            double level = geometry(opts, xi, yi, opts.stepthickness*opts.L, opts.L, opts.w, opts.stepheight);

            result.current_density_profile[xaxis].push_back(level);
        }
    }
}

void write_csv(const vector<string> &labels, const vector< vector<double> > &data)
{
    // output labels
    for(size_t i = 0; i != labels.size(); ++i)
    {
        if (i != 0) cout << ",";
        cout << labels[i];
    }
    cout << endl;

    // output data
    for(const auto& datum: data) {
        for(size_t i = 0; i != datum.size(); ++i)
        {
            if (i != 0) cout << ",";
            cout << datum[i];
        }
        cout << endl;
    }
}

void write_tsv(const vector< vector<double> > &data)
{
    // output data
    for(const auto& datum: data) {
        for(size_t i = 0; i != datum.size(); ++i)
        {
            if (i != 0) cout << "\t";
            cout << datum[i];
        }
        cout << endl;
    }
}

void file_output(const Options &opts, Results &result)
{
    if (opts.view == "profile")
    {
        for(int xaxis = 0; xaxis < opts.n; ++xaxis)
	    {
	        for (int yaxis = 0; yaxis < opts.n; ++yaxis)
	        {
                if (opts.write_to == "csv"){
        	        cout << xaxis << "," << yaxis << "," << result.current_density_profile[xaxis][yaxis]  << endl;
	            }
                else if(opts.write_to == "tsv"){
                    cout << xaxis << "\t" << yaxis << "\t" << result.current_density_profile[xaxis][yaxis]  << endl;
                }
            }
	    }
    }
    else if (opts.view == "critical_current")
    {
        for(int angle = 0; angle <= 90/opts.angleincrement; ++angle)
	    {
            double max = *max_element(result.critical_current[angle].begin(), result.critical_current[angle].end());
            for (int flux=0; flux < opts.N; ++flux)
	        {
                if(opts.write_to == "csv"){
        	        cout << angle*opts.angleincrement << "," << result.magnetic_flux[angle][flux] << "," << result.critical_current[angle][flux]/max << endl;
	            }
                else if(opts.write_to == "tsv"){
                    cout << angle*opts.angleincrement << "\t" << result.magnetic_flux[angle][flux]<< "\t" << result.critical_current[angle][flux]/max << endl;
                }
            }
	    }
    }
    else
    {
        cerr << "Error: i do not understand the option view. Please choose from the view options --help for info" << endl;
    }

}

void getargs(Options &opts, int ac, const char * av[])
{
    //Options opts;
    try {
        po::options_description desc("Allowed options");
        po::positional_options_description p;
        generic_args(desc, p);//calling function
        po::variables_map param;
        po::store(po::command_line_parser(ac, av).options(desc).positional(p).run(), param);
        po::notify(param);

        if (param.count("help")) {
            cout << desc << "\n";
            exit(0);
        }

        opts.lowfield = param["lowfield"].as<double>();
        opts.highfield = param["highfield"].as<double>();
        opts.N = param["magres"].as<int>();
        opts.n = param["intres"].as<int>();
        opts.L = param["length"].as<double>();
        opts.w = param["width"].as<double>();
        opts.d = param["height"].as<double>();
        opts.stepheight = param["step_height"].as<double>();
        opts.stepthickness = param["step_thickness"].as<double>();
        opts.angleincrement = param["angleincrement"].as<double>();
        opts.write_to = param["write_to"].as<string>();
        opts.view = param["view"].as<string>();
        opts.distribution = param["distribution"].as<string>();
    }
    catch(exception &e) {
        cerr << "error: " << e.what() << "\n";
        throw e;
    }
}

void make_results(const Options &opts, Results &result)
{
    //define Physics constants to double precision!
    double fluxquantum = 2.06783383E-15, lambdax = 90E-9;
	complex <double> im(0,1);
    double k = 2 * M_PI * (2 * lambdax + opts.d) / fluxquantum;

    //define the upper/lower bounds of double integral
    double lowbound1 = -opts.L/2, lowbound2 = -opts.w/2;
	double upbound1 = opts.L/2, upbound2 = opts.w/2;
    double dx = (double)(upbound2-lowbound2)/opts.n;//dx
	double dy = (double)(upbound1-lowbound1)/opts.n;//dy
	double step = (opts.highfield - opts.lowfield) / (opts.N-1);
	int angleblock = 90 / opts.angleincrement;

    //create the data.

    for (int angle = 0; angle <= angleblock; ++angle)
	{

        //local values set to global values.
		float loc_lowfield = opts.lowfield;
		float loc_highfield = opts.highfield;
		double loc_step = step;

        result.critical_current.push_back(vector<double>());
        result.magnetic_flux.push_back(vector<double>());

		for (int flux = 0; flux < opts.N; ++flux)
		{
			float field = loc_lowfield;
			loc_lowfield += loc_step;
			//int theta = angle * opts.angleincrement;
            double theta = angle * opts.angleincrement;

			double Bx, By ,kx, ky;
			resolvedfields(opts, k, theta, field, &Bx, &By , &kx , &ky);

            double appendff = sumdoubleintegral(opts, Bx, By,kx * im, ky * im, lowbound1, lowbound2, opts.n, dy, dx,opts.stepthickness*opts.L,opts.L, opts.w, opts.stepheight);
            double mflux =( ( ( (Bx * opts.w) + (By * opts.L) ) * (2 * lambdax + opts.d))  / fluxquantum );//+ ( ((opts.L)* ((a / (1- (b*exp(-K * (2- C) ) ) ) ) - (a*0.5))) / fluxquantum )+ ( ((1)* ((a / (1- (b*exp(-K * (2 - C) ) ) ) ) - (a*0.5))) / fluxquantum );
            result.critical_current[angle].push_back(appendff);
            result.magnetic_flux[angle].push_back(mflux);
		}
	}

}

int main(int ac, const char* av[])
{
    //Options opts;
    try {
        //Define the user options
        Options opts;

        //parse arguments
        getargs(opts, ac, av);

        //Results(opts) output defined;
        Results result;

        //create results call Math
        make_results(opts, result);

        //make the current density profile
        make_profile(opts, result);

        //write results to a file csv, tsv, 1d 2d.
        file_output(opts, result);
    }
    catch(exception &e) {
        cerr << "error: " << e.what() << "\n";
        return 1;
    }

    return 0;
}


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

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

class EKFEstimator
{
public:
  EKFEstimator()
  : P_(EigenMatrix9d::Identity() * 100),
    var_imu_w_{0.33},
    var_imu_acc_{0.33},
    tau_gyro_bias_{1.0}
  {
    /* x  = [p v q] = [x y z vx vy vz qx qy qz qw] */
    x_ << 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;
  }

/* state
* x  = [p v q] = [x y z vx vy vz qx qy qz qw]
* dx = [dp dv dth] = [dx dy dz dvx dvy dvz dthx dthy dthz]
*
* pos_k = pos_{k-1} + vel_k * dt + (1/2) * (Rot(q_{k-1}) acc_{k-1}^{imu} - g) *dt^2
* vel_k = vel_{k-1} + (Rot(quat_{k-1})) acc_{k-1}^{imu} - g) *dt
* quat_k = Rot(w_{k-1}^{imu}*dt)*quat_{k-1}
*
* covariance
* P_{k} = F_k P_{k-1} F_k^T + L Q_k L^T
*/
  void predictionUpdate(
    const double current_time_imu,
    const Eigen::Vector3d & gyro,
    const Eigen::Vector3d & linear_acceleration
  )
  {
    double dt_imu = current_time_imu - previous_time_imu_;
    previous_time_imu_ = current_time_imu;
    if (dt_imu > 0.5 /* [sec] */) {
      std::cout << "imu time interval is too large" << std::endl;
      return;
    }

    Eigen::Quaterniond quat_wdt = Eigen::Quaterniond(
      Eigen::AngleAxisd(gyro.x() * dt_imu, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(gyro.y() * dt_imu, Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(gyro.z() * dt_imu, Eigen::Vector3d::UnitZ()));
    Eigen::Vector3d acc = Eigen::Vector3d(
      linear_acceleration.x(),
      linear_acceleration.y(),
      linear_acceleration.z());

    // state
    Eigen::Quaterniond previous_quat =
      Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ));
    Eigen::MatrixXd rot_mat = previous_quat.toRotationMatrix();

    // pos
    x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dt_imu * x_.segment(STATE::VX, 3) +
      0.5 * dt_imu * dt_imu * (rot_mat * acc - gravity_);
    // vel
    x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dt_imu * (rot_mat * acc - gravity_);
    // quat
    Eigen::Quaterniond predicted_quat = quat_wdt * previous_quat;
    x_.segment(STATE::QX, 4) = Eigen::Vector4d(
      predicted_quat.x(), predicted_quat.y(), predicted_quat.z(), predicted_quat.w());

    // F
    Eigen::MatrixXd F = EigenMatrix9d::Identity();
    F.block<3, 3>(0, 3) = dt_imu * Eigen::Matrix3d::Identity();
    Eigen::Matrix3d acc_skew;
    acc_skew <<
      0, -acc(2), acc(1),
      acc(2), 0, -acc(0),
      -acc(1), acc(0), 0;
    F.block<3, 3>(3, 6) = rot_mat * (-acc_skew) * dt_imu;

    // Q
    Eigen::MatrixXd Q = Eigen::Matrix<double, 6, 6>::Identity();
    Q.block<3, 3>(0, 0) = var_imu_acc_ * Q.block<3, 3>(0, 0);
    Q.block<3, 3>(3, 3) = var_imu_w_ * Q.block<3, 3>(3, 3);
    Q = Q * (dt_imu * dt_imu);

    // L
    Eigen::MatrixXd L = Eigen::Matrix<double, num_error_state_, 6>::Zero();
    L.block<3, 3>(3, 0) = Eigen::Matrix3d::Identity();
    L.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity();

    P_ = F * P_ * F.transpose() + L * Q * L.transpose();
  }

/*
* y = pobs = [xobs yobs zobs]
*
* K = P_k H^T (H P_k H^T + R)^{-1}
*
* dx = K (y_k - p_k )
*
* p_x = p_{k-1} + dp_k
* v_k = v_{k-1} + dv_k
* q_k = Rot(dth) q_{k-1}
*
* P_k = (I - KH)*P_{k-1}
*/
  void observationUpdate(
    const Eigen::Vector3d & y,
    const Eigen::Vector3d & variance
  )
  {
    // error state
    Eigen::Matrix3d R;
    R <<
      variance.x(), 0, 0,
      0, variance.y(), 0,
      0, 0, variance.z();
    Eigen::MatrixXd H = Eigen::Matrix<double, 3, num_error_state_>::Zero();
    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
    Eigen::MatrixXd K = P_ * H.transpose() * (H * P_ * H.transpose() + R).inverse();
    Eigen::VectorXd dx = K * (y - x_.segment(STATE::X, 3));

    // state
    x_.segment(STATE::X, 3) = x_.segment(STATE::X, 3) + dx.segment(ERROR_STATE::DX, 3);
    x_.segment(STATE::VX, 3) = x_.segment(STATE::VX, 3) + dx.segment(ERROR_STATE::DVX, 3);
    double norm_quat = sqrt(
      pow(dx(ERROR_STATE::DTHX), 2) +
      pow(dx(ERROR_STATE::DTHY), 2) +
      pow(dx(ERROR_STATE::DTHZ), 2));

    if (norm_quat < 1e-10) {
      Eigen::Quaterniond dq = Eigen::Quaterniond(0, 0, 0, cos(norm_quat / 2));
      Eigen::Quaterniond q = Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ));
      Eigen::Quaterniond q_new = q * dq;
      x_.segment(STATE::QX, 4) = Eigen::Vector4d(q_new.x(), q_new.y(), q_new.z(), q_new.w());
    } else {
      Eigen::Quaterniond dq = Eigen::Quaterniond(
        sin(norm_quat / 2) * dx(ERROR_STATE::DTHX) / norm_quat,
        sin(norm_quat / 2) * dx(ERROR_STATE::DTHY) / norm_quat,
        sin(norm_quat / 2) * dx(ERROR_STATE::DTHZ) / norm_quat,
        cos(norm_quat / 2));
      Eigen::Quaterniond q = Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ));
      Eigen::Quaterniond q_new = q * dq;
      x_.segment(STATE::QX, 4) = Eigen::Vector4d(q_new.x(), q_new.y(), q_new.z(), q_new.w());
    }

    P_ = (EigenMatrix9d::Identity() - K * H) * P_;
  }

  void setTauGyroBias(const double tau_gyro_bias)
  {
    tau_gyro_bias_ = tau_gyro_bias;
  }

  void setVarImuGyro(const double var_imu_w)
  {
    var_imu_w_ = var_imu_w;
  }

  void setVarImuAcc(const double var_imu_acc)
  {
    var_imu_acc_ = var_imu_acc;
  }

  void setInitialX(Eigen::VectorXd x)
  {
    x_ = x;
  }

  Eigen::VectorXd getX()
  {
    return x_;
  }

  Eigen::MatrixXd getCoveriance()
  {
    return P_;
  }

  int getNumState()
  {
    return num_state_;
  }

private:
  double previous_time_imu_;
  double var_imu_w_;
  double var_imu_acc_;

  static const int num_state_{10};
  static const int num_error_state_{9};

  typedef Eigen::Matrix<double, num_error_state_, num_error_state_> EigenMatrix9d;

  Eigen::Matrix<double, num_state_, 1> x_;
  EigenMatrix9d P_;

  const Eigen::Vector3d gravity_{0, 0, 9.80665};

  double tau_gyro_bias_;

  enum STATE
  {
    X  = 0, Y = 1, Z = 2,
    VX = 3, VY = 4, VZ = 5,
    QX = 6, QY = 7, QZ = 8, QW = 9,
  };
  enum ERROR_STATE
  {
    DX   = 0, DY = 1, DZ = 2,
    DVX  = 3, DVY = 4, DVZ = 5,
    DTHX = 6, DTHY = 7, DTHZ = 8,
  };
};

#endif  // KALMAN_FILTER_LOCALIZATION__EKF_HPP_