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

// @sect3{Include files}

// 前面几个文件已经在前面的例子中讲过了，因此不再做进一步的评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/utilities.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_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

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

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

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_q.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/numerics/error_estimator.h>

#include <fstream>
#include <iostream>

// 我们将在牛顿迭代之间使用自适应网格细化技术。要做到这一点，我们需要能够在新的网格上使用解决方案，尽管它是在旧的网格上计算出来的。SolutionTransfer类将解决方案从旧网格转移到新网格。

#include <deal.II/numerics/solution_transfer.h>

// 然后，我们为这个程序打开一个命名空间，像以前的程序一样，将dealii命名空间中的所有东西导入其中。

namespace Step15
{
  using namespace dealii;
// @sect3{The MinimalSurfaceProblem class template}

// 类模板与  step-6  中的基本相同。 增加了三个内容。

// - 有两个解决方案向量，一个用于牛顿更新  $\delta u^n$  ，另一个用于当前迭代  $u^n$  。

// -  setup_system 函数需要一个参数，表示这是否是第一次被调用。不同的是，第一次我们需要分配自由度，并将 $u^n$ 的解向量设置为正确的大小。接下来的几次，该函数是在我们已经完成了这些步骤，作为细化 refine_mesh 中网格的一部分之后被调用的。

// - 然后我们还需要新的函数。  set_boundary_values() 负责正确设置解向量的边界值，这在介绍的最后已经讨论过了。  compute_residual() 是一个计算非线性（离散）残差规范的函数。我们用这个函数来监测牛顿迭代的收敛性。该函数以步长 $\alpha^n$ 为参数来计算 $u^n + \alpha^n \; \delta u^n$ 的残差。这是人们通常需要的步长控制，尽管我们在这里不会使用这个功能。最后， determine_step_length() 计算每个牛顿迭代中的步长 $\alpha^n$ 。正如介绍中所讨论的，我们在这里使用一个固定的步长，并把实现一个更好的策略作为一个练习。(  step-77 的做法不同。它只是在整个求解过程中使用了一个外部包，而一个好的直线搜索策略是该包所提供的一部分）。)

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

  private:
    void   setup_system(const bool initial_step);
    void   assemble_system();
    void   solve();
    void   refine_mesh();
    void   set_boundary_values();
    double compute_residual(const double alpha) const;
    double determine_step_length() const;
    void   output_results(const unsigned int refinement_cycle) const;

    Triangulation<dim> triangulation;

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

    AffineConstraints<double> hanging_node_constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> current_solution;
    Vector<double> newton_update;
    Vector<double> system_rhs;
  };
// @sect3{Boundary condition}

// 边界条件的实现就像在  step-4  中一样。 它被选为  $g(x,y)=\sin(2 \pi (x+y))$  。

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

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> &p,
                                    const unsigned int /*component*/) const
  {
    return std::sin(2 * numbers::PI * (p[0] + p[1]));
  }
// @sect3{The MinimalSurfaceProblem class implementation}
// @sect4{MinimalSurfaceProblem::MinimalSurfaceProblem}

// 该类的构造函数和析构函数与前几篇教程中的相同。

  template <int dim>
  MinimalSurfaceProblem<dim>::MinimalSurfaceProblem()
    : dof_handler(triangulation)
    , fe(2)
  {}
// @sect4{MinimalSurfaceProblem::setup_system}

// 在setup-system函数中，我们总是设置有限元方法的变量。与 step-6 有相同的区别，因为在那里我们在每个细化周期中都要从头开始求解PDE，而在这里我们需要把以前的网格的解放到当前的网格上。因此，我们不能只是重置解向量。因此，传递给这个函数的参数表明我们是否可以分布自由度（加上计算约束）并将解向量设置为零，或者这在其他地方已经发生过了（特别是在 refine_mesh() ）。

  template <int dim>
  void MinimalSurfaceProblem<dim>::setup_system(const bool initial_step)
  {
    if (initial_step)
      {
        dof_handler.distribute_dofs(fe);
        current_solution.reinit(dof_handler.n_dofs());

        hanging_node_constraints.clear();
        DoFTools::make_hanging_node_constraints(dof_handler,
                                                hanging_node_constraints);
        hanging_node_constraints.close();
      }

// 该函数的其余部分与  step-6  中的相同。

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

    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);

    hanging_node_constraints.condense(dsp);

    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
  }
// @sect4{MinimalSurfaceProblem::assemble_system}

// 这个函数的作用与前面的教程相同，当然，现在矩阵和右手边的函数取决于上一次迭代的解。正如在介绍中所讨论的，我们需要使用牛顿更新的零边界值；我们在这个函数的最后计算它们。

// 该函数的顶部包含了通常的模板代码，设置了允许我们在正交点评估形状函数的对象，以及本地矩阵和向量的临时存储位置，以及正交点上先前解的梯度。然后我们开始在所有单元格上进行循环。

  template <int dim>
  void MinimalSurfaceProblem<dim>::assemble_system()
  {
    const QGauss<dim> quadrature_formula(fe.degree + 1);

    system_matrix = 0;
    system_rhs    = 0;

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

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

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

    std::vector<Tensor<1, dim>> old_solution_gradients(n_q_points);

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

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

        fe_values.reinit(cell);

// 为了组装线性系统，我们必须在正交点上获得前一个解的梯度值。有一个标准的方法： FEValues::get_function_gradients 函数接收一个代表定义在DoFHandler上的有限元场的向量，并评估这个场在FEValues对象最后被重新初始化的单元的正交点的梯度。然后将所有正交点的梯度值写入第二个参数中。

        fe_values.get_function_gradients(current_solution,
                                         old_solution_gradients);

// 有了这个，我们就可以对所有的正交点和形状函数进行积分循环。 在刚刚计算了正交点中旧解的梯度后，我们就可以计算这些点中的系数 $a_{n}$ 。 然后，系统本身的组装看起来与我们一贯的做法相似，除了非线性项之外，将结果从局部对象复制到全局对象中也是如此。

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            const double coeff =
              1.0 / std::sqrt(1 + old_solution_gradients[q] *
                                    old_solution_gradients[q]);

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                for (unsigned int j = 0; j < dofs_per_cell; ++j)
                  cell_matrix(i, j) +=
                    (((fe_values.shape_grad(i, q)      // ((\nabla \phi_i
                       * coeff                         //   * a_n
                       * fe_values.shape_grad(j, q))   //   * \nabla \phi_j)
                      -                                //  -
                      (fe_values.shape_grad(i, q)      //  (\nabla \phi_i
                       * coeff * coeff * coeff         //   * a_n^3
                       * (fe_values.shape_grad(j, q)   //   * (\nabla \phi_j
                          * old_solution_gradients[q]) //      * \nabla u_n)
                       * old_solution_gradients[q]))   //   * \nabla u_n)))
                     * fe_values.JxW(q));              // * dx

                cell_rhs(i) -= (fe_values.shape_grad(i, q)  // \nabla \phi_i
                                * coeff                     // * a_n
                                * old_solution_gradients[q] // * u_n
                                * fe_values.JxW(q));        // * dx
              }
          }

        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              system_matrix.add(local_dof_indices[i],
                                local_dof_indices[j],
                                cell_matrix(i, j));

            system_rhs(local_dof_indices[i]) += cell_rhs(i);
          }
      }

// 最后，我们从系统中移除悬挂的节点，并将零边界值应用到定义牛顿更新的线性系统中  $\delta u^n$  。

    hanging_node_constraints.condense(system_matrix);
    hanging_node_constraints.condense(system_rhs);

    std::map<types::global_dof_index, double> boundary_values;
    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             Functions::ZeroFunction<dim>(),
                                             boundary_values);
    MatrixTools::apply_boundary_values(boundary_values,
                                       system_matrix,
                                       newton_update,
                                       system_rhs);
  }

//  @sect4{MinimalSurfaceProblem::solve}

// 解算函数和以往一样。在求解过程的最后，我们通过设置 $u^{n+1}=u^n+\alpha^n\;\delta u^n$ 来更新当前的解决方案。

  template <int dim>
  void MinimalSurfaceProblem<dim>::solve()
  {
    SolverControl            solver_control(system_rhs.size(),
                                 system_rhs.l2_norm() * 1e-6);
    SolverCG<Vector<double>> solver(solver_control);

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

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

    hanging_node_constraints.distribute(newton_update);

    const double alpha = determine_step_length();
    current_solution.add(alpha, newton_update);
  }
// @sect4{MinimalSurfaceProblem::refine_mesh}

// 这个函数的第一部分与 step-6 中的内容相同 ... 然而，在细化网格后，我们必须将旧的解决方案转移到新的解决方案中，我们在SolutionTransfer类的帮助下完成。这个过程稍微有点复杂，所以让我们详细描述一下。

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

    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(fe.degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      current_solution,
      estimated_error_per_cell);

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

// 然后我们需要一个额外的步骤：例如，如果你标记了一个比它的邻居更精炼一次的单元，而这个邻居没有被标记为精炼，我们最终会在一个单元界面上跳过两个精炼级别。 为了避免这些情况，库将默默地也要对邻居单元进行一次细化。它通过在实际进行细化和粗化之前调用 Triangulation::prepare_coarsening_and_refinement 函数来实现。 这个函数标志着一组额外的单元格进行细化或粗化，以执行像单悬节点规则这样的规则。 调用此函数后，被标记为细化和粗化的单元格正是那些将被实际细化或粗化的单元格。通常情况下，你不需要手工操作 (Triangulation::execute_coarsening_and_refinement 为你做这个）。) 然而，我们需要初始化SolutionTransfer类，它需要知道最终将被粗化或细化的单元集，以便存储旧网格的数据并转移到新网格。因此，我们手动调用这个函数。

    triangulation.prepare_coarsening_and_refinement();

// 有了这个方法，我们用现在的DoFHandler初始化一个SolutionTransfer对象，并将解决方案向量附加到它上面，然后在新网格上进行实际的细化和自由度分配

    SolutionTransfer<dim> solution_transfer(dof_handler);
    solution_transfer.prepare_for_coarsening_and_refinement(current_solution);

    triangulation.execute_coarsening_and_refinement();

    dof_handler.distribute_dofs(fe);

// 最后，我们找回插值到新网格的旧解。由于SolutionTransfer函数实际上并不存储旧的解决方案的值，而是索引，我们需要保留旧的解决方案向量，直到我们得到新的内插值。因此，我们将新的数值写入一个临时的向量中，之后才将其写入解决方案向量对象中。

    Vector<double> tmp(dof_handler.n_dofs());
    solution_transfer.interpolate(current_solution, tmp);
    current_solution = tmp;

// 在新的网格上，有不同的悬挂节点，对于这些节点，我们必须在扔掉之前的对象内容后，重新计算约束。为了安全起见，我们还应该确保当前解决方案的向量条目满足悬空节点的约束条件（参见SolutionTransfer类文档中的讨论，了解为什么必须这样做）。我们可以通过明确调用`hanging_node_constraints.distribution(current_solution)`来做到这一点；我们省略这一步，因为这将在下面调用`set_boundary_values()`的最后发生，而且没有必要做两次。

    hanging_node_constraints.clear();

    DoFTools::make_hanging_node_constraints(dof_handler,
                                            hanging_node_constraints);
    hanging_node_constraints.close();

// 一旦我们有了内插的解决方案和所有关于悬挂节点的信息，我们必须确保我们现在的 $u^n$ 实际上有正确的边界值。正如在介绍的最后所解释的，即使细化前的解决方案有正确的边界值，也不会自动出现这种情况，因此我们必须明确地确保它现在有。

    set_boundary_values();

// 我们通过更新所有剩余的数据结构来结束这个函数，向 setup_dofs() 表明这不是第一次了，它需要保留解向量的内容。

    setup_system(false);
  }

//  @sect4{MinimalSurfaceProblem::set_boundary_values}

// 下一个函数确保解向量的条目尊重我们问题的边界值。 在细化了网格之后（或者刚刚开始计算），边界上可能会出现新的节点。这些节点的数值是在`refine_mesh()`中从之前的网格中简单插值出来的，而不是正确的边界值。这个问题可以通过将当前解决方案向量的所有边界节点明确设置为正确的值来解决。

// 但是有一个问题我们必须注意：如果我们有一个挂起的节点紧挨着一个新的边界节点，那么它的值也必须被调整以确保有限元场保持连续。这就是这个函数最后一行的调用所做的。

  template <int dim>
  void MinimalSurfaceProblem<dim>::set_boundary_values()
  {
    std::map<types::global_dof_index, double> boundary_values;
    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             BoundaryValues<dim>(),
                                             boundary_values);
    for (auto &boundary_value : boundary_values)
      current_solution(boundary_value.first) = boundary_value.second;

    hanging_node_constraints.distribute(current_solution);
  }
// @sect4{MinimalSurfaceProblem::compute_residual}

// 为了监测收敛性，我们需要一种方法来计算（离散）残差的规范，即在介绍中讨论的向量 $\left<F(u^n),\varphi_i\right>$ 与 $F(u)=-\nabla \cdot \left(\frac{1}{\sqrt{1+|\nabla u|^{2}}}\nabla u \right)$ 的规范。事实证明，（尽管我们在当前版本的程序中没有使用这个功能）在确定最佳步长时需要计算残差 $\left<F(u^n+\alpha^n\;\delta u^n),\varphi_i\right>$ ，因此这就是我们在这里实现的：该函数将步长 $\alpha^n$ 作为参数。原有的功能当然是通过传递一个零作为参数得到的。

// 在下面的函数中，我们首先为残差设置一个向量，然后为评估点设置一个向量  $u^n+\alpha^n\;\delta u^n$  。接下来是我们在所有的积分操作中使用的相同的模板代码。

  template <int dim>
  double MinimalSurfaceProblem<dim>::compute_residual(const double alpha) const
  {
    Vector<double> residual(dof_handler.n_dofs());

    Vector<double> evaluation_point(dof_handler.n_dofs());
    evaluation_point = current_solution;
    evaluation_point.add(alpha, newton_update);

    const QGauss<dim> quadrature_formula(fe.degree + 1);
    FEValues<dim>     fe_values(fe,
                            quadrature_formula,
                            update_gradients | update_quadrature_points |
                              update_JxW_values);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    Vector<double>              cell_residual(dofs_per_cell);
    std::vector<Tensor<1, dim>> gradients(n_q_points);

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

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_residual = 0;
        fe_values.reinit(cell);

// 实际的计算与  assemble_system()  中的计算差不多。我们首先评估 $u^n+\alpha^n\,\delta u^n$ 在正交点的梯度，然后计算系数 $a_n$ ，然后将其全部插入残差公式中。

        fe_values.get_function_gradients(evaluation_point, gradients);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            const double coeff =
              1. / std::sqrt(1 + gradients[q] * gradients[q]);

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              cell_residual(i) -= (fe_values.shape_grad(i, q) // \nabla \phi_i
                                   * coeff                    // * a_n
                                   * gradients[q]             // * u_n
                                   * fe_values.JxW(q));       // * dx
          }

        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          residual(local_dof_indices[i]) += cell_residual(i);
      }

// 在这个函数的最后，我们还必须处理悬挂节点的约束和边界值的问题。关于后者，我们必须将所有对应于位于边界的自由度的条目的残差向量元素设置为零。原因是，由于那里的解的值是固定的，它们当然不是 "真正的 "自由度，因此，严格来说，我们不应该在残差向量中为它们集合条目。然而，正如我们一直所做的那样，我们想在每个单元上做完全相同的事情，因此我们并不想在上面的积分中处理某个自由度是否位于边界的问题。相反，我们将简单地在事后将这些条目设置为零。为此，我们需要确定哪些自由度实际上属于边界，然后在所有这些自由度上进行循环，并将剩余条目设置为零。这发生在以下几行中，我们已经在 step-11 中看到了使用DoFTools命名空间的适当函数。

    hanging_node_constraints.condense(residual);

    for (types::global_dof_index i :
         DoFTools::extract_boundary_dofs(dof_handler))
      residual(i) = 0;

// 在函数的最后，我们返回残差的常数。

    return residual.l2_norm();
  }

//  @sect4{MinimalSurfaceProblem::determine_step_length}

// 正如介绍中所讨论的，如果我们总是采取全步，即计算 $u^{n+1}=u^n+\delta u^n$ ，牛顿方法经常不收敛。相反，我们需要一个阻尼参数（步长）  $\alpha^n$  并设置  $u^{n+1}=u^n+\alpha^n\delta u^n$  。这个函数是用来计算 $\alpha^n$  的。

// 在这里，我们简单地总是返回0.1。这当然是一个次优的选择：理想情况下，人们希望的是，当我们越来越接近解的时候，步长变成1，这样我们就可以享受牛顿方法的快速二次收敛。我们将在下面的结果部分讨论更好的策略， step-77 也涉及这方面的内容。

  template <int dim>
  double MinimalSurfaceProblem<dim>::determine_step_length() const
  {
    return 0.1;
  }

//  @sect4{MinimalSurfaceProblem::output_results}

// 从`run()`调用的最后一个函数以图形形式输出当前的解决方案（和牛顿更新），作为VTU文件。它与之前教程中使用的完全相同。

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

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

    const std::string filename =
      "solution-" + Utilities::int_to_string(refinement_cycle, 2) + ".vtu";
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }
// @sect4{MinimalSurfaceProblem::run}

// 在运行函数中，我们建立第一个网格，然后有牛顿迭代的顶层逻辑。

// 正如在介绍中所描述的，领域是围绕原点的单位圆盘，创建方式与 step-6 中所示相同。网格经过两次全局细化，然后再进行若干次适应性循环。

// 在开始牛顿循环之前，我们还需要做一些设置工作。我们需要创建基本的数据结构，并确保第一个牛顿迭代已经有了正确的边界值，这在介绍中已经讨论过了。

  template <int dim>
  void MinimalSurfaceProblem<dim>::run()
  {
    GridGenerator::hyper_ball(triangulation);
    triangulation.refine_global(2);

    setup_system(/*first time=*/true);
    set_boundary_values();

// 接下来开始牛顿迭代。我们一直迭代到上一次迭代结束时计算的残差（规范）小于 $10^{-3}$ ，正如在 "do{ ... } while "循环结束时的检查。因为我们没有一个合理的值来初始化这个变量，所以我们只是使用可以表示为`双数'的最大值。

    double       last_residual_norm = std::numeric_limits<double>::max();
    unsigned int refinement_cycle   = 0;
    do
      {
        std::cout << "Mesh refinement step " << refinement_cycle << std::endl;

        if (refinement_cycle != 0)
          refine_mesh();

// 在每个网格上，我们正好做五个牛顿步骤。我们在这里打印初始残差，然后在这个网格上开始迭代。

// 在每一个牛顿步骤中，首先要计算系统矩阵和右手边，然后我们存储右手边的规范作为残差，以便在决定是否停止迭代时进行检查。然后我们求解线性系统（该函数也会更新 $u^{n+1}=u^n+\alpha^n\;\delta u^n$ ），并在这个牛顿步骤结束时输出残差的准则。

// 在这个循环结束后，我们还将以图形形式输出当前网格上的解，并增加网格细化循环的计数器。

        std::cout << "  Initial residual: " << compute_residual(0) << std::endl;

        for (unsigned int inner_iteration = 0; inner_iteration < 5;
             ++inner_iteration)
          {
            assemble_system();
            last_residual_norm = system_rhs.l2_norm();

            solve();

            std::cout << "  Residual: " << compute_residual(0) << std::endl;
          }

        output_results(refinement_cycle);

        ++refinement_cycle;
        std::cout << std::endl;
      }
    while (last_residual_norm > 1e-3);
  }
} // namespace Step15
// @sect4{The main function}

// 最后是主函数。这遵循了所有其他主函数的方案。

int main()
{
  try
    {
      using namespace Step15;

      MinimalSurfaceProblem<2> laplace_problem_2d;
      laplace_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;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>
#include <boost/core/ignore_unused.hpp>

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

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

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

/// Calculate course (bearing) between two points. Might be moved to a "course formula" ...
template <typename Point>
static inline double course(Point const& p1, Point const& p2)
{
    // http://williams.best.vwh.net/avform.htm#Crs
    double dlon = get_as_radian<0>(p2) - get_as_radian<0>(p1);
    double cos_p2lat = cos(get_as_radian<1>(p2));

    // "An alternative formula, not requiring the pre-computation of d"
    return atan2(sin(dlon) * cos_p2lat,
        cos(get_as_radian<1>(p1)) * sin(get_as_radian<1>(p2))
        - sin(get_as_radian<1>(p1)) * cos_p2lat * cos(dlon));
}

}
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_cross_track
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename coordinate_type<P1>::type,
                                typename coordinate_type<P2>::type
                            >::type,
                        typename coordinate_type<P>::type
                    >::type,
                CalculationType
            >::type coordinate_type;

        boost::ignore_unused<coordinate_type>();

        double d1 = 0.001; // m_strategy.apply(sp1, p);
        double crs_AD = detail::course(p1, p);
        double crs_AB = detail::course(p1, p2);
        double XTD = asin(sin(d1) * sin(crs_AD - crs_AB));

        return math::equals(XTD, 0) ? 0 : XTD < 0 ? 1 : -1;
    }
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP


#ifndef QST_OBSERVERWAVEFUNCTION_HPP
#define QST_OBSERVERWAVEFUNCTION_HPP

#include <iostream>
#include <Eigen/Dense>
#include <iomanip>
#include <fstream>

namespace qst{

template<class Wavefunction> class ObserverPSI{

    Wavefunction &PSI_;

    int N_;
    int npar_;
    Eigen::VectorXcd target_psi_;
    std::vector<Eigen::VectorXcd> rotated_wf_;
    Eigen::MatrixXd basis_states_;      // Hilbert space basis
    std::map<std::string,Eigen::MatrixXcd> U_;
    std::vector<std::vector<std::string> > basisSet_;
    std::string basis_;
public:

    double KL_;
    double overlap_;
    double Z_;
    double NLL_;

    ObserverPSI(Wavefunction &PSI,std::string &basis):PSI_(PSI){

        std::cout<<"- Initializing observer module"<<std::endl;
        N_ = PSI_.N();
        npar_ = PSI_.Npar();
        basis_ = basis;
        basis_states_.resize(1<<N_,N_);
        std::bitset<10> bit;
        // Create the basis of the Hilbert space
        for(int i=0;i<1<<N_;i++){
            bit = i;
            for(int j=0;j<N_;j++){
                basis_states_(i,j) = bit[N_-j-1];
            }
        }
    }

    //Compute different estimators for the training performance
    void Scan(int i){//,Eigen::MatrixXd &nll_test,std::ofstream &obs_out){
        ExactPartitionFunction();
        ExactKL();
        Overlap();
        PrintStats(i);
    }

    //Compute the partition function by exact enumeration
    void ExactPartitionFunction() {
        Z_ = 0.0;
        for(int i=0;i<basis_states_.rows();i++){
            Z_ += norm(PSI_.psi(basis_states_.row(i)));
        }
    }

    // Compute the overlap with the target wavefunction
    void Overlap(){
        overlap_ = 0.0;
        std::complex<double> tmp;
        for(int i=0;i<basis_states_.rows();i++){
            tmp += conj(target_psi_(i))*PSI_.psi(basis_states_.row(i))/std::sqrt(Z_);
        }
        overlap_ = abs(tmp);
    }
    // Compute the fidelity with the target wavefunction
    void Fidelity(){
        Overlap();
        return overlap_*overlap_;
    }

    void NLL(Eigen::MatrixXd &data){
        //TODO NOTE THIS IS ONLY FOR REFERENCE BASIS
        NLL_ = 0.0;
        for (int i=0;i<data.rows();i++){
            NLL_ -= log(norm(PSI_.psi(data.row(i))));
            NLL_ += log(Z_);
        }
        NLL_ /= float(data.rows());
    }

    //Compute KL divergence exactly
    void ExactKL(){
        Eigen::VectorXcd rotated_psi(1<<N_);
        //KL in the standard basis
        KL_ = 0.0;
        for(int i=0;i<1<<N_;i++){
            if (norm(target_psi_(i))>0.0){
                KL_ += norm(target_psi_(i))*log(norm(target_psi_(i)));
            }
            KL_ -= norm(target_psi_(i))*log(norm(PSI_.psi(basis_states_.row(i))));
            KL_ += norm(target_psi_(i))*log(Z_);
        }
        if (basis_.compare("std")!=0){
            //KL in the rotated bases
            for (int b=1;b<basisSet_.size();b++){
                rotateRbmWF(basisSet_[b],rotated_psi);
                for(int i=0;i<1<<N_;i++){
                    if (norm(rotated_wf_[b-1](i))>0.0){
                        KL_ += norm(rotated_wf_[b-1](i))*log(norm(rotated_wf_[b-1](i)));
                    }
                    KL_ -= norm(rotated_wf_[b-1](i))*log(norm(rotated_psi(i)));
                    KL_ += norm(rotated_wf_[b-1](i))*log(Z_);
                }
            }
        }
    }

    //Print observer
    void PrintStats(int i){
        std::cout << "Epoch: " << i << "\t";
        std::cout << "KL = " << std::setprecision(10) << KL_ << "\t";
        std::cout << "Overlap = " << std::setprecision(10) << overlap_<< "\t";//<< Fcheck_;
        std::cout << std::endl;
    }

    //Set the value of the target wavefunction
    void setWavefunction(Eigen::VectorXcd & psi){
        target_psi_.resize(1<<N_);
        for(int i=0;i<1<<N_;i++){
            target_psi_(i) = psi(i);
        }
    }

    void setRotatedWavefunctions(std::vector<Eigen::VectorXcd> & psi){
        for(int b=0;b<psi.size();b++){
            rotated_wf_.push_back(psi[b]);
        }
    }
    //Set the value of the target wavefunction
    void setBasisRotations(std::map<std::string,Eigen::MatrixXcd> & U){
        U_ = U;
    }
    void setBasis(std::vector<std::vector<std::string> > basis) {
        basisSet_ = basis;
    }

    void rotateRbmWF(const std::vector<std::string> & basis, Eigen::VectorXcd &psiR){//VectorRbmT & psiR){
        int t,counter;
        std::complex<double> U,Upsi;
        std::bitset<16> bit;
        std::bitset<16> st;
        std::bitset<16> tmp;
        std::vector<int> basisIndex;
        Eigen::VectorXd state(N_);
        Eigen::VectorXd v(N_);

        for(int x=0;x<1<<N_;x++){
            U = 1.0;
            Upsi=0.0;
            basisIndex.clear();
            t = 0;
            st = x;
            for (int j=0;j<N_;j++){
                state(j) = st[N_-1-j];
            }
            for(int j=0;j<N_;j++){
                if (basis[j]!="Z"){
                    t++;
                    basisIndex.push_back(j);
                }
            }
            for(int i=0;i<1<<t;i++){
                counter  =0;
                bit = i;
                v=state;
                for(int j=0;j<N_;j++){
                    if (basis[j] != "Z"){
                        v(j) = bit[counter];
                        counter++;
                    }
                }
                U=1.0;
                for(int ii=0;ii<t;ii++){
                    U = U * U_[basis[basisIndex[ii]]](int(state(basisIndex[ii])),int(v(basisIndex[ii])));
                }
                for(int j=0;j<N_;j++){
                    tmp[j]=v(N_-1-j);
                }
                Upsi += U*PSI_.psi(v);
            }
            psiR(x) = Upsi;
        }
    }
};
}

#endif


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <functional>
#include <iomanip>
#include <string>
#include <cmath>
#include "Tools.hpp"
using namespace std;

/*****************************************************************************

This program uses the classes and a few functions from Tools.hpp to model a
block-spring system in simple harmonic motion (SHM). The program implements
two methods: Euler's method and the Euler-Cromer method. The variables of the
differential equation are stored in a classes named accordingly, along with
accessor methods and a mutator to apply the defined euler step. Since
EulerCromer1D is a subclass of Euler1D, they can be used polymorphically by
allocating memory for them and referencing them by a pointer to Euler1D.

In the applyMethod function, the line with obj->update(dt) applies the update
function of whichever class is supplied, not the one for Euler1D. It returns a
struct of type Path1D (from Tools.hpp) to avoid nasty data types like 2D
vectors or having to return multiple values.

The plotpath function simply takes the Path1D, along with some information
about what's being plotted and how big it is, and sends it to gnuplot.

Finally, the main method creates the objects, applies the method, and plots
them both. The getMaxValue function returns the amplitude of the Euler Method
path (which should be larger) and plots them both methods at the same scale.
Then, the created objects are deleted and the program ends.

*****************************************************************************/

const double x_0 = inputDouble(0, "Initial Position");		// Initial position
const double v_0 = inputDouble(10, "Initial Velocity");		// Initial velocity
const double dt = inputDouble(0.005, "Time Step");			// Time step
const double k = inputDouble(25, "Spring Constant");		// Spring Constant
const double m = inputDouble(1, "Mass");					// Mass
const double om = sqrt(k/m);								// Omega
const double SIM_TIME = inputDouble(50, "Simulation Time");	// Time to simulate

function<double(double,double)> springStep = [](double x, double v){ return -om*om*x; };	// The lambda function used as the differential equation

Path1D applyMethod(Euler1D* obj){	// Function to apply euler method to a pointer of Euler1D, allowing for polymorphism
	Path1D out;						// Define an empty path to put the results in and output

	for(double t = 0; t < SIM_TIME; t += dt){	// Iterate from 0 to SIM_TIME with the defined time step
		out.x.push_back(obj->getX());	// Add the current position to the path
		out.v.push_back(obj->getV());	// Add the current velocity to the path (for phase space)
		out.t.push_back(t);				// Add the current time to the path
		obj->update(dt);				// Apply the update method of obj (either Euler's or Euler-Cromer)
	}

	return out;	// Return the path variable
}

void plotPath(Path1D p, double amp, string title, string fname){	// Used to plot a path, specialized for this program...
	// Accepts input for the path, maximum amplitude (so graphs have the same scale and aren't terrible), the title of the method, and the file name to output
	Gnuplot gp;	// Instance of gnuplot terminal stream

	gp << setprecision(3);
	gp << "set xrange [0:" << SIM_TIME << "]\n";
	gp << "set yrange [" << -amp*1.1 << ":" << amp*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"t (s)\"\n";
	gp << "set ylabel \"x (m)\"\n";
	gp << "set title \"Position Vs. Time in Simple Harmonic Motion\\nUsing " << title << "\"\n";
	gp << "set output \"" << fname << "\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(p.t,p.x));	// Separate the path into components and plot it!
}

void plotPhaseSpace(Path1D p, string title, string fname){
	Gnuplot gp;	// Instance of gnuplot terminal stream
	vector<double> v = getMomentum(p.v, m);

	gp << setprecision(3);
	gp << "set xrange [" << -1.1*getMaxVal(p.x) << ":" << 1.1*getMaxVal(p.x) << "]\n";
	gp << "set yrange [" << -1.1*getMaxVal(v) << ":" << 1.1*getMaxVal(v) << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"x (m)\"\n";
	gp << "set ylabel \"v (m/s)\"\n";
	gp << "set title \"Phase Space Diagram of SHM using " << title << "\"\n";
	gp << "set output \"" << fname << "\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(p.x,v));	// Separate the path into components and plot it!
}

int main(){
	Euler1D* badSpring = new Euler1D(x_0, v_0, springStep);			// Make a pointer to a Euler1D object
	Euler1D* goodSpring = new EulerCromer1D(x_0, v_0, springStep);	// And one to a EulerCromer1D object

	Path1D badPath = applyMethod(badSpring);	// Apply the method for both objects
	Path1D goodPath = applyMethod(goodSpring);

	double bigAmp = getMaxVal(badPath.x);	// Amplitude of the oscillation (max value of pos)
	plotPath(badPath, bigAmp, "Euler's Method", "BadMethod.png");			// Plot them accordingly!
	plotPath(goodPath, bigAmp, "The Euler-Cromer Method", "GoodMethod.png");

	plotPhaseSpace(badPath, "Euler's Method", "BadPSD.png");
	plotPhaseSpace(goodPath, "the Euler-Cromer Method", "GoodPSD.png");

	delete badSpring;	// Garbage collection to free up allocated memory
	delete goodSpring;
	return 0;			// All done!
}


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

// @sect3{Include files}

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

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

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

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

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

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

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

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

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

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

// 最后我们包括MeshWorker框架。这个框架通过其函数loop()和integration_loop()，自动在单元格上进行循环，并将数据组装成向量、矩阵等。它自动服从约束。由于我们必须建立几个矩阵，并且必须注意几组约束，这将使我们省去很多麻烦。

#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/simple.h>
#include <deal.II/meshworker/output.h>
#include <deal.II/meshworker/loop.h>

// 为了节省精力，我们使用了在以下文件中找到的预先实现的拉普拉斯。

#include <deal.II/integrators/laplace.h>
#include <deal.II/integrators/l2.h>

// 这就是C++。

#include <iostream>
#include <fstream>

using namespace dealii;

namespace Step16
{
// @sect3{The integrator on each cell}

//  MeshWorker::integration_loop() 希望有一个类能够提供在单元格和边界及内部面的积分功能。这是由下面的类来完成的。在构造函数中，我们告诉循环应该计算单元格积分（"真"），但不应该计算边界和内部面的积分（两个 "假"）。因此，我们只需要一个单元格函数，而不需要面的函数。

  template <int dim>
  class LaplaceIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    LaplaceIntegrator();
    virtual void cell(MeshWorker::DoFInfo<dim> &        dinfo,
                      MeshWorker::IntegrationInfo<dim> &info) const override;
  };

  template <int dim>
  LaplaceIntegrator<dim>::LaplaceIntegrator()
    : MeshWorker::LocalIntegrator<dim>(true, false, false)
  {}

// 接下来是每个单元上的实际积分器。我们解决一个泊松问题，在右半平面上的系数为1，在左半平面上的系数为十分之一。

//  MeshWorker::LocalResults 的基类 MeshWorker::DoFInfo 包含可以在这个局部积分器中填充的对象。在MeshWorker框架内，有多少对象被创建是由装配器类决定的。在这里，我们举例测试一下，需要一个矩阵 (MeshWorker::LocalResults::n_matrices()).  矩阵是通过 MeshWorker::LocalResults::matrix(), 来访问的，它的第一个参数是矩阵的编号。第二个参数只用于面的积分，当每个测试函数使用两个矩阵时。那么，第二个指标为 "true "的矩阵将以相同的索引存在。

//  MeshWorker::IntegrationInfo 提供了一个或几个FEValues对象，下面这些对象被 LocalIntegrators::Laplace::cell_matrix() 或 LocalIntegrators::L2::L2(). 使用，因为我们只组装一个PDE，所以也只有一个索引为0的对象。

// 此外，我们注意到这个积分器的作用是计算多级预处理的矩阵，以及全局系统的矩阵和右手边。由于系统的汇编器需要一个额外的向量， MeshWorker::LocalResults::n_vectors() 要返回一个非零值。相应地，我们在这个函数的末尾填充了一个右边的向量。由于LocalResults可以处理多个BlockVector对象，但我们这里又是最简单的情况，所以我们将信息输入到零号向量的零号块中。

  template <int dim>
  void
  LaplaceIntegrator<dim>::cell(MeshWorker::DoFInfo<dim> &        dinfo,
                               MeshWorker::IntegrationInfo<dim> &info) const
  {
    AssertDimension(dinfo.n_matrices(), 1);
    const double coefficient = (dinfo.cell->center()(0) > 0.) ? .1 : 1.;

    LocalIntegrators::Laplace::cell_matrix(dinfo.matrix(0, false).matrix,
                                           info.fe_values(0),
                                           coefficient);

    if (dinfo.n_vectors() > 0)
      {
        std::vector<double> rhs(info.fe_values(0).n_quadrature_points, 1.);
        LocalIntegrators::L2::L2(dinfo.vector(0).block(0),
                                 info.fe_values(0),
                                 rhs);
      }
  }
// @sect3{The LaplaceProblem class template}

// 这个主类与  step-6  中的类基本相同。就成员函数而言，唯一增加的是 assemble_multigrid 函数，它组装了对应于中间层离散运算符的矩阵。

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

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

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

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    AffineConstraints<double> constraints;

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

    const unsigned int degree;

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

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

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

    MGLevelObject<SparsityPattern>      mg_sparsity_patterns;
    MGLevelObject<SparseMatrix<double>> mg_matrices;
    MGLevelObject<SparseMatrix<double>> mg_interface_in;
    MGLevelObject<SparseMatrix<double>> mg_interface_out;
    MGConstrainedDoFs                   mg_constrained_dofs;
  };
// @sect3{The LaplaceProblem class implementation}

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

//  @image html limit_level_difference_at_vertices.png ""

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

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

//  @sect4{LaplaceProblem::setup_system}

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

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

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

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);

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

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

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

// 多网格约束必须被初始化。他们也需要知道边界值，所以我们也在这里传递 dirichlet_boundary 。

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

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

    const unsigned int n_levels = triangulation.n_levels();

    mg_interface_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_matrices.resize(0, n_levels - 1);
    mg_matrices.clear_elements();
    mg_sparsity_patterns.resize(0, n_levels - 1);

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

// 值得指出的是，界面矩阵只有位于较粗的网格和较细的网格之间的界面上的自由度条目。因此，它们甚至比我们多网格层次结构中的各个层次的矩阵还要稀少。如果我们更关心内存的使用（可能还有我们使用这些矩阵的速度），我们应该对这两种矩阵使用不同的稀疏性模式。

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

        mg_sparsity_patterns[level].copy_from(dsp);

        mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        mg_interface_in[level].reinit(mg_sparsity_patterns[level]);
        mg_interface_out[level].reinit(mg_sparsity_patterns[level]);
      }
  }
// @sect4{LaplaceProblem::assemble_system}

// 下面的函数将线性系统装配在网格的最细层上。由于我们想在下面的层次装配中重用这里的代码，我们使用本地积分器类LaplaceIntegrator，而将循环留给MeshWorker框架。因此，这个函数首先设置了这个框架所需的对象，即

// - 一个 MeshWorker::IntegrationInfoBox 对象，它将提供单元格上正交点的所有需要的数据。这个对象可以看作是FEValues的扩展，提供更多的有用信息。

// - 一个 MeshWorker::DoFInfo 对象，它一方面扩展了单元格迭代器的功能，另一方面也为其基类LocalResults的返回值提供了空间。

// - 一个汇编器，在这里是指整个系统。这里的 "简单 "指的是全局系统没有一个块状结构。

// - 本地集成器，它实现了实际的形式。

// 在循环将所有这些组合成一个矩阵和一个右手边之后，还有一件事要做：集合器对受限自由度的矩阵行和列不做任何处理。因此，我们在对角线上放一个一，使整个系统摆好。一的值或任何固定的值都有一个好处，即它对矩阵的频谱的影响很容易理解。由于相应的特征向量形成了一个不变的子空间，所选择的值不会影响Krylov空间求解器的收敛性。

  template <int dim>
  void LaplaceProblem<dim>::assemble_system()
  {
    MappingQ1<dim>                      mapping;
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags                         update_flags =
      update_values | update_gradients | update_hessians;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    MeshWorker::Assembler::SystemSimple<SparseMatrix<double>, Vector<double>>
      assembler;
    assembler.initialize(constraints);
    assembler.initialize(system_matrix, system_rhs);

    LaplaceIntegrator<dim> matrix_integrator;
    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_active(),
                                           dof_handler.end(),
                                           dof_info,
                                           info_box,
                                           matrix_integrator,
                                           assembler);

    for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
      if (constraints.is_constrained(i))
        system_matrix.set(i, i, 1.);
  }
// @sect4{LaplaceProblem::assemble_multigrid}

// 下一个函数是建立线性算子（矩阵），定义每一级网格上的多栅方法。积分的核心和上面的一样，但是下面的循环会遍历所有已有的单元，而不仅仅是活动的单元，而且结果必须输入正确的层次矩阵。幸运的是，MeshWorker对我们隐藏了大部分的内容，因此这个函数和之前的函数的区别只在于汇编器的设置和循环中不同的迭代器。另外，最后修复矩阵的过程也比较复杂。

  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid()
  {
    MappingQ1<dim>                      mapping;
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags                         update_flags =
      update_values | update_gradients | update_hessians;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    MeshWorker::Assembler::MGMatrixSimple<SparseMatrix<double>> assembler;
    assembler.initialize(mg_constrained_dofs);
    assembler.initialize(mg_matrices);
    assembler.initialize_interfaces(mg_interface_in, mg_interface_out);

    LaplaceIntegrator<dim> matrix_integrator;
    MeshWorker::integration_loop<dim, dim>(dof_handler.begin_mg(),
                                           dof_handler.end_mg(),
                                           dof_info,
                                           info_box,
                                           matrix_integrator,
                                           assembler);

    const unsigned int nlevels = triangulation.n_levels();
    for (unsigned int level = 0; level < nlevels; ++level)
      {
        for (unsigned int i = 0; i < dof_handler.n_dofs(level); ++i)
          if (mg_constrained_dofs.is_boundary_index(level, i) ||
              mg_constrained_dofs.at_refinement_edge(level, i))
            mg_matrices[level].set(i, i, 1.);
      }
  }

//  @sect4{LaplaceProblem::solve}

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

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

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

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

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

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

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

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

// --这不是对称的

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

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

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

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

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

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

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

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

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

    solution = 0;

    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    constraints.distribute(solution);
  }

//  @sect4{Postprocessing}

// 下面两个函数在计算出解决方案后对其进行后处理。特别是，第一个函数在每个周期开始时细化网格，第二个函数在每个周期结束时输出结果。这些函数与 step-6 中的函数几乎没有变化，只有一个小的区别：我们以VTK格式生成输出，以使用当今更现代的可视化程序，而不是 step-6 编写时的那些。

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

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

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

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

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

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

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

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

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

        setup_system();

        assemble_system();
        assemble_multigrid();

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

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

int main()
{
  try
    {
      using namespace Step16;

      deallog.depth_console(2);

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

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

  return 0;
}


/**
 * \file path/to/include/file.hpp
 *
 * \brief Rotate matrix 90 degrees.
 *
 * Inspired by the \c rot90 MATLAB function.
 * See http://www.mathworks.com/help/techdoc/ref/rot90.html.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP

//#include <boost/numeric/ublasx/detail/temporary.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename MatrixT>
struct rot90_matrix_traits
{
//Not all matrix types have this type (e.g., scalar_matrix<>)
//	typedef typename detail::matrix_temporary_traits<MatrixT>::type result_type;
	typedef matrix<typename matrix_traits<MatrixT>::value_type> result_type;
};

template <typename VectorT>
struct rot90_vector_traits
{
//Not all vector types have this type (e.g., scalar_vector<>)
//	typedef typename detail::vector_temporary_traits<VectorT>::type result_type;
	typedef vector<typename vector_traits<VectorT>::value_type> result_type;
};

} // Namespace detail

/// Rotate the given vector counterclockwise by (a multiple of) 90 degrees.
template <typename VectorT>
typename detail::rot90_vector_traits<VectorT>::result_type rot90(vector_expression<VectorT> const& v, int k=1)
{
	typedef typename detail::rot90_vector_traits<VectorT>::result_type result_type;
	typedef typename vector_traits<VectorT>::size_type size_type;

	size_type n(ublasx::size(v));

	result_type x;

	// Make sure k \in {0, 1, 2, 3}
	k %= 4;
	if (k < 0)
	{
		k += 4;
	}

	// NOTE: uBLAS makes no distinction between row and column vector.
	//       So rotations for k=0 and k=1 are considered identical.
	//       The same applies for rotations for k=2 and k=3.

	if (k == 2 || k == 3)
	{
		x.resize(n, false);

		for (size_type i = 0; i < n; ++i)
		{
			x(n-i-1) = v()(i);
		}
	}
	else
	{
		x = v;
	}

	return x;
}

/// Rotate the given vector counterclockwise by (a multiple of) 90 degrees.
template <typename VectorT>
BOOST_UBLAS_INLINE
void rot90_inplace(vector_container<VectorT>& v, int k=1)
{
	v() = rot90(v, k);
}

/// Rotate the given matrix counterclockwise by (a multiple of) 90 degrees
template <typename MatrixT>
typename detail::rot90_matrix_traits<MatrixT>::result_type rot90(matrix_expression<MatrixT> const& A, int k=1)
{
	typedef typename detail::rot90_matrix_traits<MatrixT>::result_type result_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;

	size_type nr(ublasx::num_rows(A));
	size_type nc(ublasx::num_columns(A));

	result_type X;

	// Make sure k \in {0, 1, 2, 3}
	k %= 4;
	if (k < 0)
	{
		k += 4;
	}

	if (k == 1)
	{
		X.resize(nc, nr, false);

		for (size_type c = 0; c < nc; ++c)
		{
			row(X, nc-c-1) = column(A(), c);
		}
	}
	else if (k == 2)
	{
		X.resize(nr, nc, false);

		for (size_type r = 0; r < nr; ++r)
		{
			for (size_type c = 0; c < nc; ++c)
			{
				X(nr-r-1,nc-c-1) = A()(r,c);
			}
		}
	}
	else if (k == 3)
	{
		X.resize(nc, nr, false);

		for (size_type r = 0; r < nr; ++r)
		{
			column(X, nr-r-1) = row(A(), r);
		}
	}
	else
	{
		X = A;
	}

	return X;
}

/// Rotate the given matrix counterclockwise by (a multiple of) 90 degrees
template <typename MatrixT>
BOOST_UBLAS_INLINE
void rot90_inplace(matrix_container<MatrixT>& A, int k=1)
{
	A() = rot90(A, k);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_ROT90_HPP


#ifndef _COMPLEMENTARY_FILTER_HPP_
#define _COMPLEMENTARY_FILTER_HPP_

#include <cmath>
#include <stdio.h>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <boost/math/special_functions.hpp>

// #define LOCAL_MAG_X 225.1 //miligauss
// #define LOCAL_MAG_Y 5.75
// #define LOCAL_MAG_Z 415.2

constexpr double gravity = 9.80665;
constexpr double local_mx = 208.762; //in miligauss
constexpr double local_my = 3.448;
constexpr double local_mz = 434.129;

using namespace std;

    class ComplementaryFilter{

        // constructor
        public:

        ComplementaryFilter()
            {
                is_init = false;
                data_fused = false;
                filter_updated = false;
                magFieldEarth = Eigen::Vector3d(local_mx,local_my,local_mz);
                mag = Eigen::Vector3d(0,0,0);
                accel = Eigen::Vector3d(0,0,0);
                gyro = Eigen::Vector3d(0,0,0);
            }

        struct parameters
        {
            double k1;
            double k2;
            double k3;
            double k4;
            double kb;
            double sat;
        };

        // Maths Functions
        Eigen::Matrix3d quatToRotationMatrix(Eigen::Vector4d quat);  // transforme quaternion to rotation matrix

        // Filter Methods
        void initFilter();
        void updateSensorData(Eigen::Vector3d gyro, Eigen::Vector3d accel, Eigen::Vector3d mag);
        void fuseInertialData();
        void updateFilter();

        private:

        // filter variables;
        double dt;
        Eigen::Vector3d magFieldEarth;
        Eigen::Matrix3d rotationMatrix;
        Eigen::Vector3d mag;
        Eigen::Vector3d accel;
        Eigen::Vector3d gyro;
        Eigen::Vector3d bias;
        Eigen::Vector3d sigR;
        Eigen::Vector3d sigB;
        Eigen::Vector3d gyro_estimate;
        Eigen::Quaterniond quaternion;
        Eigen::Matrix3d rotation_matrix;

        // filter flags
        bool is_init;
        bool data_fused;
        bool filter_updated;

        // filter parameters
        parameters params;

    };

#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "videoray/Throttle.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Float32.h"

#include <iostream>
#include <sstream>

#include <boost/numeric/odeint.hpp>

using std::cout;
using std::endl;

using namespace boost::numeric::odeint;

typedef boost::array< double , 12 > state_type;

#define PI (3.14159265359)

nav_msgs::Odometry odom_;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
     odom_ = *msg;
}

state_type x_ = {0,0,0,0,0,0,0,0,0,0,0,0};

// Linear and angular velocity states
double u;
double v;
double w;
double p;
double q;
double r;

// Added Mass Terms
double X_udot = 1.94; // inertia matrix M (m11)
double Y_vdot = 6.05; // inertia matrix M (m22)
double Z_wdot = 3.95; // m33
double N_rdot = 0.1;
//double N_rdot = 1.18e-2; // vehicle's motion of inertia about z-axis
// (6,6) entry of the vehicle inertia Matrix M

// Linear Drag Coefficients
double Xu = -0.95;
double Yv = -5.87;
double Nr = -0.023;
double Zw = -3.70;

// Quadratic Drag Coefficients
double Xuu = -6.04;
double Yvv = -30.73;
double Nrr = -0.45;
double Zww = -26.36;

double Ct_forw = 0.026667;
double Ct_back = 0.026667;
double Ct_vert_forw = 0.026667;
double Ct_vert_back = 0.026667;

double u_sat_low = -150;
double u_sat_high = 150;

// Control inputs
double X = 0;
double N = 0;
double Z = 0;

double u_port = 0;
double u_star = 0;
double u_vert = 0;

double xpos  = 0;
double ypos  = 0;
double zpos  = 0;
double phi   = 0;
double theta = 0;
double psi   = 0;

double c1 = 0;
double c2 = 0;
double c3 = 0;
double s1 = 0;
double s2 = 0;
double s3 = 0;
double t2 = 0;

videoray::Throttle throttle_;

void videoray_model( const state_type &x , state_type &dxdt , double t )
{
/// States:
/// 0:  u     : surge velocity
/// 1:  v     : sway velocity
/// 2:  w     : heave velocity
/// 3:  p     : roll rate
/// 4:  q     : pitch rate
/// 5:  r     : yaw rate
/// 6:  xpos  : earth x-pos
/// 7:  ypos  : earth y-pos
/// 8:  zpos  : earth z-pos
/// 9:  phi   : roll angle
/// 10: theta : pitch angle
/// 11: psi   : yaw angle
     u = x[0];
     v = x[1];
     w = x[2];
     p = x[3];
     q = x[4];
     r = x[5];
     xpos  = x[6];
     ypos  = x[7];
     zpos  = x[8];
     phi   = x[9];
     theta = x[10];
     psi   = x[11];

     // Calculate fixed frame velocity rates
     dxdt[0] = (-Y_vdot*v*r + Xu*u + Xuu*u*abs(u) + X) / X_udot;
     dxdt[1] = (X_udot*u*r + Yv*v + Yvv*v*abs(v)) / Y_vdot;
     dxdt[2] = (Zw*w + Zww*w*abs(w) + Z) / Z_wdot;

     // Calculate fixed frame orientation rates
     dxdt[3] = 0;
     dxdt[4] = 0;
     dxdt[5] = (Nr*r + Nrr*r*abs(r) + N) / N_rdot;

     c1 = cos(phi);
     c2 = cos(theta);
     c3 = cos(psi);
     s1 = sin(phi);
     s2 = sin(theta);
     s3 = sin(psi);
     t2 = tan(theta);

     // Calculate inertial frame position
     dxdt[6] = c3*c2*u + (c3*s2*s1-s3*c1)*v + (s3*s1+c3*c1*s2)*w;
     dxdt[7] = s3*c2*u + (c1*c3+s1*s2*s3)*v + (c1*s2*s3-c3*s1)*w;
     dxdt[8] = -s2*u + c2*s1*v + c1*c2*w;

     // Calculate inertial frame orientations
     dxdt[9] = p + (q*s1 + r*c1)*t2;
     dxdt[10] = q*c1 - r*s1;
     dxdt[11] = (q*s1 + r*c1)* (1 / cos(theta));
}

//
// Converts input throttle commands to simulated linear and angular velocities.
//
geometry_msgs::Twist velocity_cmd_;
geometry_msgs::Vector3 velocity_linear_;
geometry_msgs::Vector3 velocity_angular_;

double saturate(double input, const double &min, const double &max)
{
     if (min > max) {
          ROS_INFO("saturate(): Invalid Min / Max Combo");
          return 0;
     } else if (input < min) {
          input = min;
     } else if(input > max) {
          input = max;
     }
     return input;
}

// Assumes that input has already been saturated within the in_min and in_max
// boundaries. Use the saturate() function on input before calling normalize
double normalize(double input, const double &in_min, const double &in_max,
                 const double &out_min, const double &out_max)
{
     if (in_min >= in_max || out_min >= out_max) {
          ROS_INFO("normalize(): Invalid Min / Max Combo");
          return 0;
     }

     double ratio = input / (in_max - in_min);
     return ratio * (out_max - out_min);

     return input;
}

double thrust_port = 0, thrust_star = 0;
void processThrottleCmds()
{
     u_port = saturate(throttle_.PortInput, u_sat_low, u_sat_high);
     u_star = saturate(throttle_.StarInput, u_sat_low, u_sat_high);
     u_vert = saturate(throttle_.VertInput, u_sat_low, u_sat_high);

     // Ct is different for reverse and forward
     if ( u_port >= 0 ) {
          thrust_port = u_port * Ct_forw;
     } else {
          thrust_port = u_port * Ct_back;
     }

     if ( u_star >= 0 ) {
          thrust_star = u_star * Ct_forw;
     } else {
          thrust_star = u_star * Ct_back;
     }

     X = thrust_port + thrust_star;
     N = thrust_star - thrust_port;

     // Ct is different for reverse and forward
     if ( u_vert >= 0 ) {
          Z = u_vert * Ct_vert_forw;
     } else {
          Z = u_vert * Ct_vert_back;
     }

}

double normDegrees(double input)
{
     if (input < 0) {
          input += 360;
     } else if(input >= 360) {
          input -= 360;
     }
     return input;
}

void quaternionToEuler(const double &q0, const double &q1,
                       const double &q2, const double &q3,
                       double &roll, double &pitch, double &yaw)
{
     roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) );
     pitch = asin(2*(q0*q2-q3*q1));
     yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) );
}

void eulerToQuaternion(const double &roll, const double &pitch,
                       const double &yaw,
                       double &q0, double &q1,
                       double &q2, double &q3)
{
     q0 = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);
     q1 = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2);
     q2 = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
     q3 = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);
}

double depth_ref = 0;
double speed_ref = 0;
double heading_ref = 0;

void desiredVelocityCallback(const std_msgs::Float32::ConstPtr& msg)
{
     speed_ref = msg->data;
}

void desiredHeadingCallback(const std_msgs::Float32::ConstPtr& msg)
{
     heading_ref = normDegrees(msg->data - 90);
     //heading_ref = msg->data;
}

void desiredDepthCallback(const std_msgs::Float32::ConstPtr& msg)
{
     depth_ref = msg->data;
}

//geometry_msgs::Quaternion quat_;
double depth_err = 0;
double speed_err = 0;
double heading_err = 0;
double heading = 0;
double heading_port, heading_star, speed_port, speed_star;
double heading_weight = 0.5;
double speed_weight = 0.5;
//double K_heading = 0.25;
double K_heading = 0.01;
double K_speed = 10;
double K_depth = 50;
double roll_ = 0;
double pitch_ = 0;
double yaw_ = 0;

void execControlLaw()
{
     //quat_ = odom_.pose.pose.orientation;
     //quaternionToEuler(quat_.w, quat_.x, quat_.y, quat_.z,
     //                  roll_, pitch_, yaw_);

     //depth_err = depth_ref - odom_.pose.pose.position.z;
     //speed_err = speed_ref - odom_.twist.twist.linear.x;

     roll_ = x_[9];
     pitch_ = x_[10];
     yaw_ = x_[11];

     depth_err = depth_ref - x_[8];
     speed_err = speed_ref - x_[0];

     heading = normDegrees(yaw_*180/PI);
     heading_err = heading_ref - heading;

     if (abs(heading_err) < 180) {
          heading_port = -K_heading*heading_err;
          heading_star = K_heading*heading_err;
     } else  {
          heading_port = K_heading*heading_err;
          heading_star = -K_heading*heading_err;
     }

     speed_port = K_speed*speed_err;
     speed_star = K_speed*speed_err;

     throttle_.PortInput = heading_weight*heading_port + speed_weight*speed_port;
     throttle_.StarInput = heading_weight*heading_star + speed_weight*speed_star;
     throttle_.VertInput = K_depth*depth_err;

     //throttle_.PortInput = 100;
     //throttle_.StarInput = 95;
     //throttle_.VertInput = 0;
}

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

     //ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("motion", 1);
     //ros::Subscriber odom_sub = n.subscribe("odometry", 1,
     //                                       odomCallback);

     ros::Publisher pose_pub = n.advertise<geometry_msgs::Pose>("motion",1);
     geometry_msgs::Pose pose_;

     ros::Publisher pub_nav_x = n.advertise<std_msgs::Float32>("NAV_X",1);
     ros::Publisher pub_nav_y = n.advertise<std_msgs::Float32>("NAV_Y",1);
     ros::Publisher pub_nav_depth = n.advertise<std_msgs::Float32>("NAV_DEPTH",1);
     ros::Publisher pub_nav_heading = n.advertise<std_msgs::Float32>("NAV_HEADING",1);
     ros::Publisher pub_nav_speed = n.advertise<std_msgs::Float32>("NAV_SPEED",1);

     std_msgs::Float32 nav_x, nav_y, nav_depth, nav_heading, nav_speed;

     ros::Subscriber desired_vel_sub = n.subscribe("desired_velocity",
                                                   1,
                                                   desiredVelocityCallback);

     ros::Subscriber desired_head_sub = n.subscribe("desired_heading",
                                                    1,
                                                    desiredHeadingCallback);

     ros::Subscriber desired_depth_sub = n.subscribe("desired_depth",
                                                     1,
                                                     desiredDepthCallback);

     //double rate = 30;
     double rate = 10;
     ros::Rate loop_rate(rate);

     ros::Time begin = ros::Time::now();
     ros::Time curr_time = begin;
     ros::Time prev_time = begin;
     ros::Duration dt = curr_time - prev_time;

     geometry_msgs::Quaternion quat;

     runge_kutta4< state_type > stepper;
     //runge_kutta_dopri5< state_type > stepper;
     //adams_bashforth_moulton< 2 , state_type > stepper;

     while (ros::ok())
     {
          //cout << "*" << std::flush;

          curr_time = ros::Time::now();
          dt = curr_time - prev_time;
          prev_time = curr_time;

          ROS_INFO("dt: %f\n", dt.toSec());

          //cout << dt.toSec() << endl << std::flush;

          // Update state vector with odometry data from morse...
          //state_type x = {0,0,0,0,0,0,0,0,0,0,0,0};
          //x[0] = odom_.twist.twist.linear.x;
          //x[1] = odom_.twist.twist.linear.y;
          //x[2] = odom_.twist.twist.linear.z;
          //x[3] = odom_.twist.twist.angular.x;
          //x[4] = odom_.twist.twist.angular.y;
          //x[5] = odom_.twist.twist.angular.z;

          execControlLaw();

          processThrottleCmds();

          //geometry_msgs::Quaternion quat = odom_.pose.pose.orientation;
          //quaternionToEuler(quat.x, quat.y, quat.z, quat.w,
          //                  roll_, pitch_, yaw_);

          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x_,
          //                                  curr_time.toSec() ,
          //                                  (curr_time + dt).toSec(),
          //                                  dt.toSec());
          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x_,
          //                                  curr_time.toSec() ,
          //                                  curr_time.toSec() + 1.0/rate,
          //                                  1.0/rate);

          stepper.do_step(videoray_model, x_ , curr_time.toSec() , dt.toSec() );

          ROS_INFO("========================");
          ROS_INFO("Current: %f, \tdt: %f", curr_time.toSec(), dt.toSec());
          ROS_INFO("Surge: %f", x_[0]);
          ROS_INFO("Sway: %f", x_[1]);
          ROS_INFO("Heave: %f", x_[2]);

          ROS_INFO("3: %f", x_[3]);
          ROS_INFO("4: %f", x_[4]);
          ROS_INFO("5: %f", x_[5]);
          ROS_INFO("6: %f", x_[6]);
          ROS_INFO("7: %f", x_[7]);
          ROS_INFO("8: %f", x_[8]);
          ROS_INFO("9: %f", x_[9]);
          ROS_INFO("10: %f", x_[10]);
          ROS_INFO("11: %f", x_[11]);

          //velocity_linear_.x = x[0];
          //velocity_linear_.y = x[1];
          //velocity_linear_.z = x[2];
          //velocity_angular_.z = x[5];
          //
          //velocity_cmd_.linear = velocity_linear_;
          //velocity_cmd_.angular = velocity_angular_;
          //twist_pub.publish(velocity_cmd_);

          //quaternionToEuler(quat.w, quat.x, quat.y, quat.z,
          //                  roll_, pitch_, yaw_);
          //
          //eulerToQuaternion(x_[9],
          //                  x_[10],
          //                  x_[11],
          //                  quat.w, quat.x, quat.y, quat.x);

          //quat.w = 0.1; quat.x = 0.2; quat.y = 0.3 ; quat.z = 0.4;

          //quaternionToEuler(quat.w, quat.x, quat.y, quat.z,
          //                  roll_, pitch_, yaw_);

          roll_  = x_[9];
          pitch_ = x_[10];
          yaw_   = x_[11];

          eulerToQuaternion(roll_,
                            pitch_,
                            yaw_,
                            quat.w, quat.x, quat.y, quat.z);

          pose_.position.x = x_[6];
          pose_.position.y = x_[7];
          pose_.position.z = x_[8];
          pose_.orientation.x = quat.x;
          pose_.orientation.y = quat.y;
          pose_.orientation.z = quat.z;
          pose_.orientation.w = quat.w;

          pose_pub.publish(pose_);

          nav_x.data = x_[6];
          nav_y.data = x_[7];
          nav_depth.data = x_[8];
          nav_speed.data = x_[0];
          nav_heading.data = normDegrees(x_[11]*180.0/PI + 90);

          pub_nav_x.publish(nav_x);
          pub_nav_y.publish(nav_y);
          pub_nav_depth.publish(nav_depth);
          pub_nav_heading.publish(nav_heading);
          pub_nav_speed.publish(nav_speed);

          ros::spinOnce();

          loop_rate.sleep();
     }
     return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SQRT1PM1_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SQRT1PM1_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt1pm1_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      A0 tmp =  bs::sqrt(inc(a0));
      return  ((bs::abs(a0) < bs::Half<A0>())? a0/bs::inc(tmp) : bs::dec(tmp));
    }
  };
} } }

#endif


// -------------------------------------------------------------------
//
// basho_metrics: fast performance metrics for Erlang.
//
// inspired and partially derived from Coda Hale's 'metrics'
// Copyright (c) 2010-2001 Coda Hale
// https://github.com/codahale/metrics/blob/development/LICENSE.md
//
// Copyright (c) 2011 Basho Technologies, Inc. All Rights Reserved.
//
// This file is provided to you 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.
//
// -------------------------------------------------------------------
#ifndef SAMPLE_HPP_
#define SAMPLE_HPP_

#include <map>
#include <vector>
#include <cmath>
#include <ctime>
#include <cstdlib>
#include <stdint.h>
#include <sys/time.h>
#include <boost/circular_buffer.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

/**
 * An exponentially-decaying random sample of {@code long}s. Uses Cormode et
 * al's forward-decaying priority reservoir sampling method to produce a
 * statistically representative sample, exponentially biased towards newer
 * entries.
 *
 * @see
 * Cormode et al. Forward Decay: A Practical Time Decay Model for Streaming
 * Systems. ICDE '09: Proceedings of the 2009 IEEE International Conference on
 * Data Engineering (2009)
 */
template <typename IntType=unsigned long>
struct exponentially_decaying_sample
{
    exponentially_decaying_sample(std::size_t size, double alpha=0.015)
        : size_(size),
          alpha_(alpha),
          count_(0),
          start_time_(tick()),
          next_scale_time_(start_time_ + RESCALE_THRESHOLD)
    {
    }

    /**
     * Clears all recorded values.
     */
    void clear()
    {
        values_.clear();
        count_ = 0;
        start_time_ = tick();
        next_scale_time_ = start_time_ + RESCALE_THRESHOLD;
    }

   /**
     * Returns the number of values recorded.
     */
    std::size_t size() const
    {
        return std::min(size_, count_);
    }

    double weight(long t) const
    {
        return std::exp(alpha_ * t);
    }

    /**
     * Adds a new recorded value to the sample.
     */
    void update(IntType value)
    {
        update(value, tick());
    }

    void update(IntType value, long timestamp)
    {
        double priority = weight(timestamp - start_time_) / next_random();
        if (++count_ <= size_)
            values_[priority] = value;
        else
        {
            double first = values_.begin()->first;
            if (first < priority)
            {
                if (values_.find(priority) == values_.end() )
                    values_[priority] = value;
                values_.erase(values_.begin());
            }
        }
        long now = tick();
        if (now > next_scale_time_)
            rescale(now, next_scale_time_);
    }

    /* "A common feature of the above techniques—indeed, the key technique
     * that allows us to track the decayed weights efficiently—is that they
     * maintain counts and other quantities based on g(ti − L), and only scale
     * by g(t − L) at query time. But while g(ti −L)/g(t−L) is guaranteed to
     * lie between zero and one, the intermediate values of g(ti − L) could
     * become very large. For polynomial functions, these values should not
     * grow too large, and should be effectively represented in practice by
     * floating point values without loss of precision. For exponential
     * functions, these values could grow quite large as new values of (ti − L)
     * become large, and potentially exceed the capacity of common floating
     * point types. However, since the values stored by the algorithms are
     * linear combinations of g values (scaled sums), they can be rescaled
     * relative to a new landmark. That is, by the analysis of exponential
     * decay in Section III-A, the choice of L does not affect the final
     * result. We can therefore multiply each value based on L by a factor of
     * exp(−α(L′ − L)), and obtain the correct value as if we had instead
     * computed relative to a new landmark L′ (and then use this new L′ at
     * query time). This can be done with a linear pass over whatever data
     * structure is being used."
     */
    void rescale(long now, long next)
    {
        next_scale_time_ = now + RESCALE_THRESHOLD;
        long old_start_time = start_time_;
        start_time_ = tick();
        std::map<double, IntType> new_values;
        for (typename std::map<double, IntType>::const_iterator
             it=values_.begin();
             it != values_.end();
             ++it)
        {
            IntType value = it->second;
            new_values[it->first * std::exp(-alpha_ * (start_time_-old_start_time))] = value;
        }
        values_.swap(new_values);
    }

    /**
     * Returns a copy of the sample's values.
     */
    std::vector<IntType> values() const
    {
        std::vector<IntType> v;
        for (typename std::map<double, IntType>::const_iterator
                 it=values_.begin();
             it != values_.end();
             ++it)
        {
            v.push_back(it->second);
        }
        return v;
    }

private:
    long tick() const
    {
        return time(NULL);
    }

   double next_random()
   {
       return dist_(gen_) / static_cast<double>(std::numeric_limits<IntType>::max());
   }
private:
    std::size_t size_;
    double alpha_;
    std::size_t count_;
    long start_time_;
    long next_scale_time_;
    std::map<double, IntType> values_;
    boost::random::uniform_int_distribution<IntType> dist_;
    boost::random::mt19937 gen_;
    const static long RESCALE_THRESHOLD = 60;
};

/**
 * Sliding sample of a stream of {@code long}s. Operates on fixed-time window,
 * expired points are simply dropped.
 */
template <typename IntType=unsigned long>
struct sliding_sample
{
    sliding_sample(std::size_t size, std::size_t width_in_ms)
        : size_(size),
          width_(width_in_ms * 1000),
          ticks_(size),
          values_(size)
    {
    }

    typedef uint64_t tick_t;

public:
    void clear()
    {
        ticks_.clear();
        values_.clear();
    }

    std::size_t size() const
    {
        return values_.size();
    }

    void update(IntType value)
    {
        tick_t ts = tick();
        cut(ts);
        values_.push_back(value);
        ticks_.push_back(ts);
    }

    std::vector<IntType> values() const
    {
        std::size_t expired = std::min(expired_before(tick()), size() - 1);
        return std::vector<IntType>(values_.begin() + expired, values_.end());
    }

private:
    tick_t tick() const
    {
        timeval tv;
        if (0 == gettimeofday(&tv, 0)) {
            return uint64_t(tv.tv_sec) * 1000000 + tv.tv_usec;
        }
        return 0;
    }

    void cut(tick_t ts)
    {
        std::size_t cut = expired_before(ts);
        if (cut) {
            values_.erase(values_.begin(), values_.begin() + cut);
            ticks_.erase(ticks_.begin(), ticks_.begin() + cut);
        }
    }

    std::size_t expired_before(tick_t ts) const {
        tick_t cutoff = ts - width_;
        tick_buffer::const_iterator it = ticks_.begin(), end = ticks_.end();
        while (it != end && *it < cutoff) {
            ++it;
        }
        return it - ticks_.begin();
    }

private:
    std::size_t size_;
    tick_t width_;
    typedef boost::circular_buffer<tick_t> tick_buffer;
    tick_buffer ticks_;
    boost::circular_buffer<IntType> values_;

};

/**
 * A random sample of a stream of {@code long}s. Uses Vitter's Algorithm R to
 * produce a statistically representative sample.
 *
 * @see Random Sampling
 *      with a Reservoir
 */
template <typename IntType=unsigned long>
struct uniform_sample
{
    uniform_sample(std::size_t reservoir_size)
        : size_(reservoir_size),
          count_(0),
          values_(reservoir_size, 0)
    {
    }
public:
    void clear()
    {
        std::fill_n(values_.begin(), size_, 0);
    }

    std::size_t size() const
    {
        return std::min(count_, size_);
    }

    void update(IntType value)
    {
        std::size_t c = ++count_;
        if (c <= size_) { values_[c-1] = value; }
        else
        {
            std::size_t r = next_random() % c;
            if (r < size_) { values_[r] = value; }
        }
    }

    std::vector<IntType> values() const
    {
        return std::vector<IntType>(values_.begin(), values_.begin()+size());
    }

private:
    IntType next_random()
    {
        return dist_(gen_);
    }

private:
    std::size_t size_;
    std::size_t count_;
    std::vector<IntType> values_;
    boost::random::uniform_int_distribution<IntType> dist_;
    boost::random::mt19937 gen_;
};

#endif // include guard


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/Dual.h>
#include <CGAL/boost/graph/helpers.h>

#include <iostream>
#include <fstream>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/connected_components.hpp>

typedef CGAL::Simple_cartesian<double>             Kernel;
typedef Kernel::Point_3                            Point;
typedef CGAL::Surface_mesh<Point>                  Mesh;
typedef CGAL::Dual<Mesh>                           Dual;
typedef boost::graph_traits<Dual>::edge_descriptor edge_descriptor;

template <typename G>
struct noborder {
  noborder() : g(nullptr) {} // default-constructor required by filtered_graph
  noborder(G& g) : g(&g) {}

  bool operator()(const edge_descriptor& e) const
  { return !is_border(e,*g); }

  G* g;
};

// A dual border edge has a null_face as the source or target "vertex"
// BGL algorithms won't like that, so we remove border edges through a
// boost::filtered_graph.
typedef boost::filtered_graph<Dual, noborder<Mesh> >   FiniteDual;
typedef boost::graph_traits<Mesh>::vertex_descriptor   vertex_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor     face_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/prim.off";

  Mesh primal;
  if(!CGAL::IO::read_polygon_mesh(filename, primal))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  Dual dual(primal);
  FiniteDual finite_dual(dual,noborder<Mesh>(primal));

  std::cout << "dual has " << num_vertices(dual) << " vertices" << std::endl;

  std::cout << "The vertices of dual are faces in primal"<< std::endl;
  for(boost::graph_traits<Dual>::vertex_descriptor dvd : vertices(dual)) {
    std::cout << dvd << std::endl;
  }

  std::cout << "The edges in primal and dual with source and target" << std::endl;
  for(edge_descriptor e : edges(dual)) {
   std::cout << e << " in primal:  " << source(e,primal)      << " -- " << target(e,primal)       << "   "
             <<      " in dual  :  " << source(e,finite_dual) << " -- " << target(e,finite_dual)  << std::endl;
  }

 std::cout << "edges of the finite dual graph" << std::endl;
 for(boost::graph_traits<FiniteDual>::edge_descriptor e : CGAL::make_range(edges(finite_dual))) {
   std::cout << e << "  " << source(e,primal) << " " << source(e,finite_dual)  << std::endl;
 }

 // the storage of a property map is in primal
 Mesh::Property_map<face_descriptor,int> fccmap;
 fccmap = primal.add_property_map<face_descriptor,int>("f:CC").first;
 int num = connected_components(finite_dual, fccmap);

 std::cout << "The graph has " << num << " connected components (face connectivity)" << std::endl;
 for(face_descriptor f : faces(primal)) {
   std::cout << f << " in connected component " << fccmap[f] << std::endl;
 }

 Mesh::Property_map<vertex_descriptor,int> vccmap;
 vccmap = primal.add_property_map<vertex_descriptor,int>("v:CC").first;
 num = connected_components(primal, vccmap);

 std::cout << "The graph has " << num << " connected components (edge connectvity)" << std::endl;
 for(vertex_descriptor v : vertices(primal)) {
   std::cout << v << " in connected component " << vccmap[v] << std::endl;
 }
  return 0;
}


#pragma once
#include <vector>
#include <Eigen/Dense>

template <typename T>
struct PoseBase {
    using Vector2 = Eigen::Matrix<T, 2, 1>;
    using Vector3 = Eigen::Matrix<T, 3, 1>;
    using Matrix2 = Eigen::Matrix<T, 2, 2>;
    using Matrix3 = Eigen::Matrix<T, 3, 3>;
    T x;
    T y;
    T theta;
    PoseBase() : x(0), y(0), theta(0) {}
    // loop: using while loop for normalizing angle to (-pi, pi]
    PoseBase(T x, T y, T theta, bool loop = false): x(x), y(y) {
        if (loop == false)
            this->theta = goodAngle(theta);
        else
            this->theta = loopNormalize(theta);
    }
    PoseBase(const PoseBase& p): x(p.x), y(p.y), theta(p.theta) {}
    PoseBase(Vector3 p, bool loop = false) : x(p(0)), y(p(1)) {
        if (loop == false)
            this->theta = goodAngle(p(2));
        else
            this->theta = loopNormalize(p(2));
    }
    PoseBase(Vector2 p, T theta):
        x(p(0)), y(p(1)), theta(theta) {}
    PoseBase(const Matrix2& R, const Vector2& t):
        x(t.x()), y(t.y()), theta(atan2(R(1, 0), R(0, 0))) {}
    PoseBase(const Matrix3& M):
        x(M(0, 2)), y(M(1, 2)),
        theta(atan2(M(1, 0), M(0, 0))) {}
    Vector3 eigen() const {
        return Vector3(x, y, theta);
    }
    Vector3 weighted_eigen(T theta_weight) const {
        return Vector3(x, y, theta * theta_weight);
    }
    T norm2d() const {
        return std::sqrt(x * x + y * y);
    }
    void operator+=(Vector3 p) {
        x += p(0);
        y += p(1);
        theta = goodAngle(theta + p(2));
    }
    void operator-=(Vector3 p) {
        x -= p(0);
        y -= p(1);
        theta = goodAngle(theta - p(2));
    }
    PoseBase operator-(const PoseBase& p) const {
        return PoseBase(x - p.x, y - p.y, goodAngle(theta - p.theta));
    }
    PoseBase operator*(const PoseBase& p) const {
        return PoseBase(rotation() * p.translation() + translation(), goodAngle(theta + p.theta));
    }
    Vector2 translation() const {
        return Vector2(x, y);
    }
    Matrix2 rotation() const {
        Matrix2 rotate_mat;
        const T cosa = cos(theta), sina = sin(theta);
        rotate_mat << cosa, -sina, sina, cosa;
        return rotate_mat;
    }
    PoseBase inverse() const {
        T cos_theta = cos(theta);
        T sin_theta = sin(theta);
        T trans_x = x * cos_theta + y * sin_theta;
        T trans_y = x * sin_theta - y * cos_theta;
        return PoseBase(-trans_x, trans_y, -theta);
    }
    Matrix3 transform() const {
        const T cosa = cos(theta), sina = sin(theta);
        const Matrix3 res = {cosa, -sina, x, sina, cosa, y, 0, 0, 1};
        return res;
    }
    bool isnan() const {
        return (std::isnan(x) || std::isnan(y) || std::isnan(theta));
    }
    static PoseBase Identity() {
        return PoseBase(0, 0, 0);
    }
    static T goodAngle(T angle) {
        if (angle > M_PI)
            angle -= 2 * M_PI;
        if (angle < -M_PI)
            angle += 2 * M_PI;
        return angle;
    }
    static T loopNormalize(T angle) {
        while (angle > M_PI)
            angle -= 2 * M_PI;
        while (angle < -M_PI)
            angle += 2 * M_PI;
        return angle;
    }
};

typedef PoseBase<double> Pose;


#include "IKinematics.hpp"

#include <cmath>

#include <Eigen/Eigen>

using namespace Eigen;

static float crop(float angle)
{
   while (angle <= -M_PI) angle += 2*M_PI;
   while (angle >   M_PI) angle -= 2*M_PI;

   return angle;
}

namespace IKinematics
{

/**
 * (Very) luckily for us, the nao's leg joint chain can be separated out
 * into reasonably simple independent chains. For simplicity, we assume that
 * the chain link from the ankle to the ground in negligible.
 *
 * Define the following:
 *   x:      axis extending forwards from the robot's torso (sagittal plane)
 *   y:      axis extending rightwards of the robot's torso (coronal plane )
 *   z:      axis extending upwards of the robot's torso
 *   rot:    rotation of the foot relative to the x axis, 0 is forward
 *
 * When rot is 0, the x and y chains are easily separable as only the
 * thigh roll can affects the y axis. Therefore
 *
 *    thighRoll = atan2(z, y)
 *
 * Two joints affect the x axis, namely thighPitch and kneePitch. Note
 * however that the chain has been rotated however, and the new target z'
 * is now the hypotenuse of the triangle created by target z y.
 *
 *    z' = cos(thighPitch + kneePitch)thighL + cos(kneePitch)shinL
 *    x  = sin(thighPitch + kneePitch)thighL + sin(kneePitch)shinL
 *
 * It's pretty obvious that there is a periodic solution to the above
 * equations. However, it is a hell of a lot easier just to use the
 * law of cosines to get the angles.
 *
 *  |\        -     |  We know `thigh', `shin' and the third length of
 *  |A\ thigh |     |  the triangle is simply hypot(z', x). Villa, solve
 *   | \      | z'  |  for B and you're done.
 *   | B\     |     |
 *    |C/     |     |  Note that thigh pitch is not the same as A. In
 *    |/ shin -     |  actual fact, it is a lot easier to calculate
 *                  |  thigh pitch by simply substituting the value of
 *  |-|             |  knee pitch into the above equations.
 *    x             |
 *
 * Relating the above logic back to the original equation, we note that
 * kneePos = pi + B. A rather nice result is that B is always positive
 * assuming we take the positive arc-cosine. We do not want to consider
 * the negative case because this would imply the knee is bending backwards.
 *
 * For rotation, note that the only way to rotate the foot in the lateral
 * plane is to alter the hip pitch (the joint is diagonal). Rotating the
 * chain causes the x and y chains to now depend on each other, as the pitch
 * and roll of each joint now affect both x and y. Easy solution, transform
 * the target using the rotation matrix of the hip. Now the x and y axes are
 * independent again.
 *
 * Now to deal with the negligible foot joint, it turns out its actually pretty
 * easy to add to the chain.
 *
 *    let a = thighPitch
 *        b = kneePitch
 *        c = anklePitch
 *
 *    0  = a + b + c
 *    z' = cos(a + b + c)footL + cos(a + b)shinL + cos(a)thighL
 *    x  = sin(a + b + c)footL + sin(a + b)shinL + sin(a)thighL
 *
 * cos(a + b + c) is a constant, so we can simply remove it both sides of
 * the equation and solve using the same triangle as above. Too easy. In
 * the same manner, we can add the ankle roll to the chain to reach our
 * y-target.
 */

NaoLegChain NaoSolve(const NaoFootTarget &target, float left)
{
   const float thighL = 0.10000;
   const float shinL  = 0.10290;
   const float footL  = 0.04519;

   NaoLegChain r;
   float x, y, z;

   /* For now assume hip yaw is exactly equal to foot yaw.
    * Fuck my life http://en.wikipedia.org/wiki/Universal_joint
    * Well, its actually reasonably linear:
    * http://www.wolframalpha.com/input/?i=plot+atan(tan(x)/cos(pi/4)))
    *
    * This is an expensive operation so dont do it unless we actually need to.
    */
   r.hip = target.yaw;
   if (target.yaw == 0) {
      x = target.x;
      y = target.y;
      z = target.z;
   } else {
      Vector3f v(target.x, target.y, target.z);
      AngleAxis<float> aa(-target.yaw, Vector3f(0, sqrtf(2)/2, sqrtf(2)/2));
      v = aa * v;
      x = v.x(); y = v.y(); z = v.z();
   }

   /* Subtract foot length from target. This can be seen as the vector
    *    target' = target - Rot_roll * Rot_pitch * foot
    *
    * The fact that pitch is applied before roll is due to the robot's
    * physical design.
    */

   x -= footL * sinf(target.pitch);
   y -= footL * cosf(target.pitch) * sinf(target.roll);
   z += footL * cosf(target.pitch) * cosf(target.roll);

   r.thighRoll = atan2f(y, -z);

   /* Solve for target.x and target.z */
   const float z_prime = z / cosf(r.thighRoll);
   const float h = hypotf(z_prime, x);

   /* A B and C are as above comment */
   const float A = acosf((shinL*shinL - h*h - thighL*thighL) / (-2*thighL*h));
   const float B = acosf((h*h - thighL*thighL - shinL*shinL) / (-2*thighL*shinL));
   r.thighPitch = A + asin(x/h) + 3*M_PI_2;
   r.kneePitch  = B + M_PI;

   /* Calculate the ankle pitch and roll */
   r.anklePitch = target.pitch - r.kneePitch - r.thighPitch;
   r.ankleRoll  = target.roll - r.thighRoll;

   /* Currently all angles are wound counter clockwise from the normal.
    * Joints on the actual robot each have their own normals. Basically,
    * when all joint values are set to zero the robot is standing upright.
    */
   r.thighPitch = 3*M_PI_2 - r.thighPitch;
   r.kneePitch  =          - r.kneePitch;
   r.anklePitch = 1*M_PI_2 - r.anklePitch;

   /* Finally make sure all angles are bound by (-pi, pi] */
   r.hip        = crop(r.hip       );
   r.thighRoll  = crop(r.thighRoll );
   r.thighPitch = crop(r.thighPitch);
   r.kneePitch  = crop(r.kneePitch );
   r.ankleRoll  = crop(r.ankleRoll );
   r.anklePitch = crop(r.anklePitch);

   return r;
}

std::ostream &operator <<(std::ostream &out, const NaoLegChain &chain)
{
   out << "chain.hip        = " << chain.hip        << std::endl;
   out << "chain.thighRoll  = " << chain.thighRoll  << std::endl;
   out << "chain.thighPitch = " << chain.thighPitch << std::endl;
   out << "chain.kneePitch  = " << chain.kneePitch  << std::endl;
   out << "chain.ankleRoll  = " << chain.ankleRoll  << std::endl;
   out << "chain.anklePitch = " << chain.anklePitch << std::endl;

   return out;
}

std::ostream &operator <<(std::ostream &out, const NaoFootTarget &target)
{
   out << "x     =" << target.x     << std::endl;
   out << "y     =" << target.y     << std::endl;
   out << "z     =" << target.z     << std::endl;
   out << "pitch =" << target.pitch << std::endl;
   out << "roll  =" << target.roll  << std::endl;
   out << "yaw   =" << target.yaw   << std::endl;

   return out;
}

} /* namespace IKinematics */