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

// 本教程的大部分内容是对  step-15  的完全复制。因此，为了简洁起见，并保持对这里所实现的变化的关注，我们将只记录新的内容，并简单地指出哪些部分的代码是对以前内容的重复。

//  @sect3{Include files}

// 本教程中包含了几个新的头文件。第一个是提供ParameterAcceptor类的声明的文件。

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

// 这是第二个，这是一个包罗万象的头，它将使我们能够在这段代码中纳入自动区分（AD）功能。

#include <deal.II/differentiation/ad.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_values_extractors.h>
#include <deal.II/fe/fe_q.h>

// 而接下来的三个提供了一些使用通用 MeshWorker::mesh_loop() 框架的多线程能力。

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/mesh_loop.h>
#include <deal.II/meshworker/scratch_data.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>

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

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

namespace Step72
{
  using namespace dealii;
// @sect3{The MinimalSurfaceProblemParameters class}

// 在本教程中，我们将实现三种不同的方法来组装线性系统。其中一种反映了最初在 step-15 中提供的手工实现，而另外两种则使用作为Trilinos框架的一部分提供的Sacado自动微分库。

// 为了方便在三种实现之间进行切换，我们有这个非常基本的参数类，它只有两个可配置的选项。

  class MinimalSurfaceProblemParameters : public ParameterAcceptor
  {
  public:
    MinimalSurfaceProblemParameters();

// 选择要使用的配方和相应的AD框架。

// - formulation = 0 : 无辅助执行（全手工线性化）。

// - 配方 = 1 : 有限元残差的自动线性化。

// - formulation = 2 : 使用变量公式自动计算有限元残差和线性化。

    unsigned int formulation = 0;

// 线性系统残差的最大可接受公差。我们将看到，一旦我们使用AD框架，装配时间就会变得很明显，所以我们将 step-15 中选择的公差提高了一个数量级。这样，计算就不会花费太长时间来完成。

    double tolerance = 1e-2;
  };

  MinimalSurfaceProblemParameters::MinimalSurfaceProblemParameters()
    : ParameterAcceptor("Minimal Surface Problem/")
  {
    add_parameter(
      "Formulation", formulation, "", this->prm, Patterns::Integer(0, 2));
    add_parameter("Tolerance", tolerance, "", this->prm, Patterns::Double(0.0));
  }

//  @sect3{The MinimalSurfaceProblem class template}

// 该类模板与  step-15  中的内容基本相同。该类的唯一功能变化是：。

// - run()函数现在接收两个参数：一个是选择采用哪种装配方式，一个是允许的最终残差的公差，以及

// - 现在有三个不同的装配函数来实现线性系统的三种装配方法。我们将在后面提供关于这些的细节。

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

    void run(const int formulation, const double tolerance);

  private:
    void   setup_system(const bool initial_step);
    void   assemble_system_unassisted();
    void   assemble_system_with_residual_linearization();
    void   assemble_system_using_energy_functional();
    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;
    QGauss<dim>     quadrature_formula;

    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}

//应用于该问题的边界条件没有变化。

  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)
    , quadrature_formula(fe.degree + 1)
  {}
// @sect4{MinimalSurfaceProblem::setup_system}

// 设置类数据结构的函数没有任何变化，即DoFHandler、应用于问题的悬挂节点约束以及线性系统。

  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();
      }

    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{Assembling the linear system}
// @sect5{Manual assembly}

// 汇编函数是本教程的有趣贡献。assemble_system_unassisted()方法实现了与 step-15 中详述的完全相同的装配函数，但在这个例子中，我们使用 MeshWorker::mesh_loop() 函数来多线程装配过程。这样做的原因很简单。当使用自动分化时，我们知道会有一些额外的计算开销产生。为了减轻这种性能损失，我们希望尽可能多地利用（容易获得的）计算资源。 MeshWorker::mesh_loop() 的概念使这成为一个相对简单的任务。同时，为了公平比较，我们需要对在计算残差或其线性化时不使用任何援助的实现做同样的事情。( MeshWorker::mesh_loop() 函数首先在 step-12 和 step-16 中讨论，如果你想阅读它的话。)

// 实现多线程所需的步骤在这三个函数中是相同的，所以我们将利用assemble_system_unassisted()函数的机会，重点讨论多线程本身。

  template <int dim>
  void MinimalSurfaceProblem<dim>::assemble_system_unassisted()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

//  MeshWorker::mesh_loop() 希望我们提供两个示范性的数据结构。第一个，`ScratchData`，是用来存储所有要在线程间重复使用的大数据。`CopyData`将保存来自每个单元的对线性系统的贡献。这些独立的矩阵-向量对必须按顺序累积到全局线性系统中。由于我们不需要 MeshWorker::ScratchData 和 MeshWorker::CopyData 类已经提供的东西，所以我们使用这些确切的类定义来解决我们的问题。请注意，我们只需要一个局部矩阵、局部右手向量和单元自由度索引向量的单个实例--因此 MeshWorker::CopyData 的三个模板参数都是`1`。

    using ScratchData = MeshWorker::ScratchData<dim>;
    using CopyData    = MeshWorker::CopyData<1, 1, 1>;

// 我们还需要知道我们在装配过程中要处理的迭代器的类型。为了简单起见，我们只要求编译器使用decltype()指定器为我们解决这个问题，知道我们将在由  @p dof_handler.  拥有的活动单元上迭代。
    using CellIteratorType = decltype(dof_handler.begin_active());

// 在这里我们初始化示例的数据结构。因为我们知道我们需要计算形状函数梯度、加权雅各布和四分位点在实空间的位置，所以我们把这些标志传给类的构造函数。

    const ScratchData sample_scratch_data(fe,
                                          quadrature_formula,
                                          update_gradients |
                                            update_quadrature_points |
                                            update_JxW_values);
    const CopyData    sample_copy_data(dofs_per_cell);

// 现在我们定义一个lambda函数，它将在一个单元格上执行装配。三个参数是由于我们将传递给该最终调用的参数，将被 MeshWorker::mesh_loop(), 所期望的参数。我们还捕获了 @p this 指针，这意味着我们将可以访问 "this"（即当前的`MinimalSurfaceProblem<dim>`）类实例，以及它的私有成员数据（因为lambda函数被定义在MinimalSurfaceProblem<dim>方法中）。

// 在函数的顶部，我们初始化了依赖于正在执行工作的单元的数据结构。请注意，重新初始化的调用实际上返回了一个FEValues对象的实例，该对象被初始化并存储在`scratch_data`对象中（因此，被重复使用）。

// 同样地，我们从 MeshWorker::mesh_loop() 提供的`copy_data`实例中获得本地矩阵、本地RHS向量和本地单元格DoF指数的别名。然后我们初始化单元格的DoF指数，因为我们知道本地矩阵和向量的大小已经正确。

    const auto cell_worker = [this](const CellIteratorType &cell,
                                    ScratchData &           scratch_data,
                                    CopyData &              copy_data) {
      const auto &fe_values = scratch_data.reinit(cell);

      FullMatrix<double> &                  cell_matrix = copy_data.matrices[0];
      Vector<double> &                      cell_rhs    = copy_data.vectors[0];
      std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];
      cell->get_dof_indices(local_dof_indices);

// 对于牛顿方法，我们需要问题被线性化的那一点的解的梯度。

// 一旦我们有了这个梯度，我们就可以用通常的方法对这个单元进行装配。 与 step-15 的一个小区别是，我们使用了（相当方便的）基于范围的循环来迭代所有的正交点和自由度。

      std::vector<Tensor<1, dim>> old_solution_gradients(
        fe_values.n_quadrature_points);
      fe_values.get_function_gradients(current_solution,
                                       old_solution_gradients);

      for (const unsigned int q : fe_values.quadrature_point_indices())
        {
          const double coeff =
            1.0 / std::sqrt(1.0 + old_solution_gradients[q] *
                                    old_solution_gradients[q]);

          for (const unsigned int i : fe_values.dof_indices())
            {
              for (const unsigned int j : fe_values.dof_indices())
                cell_matrix(i, j) +=
                  (((fe_values.shape_grad(i, q)      // ((\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
            }
        }
    };

//  MeshWorker::mesh_loop() 要求的第二个lambda函数是一个执行累积全局线性系统中的局部贡献的任务。这正是这个函数所做的，实现的细节在前面已经看到过。需要认识的主要一点是，局部贡献被存储在传入该函数的`copy_data`实例中。这个`copy_data`在 @a 对`cell_worker`的一些调用中已经被填满了数据。

    const auto copier = [dofs_per_cell, this](const CopyData &copy_data) {
      const FullMatrix<double> &cell_matrix = copy_data.matrices[0];
      const Vector<double> &    cell_rhs    = copy_data.vectors[0];
      const std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];

      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);
        }
    };

// 我们已经有了所有需要的函数定义，所以现在我们调用 MeshWorker::mesh_loop() 来执行实际的装配。 我们传递一个标志作为最后的参数，说明我们只想对单元格进行装配。在内部， MeshWorker::mesh_loop() 然后将可用的工作分配给不同的线程，有效地利用当今几乎所有的处理器所提供的多核。

    MeshWorker::mesh_loop(dof_handler.active_cell_iterators(),
                          cell_worker,
                          copier,
                          sample_scratch_data,
                          sample_copy_data,
                          MeshWorker::assemble_own_cells);

// 最后，正如在  step-15  中所做的那样，我们从系统中移除悬空的节点，并对定义牛顿更新的线性系统应用零边界值  $\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);
  }
// @sect5{Assembly via differentiation of the residual vector}

// 正如介绍中所述，我们需要为第二种方法做的是实现 $F(U)^K$ 单元对残差向量的局部贡献，然后让AD机器处理如何计算它的导数 $J(U)_{ij}^K=\frac{\partial F(U)^K_i}{\partial U_j}$ 。

// 对于下面的内容，请记住，
// @f[
//    F(U)_i^K \dealcoloneq
//    \int\limits_K\nabla \varphi_i \cdot \left[ \frac{1}{\sqrt{1+|\nabla
//    u|^{2}}} \nabla u \right] \, dV ,
//  @f]
//  其中 $u(\mathbf x)=\sum_j U_j \varphi_j(\mathbf x)$  。

// 我们来看看这在实践中是如何实现的。

  template <int dim>
  void MinimalSurfaceProblem<dim>::assemble_system_with_residual_linearization()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    using ScratchData      = MeshWorker::ScratchData<dim>;
    using CopyData         = MeshWorker::CopyData<1, 1, 1>;
    using CellIteratorType = decltype(dof_handler.begin_active());

    const ScratchData sample_scratch_data(fe,
                                          quadrature_formula,
                                          update_gradients |
                                            update_quadrature_points |
                                            update_JxW_values);
    const CopyData    sample_copy_data(dofs_per_cell);

// 我们将利用  step-71  中所示的技术，预先定义我们要使用的AD数据结构。在这种情况下，我们选择辅助类，它将使用Sacado向前自动微分类型自动计算有限元残差的线性化。这些数字类型可以只用来计算一阶导数。这正是我们想要的，因为我们知道我们将只对残差进行线性化，这意味着我们只需要计算一阶导数。计算的返回值将是`double`类型。

// 我们还需要一个提取器来检索一些与问题的现场解决方案有关的数据。

    using ADHelper = Differentiation::AD::ResidualLinearization<
      Differentiation::AD::NumberTypes::sacado_dfad,
      double>;
    using ADNumberType = typename ADHelper::ad_type;

    const FEValuesExtractors::Scalar u_fe(0);

// 有了这个，让我们定义lambda函数，它将被用来计算单元格对雅各布矩阵和右手边的贡献。

    const auto cell_worker = [&u_fe, this](const CellIteratorType &cell,
                                           ScratchData &           scratch_data,
                                           CopyData &              copy_data) {
      const auto &       fe_values     = scratch_data.reinit(cell);
      const unsigned int dofs_per_cell = fe_values.get_fe().n_dofs_per_cell();

      FullMatrix<double> &                  cell_matrix = copy_data.matrices[0];
      Vector<double> &                      cell_rhs    = copy_data.vectors[0];
      std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];
      cell->get_dof_indices(local_dof_indices);

// 我们现在要创建并初始化一个AD辅助类的实例。要做到这一点，我们需要指定有多少个自变量和因变量。自变量将是我们的解向量所具有的局部自由度的数量，即离散化解向量 $u (\mathbf{x})|_K = \sum\limits_{j} U^K_i \varphi_j(\mathbf{x})$ 的每元素表示中的数字 $j$ ，它表示每个有限元素有多少个解系数。在deal.II中，这等于 FiniteElement::dofs_per_cell. ，自变量的数量将是我们要形成的局部残差向量的条目数。在这个特定的问题中（就像许多其他采用[标准Galerkin方法](https:en.wikipedia.org/wiki/Galerkin_method)的问题一样），局部求解系数的数量与局部残差方程的数量相符。

      const unsigned int n_independent_variables = local_dof_indices.size();
      const unsigned int n_dependent_variables   = dofs_per_cell;
      ADHelper ad_helper(n_independent_variables, n_dependent_variables);

// 接下来，我们将解决方案的值告知帮助器，即我们希望线性化的 $U_j$ 的实际值。由于这是在每个元素上单独进行的，我们必须从全局解决方案向量中提取解决方案的系数。换句话说，我们将所有这些系数 $U_j$ （其中 $j$ 是一个局部自由度）定义为进入向量 $F(U)^{K}$ （因果函数）计算的自变量。
//然后，
//我们就得到了由可自动微分的数字表示的自由度值的完整集合。对这些变量进行的操作从这一点开始被AD库跟踪，直到对象超出范围。所以正是这些变量    ，我们将对其计算残差项的导数。

      ad_helper.register_dof_values(current_solution, local_dof_indices);

      const std::vector<ADNumberType> &dof_values_ad =
        ad_helper.get_sensitive_dof_values();

// 然后我们做一些特定问题的任务，首先是根据 "敏感 "的AD自由度值计算所有数值、（空间）梯度等。在这个例子中，我们要检索每个正交点的解梯度。请注意，现在解梯度对自由度值很敏感，因为它们使用 @p ADNumberType 作为标量类型， @p dof_values_ad 矢量提供局部自由度值。

      std::vector<Tensor<1, dim, ADNumberType>> old_solution_gradients(
        fe_values.n_quadrature_points);
      fe_values[u_fe].get_function_gradients_from_local_dof_values(
        dof_values_ad, old_solution_gradients);

// 我们声明的下一个变量将存储单元格残余向量贡献。这是相当不言自明的，除了一个very important的细节。请注意，向量中的每个条目都是手工初始化的，数值为0。这是一个  强烈推荐的  做法，因为一些AD库似乎没有安全地初始化这些数字类型的内部数据结构。不这样做可能会导致一些非常难以理解或检测的错误（感谢这个程序的作者出于一般的坏经验而提到这一点）。因此，出于谨慎考虑，值得明确地将初始值归零。在这之后，除了符号的改变，残差集看起来和我们之前看到的单元格RHS向量差不多。我们在所有正交点上循环，确保系数现在通过使用正确的`ADNumberType'来编码它对（敏感的）有限元DoF值的依赖性，最后我们组装残差向量的组件。为了完全清楚，有限元形状函数（及其梯度等）以及 "JxW "值仍然是标量值，但每个正交点的 @p coeff 和 @p old_solution_gradients 是以独立变量计算的。

      std::vector<ADNumberType> residual_ad(n_dependent_variables,
                                            ADNumberType(0.0));
      for (const unsigned int q : fe_values.quadrature_point_indices())
        {
          const ADNumberType coeff =
            1.0 / std::sqrt(1.0 + old_solution_gradients[q] *
                                    old_solution_gradients[q]);

          for (const unsigned int i : fe_values.dof_indices())
            {
              residual_ad[i] += (fe_values.shape_grad(i, q)   // \nabla \phi_i
                                 * coeff                      // * a_n
                                 * old_solution_gradients[q]) // * u_n
                                * fe_values.JxW(q);           // * dx
            }
        }

// 一旦我们计算出完整的单元格残差向量，我们就可以将其注册到辅助类。

// 此后，我们在评估点计算残差值（基本上是从我们已经计算出来的东西中提取出真实的值）和它们的Jacobian（每个残差分量相对于所有单元DoF的线性化）。为了组装成全局线性系统，我们必须尊重残差和RHS贡献之间的符号差异。对于牛顿方法，右手边的向量需要等于*负的残差向量。

      ad_helper.register_residual_vector(residual_ad);

      ad_helper.compute_residual(cell_rhs);
      cell_rhs *= -1.0;

      ad_helper.compute_linearization(cell_matrix);
    };

// 该函数的剩余部分等于我们之前的内容。

    const auto copier = [dofs_per_cell, this](const CopyData &copy_data) {
      const FullMatrix<double> &cell_matrix = copy_data.matrices[0];
      const Vector<double> &    cell_rhs    = copy_data.vectors[0];
      const std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];

      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);
        }
    };

    MeshWorker::mesh_loop(dof_handler.active_cell_iterators(),
                          cell_worker,
                          copier,
                          sample_scratch_data,
                          sample_copy_data,
                          MeshWorker::assemble_own_cells);

    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);
  }
// @sect5{Assembly via differentiation of the energy functional}

// 在这第三种方法中，我们将残差和雅各布作为局部能量函数
// @f[
//     E\left( U \right)^K
//      \dealcoloneq \int\limits_{K} \Psi \left( u \right) \, dV
//      \approx \sum\limits_{q}^{n_{\textrm{q-points}}} \Psi \left( u \left(
//      \mathbf{X}_{q} \right) \right) \underbrace{\vert J_{q} \vert \times
//      W_{q}}_{\text{JxW(q)}}
//  @f]
//  的第一和第二导数来计算，能量密度由
//  @f[
//    \Psi \left( u \right) = \sqrt{1+|\nabla u|^{2}} .
//  @f]给出。

// 我们再来看看这是如何做到的。

  template <int dim>
  void MinimalSurfaceProblem<dim>::assemble_system_using_energy_functional()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    using ScratchData      = MeshWorker::ScratchData<dim>;
    using CopyData         = MeshWorker::CopyData<1, 1, 1>;
    using CellIteratorType = decltype(dof_handler.begin_active());

    const ScratchData sample_scratch_data(fe,
                                          quadrature_formula,
                                          update_gradients |
                                            update_quadrature_points |
                                            update_JxW_values);
    const CopyData    sample_copy_data(dofs_per_cell);

// 在这个装配过程的实现中，我们选择了辅助类，它将使用嵌套的Sacado前向自动微分类型自动计算残差及其从单元贡献到能量函数的线性化。所选的数字类型可以用来计算第一和第二导数。我们需要这样做，因为残差定义为势能对DoF值的敏感性（即其梯度）。然后我们需要将残差线性化，这意味着必须计算势能的二阶导数。你可能想把这与之前函数中使用的 "ADHelper "的定义进行比较，在那里我们使用 `Differentiation::AD::ResidualLinearization<Differentiation::AD::NumberTypes::sacado_dfad,double>`. 。
    using ADHelper = Differentiation::AD::EnergyFunctional<
      Differentiation::AD::NumberTypes::sacado_dfad_dfad,
      double>;
    using ADNumberType = typename ADHelper::ad_type;

    const FEValuesExtractors::Scalar u_fe(0);

// 然后让我们再次定义lambda函数，对一个单元进行积分。

// 为了初始化辅助类的实例，我们现在只需要预先知道自变量的数量（即与元素解向量相关的自由度数量）。这是因为由能量函数产生的二阶导数矩阵必然是平方的（顺便说一下，也是对称的）。

    const auto cell_worker = [&u_fe, this](const CellIteratorType &cell,
                                           ScratchData &           scratch_data,
                                           CopyData &              copy_data) {
      const auto &fe_values = scratch_data.reinit(cell);

      FullMatrix<double> &                  cell_matrix = copy_data.matrices[0];
      Vector<double> &                      cell_rhs    = copy_data.vectors[0];
      std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];
      cell->get_dof_indices(local_dof_indices);

      const unsigned int n_independent_variables = local_dof_indices.size();
      ADHelper           ad_helper(n_independent_variables);

// 再一次，我们将所有的单元格DoFs值注册到帮助器中，然后提取这些值的 "敏感 "变体，用于后续必须区分的操作--其中之一是计算解决方案的梯度。

      ad_helper.register_dof_values(current_solution, local_dof_indices);

      const std::vector<ADNumberType> &dof_values_ad =
        ad_helper.get_sensitive_dof_values();

      std::vector<Tensor<1, dim, ADNumberType>> old_solution_gradients(
        fe_values.n_quadrature_points);
      fe_values[u_fe].get_function_gradients_from_local_dof_values(
        dof_values_ad, old_solution_gradients);

// 我们接下来创建一个变量来存储电池的总能量。我们再一次强调，我们明确地对这个值进行零初始化，从而确保这个起始值的数据的完整性。

// 我们的目的是计算细胞总能量，它是内部能量（由于右手函数，通常是 $U$ 的线性）和外部能量的总和。在这种特殊情况下，我们没有外部能量（例如，来自源项或诺伊曼边界条件），所以我们将关注内部能量部分。

// 事实上，计算 $E(U)^K$ 几乎是微不足道的，只需要以下几行。

      ADNumberType energy_ad = ADNumberType(0.0);
      for (const unsigned int q : fe_values.quadrature_point_indices())
        {
          const ADNumberType psi = std::sqrt(1.0 + old_solution_gradients[q] *
                                                     old_solution_gradients[q]);

          energy_ad += psi * fe_values.JxW(q);
        }

// 在我们计算出这个单元的总能量后，我们将把它注册到帮助器上。 在此基础上，我们现在可以计算出所需的数量，即残差值和它们在评估点的雅各布系数。和以前一样，牛顿的右手边需要是残差的负数。

      ad_helper.register_energy_functional(energy_ad);

      ad_helper.compute_residual(cell_rhs);
      cell_rhs *= -1.0;

    };

// 与前两个函数一样，函数的剩余部分与之前一样。

    const auto copier = [dofs_per_cell, this](const CopyData &copy_data) {
      const FullMatrix<double> &cell_matrix = copy_data.matrices[0];
      const Vector<double> &    cell_rhs    = copy_data.vectors[0];
      const std::vector<types::global_dof_index> &local_dof_indices =
        copy_data.local_dof_indices[0];

      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);
        }
    };

    MeshWorker::mesh_loop(dof_handler.active_cell_iterators(),
                          cell_worker,
                          copier,
                          sample_scratch_data,
                          sample_copy_data,
                          MeshWorker::assemble_own_cells);

    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}

// 解算函数与  step-15  中使用的相同。

  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-15 以来，在网格细化程序和适应性网格之间的解决方案的转移方面没有任何变化。

  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();
    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);

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

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

    set_boundary_values();

  }

//  @sect4{MinimalSurfaceProblem::set_boundary_values}

// 边界条件的选择仍然与 step-15 相同 ...

  template <int dim>
  void MinimalSurfaceProblem<dim>::set_boundary_values()
  {
    std::map<types::global_dof_index, double> boundary_values;
  };
  template <int dim>
                                             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}

// ...就像在求解迭代过程中用来计算残差的函数一样。如果真的需要，我们可以用能量函数的微分来代替它，但是为了简单起见，我们在这里只是简单地复制我们在 step-15 中已经有的东西。

  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);

        fe_values.get_function_gradients(evaluation_point, gradients);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            const double coeff =
              1.0 / std::sqrt(1.0 + 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);
      }

    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}

// 非线性迭代程序的步长（或欠松系数）的选择仍然固定在  step-15  中选择和讨论的值。

  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-15  中实现的相同。唯一可以观察到的变化是，我们现在可以（通过参数文件）选择系统残差的最终可接受的公差是什么，并且我们可以选择我们希望利用的装配方法。为了使第二个选择明确，我们向控制台输出一些信息，表明选择。由于我们对比较三种方法中每一种的装配时间感兴趣，我们还添加了一个计时器，跟踪装配过程中所花费的时间。我们还跟踪了解决线性系统所需的时间，这样我们就可以将这些数字与通常需要最长时间执行的那部分代码进行对比。

  template <int dim>
  void MinimalSurfaceProblem<dim>::run(const int    formulation,
                                       const double tolerance)
  {
    std::cout << "******** Assembly approach ********" << std::endl;
    const std::array<std::string, 3> method_descriptions = {
      {"Unassisted implementation (full hand linearization).",
       "Automated linearization of the finite element residual.",
       "Automated computation of finite element residual and linearization using a variational formulation."}};
    AssertIndexRange(formulation, method_descriptions.size());
    std::cout << method_descriptions[formulation] << std::endl << std::endl;

    TimerOutput timer(std::cout, TimerOutput::summary, TimerOutput::wall_times);

    GridGenerator::hyper_ball(triangulation);
    triangulation.refine_global(2);

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

    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();

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

        for (unsigned int inner_iteration = 0; inner_iteration < 5;
             ++inner_iteration)
          {
            {
              TimerOutput::Scope t(timer, "Assemble");

              if (formulation == 0)
                assemble_system_unassisted();
              else if (formulation == 1)
                assemble_system_with_residual_linearization();
              else if (formulation == 2)
                assemble_system_using_energy_functional();
              else
                AssertThrow(false, ExcNotImplemented());
            }

            last_residual_norm = system_rhs.l2_norm();

            {
              TimerOutput::Scope t(timer, "Solve");
              solve();
            }

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

        output_results(refinement_cycle);

        ++refinement_cycle;
        std::cout << std::endl;
      }
    while (last_residual_norm > tolerance);
  }
} // namespace Step72
// @sect4{The main function}

// 最后是主函数。它遵循大多数其他主函数的方案，但有两个明显的例外。

// - 我们调用 Utilities::MPI::MPI_InitFinalize ，以便（通过一个隐藏的默认参数）设置使用多线程任务执行的线程数。

// - 我们还有几行专门用于读取或初始化用户定义的参数，这些参数将在程序执行过程中被考虑。

int main(int argc, char *argv[])
{
  try
    {
      using namespace Step72;

      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv);

      std::string prm_file;
      if (argc > 1)
        prm_file = argv[1];
      else
        prm_file = "parameters.prm";

      const MinimalSurfaceProblemParameters parameters;
      ParameterAcceptor::initialize(prm_file);

      MinimalSurfaceProblem<2> minimal_surface_problem_2d;
      minimal_surface_problem_2d.run(parameters.formulation,
                                     parameters.tolerance);
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/data_io.h>
#include "create_iris_datafile.h"
#include <vector>
#include <sstream>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.svm_multiclass_trainer");

    class test_svm_multiclass_trainer : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        test_svm_multiclass_trainer (
        ) :
            tester (
                "test_svm_multiclass_trainer",       // the command line argument name for this test
                "Run tests on the svm_multiclass_linear_trainer stuff.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        template <typename sample_type>
        void run_test()
        {
            print_spinner();

            typedef typename sample_type::value_type::second_type scalar_type;

            std::vector<sample_type> samples;
            std::vector<scalar_type> labels;

            load_libsvm_formatted_data("iris.scale",samples, labels);

            DLIB_TEST(samples.size() == 150);
            DLIB_TEST(labels.size() == 150);

            typedef sparse_linear_kernel<sample_type> kernel_type;
            svm_multiclass_linear_trainer<kernel_type> trainer;
            trainer.set_c(100);

            randomize_samples(samples, labels);
            matrix<double> cv = cross_validate_multiclass_trainer(trainer, samples, labels, 4);

            dlog << LINFO << "confusion matrix: \n" << cv;
            const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
            dlog << LINFO << "cv accuracy: " << cv_accuracy;
            DLIB_TEST(cv_accuracy > 0.97);

            {
                print_spinner();
                typedef matrix<scalar_type,0,1> dsample_type;
                std::vector<dsample_type> dsamples = sparse_to_dense(samples);
                DLIB_TEST(dsamples.size() == 150);

                typedef linear_kernel<dsample_type> kernel_type;
                svm_multiclass_linear_trainer<kernel_type> trainer;
                trainer.set_c(100);

                cv = cross_validate_multiclass_trainer(trainer, dsamples, labels, 4);

                dlog << LINFO << "dense confusion matrix: \n" << cv;
                const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
                dlog << LINFO << "dense cv accuracy: " << cv_accuracy;
                DLIB_TEST(cv_accuracy > 0.97);
            }

        }

        void perform_test (
        )
        {
            print_spinner();
            create_iris_datafile();

            run_test<std::map<unsigned int, double> >();
            run_test<std::map<unsigned int, float> >();
            run_test<std::vector<std::pair<unsigned int, float> > >();
            run_test<std::vector<std::pair<unsigned long, double> > >();
        }
    };

    test_svm_multiclass_trainer a;

}


//  Copyright (c) 2006 Xiaogang Zhang
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to handle
//  types longer than 80-bit reals.
//
#ifndef BOOST_MATH_ELLINT_RF_HPP
#define BOOST_MATH_ELLINT_RF_HPP

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>

#include <boost/math/policies/error_handling.hpp>

// Carlson's elliptic integral of the first kind
// R_F(x, y, z) = 0.5 * \int_{0}^{\infty} [(t+x)(t+y)(t+z)]^{-1/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rf_imp(T x, T y, T z, const Policy& pol)
{
    T value, X, Y, Z, E2, E3, u, lambda, tolerance;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

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

    if (x < 0 || y < 0 || z < 0)
    {
       return policies::raise_domain_error<T>(function,
            "domain error, all arguments must be non-negative, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }
    if (x + y == 0 || y + z == 0 || z + x == 0)
    {
       return policies::raise_domain_error<T>(function,
            "domain error, at most one argument can be zero, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }

    // Carlson scales error as the 6th power of tolerance,
    // but this seems not to work for types larger than
    // 80-bit reals, this heuristic seems to work OK:
    if(policies::digits<T, Policy>() > 64)
    {
      tolerance = pow(tools::epsilon<T>(), T(1)/4.25f);
      BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    }
    else
    {
      tolerance = pow(4*tools::epsilon<T>(), T(1)/6);
      BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    }

    // duplication
    k = 1;
    do
    {
        u = (x + y + z) / 3;
        X = (u - x) / u;
        Y = (u - y) / u;
        Z = (u - z) / u;

        // Termination condition:
        if ((tools::max)(abs(X), abs(Y), abs(Z)) < tolerance)
           break;

        T sx = sqrt(x);
        T sy = sqrt(y);
        T sz = sqrt(z);
        lambda = sy * (sx + sz) + sz * sx;
        x = (x + lambda) / 4;
        y = (y + lambda) / 4;
        z = (z + lambda) / 4;
        ++k;
    }
    while(k < policies::get_max_series_iterations<Policy>());

    // Check to see if we gave up too soon:
    policies::check_series_iterations(function, k, pol);
    BOOST_MATH_INSTRUMENT_VARIABLE(k);

    // Taylor series expansion to the 5th order
    E2 = X * Y - Z * Z;
    E3 = X * Y * Z;
    value = (1 + E2*(E2/24 - E3*T(3)/44 - T(0.1)) + E3/14) / sqrt(u);
    BOOST_MATH_INSTRUMENT_VARIABLE(value);

    return value;
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rf(T1 x, T2 y, T3 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rf_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z), pol), "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rf(T1 x, T2 y, T3 z)
{
   return ellint_rf(x, y, z, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RF_HPP


#pragma once
#include "dofs.hpp"
#include <Eigen/Core>
#include <igl/boundary_facets.h>
#include <igl/colon.h>
#include <igl/setdiff.h>
#include <igl/unique.h>

//! Finds the boundary and interior vertices, and evaluates
//! the function g on the boundary vertices.
//!
//! @param[out] u should be of size <number of vertices>. At the end:
//!             u(index) = g(vertices(index))  for each boundary index.
//! @param[out] interiorDofs the list of dofs that are not on the boundary.
//! @param[in]  quadraticDofs
//! @param[in]  g the boundary value function g.
void setDirichletBoundary(Eigen::VectorXd &u,
                          Eigen::VectorXi &interiorDofs,
                          const QDofs &    quadraticDofs,
                          const std::function<double(double, double)> &g) {
	//get dofs
	Eigen::MatrixXi qdof;
	int             N = quadraticDofs.get_dofs(qdof);

	// Find boundary edges
	Eigen::MatrixXi boundaryEdges;
	Eigen::MatrixXi triangles = qdof.block(0, 0, qdof.rows(), 3);
	igl::boundary_facets(triangles, boundaryEdges);

	// Find dof indices corresponding to boundary edges
	Eigen::VectorXi boundaryEdgeIndices(boundaryEdges.rows());
	auto            edgemap = quadraticDofs.vertex2edge;
	for (int k = 0; k < boundaryEdges.rows(); k++) {
		auto edge = boundaryEdges.row(k);
		auto v0   = edge(0);
		auto v1   = edge(1);
		// get edges connected to these vertices
		auto edgesv0 = (*edgemap.find(v0)).second;
		auto edgesv1 = (*edgemap.find(v1)).second;
		// traverse them and find edge (intersection)
		for (auto e0 : edgesv0) {
			for (auto e1 : edgesv1) {
				if (e0 == e1) { //found edge!
					// mark as boundary
					boundaryEdgeIndices(k) = e0;
					break;
					break;
				}
			}
		}
	}

	// Find boundary vertices
	Eigen::VectorXi boundaryVertexIndices, IA, IC;
	igl::unique(boundaryEdges, boundaryVertexIndices, IA, IC);

	// set boundary data
	auto nodes = quadraticDofs.nodes;
	for (int i = 0; i < boundaryVertexIndices.size(); ++i) {
		const int   index = boundaryVertexIndices(i);
		const auto &x     = nodes.row(index);
		u(index)          = g(x(0), x(1));
	}

	// Combine this information to get boundary dofs
	Eigen::VectorXi boundaryDofs(boundaryEdges.rows() + boundaryVertexIndices.rows());
	boundaryDofs << boundaryVertexIndices, boundaryEdgeIndices;
	// Get interior dofs
	Eigen::VectorXi allIndices;
	igl::colon<int>(0, N - 1, allIndices);
	igl::setdiff(allIndices, boundaryDofs, interiorDofs, IA);
}


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Base implementations for math library.
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_math
 */

#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"

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

#include <assert.h>

/*
 *
 * Copy helpers.
 *
 */

static inline Eigen::Quaternionf
copy(const struct xrt_quat &q)
{
	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
	//  Eigen: `float w, x, y, z`.
	// OpenXR: `float x, y, z, w`.
	return Eigen::Quaternionf(q.w, q.x, q.y, q.z);
}

static inline Eigen::Quaternionf
copy(const struct xrt_quat *q)
{
	return copy(*q);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 &v)
{
	return Eigen::Vector3f(v.x, v.y, v.z);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 *v)
{
	return copy(*v);
}

/*
 *
 * Exported vector functions.
 *
 */

extern "C" bool
math_vec3_validate(const struct xrt_vec3 *vec3)
{
	assert(vec3 != NULL);

	return map_vec3(*vec3).allFinite();
}

extern "C" void
math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut)
{
	assert(additional != NULL);
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) += map_vec3(*additional);
}

extern "C" void
math_vec3_cross(const struct xrt_vec3 *l,
                const struct xrt_vec3 *r,
                struct xrt_vec3 *result)
{
	map_vec3(*result) = map_vec3(*l).cross(map_vec3(*r));
}

/*
 *
 * Exported quaternion functions.
 *
 */

extern "C" void
math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat,
                          struct xrt_quat *result)
{
	Eigen::Matrix3f m;
	m << mat->v[0], mat->v[1], mat->v[2], mat->v[3], mat->v[4], mat->v[5],
	    mat->v[6], mat->v[7], mat->v[8];

	Eigen::Quaternionf q(m);
	map_quat(*result) = q;
}

extern "C" void
math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x,
                        const struct xrt_vec3 *plus_z,
                        struct xrt_quat *result)
{
	xrt_vec3 plus_y;
	math_vec3_cross(plus_z, plus_x, &plus_y);

	xrt_matrix_3x3 m = {{
	    plus_x->x,
	    plus_y.x,
	    plus_z->x,
	    plus_x->y,
	    plus_y.y,
	    plus_z->y,
	    plus_x->z,
	    plus_y.z,
	    plus_z->z,
	}};

	math_quat_from_matrix_3x3(&m, result);
}

extern "C" bool
math_quat_validate(const struct xrt_quat *quat)
{
	assert(quat != NULL);
	auto rot = copy(*quat);

	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
	auto norm = rot.squaredNorm();
	if (norm > 1.0f + FLOAT_EPSILON || norm < 1.0f - FLOAT_EPSILON) {
		return false;
	}

	// Technically not yet a required check, but easier to stop problems
	// now than once denormalized numbers pollute the rest of our state.
	// see https://gitlab.khronos.org/openxr/openxr/issues/922
	if (!rot.coeffs().allFinite()) {
		return false;
	}

	return true;
}

extern "C" void
math_quat_normalize(struct xrt_quat *inout)
{
	assert(inout != NULL);
	map_quat(*inout).normalize();
}

extern "C" void
math_quat_rotate(const struct xrt_quat *left,
                 const struct xrt_quat *right,
                 struct xrt_quat *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto q = l * r;

	map_quat(*result) = q;
}

extern "C" void
math_quat_rotate_vec3(const struct xrt_quat *left,
                      const struct xrt_vec3 *right,
                      struct xrt_vec3 *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto v = l * r;

	map_vec3(*result) = v;
}

/*
 *
 * Exported pose functions.
 *
 */

extern "C" bool
math_pose_validate(const struct xrt_pose *pose)
{
	assert(pose != NULL);

	return math_vec3_validate(&pose->position) &&
	       math_quat_validate(&pose->orientation);
}

extern "C" void
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(outPose != NULL);

	// Store results to temporary locals so we can do this "in-place"
	// (pose == outPose) if desired. Pure copies here.
	Eigen::Vector3f newPosition = position(*pose);
	Eigen::Quaternionf newOrientation = orientation(*pose);

	// Conjugate legal here since pose must be normalized/unit length.
	newOrientation = newOrientation.conjugate();
	// Use the newly inverted rotation, to rotate position.
	newPosition = -(newOrientation * newPosition);

	position(*outPose) = newPosition;
	orientation(*outPose) = newOrientation;
}

/*!
 * Return the result of transforming a point by a pose/transform.
 */
static inline Eigen::Vector3f
transform_point(const xrt_pose &transform, const xrt_vec3 &point)
{
	return orientation(transform) * map_vec3(point) + position(transform);
}

/*!
 * Return the result of transforming a pose by a pose/transform.
 */
static inline xrt_pose
transform_pose(const xrt_pose &transform, const xrt_pose &pose)
{
	xrt_pose ret;
	position(ret) = transform_point(transform, pose.position);
	orientation(ret) = orientation(transform) * orientation(pose);
	return ret;
}

extern "C" void
math_pose_transform(const struct xrt_pose *transform,
                    const struct xrt_pose *pose,
                    struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(transform != NULL);
	assert(outPose != NULL);

	xrt_pose newPose = transform_pose(*transform, *pose);
	memcpy(outPose, &newPose, sizeof(xrt_pose));
}

extern "C" void
math_pose_transform_point(const struct xrt_pose *transform,
                          const struct xrt_vec3 *point,
                          struct xrt_vec3 *out_point)
{
	assert(transform != NULL);
	assert(point != NULL);
	assert(out_point != NULL);

	map_vec3(*out_point) = transform_point(*transform, *point);
}

extern "C" void
math_pose_openxr_locate(const struct xrt_pose *space_pose,
                        const struct xrt_pose *relative_pose,
                        const struct xrt_pose *base_space_pose,
                        struct xrt_pose *result)
{
	assert(space_pose != NULL);
	assert(relative_pose != NULL);
	assert(base_space_pose != NULL);
	assert(result != NULL);

	// Compilers are slightly better optimizing
	// if we copy the arguments in one go.
	const auto bsp = *base_space_pose;
	const auto rel = *relative_pose;
	const auto spc = *space_pose;
	struct xrt_pose pose;

	// Apply the invert of the base space to identity.
	math_pose_invert(&bsp, &pose);

	// Apply the pure pose from the space relation.
	math_pose_transform(&pose, &rel, &pose);

	// Apply the space pose.
	math_pose_transform(&pose, &spc, &pose);

	*result = pose;
}

/*!
 * Return the result of rotating a derivative vector by a matrix.
 *
 * This is a differential transform.
 */
static inline Eigen::Vector3f
rotate_deriv(Eigen::Matrix3f const &rotation,
             const xrt_vec3 &derivativeVector,
             Eigen::Matrix3f const &rotationInverse)
{
	return ((rotation * map_vec3(derivativeVector)).transpose() *
	        rotationInverse)
	    .transpose();
}

#ifndef XRT_DOXYGEN

#define MAKE_REL_FLAG_CHECK(NAME, MASK)                                        \
	static inline bool NAME(xrt_space_relation_flags flags)                \
	{                                                                      \
		return ((flags & (MASK)) != 0);                                \
	}

MAKE_REL_FLAG_CHECK(has_some_pose_component,
                    XRT_SPACE_RELATION_POSITION_VALID_BIT |
                        XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_position, XRT_SPACE_RELATION_POSITION_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_orientation, XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_lin_vel, XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_ang_vel, XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_lin_acc,
                    XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_ang_acc,
                    XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT)
MAKE_REL_FLAG_CHECK(has_some_derivative,
                    XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT |
                        XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT |
                        XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT |
                        XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT)

#undef MAKE_REL_FLAG_CHECK

#endif // !XRT_DOXYGEN

enum accumulate_pose_flags
{
	OFFSET,
	LEGACY,
};

/*!
 * Apply a transform to a space relation.
 */
static inline void
transform_accumulate_pose(const xrt_pose &transform,
                          xrt_space_relation &relation,
                          enum accumulate_pose_flags accum_flags,
                          bool do_translation = true,
                          bool do_rotation = true)
{
	assert(do_translation || do_rotation);

	// Save the quat in case we are self-transforming.
	Eigen::Quaternionf quat = orientation(transform);

	auto flags = relation.relation_flags;
	// so code looks similar
	auto in_out_relation = &relation;

	// transform (rotate and translate) the pose, if applicable.
	if (has_some_pose_component(flags)) {
		// Zero out transform parts we don't want to use,
		// because math_pose_transform doesn't take flags.
		xrt_pose transform_copy = transform;
		if (!do_translation) {
			position(transform_copy) = Eigen::Vector3f::Zero();
		}
		if (!do_rotation) {
			orientation(transform_copy) =
			    Eigen::Quaternionf::Identity();
		}

		//! @todo This is just a big hack.
		if (accum_flags == OFFSET) {
			math_pose_transform(&transform, &in_out_relation->pose,
			                    &in_out_relation->pose);
		} else {
			math_pose_transform(&in_out_relation->pose, &transform,
			                    &in_out_relation->pose);
		}
	}

	if (do_rotation && has_some_derivative(flags)) {

		// prepare matrices required for rotating derivatives from the
		// saved quat.
		Eigen::Matrix3f rot = quat.toRotationMatrix();
		Eigen::Matrix3f rotInverse = rot.inverse();

		// Rotate derivatives, if applicable.
		if (has_lin_vel(flags)) {
			map_vec3(in_out_relation->linear_velocity) =
			    rotate_deriv(rot, in_out_relation->linear_velocity,
			                 rotInverse);
		}

		if (has_ang_vel(flags)) {
			map_vec3(in_out_relation->angular_velocity) =
			    rotate_deriv(rot, in_out_relation->angular_velocity,
			                 rotInverse);
		}

		if (has_lin_acc(flags)) {
			map_vec3(in_out_relation->linear_acceleration) =
			    rotate_deriv(rot,
			                 in_out_relation->linear_acceleration,
			                 rotInverse);
		}

		if (has_ang_acc(flags)) {
			map_vec3(in_out_relation->angular_acceleration) =
			    rotate_deriv(rot,
			                 in_out_relation->angular_acceleration,
			                 rotInverse);
		}
	}
}

static const struct xrt_space_relation BLANK_RELATION = {
    XRT_SPACE_RELATION_BITMASK_ALL,
    {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}},
    {0, 0, 0},
    {0, 0, 0},
    {0, 0, 0},
    {0, 0, 0},
};

extern "C" void
math_relation_reset(struct xrt_space_relation *out)
{
	*out = BLANK_RELATION;
}

extern "C" void
math_relation_apply_offset(const struct xrt_pose *offset,
                           struct xrt_space_relation *in_out_relation)
{
	assert(offset != nullptr);
	assert(in_out_relation != nullptr);

	// No modifying the validity flags here.
	transform_accumulate_pose(*offset, *in_out_relation, OFFSET);
}

void
accumulate_transform(const struct xrt_pose *transform,
                     struct xrt_space_relation *in_out_relation)
{
	assert(transform != nullptr);
	assert(in_out_relation != nullptr);

	// No modifying the validity flags here.
	transform_accumulate_pose(*transform, *in_out_relation, LEGACY);
}

extern "C" void
math_relation_accumulate_relation(
    const struct xrt_space_relation *additional_relation,
    struct xrt_space_relation *in_out_relation)
{
	assert(additional_relation != NULL);
	assert(in_out_relation != NULL);

	// Update the flags.
	xrt_space_relation_flags flags = (enum xrt_space_relation_flags)(
	    in_out_relation->relation_flags &
	    additional_relation->relation_flags);
	in_out_relation->relation_flags = flags;

	if (has_some_pose_component(flags)) {
		// First, just do the pose part (including rotating
		// derivatives, if applicable).
		transform_accumulate_pose(
		    additional_relation->pose, *in_out_relation, LEGACY,
		    has_position(flags), has_orientation(flags));
	}

	// Then, accumulate the derivatives, if required.
	if (has_lin_vel(flags)) {
		map_vec3(in_out_relation->linear_velocity) +=
		    map_vec3(additional_relation->linear_velocity);
	}

	if (has_ang_vel(flags)) {
		map_vec3(in_out_relation->angular_velocity) +=
		    map_vec3(additional_relation->angular_velocity);
	}

	if (has_lin_acc(flags)) {
		map_vec3(in_out_relation->linear_acceleration) +=
		    map_vec3(additional_relation->linear_acceleration);
	}

	if (has_ang_acc(flags)) {
		map_vec3(in_out_relation->angular_acceleration) +=
		    map_vec3(additional_relation->angular_acceleration);
	}
}

extern "C" void
math_relation_openxr_locate(const struct xrt_pose *space_pose,
                            const struct xrt_space_relation *relative_relation,
                            const struct xrt_pose *base_space_pose,
                            struct xrt_space_relation *result)
{
	assert(space_pose != NULL);
	assert(relative_relation != NULL);
	assert(base_space_pose != NULL);
	assert(result != NULL);

	// Compilers are slightly better optimizing
	// if we copy the arguments in one go.
	const auto bsp = *base_space_pose;
	const auto spc = *space_pose;
	struct xrt_space_relation accumulating_relation = BLANK_RELATION;

	// Apply the invert of the base space to identity.
	math_pose_invert(&bsp, &accumulating_relation.pose);

	// Apply the pure relation between spaces.
	math_relation_accumulate_relation(relative_relation,
	                                  &accumulating_relation);

	// Apply the space pose.
	accumulate_transform(&spc, &accumulating_relation);

	*result = accumulating_relation;
}


#include <boost/math/special_functions/ellint_rj.hpp>


#include <igl/copyleft/marching_cubes.h>
#include <igl/signed_distance.h>
#include <igl/read_triangle_mesh.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Core>
#include <iostream>

#include "tutorial_shared_path.h"

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  MatrixXi F;
  MatrixXd V;
  // Read in inputs as double precision floating point meshes
  read_triangle_mesh(
      TUTORIAL_SHARED_PATH "/armadillo.obj",V,F);
  // number of vertices on the largest side
  const int s = 50;
  const RowVector3d Vmin = V.colwise().minCoeff();
  const RowVector3d Vmax = V.colwise().maxCoeff();
  const double h = (Vmax-Vmin).maxCoeff()/(double)s;
  const RowVector3i res = (s*((Vmax-Vmin)/(Vmax-Vmin).maxCoeff())).cast<int>();
  // create grid
  cout<<"Creating grid..."<<endl;
  MatrixXd GV(res(0)*res(1)*res(2),3);
  for(int zi = 0;zi<res(2);zi++)
  {
    const auto lerp = [&](const int di, const int d)->double
      {return Vmin(d)+(double)di/(double)(res(d)-1)*(Vmax(d)-Vmin(d));};
    const double z = lerp(zi,2);
    for(int yi = 0;yi<res(1);yi++)
    {
      const double y = lerp(yi,1);
      for(int xi = 0;xi<res(0);xi++)
      {
        const double x = lerp(xi,0);
        GV.row(xi+res(0)*(yi + res(1)*zi)) = RowVector3d(x,y,z);
      }
    }
  }
  // compute values
  cout<<"Computing distances..."<<endl;
  VectorXd S,B;
  {
    VectorXi I;
    MatrixXd C,N;
    signed_distance(GV,V,F,SIGNED_DISTANCE_TYPE_PSEUDONORMAL,S,I,C,N);
    // Convert distances to binary inside-outside data --> aliasing artifacts
    B = S;
    for_each(B.data(),B.data()+B.size(),[](double& b){b=(b>0?1:(b<0?-1:0));});
  }
  cout<<"Marching cubes..."<<endl;
  MatrixXd SV,BV;
  MatrixXi SF,BF;
  igl::copyleft::marching_cubes(S,GV,res(0),res(1),res(2),SV,SF);
  igl::copyleft::marching_cubes(B,GV,res(0),res(1),res(2),BV,BF);

  cout<<R"(Usage:
'1'  Show original mesh.
'2'  Show marching cubes contour of signed distance.
'3'  Show marching cubes contour of indicator function.
)";
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(SV,SF);
  viewer.callback_key_down =
    [&](igl::opengl::glfw::Viewer & viewer, unsigned char key, int mod)->bool
    {
      switch(key)
      {
        default:
          return false;
        case '1':
          viewer.data().clear();
          viewer.data().set_mesh(V,F);
          break;
        case '2':
          viewer.data().clear();
          viewer.data().set_mesh(SV,SF);
          break;
        case '3':
          viewer.data().clear();
          viewer.data().set_mesh(BV,BF);
          break;
      }
      viewer.data().set_face_based(true);
      return true;
    };
  viewer.launch();
}


#include <iostream>
#include <unordered_set>

#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>
#include <boost/dynamic_bitset.hpp>

#include "edlib/Basis/BasisFull.hpp"
#include "edlib/Basis/BasisFullZ2.hpp"
#include "edlib/Basis/ToOriginalBasis.hpp"
#include "edlib/EDP/ConstructSparseMat.hpp"
#include "edlib/Op/NodeMV.hpp"

#include "utils.hpp"
#include "XXZ.hpp"

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

using namespace Eigen;
using namespace edlib;

constexpr uint32_t MAX_N = 14;

template<typename UINT>
class OpenTFI
{
private:
	const edlib::AbstractBasis<UINT>& basis_;
	double J_;
	double h_;

public:
	OpenTFI(const edlib::AbstractBasis<UINT>& basis, double J, double h)
		: basis_{basis}, J_{J}, h_{h}
	{
	}

	std::map<int, double> getCol(UINT n) const
	{
		std::map<int, double> m;
		const uint32_t N = basis_.getN();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(N, a);

		for(uint32_t i = 0; i < N-1; ++i)
		{
			int zz = (1-2*bs[i])*(1-2*bs[i+1]);

			m[n] += -J_*zz;

			UINT s = a;
			s ^= basis_.mask({i});

			int bidx;
			double coeff;

			std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

			if(bidx >= 0)
			{
				m[bidx] += -h_*coeff;
			}
		}
		return m;
	}

	std::map<int, double> operator()(UINT n) const
	{
		return getCol(n);
	}
};

TEST_CASE("Test BasisFull and BasisFullZ2 using the transverse field Ising model", "[basisfull]")
{
	double h = 0.5;
	for(int N = 4; N <= MAX_N; N += 2)
	{
		BasisFull<uint32_t> basisFull(N);
		BasisFullZ2<uint32_t> basisFullP(N, 1);
		BasisFullZ2<uint32_t> basisFullM(N, -1);

		auto hamFull = OpenTFI(basisFull, 1.0, h);
		auto hamFullP = OpenTFI(basisFullP, 1.0, h);
		auto hamFullM = OpenTFI(basisFullM, 1.0, h);

		VectorXd evFull;
		VectorXd evFullP;
		VectorXd evFullM;

		{
			const auto dim = basisFull.getDim();

			NodeMV mv(dim, 0, dim, hamFull);

			Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
			eigs.init();
			eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
			if(eigs.info() != Spectra::SUCCESSFUL)
				REQUIRE(false);
			evFull = eigs.eigenvalues();
		}

		{
			const auto dim = basisFullP.getDim();

			NodeMV mv(dim, 0, dim, hamFullP);

			Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
			eigs.init();
			eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
			if(eigs.info() != Spectra::SUCCESSFUL)
				REQUIRE(false);
			evFullP = eigs.eigenvalues();
		}

		{
			const auto dim = basisFullM.getDim();

			NodeMV mv(dim, 0, dim, hamFullM);

			Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
			eigs.init();
			eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
			if(eigs.info() != Spectra::SUCCESSFUL)
				REQUIRE(false);
			evFullM = eigs.eigenvalues();
		}
		using Catch::WithinAbs;
		REQUIRE_THAT(evFull(0), WithinAbs(evFullP(0), 1e-8));
		REQUIRE_THAT(evFull(1), WithinAbs(evFullM(0), 1e-8));
	}
}


/*
 * exprtk_expression.hpp
 *
 *  Created on: Jul 6, 2015
 *      Author: dimitar
 */

#include "expression.hpp"
#include <exprtk/exprtk.hpp>
#include "context.hpp"
#include <list>
#include <boost/noncopyable.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>

#ifndef EXPRTK_EXPRESSION_HPP_
#define EXPRTK_EXPRESSION_HPP_

namespace kaluun {

namespace exprtk{

template<class Context>
struct expression : public boost::noncopyable{
	typedef double 										result_type;
	typedef ::exprtk::symbol_table<result_type> 		symbol_table_t;
	typedef ::exprtk::expression<result_type> 			expression_t;
	typedef ::exprtk::parser<result_type> 				parser_t;

	struct variable_function: public ::exprtk::ifunction<result_type> , boost::noncopyable {
		const std::string&				variable_;
		Context*						context_;

		variable_function(const std::string& variable): ::exprtk::ifunction<result_type>(0), variable_(variable), context_(nullptr) { }
		inline void set_context(Context* target){
			context_ = target;
		}
		inline result_type operator()() {
			if(context_ != NULL)
				return boost::lexical_cast<result_type>(context_->operator[](variable_).to_string());
			else
				throw std::logic_error("context has not been set");
		}
	};

	struct unknown_symbol_resolver_helper: public parser_t::unknown_symbol_resolver , boost::noncopyable {
		std::unordered_set<std::string>& operands_;

		unknown_symbol_resolver_helper(std::unordered_set<std::string>& operands):operands_(operands){}
		virtual bool process(const std::string& unknown_symbol, usr_symbol_type& st, result_type & default_value, std::string& error_message) {
			st = e_usr_variable_type;
			default_value = 13;
			error_message = "";
			operands_.insert(unknown_symbol);
			return true;
		}
	};

	symbol_table_t 							symbol_table_;
	expression_t 							expression_;
	std::list<variable_function>			functors_;
	std::unordered_set<std::string> 		variables_;

	expression() {
		expression_.register_symbol_table(symbol_table_);
	}

	std::string operator()(Context& ctx) {
		for(auto itFunc = functors_.begin(); itFunc != functors_.end(); itFunc++)
			itFunc->set_context(&ctx);
		expression::result_type res = expression_.value();
		return boost::lexical_cast<std::string>(res);
	}

	template<class Holder>
	static void parse(Holder expression_string, expression& expr) {
		unknown_symbol_resolver_helper 		resolver(expr.variables_);
		expression::parser_t 					parser;
		expression_t 						expression1;
		symbol_table_t 						symbol_table1;

		expression1.register_symbol_table(symbol_table1);
		parser.enable_unknown_symbol_resolver(&resolver);

		parser.compile(std::string(expression_string), expression1); //first run
		for(auto it = expr.variables_.begin(); it != expr.variables_.end(); it++){
			expr.functors_.emplace_back(*it);
			expr.symbol_table_.add_function(*it, expr.functors_.back());
		}
		parser.disable_unknown_symbol_resolver();
		parser.compile(expression_string, expr.expression_);
	}

	static void parse(const std::string& expression_string, expression& expr) {
		unknown_symbol_resolver_helper 		resolver(expr.variables_);
		expression::parser_t 					parser;
		expression_t 						expression1;
		symbol_table_t 						symbol_table1;

		expression1.register_symbol_table(symbol_table1);
		parser.enable_unknown_symbol_resolver(&resolver);

		parser.compile(expression_string, expression1); //first run
		for(auto it = expr.variables_.begin(); it != expr.variables_.end(); it++){
			expr.functors_.emplace_back(*it);
			expr.symbol_table_.add_function(*it, expr.functors_.back());
		}
		parser.disable_unknown_symbol_resolver();
		parser.compile(expression_string, expr.expression_);
	}

};

template<class Context>
struct condition : public boost::noncopyable{
	typedef double 										result_type;
	typedef ::exprtk::symbol_table<result_type> 		symbol_table_t;
	typedef ::exprtk::expression<result_type> 			expression_t;
	typedef ::exprtk::parser<result_type> 				parser_t;

	struct variable_function: public ::exprtk::ifunction<result_type> , boost::noncopyable{
		const std::string&				variable_;
		Context*						context_;

		variable_function(const std::string& variable): ::exprtk::ifunction<result_type>(0), variable_(variable), context_(nullptr) { }

		inline void set_context(Context* target){
			context_ = target;
		}
		inline result_type operator()() {
			if(context_ != NULL)
				return std::stod(context_->operator[](variable_).to_string());
			else
				throw std::logic_error("context has not been set");
		}
	};

	struct unknown_symbol_resolver_helper: public parser_t::unknown_symbol_resolver , boost::noncopyable {
		std::unordered_set<std::string>& operands_;

		unknown_symbol_resolver_helper(std::unordered_set<std::string>& operands):operands_(operands){}
		virtual bool process(const std::string& unknown_symbol, usr_symbol_type& st, result_type & default_value, std::string& error_message) {
			st = e_usr_variable_type;
			default_value = 13;
			error_message = "";
			operands_.insert(unknown_symbol);
			return true;
		}
	};

	symbol_table_t 							symbol_table_;
	expression_t 							expression_;
	std::list<variable_function>			functors_;
	std::unordered_set<std::string> 		variables_;

	condition() {
		expression_.register_symbol_table(symbol_table_);
	}

	bool operator()(Context& ctx) {
		for(auto itFunc = functors_.begin(); itFunc != functors_.end(); itFunc++)
			itFunc->set_context(&ctx);
		return expression_.operator bool();
	}

	template<class Holder>
	static void parse(Holder expression_string, condition& expr) {
		unknown_symbol_resolver_helper 		resolver(expr.variables_);
		condition::parser_t 				parser;
		expression_t 						expression1;
		symbol_table_t 						symbol_table1;

		expression1.register_symbol_table(symbol_table1);
		parser.enable_unknown_symbol_resolver(&resolver);

		parser.compile(std::string(expression_string), expression1); //first run
		for(auto it = expr.variables_.begin(); it != expr.variables_.end(); it++){
			//variable_function functor(expr, *it);
			expr.functors_.emplace_back(*it);
			expr.symbol_table_.add_function(*it, expr.functors_.back());
		}
		parser.disable_unknown_symbol_resolver();
		parser.compile(expression_string, expr.expression_);
	}

	static void parse(const std::string& expression_string, condition& expr) {
		unknown_symbol_resolver_helper 		resolver(expr.variables_);
		condition::parser_t 				parser;
		expression_t 						expression1;
		symbol_table_t 						symbol_table1;

		expression1.register_symbol_table(symbol_table1);
		parser.enable_unknown_symbol_resolver(&resolver);

		parser.compile(expression_string, expression1); //first run
		for(auto it = expr.variables_.begin(); it != expr.variables_.end(); it++){
			//variable_function functor(expr, *it);
			expr.functors_.emplace_back(*it);
			expr.symbol_table_.add_function(*it, expr.functors_.back());
		}
		parser.disable_unknown_symbol_resolver();
		parser.compile(expression_string, expr.expression_);
	}

}; } }

#endif /* EXPRTK_EXPRESSION_HPP_ */


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

  @copyright 2016 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_FUNCTION_ACSC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing acsc capabilities

    inverse cosecant in radian: \f$\arcsin(1/x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = acsc(x);
    @endcode

    @see acscd, acscpi, asin, asin, sin, rec

  **/
  Value acsc(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/acsc.hpp>
#include <boost/simd/function/simd/acsc.hpp>

#endif


/**
 * \file PanFilter.cpp
 */

#include "PanFilter.h"

#include <cmath>
#include <cstdint>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType_>
  PanFilter<DataType_>::PanFilter()
  :Parent(1, 2), law(PAN_LAWS::SINCOS_0_CENTER), pan(0)
  {

  }

  template<typename DataType_>
  PanFilter<DataType_>::~PanFilter()
  {

  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan_law(PAN_LAWS law)
  {
    this->law = law;
  }

  template<typename DataType_>
  typename PanFilter<DataType_>::PAN_LAWS PanFilter<DataType_>::get_pan_law() const
  {
    return law;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::set_pan(double pan)
  {
    if(pan < -1 || pan > 1)
    {
      throw std::out_of_range("Pan must be a value between -1 and 1");
    }
    this->pan = pan;
  }

  template<typename DataType_>
  double PanFilter<DataType_>::get_pan() const
  {
    return pan;
  }

  template<typename DataType_>
  void PanFilter<DataType_>::process_impl(int64_t size) const
  {
    double left_coeff = 1;
    double right_coeff = 1;

    switch(law)
    {
    case PAN_LAWS::SINCOS_0_CENTER:
        left_coeff = std::sqrt(2) * std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
        right_coeff = std::sqrt(2) * std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
        break;
      case PAN_LAWS::SINCOS_3_CENTER:
        left_coeff = std::cos((pan + 1) / 4 * boost::math::constants::pi<double>());
        right_coeff = std::sin((pan + 1) / 4 * boost::math::constants::pi<double>());
        break;
      case PAN_LAWS::SQUARE_0_CENTER:
        left_coeff = std::sqrt(2) * std::sqrt((1 - pan) / 2);
        right_coeff = std::sqrt(2) * std::sqrt((1 + pan) / 2);
        break;
      case PAN_LAWS::SQUARE_3_CENTER:
        left_coeff = std::sqrt((1 - pan) / 2);
        right_coeff = std::sqrt((1 + pan) / 2);
        break;
      case PAN_LAWS::LINEAR_TAPER:
        left_coeff = (1 - pan) / 2;
        right_coeff = (1 + pan) / 2;
        break;
      case PAN_LAWS::BALANCE:
        left_coeff = pan < 0 ? 1 : 1 - pan;
        right_coeff = pan > 0 ? 1 : 1 + pan;
        break;
    }

    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output0 = outputs[0];
    DataType* ATK_RESTRICT output1 = outputs[1];
    for(int64_t i = 0; i < size; ++i)
    {
      *(output0++) = static_cast<DataType>(left_coeff * *input);
      *(output1++) = static_cast<DataType>(right_coeff * *(input++));
    }

  }

  template class PanFilter<std::int16_t>;
  template class PanFilter<std::int32_t>;
  template class PanFilter<int64_t>;
  template class PanFilter<float>;
  template class PanFilter<double>;
}


//    |  /           |
//    ' /   __| _` | __|  _ \   __|
//    . \  |   (   | |   (   |\__ \.
//   _|\_\_|  \__,_|\__|\___/ ____/
//                   Multi-Physics
//
//  License:		 BSD License
//					 Kratos default license: kratos/license.txt
//
//  Main authors:    Michael Andre, https://github.com/msandre
//

// System includes

// External includes
#include <boost/numeric/ublas/matrix_proxy.hpp>

// Project includes
#include "utilities/geometrical_sensitivity_utility.h"
#include "utilities/math_utils.h"

namespace Kratos
{

GeometricalSensitivityUtility::GeometricalSensitivityUtility(const JacobianType& rJ, const ShapeFunctionsLocalGradientType& rDN_De)
: mrJ(rJ), mrDN_De(rDN_De)
{
    KRATOS_TRY;

    Initialize();

    KRATOS_CATCH("");
}

void GeometricalSensitivityUtility::Initialize()
{
    KRATOS_TRY;

    KRATOS_ERROR_IF(mrJ.size1() != mrJ.size2()) << "Non-square Jacobian matrix." << std::endl;

    KRATOS_ERROR_IF(mrJ.size2() != mrDN_De.size2())
        << "Jacobian local-coordinates size (" << mrJ.size2()
        << ") != shape function local-coordinates size(" << mrDN_De.size2()
        << ")." << std::endl;

    mCofactorJ = MathUtils<double>::CofactorMatrix(mrJ);
    mDetJ = MathUtils<double>::DetMat(mrJ);

    KRATOS_CATCH("");
}

void GeometricalSensitivityUtility::CalculateSensitivity(ShapeParameter Deriv, double& rDetJ_Deriv, ShapeFunctionsGradientType& rDN_DX_Deriv) const
{
    KRATOS_TRY;

    rDetJ_Deriv = CalculateDeterminantOfJacobianSensitivity(Deriv);

    MatrixType cofactorJ_deriv = CalculateCofactorOfJacobianSensitivity(Deriv);
    if (rDN_DX_Deriv.size1() != mrDN_De.size1() || rDN_DX_Deriv.size2() != mCofactorJ.size1())
        rDN_DX_Deriv.resize(mrDN_De.size1(), mCofactorJ.size1());
    noalias(rDN_DX_Deriv) = (1.0 / mDetJ) * prod(mrDN_De, trans(cofactorJ_deriv));
    rDN_DX_Deriv += -(rDetJ_Deriv / (mDetJ * mDetJ)) * prod(mrDN_De, trans(mCofactorJ));

    KRATOS_CATCH("");
}

double GeometricalSensitivityUtility::CalculateDeterminantOfJacobianSensitivity(ShapeParameter Deriv) const
{
    return inner_prod(
        row(mCofactorJ, Deriv.Direction),
        row(mrDN_De, Deriv.NodeIndex));
}

GeometricalSensitivityUtility::MatrixType GeometricalSensitivityUtility::CalculateCofactorOfJacobianSensitivity(
    ShapeParameter Deriv) const
{
    KRATOS_TRY;
    MatrixType result(mrJ.size1(), mrJ.size2());

    IndirectArrayType ia3(mrDN_De.size1());
    for (std::size_t k = 0; k < ia3.size(); ++k)
        ia3[k] = k;

    for (unsigned i = 0; i < mrJ.size1(); ++i)
    {
        if (i == Deriv.Direction)
        {
            // Here the derivative is automatically zero.
            for (unsigned j = 0; j < mrJ.size2(); ++j)
                result(i, j) = 0.0;
        }
        else
        {
            // Decrement the coordinate index if it's greater than the deleted row.
            IndexType i_coord_sub = (Deriv.Direction > i) ? Deriv.Direction - 1 : Deriv.Direction;
            for (unsigned j = 0; j < mrJ.size2(); ++j)
            {
#ifdef KRATOS_USE_AMATRIX   // This macro definition is for the migration period and to be removed afterward please do not use it
				DenseVector<std::size_t> ia1(mrJ.size1() - 1), ia2(mrJ.size2() - 1);
#else
				IndirectArrayType ia1(mrJ.size1() - 1), ia2(mrJ.size2() - 1);
#endif // ifdef KRATOS_USE_AMATRIX

                // Construct the Jacobian submatrix structure for the first minor.
                unsigned i_sub = 0;
                for (unsigned k = 0; k < mrJ.size1(); ++k)
                    if (k != i)
                        ia1(i_sub++) = k;

                unsigned j_sub = 0;
                for (unsigned k = 0; k < mrJ.size2(); ++k)
                    if (k != j)
                        ia2(j_sub++) = k;

                const SubMatrixType sub_jacobian(mrJ, ia1, ia2);
                const MatrixType cofactor_sub_jacobian = MathUtils<double>::CofactorMatrix(sub_jacobian);

                // Construct the corresponding shape function local gradients
                // submatrix.
                const SubMatrixType sub_DN_De(mrDN_De, ia3, ia2);

                const double first_minor_deriv = inner_prod(
                    row(cofactor_sub_jacobian, i_coord_sub),
                    row(sub_DN_De, Deriv.NodeIndex));

                result(i, j) = ((i + j) % 2) ? -first_minor_deriv : first_minor_deriv;
            }
        }
    }

    return result;
    KRATOS_CATCH("");
}

} /* namespace Kratos.*/


#pragma once

#include <boost/range.hpp>
#include <math/Vec.hpp>
#include <OpenGLES/ES2/gl.h>
#include <renderer/Camera.hpp>
#include <renderer/Renderable.hpp>

struct Vertex {
	GLfloat pos[3];
	GLfloat tex[2];

	Vec4 get_pos() const { return Vec4(pos[0], pos[1], pos[2], 1.0f); }
};

namespace detail {
	struct ApplyTransformAndProject {
		ApplyTransformAndProject(Camera const * camera, Matrix<4, 4, float> * M) :
			camera_(camera), M_(M) { }

		Vec2 operator () (Vertex const & v) const {
			return camera_->project_to_device_independent(v.get_pos() *  *M_);
		}

		private:
			Camera const * camera_;
			Matrix<4, 4, float> * M_;
	};
}

template <class Iterator>
inline Rectangle<float> get_bounding_rectangle(Camera const & camera, Renderable::FrameType const & f, Iterator begin, Iterator end) {
	float F[] = {
		f[0][0], f[0][1], f[0][2], 0,
		f[1][0], f[1][1], f[1][2], 0,
			  0,       0,       1, 0,
		f[2][0], f[2][1], f[2][2], 1
	};

	Matrix<4, 4, float> M(F);

	return get_bounding_rectangle(begin, end, detail::ApplyTransformAndProject(&camera, &M));
}

template <class Container>
inline Rectangle<float> get_bounding_rectangle(Camera const & camera, Renderable::FrameType const & f, Container const & container) {
	return get_bounding_rectangle(camera, f, boost::begin(container), boost::end(container));
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include "perception/behaviour/KinematicsCalibrationSkill.hpp"
#include "types/RRCoord.hpp"
#include "blackboard/Blackboard.hpp"
#include "types/ActionCommand.hpp"
#include "perception/kinematics/Kinematics.hpp"
#include "perception/kinematics/Pose.hpp"
#include "utils/Logger.hpp"
#include "FADBAD++/fadiff.h"

fadbad::F<float> fadbadAtan2(fadbad::F<float> y, fadbad::F<float> x) {
   fadbad::F<float> PI = 3.1415926535;
   if (x == 0) {
      if (y > 0) {
         return PI / 2;
      } else if (y < 0) {
         return -PI / 2;
      } else {
         return 0;
      }
   } else if (x > 0) {
      return fadbad::atan(y / x);
   } else if (y >= 0 && x < 0) {
      return fadbad::atan(y / x) + PI;
   } else if (y < 0 && x < 0) {
      return fadbad::atan(y / x) - PI;
   } else {
      return 0;
   }
}

KinematicsCalibrationSkill::KinematicsCalibrationSkill(Blackboard *bb) : Adapter(bb) {
   points.push_back(std::make_pair(-3020 + 1200, -2025));
   points.push_back(std::make_pair(1200, -2025));
   points.push_back(std::make_pair(1200, 0));
   points.push_back(std::make_pair(1200, 2025));
   points.push_back(std::make_pair(-3020 + 1200, 2025));
   currentWaypoint = 0;
   beenAtFrameFor = 0;
   takenReading = false;
   resetGradients();
   alpha = MAX_ALPHA;
   isTop = false;
}

KinematicsCalibrationSkill::~KinematicsCalibrationSkill() {}

/* Input
      - Joint Angles
      - Offsets
      - Perceived Location of Object in the Image

   Output
      - Euclidian distance between Perceived and Actual Position

   Processing Steps
      - Set up FADBAD variables for each of the offsets
      - Feed 3 Inputs into Kinematics (May have to change some of the
        the interfaces
      - Deterime Euclidian distance
*/

fadbad::F<float> KinematicsCalibrationSkill::objectiveFunction(
   Parameters<fadbad::F<float> > &parameters) {
   Kinematics kinematics;
   kinematics.sensorValues = readFrom(motion, sensors);
   kinematics.parameters = parameters.cast<float>();

   kinematics.updateDHChain();
   Pose pose = kinematics.getPose();

   /* Image to RR */
   Point cc_p = readFrom(vision, balls[0].imageCoords);
   std::pair<uint16_t, uint16_t>  cc(cc_p.x(), cc_p.y());

   // calculate vector to pixel in camera space
   boost::numeric::ublas::matrix<fadbad::F<float> > lOrigin, lOrigin2;
   fadbad::F<float> imgCols = IMAGE_COLS;
   fadbad::F<float> imgRows = IMAGE_ROWS;
   fadbad::F<float> pixelSize = TOP_PIXEL_SIZE; // TODO: properly if used
   fadbad::F<float> x = cc.first;
   fadbad::F<float> y = cc.second;
   lOrigin2 = vec4<fadbad::F<float> >(
      (((imgCols) / 2.0 - x) *  pixelSize),
      (((imgRows) / 2.0 - y) * pixelSize),
      0,
      1);

   lOrigin = prod(
      pose.getC2wTransform(),
      lOrigin2);

   boost::numeric::ublas::matrix<fadbad::F<float> > toPixel, toPixel2;
   toPixel2 = vec4<fadbad::F<float> >(0, 0, FOCAL_LENGTH, 1);
   toPixel = prod(
      pose.getC2wTransform(),
      toPixel2);

   boost::numeric::ublas::matrix<fadbad::F<float> > cdir(toPixel - lOrigin);

   fadbad::F<float> lambda = (35 - lOrigin(2, 0)) / (1.0 * cdir(2, 0));
   boost::numeric::ublas::matrix<fadbad::F<float> > intercept;
   intercept = vec4<fadbad::F<float> >(lOrigin(0, 0) + lambda * cdir(0, 0),
                                       lOrigin(1, 0) + lambda * cdir(1, 0),
                                       35,
                                       1);
   fadbad::F<float> distance, heading;
   distance = fadbad::sqrt(fadbad::pow(intercept(0, 0), 2) +
                           fadbad::pow(intercept(1, 0), 2));
   heading = fadbadAtan2(intercept(1, 0), intercept(0, 0));
   fadbad::F<float> px, py;
   px = fadbad::cos(heading) * distance;
   py = fadbad::sin(heading) * distance;

   // find which point is closest to the one we see
   float tx, ty;
   tx = px.x();
   ty = py.x();

   int minIndex = 0;
   float rx, ry;
   rx = points[0].first;
   ry = points[0].second;
   float bDistance = sqrt(pow(rx - tx, 2) + pow(ry - ty, 2));
   for (unsigned int i = 1; i < points.size(); i++) {
      rx = points[i].first;
      ry = points[i].second;
      float cDistance = sqrt(pow(rx - tx, 2) + pow(ry - ty, 2));
      if (cDistance < bDistance) {
         bDistance = cDistance;
         minIndex = i;
      }
   }

   fadbad::F<float> bx, by;
   bx = points[minIndex].first;
   by = points[minIndex].second;
   return fadbad::sqrt(fadbad::pow(bx - px, 2) +
                       fadbad::pow(by - py, 2));
}

void KinematicsCalibrationSkill::updateGradient() {
   // read in the parameters
   Parameters<fadbad::F<float> > parameters;
   parameters.cameraYawBottom = readFrom(kinematics,
                                         parameters.cameraYawBottom);
   parameters.cameraPitchBottom = readFrom(kinematics,
                                           parameters.cameraPitchBottom);
   parameters.cameraRollBottom = readFrom(kinematics,
                                          parameters.cameraRollBottom);
   parameters.cameraYawTop = readFrom(kinematics,
                                      parameters.cameraYawTop);
   parameters.cameraPitchTop = readFrom(kinematics,
                                        parameters.cameraPitchTop);
   parameters.cameraRollTop = readFrom(kinematics,
                                       parameters.cameraRollTop);
   parameters.bodyPitch = readFrom(kinematics, parameters.bodyPitch);
   lastParams = parameters;

   // set them to be derived
   parameters.cameraYawBottom.diff(0, 7);
   parameters.cameraPitchBottom.diff(1, 7);
   parameters.cameraRollBottom.diff(2, 7);

   parameters.cameraYawTop.diff(3, 7);
   parameters.cameraPitchTop.diff(4, 7);
   parameters.cameraRollTop.diff(5, 7);

   parameters.bodyPitch.diff(6, 7);

   // calculate objective function
   fadbad::F<float> f = objectiveFunction(parameters);

   value += f.x();
   n += 1;
   // store gradients
   gradients.cameraYawBottom += f.d(0);
   gradients.cameraPitchBottom += f.d(1);
   gradients.cameraRollBottom += f.d(2);
   gradients.cameraYawTop += f.d(3);
   gradients.cameraPitchTop += f.d(4);
   gradients.cameraRollTop += f.d(5);

   gradients.bodyPitch += f.d(6);
}

BehaviourRequest KinematicsCalibrationSkill::execute() {
   BehaviourRequest request;
   if (readFrom(vision, balls).size() > 0 &&
       beenAtFrameFor > STABALIZE_FRAMES) {
      updateGradient();
      takenReading = true;
   }

   // we have gathered point.size number of samples. update and get new batch
   if (true && currentWaypoint == points.size() - 1 &&
       takenReading == true) {
      Parameters<fadbad::F<float> > parameters;
      parameters.cameraYawBottom = readFrom(kinematics,
                                            parameters.cameraYawBottom);
      parameters.cameraPitchBottom = readFrom(kinematics,
                                              parameters.cameraPitchBottom);
      parameters.cameraRollBottom = readFrom(kinematics,
                                             parameters.cameraRollBottom);
      parameters.cameraYawTop = readFrom(kinematics,
                                         parameters.cameraYawTop);
      parameters.cameraPitchTop = readFrom(kinematics,
                                           parameters.cameraPitchTop);
      parameters.cameraRollTop = readFrom(kinematics,
                                          parameters.cameraRollTop);
      parameters.bodyPitch = readFrom(kinematics, parameters.bodyPitch);

      // optimization algorithm goes here...
      gradientDescent(parameters, gradients);

      // write values back for next iteration
      writeTo(kinematics, parameters.cameraYawBottom,
              parameters.cameraYawBottom.x());
      writeTo(kinematics, parameters.cameraPitchBottom,
              parameters.cameraPitchBottom.x());
      writeTo(kinematics, parameters.cameraRollBottom,
              parameters.cameraRollBottom.x());
      writeTo(kinematics, parameters.cameraYawTop,
              parameters.cameraYawTop.x());
      writeTo(kinematics, parameters.cameraPitchTop,
              parameters.cameraPitchTop.x());
      writeTo(kinematics, parameters.cameraRollTop,
              parameters.cameraRollTop.x());

      writeTo(kinematics, parameters.bodyPitch, parameters.bodyPitch.x());
      resetGradients();
      isTop = !isTop;
   }

   std::pair<float, float> cwp = points[currentWaypoint];
   float angle = atan2(cwp.second, cwp.first);
   if (abs(readFrom(motion, sensors).joints.angles[Joints::HeadYaw] - angle) < .01) {
      beenAtFrameFor++;
   }

   if (takenReading == true) {
      beenAtFrameFor = 0;
      currentWaypoint = (currentWaypoint + 1) % points.size();
      takenReading = false;
   }

   // move head to correct spot
   if (isTop) {
      request.actions.head = ActionCommand::Head(angle,
                                                 DEG2RAD(5), false, 0.5f, 0.5f);
   } else {
      request.actions.head = ActionCommand::Head(angle,
                                                 DEG2RAD(-25), false, 0.5f, 0.5f);
   }
   request.actions.body = ActionCommand::Body::STAND;

   return request;
}

void KinematicsCalibrationSkill::resetGradients() {
   n = 0;
   value = 0;
   gradients.cameraYawBottom = 0;
   gradients.cameraPitchBottom = 0;
   gradients.cameraRollBottom = 0;
   gradients.cameraYawTop = 0;
   gradients.cameraPitchTop = 0;
   gradients.cameraRollTop = 0;

   gradients.bodyPitch = 0;
}

bool KinematicsCalibrationSkill::gradientDescent(
   Parameters<fadbad::F<float> > &parameters,
   Parameters<fadbad::F<float> > gradients) {
   parameters.cameraYawBottom -= alpha * gradients.cameraYawBottom / n;
   parameters.cameraPitchBottom -= alpha * gradients.cameraPitchBottom / n;
   parameters.cameraRollBottom -= alpha * gradients.cameraRollBottom / n;
   parameters.cameraYawTop -= alpha * gradients.cameraYawTop / n;
   parameters.cameraPitchTop -= alpha * gradients.cameraPitchTop / n;
   parameters.cameraRollTop -= alpha * gradients.cameraRollTop / n;
   parameters.bodyPitch -= alpha * gradients.bodyPitch / n;
   alpha -= 0.0005;
   if (alpha < MIN_ALPHA) {
      alpha = MIN_ALPHA;
   }

   //static int t = 0;
   std::cout << printParams(parameters) << std::endl;
   std::cout << "Gradients: ";
   std::cout << gradients.cameraYawBottom.x() << " " <<
   gradients.cameraPitchBottom.x() << " " <<
   gradients.cameraRollBottom.x() << " " <<
   gradients.bodyPitch.x() << std::endl;
   std::cout << "Value: " << value / n << std::endl;
   std::cout << "Alpha: " << alpha << std::endl;
   std::cout << std::endl;

   return false;
}

std::string KinematicsCalibrationSkill::printParams(
   Parameters<fadbad::F<float> > &parameters) {
   std::stringstream s;
   std::vector<std::pair<std::string, float> > plist;
   plist.push_back(std::make_pair(
                      "cameraYawBottom", parameters.cameraYawBottom.x()));
   plist.push_back(std::make_pair(
                      "cameraPitchBottom", parameters.cameraPitchBottom.x()));
   plist.push_back(std::make_pair(
                      "cameraRollBottom", parameters.cameraRollBottom.x()));
   plist.push_back(std::make_pair(
                      "cameraYawTop", parameters.cameraYawTop.x()));
   plist.push_back(std::make_pair(
                      "cameraPitchTop", parameters.cameraPitchTop.x()));
   plist.push_back(std::make_pair(
                      "cameraRollTop", parameters.cameraRollTop.x()));

   plist.push_back(std::make_pair(
                      "bodyPitch", parameters.bodyPitch.x()));

   for (unsigned int i = 0; i < plist.size(); i++) {
      s << plist[i].first << "=" << plist[i].second << std::endl;
   }
   return s.str();
}

std::string KinematicsCalibrationSkill::printParams(
   Parameters<float> &parameters) {
   std::stringstream s;
   std::vector<std::pair<std::string, float> > plist;
   plist.push_back(std::make_pair(
                      "cameraYawBottom", parameters.cameraYawBottom));
   plist.push_back(std::make_pair(
                      "cameraPitchBottom", parameters.cameraPitchBottom));
   plist.push_back(std::make_pair(
                      "cameraRollBottom", parameters.cameraRollBottom));
   plist.push_back(std::make_pair(
                      "cameraYawTop", parameters.cameraYawTop));
   plist.push_back(std::make_pair(
                      "cameraPitchTop", parameters.cameraPitchTop));
   plist.push_back(std::make_pair(
                      "cameraRollTop", parameters.cameraRollTop));

   plist.push_back(std::make_pair(
                      "bodyPitch", parameters.bodyPitch));
   for (unsigned int i = 0; i < plist.size(); i++) {
      s << plist[i].first << "=" << plist[i].second << std::endl;
   }
   return s.str();
}


/**
 * \file RIAAFilter.hxx
 */

#include "RIAAFilter.h"
#include <ATK/EQ/helpers.h>

#include <boost/math/tools/polynomial.hpp>

#include <cmath>

namespace ATK
{
  template<typename DataType>
  RIAACoefficients<DataType>::RIAACoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template<typename T>
  void generate_RIAA_coeffs(EQUtilities::ZPK<T>& zpk, gsl::index input_sampling_rate)
  {
    auto pi = boost::math::constants::pi<T>();
    T t1 = 1 / (input_sampling_rate * std::tan(pi / (75e-6 * input_sampling_rate)));
    T t2 = 1 / (input_sampling_rate * std::tan(pi / (318e-6 * input_sampling_rate)));
    T t3 = 1 / (input_sampling_rate * std::tan(pi / (3180e-6 * input_sampling_rate)));

    zpk.k = 318e-6/75e-6 * t2/(t1*t3);
    zpk.z.push_back(-1/t2);
    zpk.p.push_back(-1/t1);
    zpk.p.push_back(-1/t3);

    EQUtilities::zpk_bilinear(input_sampling_rate, zpk);
  }

  template <typename DataType>
  void RIAACoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    EQUtilities::ZPK<CoeffDataType> zpk;

    boost::math::tools::polynomial<CoeffDataType> b{ 1 };
    boost::math::tools::polynomial<CoeffDataType> a{ 1 };

    generate_RIAA_coeffs(zpk, input_sampling_rate);
    EQUtilities::zpk2ba(zpk, b, a);

    auto in_size = std::min(in_order + 1, static_cast<gsl::index>(b.size()));
    for (gsl::index i = 0; i < in_size; ++i)
    {
      coefficients_in[i] = b[i];
    }
    auto out_size = std::min(out_order, static_cast<gsl::index>(a.size() - 1));
    for (gsl::index i = 0; i < out_size; ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  InverseRIAACoefficients<DataType>::InverseRIAACoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void InverseRIAACoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    EQUtilities::ZPK<CoeffDataType> zpk;

    boost::math::tools::polynomial<CoeffDataType> b{ 1 };
    boost::math::tools::polynomial<CoeffDataType> a{ 1 };

    generate_RIAA_coeffs(zpk, input_sampling_rate);
    zpk.z.back() = -.8;
    EQUtilities::zpk2ba(zpk, b, a);

    auto in_size = std::min(in_order + 1, static_cast<gsl::index>(a.size()));
    for (gsl::index i = 0; i < in_size; ++i)
    {
      coefficients_in[i] = a[i] / b[b.size() - 1];
    }
    auto out_size = std::min(out_order, static_cast<gsl::index>(b.size() - 1));
    for (gsl::index i = 0; i < out_size; ++i)
    {
      coefficients_out[i] = -b[i] / b[b.size() - 1];
    }
  }
}