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

// @sect3{Include files}

// 标准deal.II需要的典型文件。

#include <deal.II/base/tensor_function.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/parameter_handler.h>

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

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

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

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

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

// 包括所有相关的多层次文件。

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

// C++:

#include <algorithm>
#include <fstream>
#include <iostream>
#include <random>

// 我们将使用 MeshWorker::mesh_loop 功能来组装矩阵。

#include <deal.II/meshworker/mesh_loop.h>
// @sect3{MeshWorker data}

// 像往常一样，我们将把所有与这个程序有关的东西放到一个自己的命名空间中。

// 由于我们将使用MeshWorker框架，第一步是定义以下由 MeshWorker::mesh_loop(): 使用的assemble_cell()函数所需要的结构 `ScratchData`包含一个FEValues对象，这是组装一个单元的局部贡献所需要的，而`CopyData`包含一个单元的局部贡献的输出和复制到全局系统的必要信息。它们的目的在WorkStream类的文档中也有解释）。

namespace Step63
{
  using namespace dealii;

  template <int dim>
  struct ScratchData
  {
    ScratchData(const FiniteElement<dim> &fe,
                const unsigned int        quadrature_degree)
      : fe_values(fe,
                  QGauss<dim>(quadrature_degree),
                  update_values | update_gradients | update_hessians |
                    update_quadrature_points | update_JxW_values)
    {}

    ScratchData(const ScratchData<dim> &scratch_data)
      : fe_values(scratch_data.fe_values.get_fe(),
                  scratch_data.fe_values.get_quadrature(),
                  update_values | update_gradients | update_hessians |
                    update_quadrature_points | update_JxW_values)
    {}

    FEValues<dim> fe_values;
  };

  struct CopyData
  {
    CopyData() = default;

    unsigned int level;
    unsigned int dofs_per_cell;

    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;
  };

//  @sect3{Problem parameters}

// 第二步是定义处理要从输入文件中读取的运行时参数的类。

// 我们将使用ParameterHandler在运行时传入参数。结构`Settings`解析并存储整个程序要查询的参数。

  struct Settings
  {
    enum DoFRenumberingStrategy
    {
      none,
      downstream,
      upstream,
      random
    };

    void get_parameters(const std::string &prm_filename);

    double                 epsilon;
    unsigned int           fe_degree;
    std::string            smoother_type;
    unsigned int           smoothing_steps;
    DoFRenumberingStrategy dof_renumbering;
    bool                   with_streamline_diffusion;
    bool                   output;
  };

  void Settings::get_parameters(const std::string &prm_filename)
  {

/* 首先声明参数...   */

    ParameterHandler prm;

    prm.declare_entry("Epsilon",
                      "0.005",
                      Patterns::Double(0),
                      "Diffusion parameter");

    prm.declare_entry("Fe degree",
                      "1",
                      Patterns::Integer(1),
                      "Finite Element degree");
    prm.declare_entry("Smoother type",
                      "block SOR",
                      Patterns::Selection("SOR|Jacobi|block SOR|block Jacobi"),
                      "Select smoother: SOR|Jacobi|block SOR|block Jacobi");
    prm.declare_entry("Smoothing steps",
                      "2",
                      Patterns::Integer(1),
                      "Number of smoothing steps");
    prm.declare_entry(
      "DoF renumbering",
      "downstream",
      Patterns::Selection("none|downstream|upstream|random"),
      "Select DoF renumbering: none|downstream|upstream|random");
    prm.declare_entry("With streamline diffusion",
                      "true",
                      Patterns::Bool(),
                      "Enable streamline diffusion stabilization: true|false");
    prm.declare_entry("Output",
                      "true",
                      Patterns::Bool(),
                      "Generate graphical output: true|false");
    /* ...然后尝试从输入文件中读取它们的值。  */
    if (prm_filename.empty())
      {
        prm.print_parameters(std::cout, ParameterHandler::Text);
        AssertThrow(
          false, ExcMessage("Please pass a .prm file as the first argument!"));
      }

    prm.parse_input(prm_filename);

    epsilon         = prm.get_double("Epsilon");
    fe_degree       = prm.get_integer("Fe degree");
    smoother_type   = prm.get("Smoother type");
    smoothing_steps = prm.get_integer("Smoothing steps");

    const std::string renumbering = prm.get("DoF renumbering");
    if (renumbering == "none")
      dof_renumbering = DoFRenumberingStrategy::none;
    else if (renumbering == "downstream")
      dof_renumbering = DoFRenumberingStrategy::downstream;
    else if (renumbering == "upstream")
      dof_renumbering = DoFRenumberingStrategy::upstream;
    else if (renumbering == "random")
      dof_renumbering = DoFRenumberingStrategy::random;
    else
      AssertThrow(false,
                  ExcMessage("The <DoF renumbering> parameter has "
                             "an invalid value."));

    with_streamline_diffusion = prm.get_bool("With streamline diffusion");
    output                    = prm.get_bool("Output");
  }
// @sect3{Cell permutations}

// 遍历单元和自由度的顺序将对乘法的收敛速度起作用。在这里，我们定义了一些函数，这些函数返回单元格的特定顺序，供块平滑器使用。

// 对于每种类型的单元格排序，我们定义了一个用于活动网格的函数和一个用于水平网格的函数（即用于多网格层次结构中的某一层的单元格）。虽然求解系统所需的唯一重新排序是在水平网格上进行的，但为了可视化的目的，我们在output_results()中包含了主动网格的重新排序。

// 对于两个下游排序函数，我们首先创建一个包含所有相关单元的数组，然后使用一个 "比较器 "对象在下游方向进行排序。然后，函数的输出是一个简单的数组，包含了刚刚计算出来的单元格的索引。

  template <int dim>
  std::vector<unsigned int>
  create_downstream_cell_ordering(const DoFHandler<dim> &dof_handler,
                                  const Tensor<1, dim>   direction,
                                  const unsigned int     level)
  {
    std::vector<typename DoFHandler<dim>::level_cell_iterator> ordered_cells;
    ordered_cells.reserve(dof_handler.get_triangulation().n_cells(level));
    for (const auto &cell : dof_handler.cell_iterators_on_level(level))
      ordered_cells.push_back(cell);

    const DoFRenumbering::
      CompareDownstream<typename DoFHandler<dim>::level_cell_iterator, dim>
        comparator(direction);
    std::sort(ordered_cells.begin(), ordered_cells.end(), comparator);

    std::vector<unsigned> ordered_indices;
    ordered_indices.reserve(dof_handler.get_triangulation().n_cells(level));

    for (const auto &cell : ordered_cells)
      ordered_indices.push_back(cell->index());

    return ordered_indices;
  }

  template <int dim>
  std::vector<unsigned int>
  create_downstream_cell_ordering(const DoFHandler<dim> &dof_handler,
                                  const Tensor<1, dim>   direction)
  {
    std::vector<typename DoFHandler<dim>::active_cell_iterator> ordered_cells;
    ordered_cells.reserve(dof_handler.get_triangulation().n_active_cells());
    for (const auto &cell : dof_handler.active_cell_iterators())
      ordered_cells.push_back(cell);

    const DoFRenumbering::
      CompareDownstream<typename DoFHandler<dim>::active_cell_iterator, dim>
        comparator(direction);
    std::sort(ordered_cells.begin(), ordered_cells.end(), comparator);

    std::vector<unsigned int> ordered_indices;
    ordered_indices.reserve(dof_handler.get_triangulation().n_active_cells());

    for (const auto &cell : ordered_cells)
      ordered_indices.push_back(cell->index());

    return ordered_indices;
  }

// 产生随机排序的函数在精神上是相似的，它们首先将所有单元的信息放入一个数组。但是，它们不是对它们进行排序，而是利用C++提供的生成随机数的设施对元素进行随机洗牌。这样做的方式是在数组的所有元素上进行迭代，为之前的另一个元素抽取一个随机数，然后交换这些元素。其结果是对数组中的元素进行随机洗牌。

  template <int dim>
  std::vector<unsigned int>
  create_random_cell_ordering(const DoFHandler<dim> &dof_handler,
                              const unsigned int     level)
  {
    std::vector<unsigned int> ordered_cells;
    ordered_cells.reserve(dof_handler.get_triangulation().n_cells(level));
    for (const auto &cell : dof_handler.cell_iterators_on_level(level))
      ordered_cells.push_back(cell->index());

    std::mt19937 random_number_generator;
    std::shuffle(ordered_cells.begin(),
                 ordered_cells.end(),
                 random_number_generator);

    return ordered_cells;
  }

  template <int dim>
  std::vector<unsigned int>
  create_random_cell_ordering(const DoFHandler<dim> &dof_handler)
  {
    std::vector<unsigned int> ordered_cells;
    ordered_cells.reserve(dof_handler.get_triangulation().n_active_cells());
    for (const auto &cell : dof_handler.active_cell_iterators())
      ordered_cells.push_back(cell->index());

    std::mt19937 random_number_generator;
    std::shuffle(ordered_cells.begin(),
                 ordered_cells.end(),
                 random_number_generator);

    return ordered_cells;
  }
// @sect3{Right-hand side and boundary values}

// 本教程中所解决的问题是对
//  Finite Elements and Fast Iterative Solvers: with Applications in
//  Incompressible Fluid Dynamics by Elman, Silvester, and Wathen第118页上的例3.1.3的修改。主要的区别是我们在域的中心增加了一个洞，其边界条件为零的Dirichlet。

// 为了获得完整的描述，我们需要首先实现零右手边的类（当然，我们可以直接使用 Functions::ZeroFunction):  。
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

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

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

    return 0.0;
  }

  template <int dim>
  void RightHandSide<dim>::value_list(const std::vector<Point<dim>> &points,
                                      std::vector<double> &          values,
                                      const unsigned int component) const
  {
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));

    for (unsigned int i = 0; i < points.size(); ++i)
      values[i] = RightHandSide<dim>::value(points[i], component);
  }

// 我们也有迪里希特的边界条件。在外部正方形边界的连接部分，我们将数值设置为1，其他地方（包括内部圆形边界）的数值设置为0。

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

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

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

// 如果  $x=1$  ，或如果  $x>0.5$  和  $y=-1$  ，则将边界设为 1。

    if (std::fabs(p[0] - 1) < 1e-8 ||
        (std::fabs(p[1] + 1) < 1e-8 && p[0] >= 0.5))
      {
        return 1.0;
      }
    else
      {
        return 0.0;
      }
  }

  template <int dim>
  void BoundaryValues<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int component) const
  {
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));

    for (unsigned int i = 0; i < points.size(); ++i)
      values[i] = BoundaryValues<dim>::value(points[i], component);
  }

//  @sect3{Streamline diffusion implementation}

// 流水线扩散方法有一个稳定常数，我们需要能够计算出来。这个参数的计算方式的选择取自于On
//  Discontinuity-Capturing Methods for Convection-Diffusion
//  Equations by Volker John and Petr Knobloch。

  template <int dim>
  double compute_stabilization_delta(const double         hk,
                                     const double         eps,
                                     const Tensor<1, dim> dir,
                                     const double         pk)
  {
    const double Peclet = dir.norm() * hk / (2.0 * eps * pk);
    const double coth =
      (1.0 + std::exp(-2.0 * Peclet)) / (1.0 - std::exp(-2.0 * Peclet));

    return hk / (2.0 * dir.norm() * pk) * (coth - 1.0 / Peclet);
  }
// @sect3{AdvectionProlem class}

// 这是程序的主类，看起来应该与  step-16  非常相似。主要的区别是，由于我们是在运行时定义我们的多网格平滑器，我们选择定义一个函数`create_smoother()`和一个类对象`mg_smoother`，这是一个  `std::unique_ptr`  派生于MGSmoother的平滑器。请注意，对于从RelaxationBlock派生的平滑器，我们必须为每个级别包括一个`smoother_data`对象。这将包含关于单元格排序和单元格矩阵倒置方法的信息。

  template <int dim>
  class AdvectionProblem
  {
  public:
    AdvectionProblem(const Settings &settings);
    void run();

  private:
    void setup_system();

    template <class IteratorType>
    void assemble_cell(const IteratorType &cell,
                       ScratchData<dim> &  scratch_data,
                       CopyData &          copy_data);
    void assemble_system_and_multigrid();

    void setup_smoother();

    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

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

    const FE_Q<dim>     fe;
    const MappingQ<dim> mapping;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

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

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

    MGLevelObject<SparseMatrix<double>> mg_matrices;
    MGLevelObject<SparseMatrix<double>> mg_interface_in;
    MGLevelObject<SparseMatrix<double>> mg_interface_out;

    mg::Matrix<Vector<double>> mg_matrix;
    mg::Matrix<Vector<double>> mg_interface_matrix_in;
    mg::Matrix<Vector<double>> mg_interface_matrix_out;

    std::unique_ptr<MGSmoother<Vector<double>>> mg_smoother;

    using SmootherType =
      RelaxationBlock<SparseMatrix<double>, double, Vector<double>>;
    using SmootherAdditionalDataType = SmootherType::AdditionalData;
    MGLevelObject<SmootherAdditionalDataType> smoother_data;

    MGConstrainedDoFs mg_constrained_dofs;

    Tensor<1, dim> advection_direction;

    const Settings settings;
  };

  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem(const Settings &settings)
    : triangulation(Triangulation<dim>::limit_level_difference_at_vertices)
    , dof_handler(triangulation)
    , fe(settings.fe_degree)
    , mapping(settings.fe_degree)
    , settings(settings)
  {
    advection_direction[0] = -std::sin(numbers::PI / 6.0);
    if (dim >= 2)
      advection_direction[1] = std::cos(numbers::PI / 6.0);
    if (dim >= 3)
      AssertThrow(false, ExcNotImplemented());
  }
// @sect4{AdvectionProblem::setup_system()}

// 在这里，我们首先为活动和多网格级别的网格设置DoFHandler、AffineConstraints和SparsityPattern对象。

// 我们可以用DoFRenumbering类对活动DoF进行重新编号，但是平滑器只作用于多网格层，因此，这对计算并不重要。相反，我们将对每个多网格层的DoFs进行重新编号。

  template <int dim>
  void AdvectionProblem<dim>::setup_system()
  {
    const unsigned int n_levels = triangulation.n_levels();

    dof_handler.distribute_dofs(fe);

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

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

    VectorTools::interpolate_boundary_values(
      mapping, dof_handler, 0, BoundaryValues<dim>(), constraints);
    VectorTools::interpolate_boundary_values(
      mapping, dof_handler, 1, BoundaryValues<dim>(), 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);

    dof_handler.distribute_mg_dofs();

// 在列举了全局自由度以及（上面最后一行）水平自由度之后，让我们对水平自由度进行重新编号，以获得一个更好的平滑器，正如介绍中所解释的。 如果需要的话，下面的第一个区块会对下游或上游方向的每个层次的自由度进行重新编号。这只对点平滑器（SOR和Jacobi）有必要，因为块平滑器是在单元上操作的（见`create_smoother()`）。然后，下面的块也实现了随机编号。

    if (settings.smoother_type == "SOR" || settings.smoother_type == "Jacobi")
      {
        if (settings.dof_renumbering ==
              Settings::DoFRenumberingStrategy::downstream ||
            settings.dof_renumbering ==
              Settings::DoFRenumberingStrategy::upstream)
          {
            const Tensor<1, dim> direction =
              (settings.dof_renumbering ==
                   Settings::DoFRenumberingStrategy::upstream ?
                 -1.0 :
                 1.0) *
              advection_direction;

            for (unsigned int level = 0; level < n_levels; ++level)
              DoFRenumbering::downstream(dof_handler,
                                         level,
                                         direction,
                                         /*dof_wise_renumbering =  */ true);

          }
        else if (settings.dof_renumbering ==
                 Settings::DoFRenumberingStrategy::random)
          {
            for (unsigned int level = 0; level < n_levels; ++level)
              DoFRenumbering::random(dof_handler, level);
          }
        else
          Assert(false, ExcNotImplemented());
      }

// 该函数的其余部分只是设置了数据结构。下面代码的最后几行与其他GMG教程不同，因为它同时设置了接口输入和输出矩阵。我们需要这样做，因为我们的问题是非对称性的。

    mg_constrained_dofs.clear();
    mg_constrained_dofs.initialize(dof_handler);

    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler, {0, 1});

    mg_matrices.resize(0, n_levels - 1);
    mg_matrices.clear_elements();
    mg_interface_in.resize(0, n_levels - 1);
    mg_interface_in.clear_elements();
    mg_interface_out.resize(0, n_levels - 1);
    mg_interface_out.clear_elements();
    mg_sparsity_patterns.resize(0, n_levels - 1);
    mg_interface_sparsity_patterns.resize(0, n_levels - 1);

    for (unsigned int level = 0; level < n_levels; ++level)
      {
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_sparsity_pattern(dof_handler, dsp, level);
          mg_sparsity_patterns[level].copy_from(dsp);
          mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        }
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_interface_sparsity_pattern(dof_handler,
                                                   mg_constrained_dofs,
                                                   dsp,
                                                   level);
          mg_interface_sparsity_patterns[level].copy_from(dsp);

          mg_interface_in[level].reinit(mg_interface_sparsity_patterns[level]);
          mg_interface_out[level].reinit(mg_interface_sparsity_patterns[level]);
        }
      }
  }
// @sect4{AdvectionProblem::assemble_cell()}

// 这里我们定义了每个单元上的线性系统的装配，以便被下面的Mesh_loop()函数使用。这个函数为活动单元或水平单元（不管它的第一个参数是什么）装配单元矩阵，并且只有在调用活动单元时才装配右手边。

  template <int dim>
  template <class IteratorType>
  void AdvectionProblem<dim>::assemble_cell(const IteratorType &cell,
                                            ScratchData<dim> &  scratch_data,
                                            CopyData &          copy_data)
  {
    copy_data.level = cell->level();

    const unsigned int dofs_per_cell =
      scratch_data.fe_values.get_fe().n_dofs_per_cell();
    copy_data.dofs_per_cell = dofs_per_cell;
    copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);

    const unsigned int n_q_points =
      scratch_data.fe_values.get_quadrature().size();

    if (cell->is_level_cell() == false)
      copy_data.cell_rhs.reinit(dofs_per_cell);

    copy_data.local_dof_indices.resize(dofs_per_cell);
    cell->get_active_or_mg_dof_indices(copy_data.local_dof_indices);

    scratch_data.fe_values.reinit(cell);

    RightHandSide<dim>  right_hand_side;
    std::vector<double> rhs_values(n_q_points);

    right_hand_side.value_list(scratch_data.fe_values.get_quadrature_points(),
                               rhs_values);

// 如果我们使用流线扩散，我们必须把它的贡献加到单元格矩阵和单元格的右手边。如果我们不使用流线扩散，设置 $\delta=0$ 就可以否定这个贡献，我们就可以使用标准的Galerkin有限元组合。

    const double delta = (settings.with_streamline_diffusion ?
                            compute_stabilization_delta(cell->diameter(),
                                                        settings.epsilon,
                                                        advection_direction,
                                                        settings.fe_degree) :
                            0.0);

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

// 本地矩阵的组装有两个部分。首先是Galerkin贡献。

              copy_data.cell_matrix(i, j) +=
                (settings.epsilon *
                 scratch_data.fe_values.shape_grad(i, q_point) *
                 scratch_data.fe_values.shape_grad(j, q_point) *
                 scratch_data.fe_values.JxW(q_point)) +
                (scratch_data.fe_values.shape_value(i, q_point) *
                 (advection_direction *
                  scratch_data.fe_values.shape_grad(j, q_point)) *
                 scratch_data.fe_values.JxW(q_point))

//然后是流线扩散贡献。

                + delta *
                    (advection_direction *
                     scratch_data.fe_values.shape_grad(j, q_point)) *
                    (advection_direction *
                     scratch_data.fe_values.shape_grad(i, q_point)) *
                    scratch_data.fe_values.JxW(q_point) -
                delta * settings.epsilon *
                  trace(scratch_data.fe_values.shape_hessian(j, q_point)) *
                  (advection_direction *
                   scratch_data.fe_values.shape_grad(i, q_point)) *
                  scratch_data.fe_values.JxW(q_point);
            }
          if (cell->is_level_cell() == false)
            {

// 同样的情况也适用于右手边。首先是Galerkin贡献。

              copy_data.cell_rhs(i) +=
                scratch_data.fe_values.shape_value(i, q_point) *
                  rhs_values[q_point] * scratch_data.fe_values.JxW(q_point)

// 然后是流线扩散贡献。

                + delta * rhs_values[q_point] * advection_direction *
                    scratch_data.fe_values.shape_grad(i, q_point) *
                    scratch_data.fe_values.JxW(q_point);
            }
        }
  }
// @sect4{AdvectionProblem::assemble_system_and_multigrid()}

// 这里我们采用 MeshWorker::mesh_loop() 来翻阅单元格，为我们组装system_matrix、system_rhs和所有mg_matrices。

  template <int dim>
  void AdvectionProblem<dim>::assemble_system_and_multigrid()
  {
    const auto cell_worker_active =
      [&](const decltype(dof_handler.begin_active()) &cell,
          ScratchData<dim> &                          scratch_data,
          CopyData &                                  copy_data) {
        this->assemble_cell(cell, scratch_data, copy_data);
      };

    const auto copier_active = [&](const CopyData &copy_data) {
      constraints.distribute_local_to_global(copy_data.cell_matrix,
                                             copy_data.cell_rhs,
                                             copy_data.local_dof_indices,
                                             system_matrix,
                                             system_rhs);
    };

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker_active,
                          copier_active,
                          ScratchData<dim>(fe, fe.degree + 1),
                          CopyData(),
                          MeshWorker::assemble_own_cells);

// 与活动层的约束不同，我们选择在这个函数的本地为每个多网格层创建约束对象，因为它们在程序的其他地方从来不需要。

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

    const auto cell_worker_mg =
      [&](const decltype(dof_handler.begin_mg()) &cell,
          ScratchData<dim> &                      scratch_data,
          CopyData &                              copy_data) {
        this->assemble_cell(cell, scratch_data, copy_data);
      };

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

// 如果 $(i,j)$ 是一个`interface_out` dof对，那么 $(j,i)$ 就是一个`interface_in` dof对。注意：对于 "interface_in"，我们加载接口条目的转置，即，dof对 $(j,i)$ 的条目被存储在 "interface_in(i,j)"。这是对对称情况的优化，允许在solve()中设置边缘矩阵时只使用一个矩阵。然而，在这里，由于我们的问题是非对称的，我们必须同时存储`interface_in`和`interface_out`矩阵。

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

    MeshWorker::mesh_loop(dof_handler.begin_mg(),
                          dof_handler.end_mg(),
                          cell_worker_mg,
                          copier_mg,
                          ScratchData<dim>(fe, fe.degree + 1),
                          CopyData(),
                          MeshWorker::assemble_own_cells);
  }
// @sect4{AdvectionProblem::setup_smoother()}

// 接下来，我们根据`.prm`文件中的设置来设置平滑器。两个重要的选项是多网格v周期每一级的平滑前和平滑后步骤的数量以及松弛参数。

// 由于乘法往往比加法更强大，所以需要较少的平滑步骤来实现收敛，与网格大小无关。块平滑器比点平滑器也是如此。这反映在下面对每种平滑器的平滑步数的选择上。

// 点平滑器的松弛参数是在试验和错误的基础上选择的，它反映了在我们细化网格时保持GMRES求解的迭代次数不变（或尽可能接近）的必要值。在`.prm`文件中给 "Jacobi "和 "SOR "的两个值是针对1度和3度有限元的。如果用户想改成其他度数，他们可能需要调整这些数字。对于块平滑器，这个参数有一个更直接的解释，即对于二维的加法，一个DoF可以有多达4个单元的重复贡献，因此我们必须将这些方法放松0.25来补偿。对于乘法来说，这不是一个问题，因为每个单元的逆向应用都会给其所有的DoF带来新的信息。

// 最后，如上所述，点平滑器只对DoF进行操作，而块平滑器对单元进行操作，因此只有块平滑器需要被赋予有关单元排序的信息。点平滑器的DoF排序已经在`setup_system()`中得到了处理。

  template <int dim>
  void AdvectionProblem<dim>::setup_smoother()
  {
    if (settings.smoother_type == "SOR")
      {
        using Smoother = PreconditionSOR<SparseMatrix<double>>;

        auto smoother =
          std::make_unique<MGSmootherPrecondition<SparseMatrix<double>,
                                                  Smoother,
                                                  Vector<double>>>();
        smoother->initialize(mg_matrices,
                             Smoother::AdditionalData(fe.degree == 1 ? 1.0 :
                                                                       0.62));
        smoother->set_steps(settings.smoothing_steps);
        mg_smoother = std::move(smoother);
      }
    else if (settings.smoother_type == "Jacobi")
      {
        using Smoother = PreconditionJacobi<SparseMatrix<double>>;
        auto smoother =
          std::make_unique<MGSmootherPrecondition<SparseMatrix<double>,
                                                  Smoother,
                                                  Vector<double>>>();
        smoother->initialize(mg_matrices,
                             Smoother::AdditionalData(fe.degree == 1 ? 0.6667 :
                                                                       0.47));
        smoother->set_steps(settings.smoothing_steps);
        mg_smoother = std::move(smoother);
      }
    else if (settings.smoother_type == "block SOR" ||
             settings.smoother_type == "block Jacobi")
      {
        smoother_data.resize(0, triangulation.n_levels() - 1);

        for (unsigned int level = 0; level < triangulation.n_levels(); ++level)
          {
            DoFTools::make_cell_patches(smoother_data[level].block_list,
                                        dof_handler,
                                        level);

            smoother_data[level].relaxation =
              (settings.smoother_type == "block SOR" ? 1.0 : 0.25);
            smoother_data[level].inversion = PreconditionBlockBase<double>::svd;

            std::vector<unsigned int> ordered_indices;
            switch (settings.dof_renumbering)
              {
                case Settings::DoFRenumberingStrategy::downstream:
                  ordered_indices =
                    create_downstream_cell_ordering(dof_handler,
                                                    advection_direction,
                                                    level);
                  break;

                case Settings::DoFRenumberingStrategy::upstream:
                  ordered_indices =
                    create_downstream_cell_ordering(dof_handler,
                                                    -1.0 * advection_direction,
                                                    level);
                  break;

                case Settings::DoFRenumberingStrategy::random:
                  ordered_indices =
                    create_random_cell_ordering(dof_handler, level);
                  break;

                case Settings::DoFRenumberingStrategy::none:
                  break;

                default:
                  AssertThrow(false, ExcNotImplemented());
                  break;
              }

            smoother_data[level].order =
              std::vector<std::vector<unsigned int>>(1, ordered_indices);
          }

        if (settings.smoother_type == "block SOR")
          {
            auto smoother = std::make_unique<MGSmootherPrecondition<
              SparseMatrix<double>,
              RelaxationBlockSOR<SparseMatrix<double>, double, Vector<double>>,
              Vector<double>>>();
            smoother->initialize(mg_matrices, smoother_data);
            smoother->set_steps(settings.smoothing_steps);
            mg_smoother = std::move(smoother);
          }
        else if (settings.smoother_type == "block Jacobi")
          {
            auto smoother = std::make_unique<
              MGSmootherPrecondition<SparseMatrix<double>,
                                     RelaxationBlockJacobi<SparseMatrix<double>,
                                                           double,
                                                           Vector<double>>,
                                     Vector<double>>>();
            smoother->initialize(mg_matrices, smoother_data);
            smoother->set_steps(settings.smoothing_steps);
            mg_smoother = std::move(smoother);
          }
      }
    else
      AssertThrow(false, ExcNotImplemented());
  }
// @sect4{AdvectionProblem::solve()}

// 在解决这个系统之前，我们必须首先设置多网格预处理程序。这需要设置各级之间的转换、粗略矩阵求解器和平滑器。这个设置几乎与 Step-16 相同，主要区别在于上面定义的各种平滑器，以及由于我们的问题是非对称的，我们需要不同的界面边缘矩阵。实际上，在本教程中，这些接口矩阵是空的，因为我们只使用全局细化，因此没有细化边。然而，我们在这里仍然包括了这两个矩阵，因为如果我们简单地切换到自适应细化方法，程序仍然可以正常运行）。)

// 最后要注意的是，由于我们的问题是非对称的，我们必须使用适当的Krylov子空间方法。我们在这里选择使用GMRES，因为它能保证在每次迭代中减少残差。GMRES的主要缺点是，每次迭代，存储的临时向量的数量都会增加一个，而且还需要计算与之前存储的所有向量的标量积。这是很昂贵的。通过使用重启的GMRES方法可以放松这一要求，该方法对我们在任何时候需要存储的向量数量设置了上限（这里我们在50个临时向量后重启，即48次迭代）。这样做的缺点是我们失去了在整个迭代过程中收集的信息，因此我们可以看到收敛速度较慢。因此，在哪里重启是一个平衡内存消耗、CPU工作量和收敛速度的问题。然而，本教程的目标是通过使用强大的GMG预处理程序来实现非常低的迭代次数，所以我们选择了重启长度，使下面显示的所有结果在重启发生之前就能收敛，因此我们有一个标准的GMRES方法。如果用户有兴趣，deal.II中提供的另一种合适的方法是BiCGStab。

  template <int dim>
  void AdvectionProblem<dim>::solve()
  {
    const unsigned int max_iters       = 200;
    const double       solve_tolerance = 1e-8 * system_rhs.l2_norm();
    SolverControl      solver_control(max_iters, solve_tolerance, true, true);
    solver_control.enable_history_data();

    using Transfer = MGTransferPrebuilt<Vector<double>>;
    Transfer mg_transfer(mg_constrained_dofs);
    mg_transfer.build(dof_handler);

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

    setup_smoother();

    mg_matrix.initialize(mg_matrices);
    mg_interface_matrix_in.initialize(mg_interface_in);
    mg_interface_matrix_out.initialize(mg_interface_out);

    Multigrid<Vector<double>> mg(
      mg_matrix, coarse_grid_solver, mg_transfer, *mg_smoother, *mg_smoother);
    mg.set_edge_matrices(mg_interface_matrix_out, mg_interface_matrix_in);

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

    std::cout << "     Solving with GMRES to tol " << solve_tolerance << "..."
              << std::endl;
    SolverGMRES<Vector<double>> solver(
      solver_control, SolverGMRES<Vector<double>>::AdditionalData(50, true));

    Timer time;
    time.start();
    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    time.stop();

    std::cout << "          converged in " << solver_control.last_step()
              << " iterations"
              << " in " << time.last_wall_time() << " seconds " << std::endl;

    constraints.distribute(solution);

    mg_smoother.release();
  }
// @sect4{AdvectionProblem::output_results()}

// 最后一个感兴趣的函数会生成图形输出。这里我们以.vtu格式输出解决方案和单元格排序。

// 在函数的顶部，我们为每个单元生成一个索引，以显示平滑器所使用的排序。请注意，我们只对活动单元而不是平滑器实际使用的层级做这个处理。对于点平滑器，我们对DoFs而不是单元进行重新编号，所以这只是对现实中发生的情况的一种近似。最后，这个随机排序不是我们实际使用的随机排序（见`create_smoother()`）。

// 然后，单元格的（整数）排序被复制到一个（浮点）矢量中，用于图形输出。

  template <int dim>
  void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
  {
    const unsigned int n_active_cells = triangulation.n_active_cells();
    Vector<double>     cell_indices(n_active_cells);
    {
      std::vector<unsigned int> ordered_indices;
      switch (settings.dof_renumbering)
        {
          case Settings::DoFRenumberingStrategy::downstream:
            ordered_indices =
              create_downstream_cell_ordering(dof_handler, advection_direction);
            break;

          case Settings::DoFRenumberingStrategy::upstream:
            ordered_indices =
              create_downstream_cell_ordering(dof_handler,
                                              -1.0 * advection_direction);
            break;

          case Settings::DoFRenumberingStrategy::random:
            ordered_indices = create_random_cell_ordering(dof_handler);
            break;

          case Settings::DoFRenumberingStrategy::none:
            ordered_indices.resize(n_active_cells);
            for (unsigned int i = 0; i < n_active_cells; ++i)
              ordered_indices[i] = i;
            break;

          default:
            AssertThrow(false, ExcNotImplemented());
            break;
        }

      for (unsigned int i = 0; i < n_active_cells; ++i)
        cell_indices(ordered_indices[i]) = static_cast<double>(i);
    }

// 考虑到以前的教程程序，该函数的其余部分就很简单了。

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");
    data_out.add_data_vector(cell_indices, "cell_index");
    data_out.build_patches();

    const std::string filename =
      "solution-" + Utilities::int_to_string(cycle) + ".vtu";
    std::ofstream output(filename.c_str());
    data_out.write_vtu(output);
  }
// @sect4{AdvectionProblem::run()}

// 和大多数教程一样，这个函数创建/细化网格并调用上面定义的各种函数来设置、装配、求解和输出结果。

// 在第0个循环中，我们在正方形 [-1,1]^dim 上生成网格，半径为3/10个单位的孔以原点为中心。对于`manifold_id`等于1的对象（即与洞相邻的面），我们指定了一个球形流形。

  template <int dim>
  void AdvectionProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < (settings.fe_degree == 1 ? 7 : 5);
         ++cycle)
      {
        std::cout << "  Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube_with_cylindrical_hole(triangulation,
                                                            0.3,
                                                            1.0);

            const SphericalManifold<dim> manifold_description(Point<dim>(0, 0));
            triangulation.set_manifold(1, manifold_description);
          }

        triangulation.refine_global();

        setup_system();

        std::cout << "     Number of active cells:       "
                  << triangulation.n_active_cells() << " ("
                  << triangulation.n_levels() << " levels)" << std::endl;
        std::cout << "     Number of degrees of freedom: "
                  << dof_handler.n_dofs() << std::endl;

        assemble_system_and_multigrid();

        solve();

        if (settings.output)
          output_results(cycle);

        std::cout << std::endl;
      }
  }
} // namespace Step63
// @sect3{The main function}

// 最后，主函数和大多数教程一样。唯一有趣的一点是，我们要求用户传递一个`.prm`文件作为唯一的命令行参数。如果没有给出参数文件，程序将在屏幕上输出一个带有所有默认值的样本参数文件的内容，然后用户可以复制并粘贴到自己的`.prm`文件中。

int main(int argc, char *argv[])
{
  try
    {
      Step63::Settings settings;
      settings.get_parameters((argc > 1) ? (argv[1]) : "");

      Step63::AdvectionProblem<2> advection_problem_2d(settings);
      advection_problem_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;
}


/*
Copyright 2016 Andrich van Wyk

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

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

#include "domain.h"
#include "particle.h"
#include "pso.h"
#include "pso_functions.h"
#include "random.h"

using Eigen::ArrayXd;
using cswarm::pso::lbest;
using std::cout;
using std::endl;

double spherical_f(const ArrayXd& solution) {
  return (solution * solution).sum();
}

int get_dimension(int argc, char* argv[]) {
  return argc < 2 ? 1000 : atoi(argv[1]);
}

int get_swarm_size(int argc, char* argv[]) {
  return argc < 3 ? 25 : atoi(argv[2]);
}

int main(int argc, char* argv[]) {
  auto rng = std::make_shared<Random>(15632435212L);
  int dimensions = get_dimension(argc, argv);
  int swarm_size = get_swarm_size(argc, argv);
  auto d = Domain(-5.0, 5.0, dimensions);
  auto p = PSOParameters(0.729844, 1.496180, 1.496180, 0.1, 5);
  auto pso = PSO(swarm_size, d, p, spherical_f, lbest, rng);

  auto result = pso.optimize(1000);
  cout << result.getFitness() << endl;
  return 0;
}


/**
 * \file
 * \author Norihiro Watanabe
 * \author Wenqing Wang
 * \date   2013-05-15, 2014-02
 * \brief  Interface tests of global matrix classes
 *
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "MathLib/LinAlg/LinAlg.h"

#if defined(USE_PETSC)
#include "MathLib/LinAlg/PETSc/PETScMatrix.h"
#else
#include "MathLib/LinAlg/Eigen/EigenMatrix.h"
#endif

#include "MathLib/LinAlg/FinalizeMatrixAssembly.h"
#include "NumLib/NumericsConfig.h"

using namespace MathLib::LinAlg;

namespace
{
template <class T_MATRIX>
void checkGlobalMatrixInterface(T_MATRIX& m)
{
    ASSERT_EQ(10u, m.getNumberOfRows());
    ASSERT_EQ(10u, m.getNumberOfColumns());
    ASSERT_EQ(0u, m.getRangeBegin());
    ASSERT_EQ(10u, m.getRangeEnd());

    m.setValue(0, 0, 1.0);
    m.add(0, 0, 1.0);
    m.setZero();

    Eigen::Matrix2d local_m;
    local_m << 1.0, 1.0, 1.0, 1.0;
    std::vector<GlobalIndexType> vec_pos(2);
    vec_pos[0] = 1;
    vec_pos[1] = 3;
    m.add(vec_pos, vec_pos, local_m);

    ASSERT_TRUE(finalizeMatrixAssembly(m));
}

#ifdef USE_PETSC  // or MPI
template <class T_MATRIX, class T_VECTOR>
void checkGlobalMatrixInterfaceMPI(T_MATRIX& m, T_VECTOR& v)
{
    int msize;
    MPI_Comm_size(PETSC_COMM_WORLD, &msize);
    int mrank;
    MPI_Comm_rank(PETSC_COMM_WORLD, &mrank);

    ASSERT_EQ(3u, msize);
    ASSERT_EQ(m.getRangeEnd() - m.getRangeBegin(), m.getNumberOfLocalRows());

    int gathered_rows;
    int local_rows = m.getNumberOfLocalRows();
    MPI_Allreduce(&local_rows, &gathered_rows, 1, MPI_INT, MPI_SUM,
                  PETSC_COMM_WORLD);
    ASSERT_EQ(m.getNumberOfRows(), gathered_rows);

    int gathered_cols;
    int local_cols = m.getNumberOfLocalColumns();
    MPI_Allreduce(&local_cols, &gathered_cols, 1, MPI_INT, MPI_SUM,
                  PETSC_COMM_WORLD);
    ASSERT_EQ(m.getNumberOfColumns(), gathered_cols);

    // Add entries
    Eigen::Matrix2d loc_m(2, 2);
    loc_m(0, 0) = 1.;
    loc_m(0, 1) = 2.;
    loc_m(1, 0) = 3.;
    loc_m(1, 1) = 4.;

    std::vector<GlobalIndexType> row_pos(2);
    std::vector<GlobalIndexType> col_pos(2);
    row_pos[0] = 2 * mrank;
    row_pos[1] = 2 * mrank + 1;
    col_pos[0] = row_pos[0];
    col_pos[1] = row_pos[1];

    m.add(row_pos, col_pos, loc_m);

    MathLib::finalizeMatrixAssembly(m);

    // Test basic assignment operator with an empty T_MATRIX._A
    T_MATRIX m_c = m;
    // Test basic assignment operator with an initialized T_MATRIX._A
    m_c = m;

    // Multiply by a vector
    // v = 1.;
    set(v, 1.);
    const bool deep_copy = false;
    T_VECTOR y(v, deep_copy);
    matMult(m_c, v, y);

    ASSERT_EQ(sqrt(3 * (3 * 3 + 7 * 7)), norm2(y));

    // set a value
    m_c.set(2 * mrank, 2 * mrank, 5.0);
    MathLib::finalizeMatrixAssembly(m);
    // add a value
    m_c.add(2 * mrank + 1, 2 * mrank + 1, 5.0);
    MathLib::finalizeMatrixAssembly(m_c);

    matMult(m_c, v, y);

    ASSERT_EQ(sqrt((3 * 7 * 7 + 3 * 12 * 12)), norm2(y));
}

// Rectanglular matrix
template <class T_MATRIX, class T_VECTOR>
void checkGlobalRectangularMatrixInterfaceMPI(T_MATRIX& m, T_VECTOR& v)
{
    int mrank;
    MPI_Comm_rank(PETSC_COMM_WORLD, &mrank);

    ASSERT_EQ(m.getRangeEnd() - m.getRangeBegin(), m.getNumberOfLocalRows());

    int gathered_rows;
    int local_rows = m.getNumberOfLocalRows();
    MPI_Allreduce(&local_rows, &gathered_rows, 1, MPI_INT, MPI_SUM,
                  PETSC_COMM_WORLD);
    ASSERT_EQ(m.getNumberOfRows(), gathered_rows);

    int gathered_cols;
    int local_cols = m.getNumberOfLocalColumns();
    MPI_Allreduce(&local_cols, &gathered_cols, 1, MPI_INT, MPI_SUM,
                  PETSC_COMM_WORLD);
    ASSERT_EQ(m.getNumberOfColumns(), gathered_cols);

    // Add entries
    Eigen::Matrix<double, 2, 3> loc_m;
    loc_m(0, 0) = 1.;
    loc_m(0, 1) = 2.;
    loc_m(0, 2) = 3.;
    loc_m(1, 0) = 1.;
    loc_m(1, 1) = 2.;
    loc_m(1, 2) = 3.;

    std::vector<GlobalIndexType> row_pos(2);
    std::vector<GlobalIndexType> col_pos(3);
    row_pos[0] = 2 * mrank;
    row_pos[1] = 2 * mrank + 1;
    col_pos[0] = 3 * mrank;
    col_pos[1] = 3 * mrank + 1;
    col_pos[2] = 3 * mrank + 2;

    m.add(row_pos, col_pos, loc_m);

    MathLib::finalizeMatrixAssembly(m);

    // Multiply by a vector
    set(v, 1);
    T_VECTOR y(m.getNumberOfRows());
    matMult(m, v, y);

    ASSERT_NEAR(6. * sqrt(6.), norm2(y), 1.e-10);
}

#endif  // end of: ifdef USE_PETSC // or MPI

}  // end namespace

#if defined(USE_PETSC)
TEST(MPITest_Math, CheckInterface_PETScMatrix_Local_Size)
{
    MathLib::PETScMatrixOption opt;
    opt.d_nz = 2;
    opt.o_nz = 0;
    opt.is_global_size = false;
    opt.n_local_cols = 2;
    MathLib::PETScMatrix A(2, opt);

    const bool is_gloabal_size = false;
    MathLib::PETScVector x(2, is_gloabal_size);

    checkGlobalMatrixInterfaceMPI(A, x);
}

TEST(MPITest_Math, CheckInterface_PETScMatrix_Global_Size)
{
    MathLib::PETScMatrixOption opt;
    opt.d_nz = 2;
    opt.o_nz = 0;
    MathLib::PETScMatrix A(6, opt);

    MathLib::PETScVector x(6);

    checkGlobalMatrixInterfaceMPI(A, x);
}

TEST(MPITest_Math, CheckInterface_PETSc_Rectangular_Matrix_Local_Size)
{
    MathLib::PETScMatrixOption opt;
    opt.d_nz = 3;
    opt.o_nz = 0;
    opt.is_global_size = false;
    opt.n_local_cols = -1;
    MathLib::PETScMatrix A(2, 3, opt);

    const bool is_gloabal_size = false;
    MathLib::PETScVector x(3, is_gloabal_size);

    checkGlobalRectangularMatrixInterfaceMPI(A, x);
}

TEST(MPITest_Math, CheckInterface_PETSc_Rectangular_Matrix_Global_Size)
{
    MathLib::PETScMatrixOption opt;
    opt.d_nz = 3;
    opt.o_nz = 0;
    MathLib::PETScMatrix A(6, 9, opt);

    MathLib::PETScVector x(9);

    checkGlobalRectangularMatrixInterfaceMPI(A, x);
}
#else
TEST(Math, CheckInterface_EigenMatrix)
{
    MathLib::EigenMatrix m(10);
    checkGlobalMatrixInterface(m);
}
#endif


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/ui/metrics_server.hpp>
#include <graphlab/util/hopscotch_set.hpp>
#include <graphlab/macros_def.hpp>
/**
 *
 * In this program we implement the "hash-set" version of the
 * "edge-iterator" algorithm described in
 *
 *    T. Schank. Algorithmic Aspects of Triangle-Based Network Analysis.
 *    Phd in computer science, University Karlsruhe, 2007.
 *
 * The procedure is quite straightforward:
 *   - each vertex maintains a list of all of its neighbors in a hash set.
 *   - For each edge (u,v) in the graph, count the number of intersections
 *     of the neighbor set on u and the neighbor set on v.
 *   - We store the size of the intersection on the edge.
 *
 * This will count every triangle exactly 3 times. Summing across all the
 * edges and dividing by 3 gives the desired result.
 *
 * The preprocessing stage take O(|E|) time, and it has been shown that this
 * algorithm takes $O(|E|^(3/2))$ time.
 *
 * If we only require total counts, we can introduce a optimization that is
 * similar to the "forward" algorithm
 * described in thesis above. Instead of maintaining a complete list of all
 * neighbors, each vertex only maintains a list of all neighbors with
 * ID greater than itself. This implicitly generates a topological sort
 * of the graph.
 *
 * Then you can see that each triangle
 *
 * \verbatim

     A----->C
     |     ^
     |   /
     v /
     B

 * \endverbatim
 * Must be counted only once. (Only when processing edge AB, can one
 * observe that A and B have intersecting out-neighbor sets).
 */

// Radix sort implementation from https://github.com/gorset/radix
// Thanks to Erik Gorset
//
/*
Copyright 2011 Erik Gorset. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer.

2. 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.

THIS SOFTWARE IS PROVIDED BY Erik Gorset ``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 Erik Gorset 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.

The views and conclusions contained in the software and documentation are those of the
authors and should not be interpreted as representing official policies, either expressed
or implied, of Erik Gorset.
*/
void radix_sort(graphlab::vertex_id_type *array, int offset, int end, int shift) {
    int x, y;
    graphlab::vertex_id_type value, temp;
    int last[256] = { 0 }, pointer[256];

    for (x=offset; x<end; ++x) {
        ++last[(array[x] >> shift) & 0xFF];
    }

    last[0] += offset;
    pointer[0] = offset;
    for (x=1; x<256; ++x) {
        pointer[x] = last[x-1];
        last[x] += last[x-1];
    }

    for (x=0; x<256; ++x) {
        while (pointer[x] != last[x]) {
            value = array[pointer[x]];
            y = (value >> shift) & 0xFF;
            while (x != y) {
                temp = array[pointer[y]];
                array[pointer[y]++] = value;
                value = temp;
                y = (value >> shift) & 0xFF;
            }
            array[pointer[x]++] = value;
        }
    }

    if (shift > 0) {
        shift -= 8;
        for (x=0; x<256; ++x) {
            temp = x > 0 ? pointer[x] - pointer[x-1] : pointer[0] - offset;
            if (temp > 64) {
                radix_sort(array, pointer[x] - temp, pointer[x], shift);
            } else if (temp > 1) {
                std::sort(array + (pointer[x] - temp), array + pointer[x]);
                //insertion_sort(array, pointer[x] - temp, pointer[x]);
            }
        }
    }
}

size_t HASH_THRESHOLD = 64;

// We on each vertex, either a vector of sorted VIDs
// or a hash set (cuckoo hash) of VIDs.
// If the number of elements is greater than HASH_THRESHOLD,
// the hash set is used. Otherwise the vector is used.
struct vid_vector{
  std::vector<graphlab::vertex_id_type> vid_vec;
  graphlab::hopscotch_set<graphlab::vertex_id_type, false> *cset;
  vid_vector(): cset(NULL) { }
  vid_vector(const vid_vector& v):cset(NULL) {
    (*this) = v;
  }

  vid_vector& operator=(const vid_vector& v) {
    if (this == &v) return *this;
    vid_vec = v.vid_vec;
    if (v.cset != NULL) {
      // allocate the cuckoo set if the other side is using a cuckoo set
      // or clear if I alrady have one
      if (cset == NULL) {
        cset = new graphlab::hopscotch_set<graphlab::vertex_id_type, false>(HASH_THRESHOLD);
      }
      else {
        cset->clear();
      }
      (*cset) = *(v.cset);
    }
    else {
      // if the other side is not using a cuckoo set, lets not use a cuckoo set
      // either
      if (cset != NULL) {
        delete cset;
        cset = NULL;
      }
    }
    return *this;
  }

  ~vid_vector() {
    if (cset != NULL) delete cset;
  }

  // assigns a vector of vertex IDs to this storage.
  // this function will clear the contents of the vid_vector
  // and reconstruct it.
  // If the assigned values has length >= HASH_THRESHOLD,
  // we will allocate a cuckoo set to store it. Otherwise,
  // we just store a sorted vector
  void assign(const std::vector<graphlab::vertex_id_type>& vec) {
    clear();
    if (vec.size() >= HASH_THRESHOLD) {
        // move to cset
        cset = new graphlab::hopscotch_set<graphlab::vertex_id_type, false>(HASH_THRESHOLD);
        foreach (graphlab::vertex_id_type v, vec) {
          cset->insert(v);
        }
    }
    else {
      vid_vec = vec;
      if (vid_vec.size() > 64) {
        radix_sort(&(vid_vec[0]), 0, vid_vec.size(), 24);
      }
      else {
        std::sort(vid_vec.begin(), vid_vec.end());
      }
      std::vector<graphlab::vertex_id_type>::iterator new_end = std::unique(vid_vec.begin(),
                                               vid_vec.end());
      vid_vec.erase(new_end, vid_vec.end());
    }
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << (cset != NULL);
    if (cset == NULL) oarc << vid_vec;
    else oarc << (*cset);
  }

  void clear() {
    vid_vec.clear();
    if (cset != NULL) {
      delete cset;
      cset = NULL;
    }
  }

  size_t size() const {
    return cset == NULL ? vid_vec.size() : cset->size();
  }

  void load(graphlab::iarchive& iarc) {
    clear();
    bool hascset;
    iarc >> hascset;
    if (!hascset) iarc >> vid_vec;
    else {
      cset = new graphlab::hopscotch_set<graphlab::vertex_id_type, false>(HASH_THRESHOLD);
      iarc >> (*cset);
    }
  }
};

/*
  A simple counting iterator which can be used as an insert iterator.
  but only counts the number of elements inserted. Useful for
  use with counting the size of an intersection using std::set_intersection
*/
template <typename T>
struct counting_inserter {
  size_t* i;
  counting_inserter(size_t* i):i(i) { }
  counting_inserter& operator++() {
    ++(*i);
    return *this;
  }
  void operator++(int) {
    ++(*i);
  }

  struct empty_val {
    empty_val operator=(const T&) { return empty_val(); }
  };

  empty_val operator*() {
    return empty_val();
  }

  typedef empty_val reference;
};

/*
 * Computes the size of the intersection of two vid_vector's
 */
static uint32_t count_set_intersect(
             const vid_vector& smaller_set,
             const vid_vector& larger_set) {

  if (smaller_set.cset == NULL && larger_set.cset == NULL) {
    size_t i = 0;
    counting_inserter<graphlab::vertex_id_type> iter(&i);
    std::set_intersection(smaller_set.vid_vec.begin(), smaller_set.vid_vec.end(),
                          larger_set.vid_vec.begin(), larger_set.vid_vec.end(),
                          iter);
    return i;
  }
  else if (smaller_set.cset == NULL && larger_set.cset != NULL) {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, smaller_set.vid_vec) {
      i += larger_set.cset->count(vid);
    }
    return i;
  }
  else if (smaller_set.cset != NULL && larger_set.cset == NULL) {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, larger_set.vid_vec) {
      i += smaller_set.cset->count(vid);
    }
    return i;
  }
  else {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, *(smaller_set.cset)) {
      i += larger_set.cset->count(vid);
    }
    return i;

  }
}

/*
 * Each vertex maintains a list of all its neighbors.
 * and a final count for the number of triangles it is involved in
 */
struct vertex_data_type {
  vertex_data_type(): num_triangles(0){ }
  // A list of all its neighbors
  vid_vector vid_set;
  // The number of triangles this vertex is involved it.
  // only used if "per vertex counting" is used
  uint32_t num_triangles;
  void save(graphlab::oarchive &oarc) const {
    oarc << vid_set << num_triangles;
  }
  void load(graphlab::iarchive &iarc) {
    iarc >> vid_set >> num_triangles;
  }
};

/*
 * Each edge is simply a counter of triangles
 */
typedef uint32_t edge_data_type;

// To collect the set of neighbors, we need a message type which is
// basically a set of vertex IDs

bool PER_VERTEX_COUNT = false;

/*
 * This is the gathering type which accumulates an array of
 * all neighboring vertices.
 * It is a simple wrapper around a vector with
 * an operator+= which simply performs a  +=
 */
struct set_union_gather {
  graphlab::vertex_id_type v;
  std::vector<graphlab::vertex_id_type> vid_vec;

  set_union_gather():v(-1) {
  }

  size_t size() const {
    if (v == (graphlab::vertex_id_type)-1) return vid_vec.size();
    else return 1;
  }
  /*
   * Combining with another collection of vertices.
   * Union it into the current set.
   */
  set_union_gather& operator+=(const set_union_gather& other) {
    if (size() == 0) {
      (*this) = other;
      return (*this);
    }
    else if (other.size() == 0) {
      return *this;
    }

    if (vid_vec.size() == 0) {
      vid_vec.push_back(v);
      v = (graphlab::vertex_id_type)(-1);
    }
    if (other.vid_vec.size() > 0) {
      size_t ct = vid_vec.size();
      vid_vec.resize(vid_vec.size() + other.vid_vec.size());
      for (size_t i = 0; i < other.vid_vec.size(); ++i) {
        vid_vec[ct + i] = other.vid_vec[i];
      }
    }
    else if (other.v != (graphlab::vertex_id_type)-1) {
      vid_vec.push_back(other.v);
    }
    return *this;
  }

  // serialize
  void save(graphlab::oarchive& oarc) const {
    oarc << bool(vid_vec.size() == 0);
    if (vid_vec.size() == 0) oarc << v;
    else oarc << vid_vec;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    bool novvec;
    v = (graphlab::vertex_id_type)(-1);
    vid_vec.clear();
    iarc >> novvec;
    if (novvec) iarc >> v;
    else iarc >> vid_vec;
  }
};

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

/*
 * This class implements the triangle counting algorithm as described in
 * the header. On gather, we accumulate a set of all adjacent vertices.
 * If per_vertex output is not necessary, we can use the optimization
 * where each vertex only accumulates neighbors with greater vertex IDs.
 */
class triangle_count :
      public graphlab::ivertex_program<graph_type,
                                      set_union_gather>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  bool do_not_scatter;

  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, figure out the ID of the "other" vertex
   * and accumulate a set of the neighborhood vertex IDs.
   */
  gather_type gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    set_union_gather gather;
    graphlab::vertex_id_type otherid = edge.target().id() == vertex.id() ?
                                       edge.source().id() : edge.target().id();

    size_t other_nbrs = (edge.target().id() == vertex.id()) ?
        (edge.source().num_in_edges() + edge.source().num_out_edges()):
        (edge.target().num_in_edges() + edge.target().num_out_edges());

    size_t my_nbrs = vertex.num_in_edges() + vertex.num_out_edges();

    if (PER_VERTEX_COUNT || (other_nbrs > my_nbrs) || (other_nbrs == my_nbrs && otherid > vertex.id())) {
    //if (PER_VERTEX_COUNT || otherid > vertex.id()) {
     gather.v = otherid;
    }
    return gather;
  }

  /*
   * the gather result now contains the vertex IDs in the neighborhood.
   * store it on the vertex.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& neighborhood) {
   do_not_scatter = false;
   if (neighborhood.vid_vec.size() == 0) {
     // neighborhood set may be empty or has only 1 element
     vertex.data().vid_set.clear();
     if (neighborhood.v != (graphlab::vertex_id_type(-1))) {
       vertex.data().vid_set.vid_vec.push_back(neighborhood.v);
     }
   }
   else {
     vertex.data().vid_set.assign(neighborhood.vid_vec);
   }
   do_not_scatter = vertex.data().vid_set.size() == 0;
  } // end of apply

  /*
   * Scatter over all edges to compute the intersection.
   * I only need to touch each edge once, so if I scatter just on the
   * out edges, that is sufficient.
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    if (do_not_scatter) return graphlab::NO_EDGES;
    else return graphlab::OUT_EDGES;
  }

  /*
   * For each edge, count the intersection of the neighborhood of the
   * adjacent vertices. This is the number of triangles this edge is involved
   * in.
   */
  void scatter(icontext_type& context,
              const vertex_type& vertex,
              edge_type& edge) const {

    //    vertex_type othervtx = edge.target();
    const vertex_data_type& srclist = edge.source().data();
    const vertex_data_type& targetlist = edge.target().data();
    if (targetlist.vid_set.size() < srclist.vid_set.size()) {
      edge.data() += count_set_intersect(targetlist.vid_set, srclist.vid_set);
    }
    else {
      edge.data() += count_set_intersect(srclist.vid_set, targetlist.vid_set);
    }
  }
};

/*
 * This class is used in a second engine call if per vertex counts are needed.
 * The number of triangles a vertex is involved in can be computed easily
 * by summing over the number of triangles each adjacent edge is involved in
 * and dividing by 2.
 */
class get_per_vertex_count :
      public graphlab::ivertex_program<graph_type, size_t>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  // We gather the number of triangles each edge is involved in
  size_t gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    return edge.data();
  }

  /* the gather result is the total sum of the number of triangles
   * each adjacent edge is involved in . Dividing by 2 gives the
   * desired result.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& num_triangles) {
    vertex.data().vid_set.clear();
    vertex.data().num_triangles = num_triangles / 2;
  }

  // No scatter
  edge_dir_type scatter_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

};

typedef graphlab::synchronous_engine<triangle_count> engine_type;

/* Used to sum over all the edges in the graph in a
 * map_reduce_edges call
 * to get the total number of triangles
 */
size_t get_edge_data(const graph_type::edge_type& e) {
  return e.data();
}

/*
 * A saver which saves a file where each line is a vid / # triangles pair
 */
struct save_triangle_count{
  std::string save_vertex(graph_type::vertex_type v) {
    double nt = v.data().num_triangles;
    double n_followed = v.num_out_edges();
    double n_following = v.num_in_edges();

    return graphlab::tostr(v.id()) + "\t" +
           graphlab::tostr(nt) + "\t" +
           graphlab::tostr(n_followed) + "\t" +
           graphlab::tostr(n_following) + "\n";
  }
  std::string save_edge(graph_type::edge_type e) {
    return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "This program counts the exact number of triangles in the "
            "provided graph.\n\n";

  graphlab::command_line_options clopts("Exact Triangle Counting. "
    "Given a graph, this program computes the total number of triangles "
    "in the graph. An option (per_vertex) is also provided which "
    "computes for each vertex, the number of triangles it is involved in."
    "The algorithm assumes that each undirected edge appears exactly once "
    "in the graph input. If edges may appear more than once, this procedure "
    "will over count.");
  std::string prefix, format;
  std::string per_vertex;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
 clopts.attach_option("ht", HASH_THRESHOLD,
                       "Above this size, hash sets are used");
  clopts.attach_option("per_vertex", per_vertex,
                       "If not empty, will count the number of "
                       "triangles each vertex belongs to and "
                       "save to file with prefix \"[per_vertex]\". "
                       "The algorithm used is slightly different "
                       "and thus will be a little slower");
  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix == "") {
    std::cout << "--graph is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (format == "") {
    std::cout << "--format is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }

  if (per_vertex != "") PER_VERTEX_COUNT = true;
  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  graphlab::launch_metric_server();
  // load graph
  graph_type graph(dc, clopts);
  graph.load_format(prefix, format);
  graph.finalize();
  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
            << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  // create engine to count the number of triangles
  dc.cout() << "Counting Triangles..." << std::endl;
  engine_type engine(dc, graph, clopts);
  engine.signal_all();
  engine.start();

  dc.cout() << "Counted in " << ti.current_time() << " seconds" << std::endl;

  if (PER_VERTEX_COUNT == false) {
    size_t count = graph.map_reduce_edges<size_t>(get_edge_data);
    dc.cout() << count << " Triangles"  << std::endl;
  }
  else {
    graphlab::synchronous_engine<get_per_vertex_count> engine(dc, graph, clopts);
    engine.signal_all();
    engine.start();
    graph.save(per_vertex,
            save_triangle_count(),
            false, /* no compression */
            true, /* save vertex */
            false, /* do not save edge */
            clopts.get_ncpus()); /* one file per machine */

  }

  graphlab::stop_metric_server();

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


#ifndef EDGE_SAMPLER_HPP
#define EDGE_SAMPLER_HPP

#include <Eigen/Dense>

namespace polyfem {

	class EdgeSampler
	{
	public:
		static void sample_2d_simplex(const int resolution, Eigen::MatrixXd &samples);
		static void sample_2d_cube(const int resolution, Eigen::MatrixXd &samples);

		static void sample_3d_simplex(const int resolution, Eigen::MatrixXd &samples);
		static void sample_3d_cube(const int resolution, Eigen::MatrixXd &samples);

	};
}

#endif //EDGE_SAMPLER_HPP


#ifndef UWB_NODE_H
#define UWB_NODE_H

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"
#include "common_msgs/UWB_FullRangeInfo.h"
#include "common_msgs/UWB_FullNeighborDatabase.h"
#include "common_msgs/UWB_DataInfo.h"
#include "common_msgs/UWB_SendData.h"
#include "common_msgs/UWB_EchoedRangeInfo.h"

#include <Eigen/Core>
#include <Eigen/Dense>

#include <boost/shared_ptr.hpp>
#include <map>

namespace uavos{

typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;

// -----------------------------
struct comp{
    template <typename T>
    inline bool operator()(const T& a, const T& b) const{
        return (a<b);
    }
};

template <typename T>
double robustAverage(std::vector<T> array){
    // the paramter is copied, so that the original data will not be affected.
    const int cut_length = 2;

    if(array.size()<=2*cut_length){
        return 0;
    }

    double sum = 0;
    std::sort(array.begin(), array.end(), comp());
    for(int i=cut_length;i<array.size()-cut_length;++i){
        sum+=array.at(i);
    }

    return sum / (array.size()-2*cut_length);

}

template <typename T>
bool medianAndVariance(std::vector<T> array, double& median, double& variance){
    if(array.size()==0){
        return false;
    }

    // get median
    std::sort(array.begin(), array.end(), comp());
    median = array.at(std::floor(array.size()/2));

    // get mean
    double sum = 0;
    for(int i=0;i<array.size();++i){
        sum+=array.at(i);
    }
    double mean = sum / array.size();

    // get variance
    double variance_sum = 0;
    for(int i=0;i<array.size();++i){
        double res = array.at(i)-mean;
        variance_sum += res*res;
    }
    variance = variance_sum / array.size();

    return true;
}

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

class UWB_Node{
public:
    explicit UWB_Node(const int node_id);

protected:
    int m_node_id;

    geometry_msgs::PointStamped m_position;
    geometry_msgs::PointStamped m_velocity;

    Matrix6d m_state_covariance;

public:
    inline int getNodeId() const{
        return m_node_id;
    }
    inline geometry_msgs::PointStamped getPosition() const{
        return m_position;
    }
    inline geometry_msgs::PointStamped* getPositionPtr(){
        return &m_position;
    }
    inline geometry_msgs::PointStamped getVelocity() const{
        return m_velocity;
    }
    inline Matrix6d getStateCovariance() const{
        return m_state_covariance;
    }

    inline void setPosition(const double x, const double y, const double z){
        m_position.header.stamp = ros::Time::now();

        m_position.point.x = x;
        m_position.point.y = y;
        m_position.point.z = z;
    }
    inline void setVelocity(const double vx, const double vy, const double vz){
        m_velocity.header.stamp = ros::Time::now();

        m_velocity.point.x = vx;
        m_velocity.point.y = vy;
        m_velocity.point.z = vz;
    }
    inline void setStateCovariance(const Matrix6d& state_covariance){
        m_state_covariance = state_covariance;
    }

    inline void printPosition(){
        std::cout<<m_node_id<<":"<<std::endl;
        std::cout<<"pose: "<<m_position.point.x<<", "<<m_position.point.y<<", "<<m_position.point.z
                <<"   cov: "<<m_state_covariance(0,0)<<", "<<m_state_covariance(1,1)<<", "<<m_state_covariance(2,2)
                <<std::endl;
    }
    inline void printVelocity(){
        std::cout<<"velo: "<<m_velocity.point.x<<", "<<m_velocity.point.y<<", "<<m_velocity.point.z
                <<"   cov: "<<m_state_covariance(3,3)<<", "<<m_state_covariance(4,4)<<", "<<m_state_covariance(5,5)
                <<std::endl;
    }

};

class UWB_Anchor : public UWB_Node{
public:
    explicit UWB_Anchor(const int node_id,
                        const double x, const double y, const double z,
                        const double var_x, const double var_y, const double var_z);

public:
    std::map<int, std::vector<double> > m_range_map;

    void insertRangeInfo(const common_msgs::UWB_EchoedRangeInfo& msg);
    void insertRangeInfo(const common_msgs::UWB_FullRangeInfo& msg);
    double getAverageRangeToNode(const int anchor_id);
    bool getMedianAndVarianceToNode(const int anchor_id, double& median, double& variance);

    inline void printPosition(){
        std::cout<<m_node_id<<": "<<m_position.point.x<<", "<<m_position.point.y<<", "<<m_position.point.z<<std::endl;
    }

};

class UWB_Mobile : public UWB_Node{
public:
    explicit UWB_Mobile(const int node_id, const std::map<int, boost::shared_ptr<UWB_Anchor> > anchor_map);

private:
    // stores last correct/accepted measurements
    std::map<int, common_msgs::UWB_FullRangeInfo> m_ranges_to_anchors;
    ros::Time m_last_range_time;

    // provided by parameter server
    std::map<int, boost::shared_ptr<UWB_Anchor> > m_anchor_map;

    double m_kalman_sigma_a;
    double m_snr_threshold;
    double m_innovation_threshold;

    // check whether the filter is properly initialized and ready to use
    bool m_is_initialized;
    std::vector<geometry_msgs::PointStamped> m_init_inspector_position_array;

public:
    // reading statistics
    std::map<int, int> m_anchor_reading_counter;
    std::map<int, int> m_anchor_successful_reading_counter;

public:
    bool checkInitialization();
    inline bool getInitializationFlag() const{
        return m_is_initialized;
    }

    inline common_msgs::UWB_FullRangeInfo getRangeInfoToAnchor(const int anchor_id) const{
        return m_ranges_to_anchors.find(anchor_id)->second;
    }
    inline double getRangeMeterToAnchor(const int anchor_id) const{
        return double(m_ranges_to_anchors.find(anchor_id)->second.precisionRangeMm) / 1000.0;
    }

    inline void setRangeInfoToAnchor(const common_msgs::UWB_FullRangeInfo& range_info){
        m_ranges_to_anchors[range_info.responderId] = range_info;
    }

    inline void readingCounterInc(const int anchor_id){
        if(m_anchor_reading_counter.find(anchor_id)==m_anchor_reading_counter.end()){
            m_anchor_reading_counter[anchor_id] = 1;
        } else {
            m_anchor_reading_counter[anchor_id] += 1;
        }
    }
    inline void successfulReadingCounterInc(const int anchor_id){
        if(m_anchor_successful_reading_counter.find(anchor_id)==m_anchor_successful_reading_counter.end()){
            m_anchor_successful_reading_counter[anchor_id] = 1;
        } else {
            m_anchor_successful_reading_counter[anchor_id] += 1;
        }
    }
    inline int getReadingCount(const int anchor_id){
        if(m_anchor_reading_counter.find(anchor_id)==m_anchor_reading_counter.end()){
            return 0;
        } else {
            return m_anchor_reading_counter.at(anchor_id);
        }
    }
    inline int getSuccessfulReadingCount(const int anchor_id){
        if(m_anchor_successful_reading_counter.find(anchor_id)==m_anchor_successful_reading_counter.end()){
            return 0;
        } else {
            return m_anchor_successful_reading_counter.at(anchor_id);
        }
    }

    // EKF from TimeDomain
    void initializeEKF();
    bool kalmanFilter3DUpdate(const common_msgs::UWB_FullRangeInfo& range_info);

};

}

#endif // UWB_NODE_H


#include "MexPackUnpack.h"
#include "mex.h"
#include <Eigen>

using namespace MexPackUnpackTypes;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  //argument 1 : real double precision scalar
  //argument 2 : single precision complex scalar
  //argument 3 : Double precision complex matrix (Eigen Map)
  //argument 4 : Double precision complex matrix represented as a pair of pointers, number of rows, and number of columns
  //Note: Requires MATLAB 2018a or newer and  the -R2018a compilation flag
  MexUnpacker<double, std::complex<float>, EDCIM,  CDIP> my_unpack(nrhs, prhs);

  try {

    auto [a, b, c, d] = my_unpack.unpackMex();
    auto [d_p, d_M, d_N] = d; //dp is std::complex<double>* (pointer to complex data), d_M is number of rows, d_N is number of columns

    Eigen::MatrixXcd e_comp = b*c;

    MexPacker<std::complex<double>, int, Eigen::MatrixXcd, CDIP> my_pack(nlhs, plhs);
    my_pack.PackMex(b, 2, e_comp, d);

  } catch (std::string s) {
    mexPrintf(s.data());
  }

}


// Boost.Geometry

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP
#define BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP

#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

//#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry {

namespace formula {

template <typename Point3d, typename PointSph>
static inline Point3d sph_to_cart3d(PointSph const& point_sph)
{
    typedef typename coordinate_type<Point3d>::type calc_t;

    Point3d res;

    calc_t lon = get_as_radian<0>(point_sph);
    calc_t lat = get_as_radian<1>(point_sph);

    calc_t const cos_lat = cos(lat);
    set<0>(res, cos_lat * cos(lon));
    set<1>(res, cos_lat * sin(lon));
    set<2>(res, sin(lat));

    return res;
}

template <typename PointSph, typename Point3d>
static inline PointSph cart3d_to_sph(Point3d const& point_3d)
{
    typedef typename coordinate_type<PointSph>::type coord_t;
    typedef typename coordinate_type<Point3d>::type calc_t;

    PointSph res;

    calc_t const x = get<0>(point_3d);
    calc_t const y = get<1>(point_3d);
    calc_t const z = get<2>(point_3d);

    set_from_radian<0>(res, atan2(y, x));
    set_from_radian<1>(res, asin(z));

    coord_t lon = get<0>(res);
    coord_t lat = get<1>(res);

    math::normalize_spheroidal_coordinates
        <
            typename coordinate_system<PointSph>::type::units,
            coord_t
        >(lon, lat);

    set<0>(res, lon);
    set<1>(res, lat);

    return res;
}

// -1 right
// 1 left
// 0 on
template <typename Point3d1, typename Point3d2>
static inline int sph_side_value(Point3d1 const& norm, Point3d2 const& pt)
{
    typedef typename select_coordinate_type<Point3d1, Point3d2>::type calc_t;
    calc_t c0 = 0;
    calc_t d = dot_product(norm, pt);
    return math::equals(d, c0) ? 0
        : d > c0 ? 1
        : -1; // d < 0
}

} // namespace formula

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP


#include <fstream>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

ros::Publisher  pub_cloud;
ros::Publisher  pub_velo;

void getCloudClusters(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr, std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& pc_vector)
{
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI>);
  tree->setInputCloud (cloud_ptr);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
  ec.setClusterTolerance (0.5); // 50cm - big since we're sure the panel is far from other obstacles (ie. barriers)
  ec.setMinClusterSize (3);
  ec.setMaxClusterSize (1000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_ptr);
  ec.extract (cluster_indices);

  // Get the cloud representing each cluster
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZI>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
      cloud_cluster->points.push_back (cloud_ptr->points[*pit]);

    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    pc_vector.push_back(cloud_cluster);
  }
}

void computeBoundingBox(std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& pc_vector,std::vector<Eigen::Vector3f>& dimension_list, std::vector<Eigen::Vector4f>& centroid_list, std::vector<std::vector<pcl::PointXYZI> >& corners)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZI> ());
  Eigen::Vector3f one_dimension;

  for (std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>::const_iterator iterator = pc_vector.begin(), end = pc_vector.end(); iterator != end; ++iterator)
  {
    cloud_plane=*iterator;

    // Compute principal directions
    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud_plane, pcaCentroid);

    Eigen::Matrix3f covariance;
    computeCovarianceMatrixNormalized(*cloud_plane, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));

    // Transform the original cloud to the origin where the principal components correspond to the axes.
    Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
    projectionTransform.block<3,3>(0,0) = eigenVectorsPCA.transpose();
    projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * pcaCentroid.head<3>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZI>);

    pcl::transformPointCloud(*cloud_plane, *cloudPointsProjected, projectionTransform);

    // Get the minimum and maximum points of the transformed cloud.
    pcl::PointXYZI minPoint, maxPoint;

    pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);

    // save the centroid into the centroid vector
    centroid_list.push_back(pcaCentroid);
    // save dimenstion into dimension list
    one_dimension[0] = maxPoint.x - minPoint.x;
    one_dimension[1] = maxPoint.y - minPoint.y;
    one_dimension[2] = maxPoint.z - minPoint.z;
    dimension_list.push_back(one_dimension);

    // Transform back
    Eigen::Matrix4f bboxTransform(Eigen::Matrix4f::Identity());
    bboxTransform.block<3,3>(0,0) = eigenVectorsPCA;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_corners_pca (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_corners_base (new pcl::PointCloud<pcl::PointXYZI>);

    cloud_corners_pca->points.push_back(minPoint);
    cloud_corners_pca->points.push_back(maxPoint);

    for (int i=0; i<cloud_corners_pca->points.size(); i++)
    {
      cloud_corners_pca->points[i].x -= projectionTransform(0,3);
      cloud_corners_pca->points[i].y -= projectionTransform(1,3);
      cloud_corners_pca->points[i].z -= projectionTransform(2,3);
    }

    pcl::transformPointCloud(*cloud_corners_pca, *cloud_corners_base, bboxTransform);

    // Extract corners
    std::vector<pcl::PointXYZI> c;
    for (int i=0; i<cloud_corners_base->points.size(); i++)
      c.push_back(cloud_corners_base->points[i]);

    // Save list of corners
    corners.push_back(c);
  }
}

struct tracked_cluster
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
  double x, y;
  double x_start, y_start;
  double distance, distance_travelled;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "process_velodyne_rosbag");
  ros::NodeHandle node;

  pub_cloud = node.advertise<sensor_msgs::PointCloud2>("/velo_rosbag/points", 10);
  pub_velo = node.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 10);

  std::string bag_path;

  // Parse input
  if (argc > 1)
  {
    bag_path = argv[1];
  }
  else
  {
    std::cout << "Error: Input the name of the bag file\n";
    return 0;
  }

  // Open rosbag
  rosbag::Bag bag;
  bag.open(bag_path, rosbag::bagmode::Read);

  std::vector<std::string> topics;
  topics.push_back(std::string("/velodyne_points"));
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  // Process velodyne messages
  double max_angle = DEG2RAD(15);
  double min_angle = DEG2RAD(0);
  double max_starting_distance = 70;
  double min_starting_distance = 60;
  double tracked_distance = -1;
  double tracking_radius = 5;
  bool is_tracking = false;

  std::vector<tracked_cluster> tracked;

  std::ofstream myfile;
  myfile.open ("velodyne_points.csv");
  myfile << "Average distance, Points, Average Intensity\n";

  foreach(rosbag::MessageInstance const m, view)
  {
    if (!ros::ok())
      break;

    sensor_msgs::PointCloud2::ConstPtr msg = m.instantiate<sensor_msgs::PointCloud2>();

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*msg, *cloud);

    // Filter out points outside a given angle
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
    for (int i=0; i<cloud->points.size(); i++)
    {
      double x, y, z;
      x = cloud->points[i].x;
      y = cloud->points[i].y;
      z = cloud->points[i].z;

      double angle = atan2(y, x);
      if (angle > max_angle || angle < min_angle)
        continue;

      cloud_filtered->points.push_back(cloud->points[i]);
    }

    // Get clusters
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> pc_vector;
    getCloudClusters(cloud_filtered, pc_vector);

    // Keep clusters with certain number of points
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> pc_vector_filtered;
    for (int i=0; i<pc_vector.size(); i++)
    {
      int pc_size = pc_vector[i]->points.size();
      if (pc_size > 20 || pc_size < 3)
        continue;

      pc_vector_filtered.push_back(pc_vector[i]);
    }

    // Get size of each cluster
    std::vector<Eigen::Vector3f> dimension_list;
    std::vector<Eigen::Vector4f> centroid_list;
    std::vector<std::vector<pcl::PointXYZI> > corners_list;
    computeBoundingBox(pc_vector, dimension_list, centroid_list, corners_list);

    /*
    for (int i=0; i<pc_vector.size(); i++)
    {
      double x = 0, y=0;

      for (int pi=0; pi<pc_vector[i]->points.size(); pi++)
      {
        x +=pc_vector[i]->points[pi].x;
        y +=pc_vector[i]->points[pi].y;
      }

      x /= pc_vector[i]->points.size();
      y /= pc_vector[i]->points.size();

      centroid_list[i][0] = x;
      centroid_list[i][1] = y;
    }
    */

    // Update tracked entries
    for (int pci=0; pci<pc_vector.size(); pci++)
    {
      // Only keep the clusters that are likely to be panels
      if (dimension_list[pci][2] > 1.5 || dimension_list[pci][1] > 1.5)
        continue;

      if (!is_tracking)
      {
        double x = centroid_list[pci][0];
        double y = centroid_list[pci][1];
        double r = sqrt(x*x + y*y);

        if (r < min_starting_distance || r > max_starting_distance)
          continue;
      }

      bool found = false;

      // Update previous tracked entries
      for (int ti=0; ti<tracked.size(); ti++)
      {
        // Check if we're in proximity
        double x1 = centroid_list[pci][0] - tracked[ti].x;
        double y1 = centroid_list[pci][1] - tracked[ti].y;
        double dist = sqrt(x1*x1 + y1*y1);

        if (dist > tracking_radius)
          continue;
        else
          found = true;

        // Check that the distance to velodyne has decreased within some tolerance
        double x2 = centroid_list[pci][0];
        double y2 = centroid_list[pci][1];
        double dist2 = sqrt(x2*x2 + y2*y2);

        double x3 = tracked[ti].x;
        double y3 = tracked[ti].y;
        double dist3 = sqrt(x3*x3 + y3*y3);

        if (dist3 + 0.5 < dist2)
          continue;

        // Update
        tracked[ti].cloud = pc_vector[pci];
        tracked[ti].x = centroid_list[pci][0];
        tracked[ti].y = centroid_list[pci][1];

        x1 = tracked[ti].x - tracked[ti].x_start;
        y1 = tracked[ti].y - tracked[ti].y_start;
        tracked[ti].distance_travelled = sqrt(x1*x1 + y1*y1);
        tracked[ti].distance = dist2;

        // Write to file
        if (is_tracking)
        {
          double intensity = 0;
          double count = tracked[ti].cloud->points.size();

          for (int idx=0; idx < count; idx++)
          {
            intensity += tracked[ti].cloud->points[idx].intensity;
          }
          intensity /= count;

          myfile << dist2 << ", " << count << ", " << intensity << "\n";
        }

      }

      // Add new tracked entry
      if (!found && !is_tracking)
      {
        tracked_cluster t;
        t.x = centroid_list[pci][0];
        t.y = centroid_list[pci][1];
        t.x_start = centroid_list[pci][0];
        t.y_start = centroid_list[pci][1];
        t.distance_travelled = 0;
        t.cloud = pc_vector[pci];

        tracked.push_back(t);
      }
    }

    // Filter out clusters that aren't likely to be our target
    std::vector<tracked_cluster> tracked_temp;
    for (int ti=0; ti<tracked.size(); ti++)
    {
      if (tracked[ti].distance_travelled > 5)
      {
        // Found our target
        tracked_temp.clear();
        tracked_temp.push_back(tracked[ti]);

        is_tracking = true;
        tracking_radius = 5;
        break;
      }

      tracked_temp.push_back(tracked[ti]);
    }

    tracked = tracked_temp;

    std::cout << "Found " << tracked.size() << " clusters \n";

    if (tracked.size() < 3)
    {
      // Publish velodyne cloud
      sensor_msgs::PointCloud2 velo_msg;
      velo_msg = *msg;
      velo_msg.header.stamp = ros::Time::now();
      pub_velo.publish(velo_msg);

      for (int i=0; i<tracked.size(); i++)
      {
        // Publish cloud
        sensor_msgs::PointCloud2 cloud_cluster_msg;
        pcl::toROSMsg(*tracked[i].cloud, cloud_cluster_msg);
        cloud_cluster_msg.header.frame_id = "velodyne";
        cloud_cluster_msg.header.stamp = ros::Time::now();
        pub_cloud.publish(cloud_cluster_msg);

        {
          ros::Rate r(30);
          ros::spinOnce();
          r.sleep();
        }
      }
    }

    /*

    // Publish cloud
    sensor_msgs::PointCloud2 cloud_cluster_msg;
    pcl::toROSMsg(*cloud_filtered, cloud_cluster_msg);
    cloud_cluster_msg.header.frame_id = "velodyne";
    cloud_cluster_msg.header.stamp = ros::Time::now();
    pub_cloud.publish(cloud_cluster_msg);

    std::cout << cloud->points.size() << " Got it \n";
    std::cout << cloud->points[0].intensity << "\n";
    */

    {
      ros::Rate r(30);
      ros::spinOnce();
      r.sleep();
    }
    //break;
  }

  myfile.close();
  bag.close();

  return 0;
}


#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

// this is just testing the nan behavior of the built-in fma
// there is no longer a stan::math::fma, just the agrad versions
// instead, the top-level ::fma should be used by including <cmath>

TEST(MathFunctions, fma) {
  EXPECT_FLOAT_EQ(5.0, fma(1.0,2.0,3.0));
  EXPECT_FLOAT_EQ(10.0, fma(2.0,3.0,4.0));
}

TEST(MathFunctions, fma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(1.0, 2.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(1.0, nan, 3.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(1.0, nan, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(nan, 2.0, 3.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(nan, 2.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(nan, nan, 3.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               fma(nan, nan, nan));
}


#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <cfenv>

#include <complex>
#include <iomanip>
#include <iostream>

#include <armadillo>

#include <omp.h>

#include "pme.h"
#include "abort_unless.h"

using namespace std;
using namespace arma;

const complex<double> I(0.0, 1.0);

const double sigma = pow(2.0, 1.0 / 6.0);

int main(int argc, char* argv[]) {
  if (argc < 7) {
    cout << "Usage: " << argv[0]
         << " dimension cell-file positions-file charges-file cut-off tolerance\n";
    return EXIT_FAILURE;
  }

  const long dim0 = atoi(argv[1]);
  const long dim1 = dim0;
  const long dim2 = dim0;

  mat::fixed<3, 3> L;
  L.load(argv[2]);
  const mat::fixed<3, 3> LinvT = trans(inv(L));
  // const double V = det(L);
  L.print("L = ");

  mat r;
  r.load(argv[3]);
  abort_unless(r.n_rows == 3);
  // r.print("r = ");
  const mat x = inv(L) * r;

  vec qq;
  qq.load(argv[4]);
  std::vector<double> q(r.n_cols);
  {
    for (size_t k = 0; k < q.size(); ++k)
      q[k] = qq(k);
  }

  const size_t num_particles = q.size();
  clog << "Read " << num_particles << " particles into memory.\n";

  std::vector<double> coor_x(num_particles);
  std::vector<double> coor_y(num_particles);
  std::vector<double> coor_z(num_particles);
  {
    for (size_t k = 0; k < num_particles; ++k) {
      coor_x[k] = r(0, k);
      coor_y[k] = r(1, k);
      coor_z[k] = r(2, k);
    }
  }

  const double cut_off = atof(argv[5]);
  const double tolerance = atof(argv[6]);
  clog << "Real space cut-off: " << cut_off << "\n"
       << "Real space tolerance: " << tolerance << "\n\n";

  mat forces = zeros<mat>(3, num_particles);
  mat forces_cutoff = zeros<mat>(3, num_particles);
  mat forces_direct = zeros<mat>(3, num_particles);
  mat forces_recip = zeros<mat>(3, num_particles);

  long double ErecipPME = 0.0L;
  std::vector<double> forces_x(num_particles);
  std::vector<double> forces_y(num_particles);
  std::vector<double> forces_z(num_particles);
  mat forcesPME = zeros<mat>(3, num_particles);
    wall_clock timer;
    timer.tic();

    double LL[3 * 3];
    for (size_t i = 0; i < 3; ++i)
      for (size_t j = 0; j < 3; ++j)
        LL[3*i+j] = L(i, j);

    ewald::pme p(dim0, dim1, dim2, LL,
                 num_particles, &q[0],
                 cut_off, tolerance,
                 omp_get_max_threads());

    double seconds = timer.toc();
    clog << "PME initialization took " << seconds << " seconds.\n\n";

    timer.tic();

    ErecipPME = p.energy(&coor_x[0], &coor_y[0], &coor_z[0],
                         &forces_x[0], &forces_y[0], &forces_z[0]);

    for (size_t k = 0; k < num_particles; ++k) {
      forcesPME(0, k) = forces_x[k];
      forcesPME(1, k) = forces_y[k];
      forcesPME(2, k) = forces_z[k];
    }

    seconds = timer.toc();
    clog << "PME took " << seconds << " seconds.\n\n";

  long double Ebrute = 0.0L, Edirect = 0.0L;
  long double Ecutoff = 0.0L;
  const long double Eextra = p.energy_extra();
  const long double Eself  = p.energy_self();

  mat forces_ewald, forces_pme;

  double elapsed = 0.0;
  for (int n = 0; n < 1; n++) {
    wall_clock timer;
    timer.tic();
    for (int nx = -n; nx <= n; nx++) {
      for (int ny = -n; ny <= n; ny++) {
        for (int nz = -n; nz <= n; nz++) {
          if (abs(nx) != n && abs(ny) != n && abs(nz) != n)
            continue;

          const bool origin = (nx == 0 && ny == 0 && nz == 0);

          const vec::fixed<3> m = { double(nx), double(ny), double(nz) };
          const vec::fixed<3> Lm = L * m;
          const vec::fixed<3> LinvTm = LinvT * m;

          for (size_t i = 0; i < num_particles; i++) {
            for (size_t j = 0; j < num_particles; j++) {
              if (origin && i == j)
                continue;

              const vec::fixed<3> v = r.col(i) - r.col(j) + Lm;
              const double r2 = dot(v, v);
              const double r6 = r2 * r2 * r2;
              const double r8 = r6 * r2;

              assert(r2 >= 1e-16);

              Ebrute  += 0.5 * q[i] * q[j] / r6;
              forces.col(i) -= -6.0 * q[i] * q[j] / r8 * v;

              if (abs(nx) < 2 && abs(ny) < 2 && abs(nz) < 2) {
                forces_cutoff.col(i) -= -6.0 * q[i] * q[j] / r8 * v;
                Ecutoff = Ebrute;
              }

              vec::fixed<3> dv;
              Edirect += 0.5 * q[i] * q[j] * p.direct_convergence_term(v.memptr(), dv.memptr());
              forces_direct.col(i) -= q[i] * q[j] * dv;
            }
          }
        }
      }
    }

    elapsed += timer.toc();

    const long double EtotalPME = Edirect + ErecipPME + Eextra - Eself;

    forces_pme   = forces_direct + forcesPME;

    cout << "Real space computation over one shell took " << elapsed << " seconds." << "\n"
         << fixed << setprecision(16)
         << "Ecutoff      = " << Ecutoff << "\n"
         << "Etotal (PME) = " << EtotalPME  << "\n"
         << "Edirect + Erecip + Eextra - Eself = "
         << Edirect << " + " << ErecipPME << " + " << Eextra << " - " << Eself << "\n";
  }

  return 0;
}