#pragma once

#include "Base/TQuaternion.h"

#include <boost/bind.hpp>

#include "vector.hpp"
#include "gmres.hpp"
#include "integrate.hpp"

template<int DIMX, int DIMUC>
class Model{
public:
	static const int DimX = DIMX;
	static const int DimU = DIMUC;

	using x_t = vector_t<DIMX>;		///< Vector for a state vector
    using u_t =  vector_t<DIMUC>;	///< Vector for a control input
	struct xu_t : vector_t<DIMX + DIMUC>{	///< Integrated structure for x and u
	x_t& x(){ return *(x_t*)this; }
	u_t& u(){ return *(u_t*)((double*)this + DIMX); }
	const x_t& x()const{ return *(const x_t*)this; }
	const u_t& u()const{ return *(const u_t*)((const double*)this + DIMX); }
	};

	x_t    x0;		///< Initial state
	u_t    u0;		///< Initial input

	x_t    x;		///< Current state
	x_t    x1;		///< State in next step
	u_t    u;		///< Control input

	/*-------------- dPhi/dx -------------- */
	virtual void phix(double t, const x_t& x, x_t& phx1) = 0;

	/*-------------- State Equation -------------- */
	virtual void xpfunc(double t, const x_t& x, const u_t& u, x_t& xprime) = 0;

	/*-------------- Costate Equation -------------- */
	virtual void lpfunc(double t, const x_t& lmd, const xu_t& linp, x_t& lprime) = 0;

	/*-------------- Error in Optimality Condition, Hu -------------- */
	virtual void hufunc(double t, const x_t& x, const x_t& lmd, const u_t& u, u_t& hui) = 0;
};

template<class MODEL, int N, int KMAX>
class Controller{
public:
	static const int DimX = MODEL::DimX;	//< Dimension of state vector
	static const int DimU = MODEL::DimU;	//< Dimension of control input
	static const int dv   = N;				//< Number of steps in prediction horizon
	static const int kmax = KMAX;			//< Iteration number of FDGMRES

	using controller_t = Controller<MODEL, N, KMAX>;
	using model_t = MODEL;
    using x_t = typename MODEL::x_t;
    using u_t = typename MODEL::u_t;
    using xu_t = typename MODEL::xu_t;

	/// State sequence
	struct X_t : vector_t<DimX * (N+1)>{
		template<class T> X_t& operator=(const T& v){ assign(v); return *this; }
		x_t& elem(int i){ return ((x_t*)this)[i]; }
		const x_t& elem(int i)const{ return ((const x_t*)this)[i]; }
	};
	/// Control input sequence
	struct U_t : vector_t<DimU * N>{
		template<class T> U_t& operator=(const T& v){ this->assign(v); return *this; }
		u_t& elem(int i){ return ((u_t*)this)[i]; }
		const u_t& elem(int i)const { return ((const u_t*)this)[i]; }
	};

	double tf;		//< Prediction time
	double ht;		//< Control period
	double alpha;	//< Time constant in case of time-dependent horizon(?)
	double zeta;	//< Feedback gain in Eq.9
	double hdir;	//< h in Eq.11
	double rtol;	//< Tolerance on r for calculating the initial control input
	int    dstep;	//< ?

	double htau;	//< Period in prediction horizon (=tf/dv)
	double onemhdir;	//< 1-hdir
	double hdirbht;		//< hdir/ht
	double onemzetahdir; 	//< 1-zeta*hdir
	double ts;		//< t+hdir
	double tauf;	//< Compensated prediction time in prediction horizon

	x_t x1s;
	x_t	lmd0;
	u_t hu0;
	X_t xtau;
	X_t xtau1;
	X_t ltau;
	U_t duvec;
	U_t utau;
	U_t utau1;
	U_t hutau;
	U_t hutau1;
	U_t hutau2;
	U_t bvec;
	//U_t dutmp;
	U_t utautmp;
	//U_t ptau; //< PrARXモデルのモード出力用　@2018.07.13

	vector_t<dv  +1> tau;
	vector_t<kmax+1> errvec;

	model_t*	model;

public:
	// Constructor
	Controller(){}
	Controller(
               double in_ht,
               double in_zeta,
               double in_hdir,
               double in_tf,
               double in_alpha,
               double in_rtol,
               int in_dstep):
            ht(in_ht), tf(in_tf), alpha(in_alpha),
            zeta(in_zeta), hdir(in_hdir),
            rtol(in_rtol), dstep(in_dstep) {}

	/*-------------- Initial Conditions -------------- */
	/*struct dhu0func{
		void operator()(const u_t& du0, u_t& dhu){
			u_t u;
			u_t hu;

			u = *(u_t*)u0 + hdir * du0;
			hufunc(tsim0, x0, lmd0, u, hu);
			dhu = (hu - hu0) / hdir;
		}
	};*/

	void init(model_t* m){
		model = m;

		onemhdir     = 1 - hdir / ht;
		hdirbht      = hdir / ht;
		onemzetahdir = 1 - zeta * hdir;

		int i;
		//double r;
		vector_t<DimU>   b;
		vector_t<DimU>   du0;
		vector_t<DimU+1> erru0;
		model->x = model->x0;
		// phi_x(x, t)
		model->phix(tsim0, model->x0, lmd0);
		// Hu(
		model->hufunc(tsim0, model->x0, lmd0, model->u0, hu0);
		/*
		du0.clear();
		r = hu0.norm();
		i = 0;

		// 0ステップの最適化問題を解き，最適入力u0を求める
		while( r > rtol && i < 100 ){
			b = -hu0;
			nfgmres<dhu0func, u_t, DIMUC>(b, du0, erru0);
			*(u_t*)u0 += du0;
			hufunc(tsim0, x0, lmd0, u0, hu0);
			r = hu0.norm();
			i++;
		}
		*/
		// 全時刻のuをu0で埋める
		model->u = model->u0;
		for(i=0; i<dv; i++){
			utau .elem(i) = model->u;
			hutau.elem(i) = hu0;
		}
		duvec.clear();
	}

	/*-------------- Control Update -------------- */

	// F(U,x,t)
	void errfunc(double t, const x_t& x, const U_t& u, U_t& hu)
	{
		int i;
		double taut;
		xu_t linp;

		// Time period in prediction horizon
		//tauf = tf * (1.0 - exp(-alpha * t));
		tauf = tf;
		htau = tauf / dv;

		// Fill the initial state with x given
		xtau.elem(0) = x;

		// Calculate state sequence by recursive forward difference calculation(Eq.(1)）
		x_t xd;
		//u_t pd;
		for(taut = t, i=0; i < dv; taut += htau, i++){
			model->xpfunc(taut, xtau.elem(i), u.elem(i), xd);
			//model->xpfunc(taut, xtau.elem(i), u.elem(i), xd, pd);
			xtau.elem(i+1) = xtau.elem(i) + htau * xd;
			tau[i] = taut;
			//ptau.elem(i) = pd;
		}
		tau[i] = taut;

		// Final condition on the costate lambda（Eq.(7)）
		model->phix(taut, xtau.elem(dv), ltau.elem(dv));

		// Calculate costate sequence by recursive backward difference calculation（Eq.(6)）
		x_t ld;
		for(i = dv-1; i >= 0; i--){
			linp.x() = xtau.elem(i);
			linp.u() = u.elem(i);
			model->lpfunc(taut, ltau.elem(i+1), linp, ld);
			ltau.elem(i) = ltau.elem(i+1) - htau * ld;
			taut -= htau;
			// Calculate Hu from predicted x and lambda（Eq.(5)）
			model->hufunc(taut, xtau.elem(i), ltau.elem(i+1), ((U_t&)u).elem(i), hu.elem(i));
		}
	}

	void adufunc(const U_t& du, U_t& adu){
		// U + h dU
		utau1 = utau + du * hdir;
		// F(U + hdU, x + h xd, t + h)
		errfunc(ts, x1s, utau1, hutau2);
		// F_U dU approx [F(U + h dU, x + h xd, t + h) - F(U, x + h xd, t + h)] / h
		adu = (hutau2 - hutau1) / hdir;
	}
	struct call_adufunc{
		controller_t* ctrl;
		void operator()(const U_t& du, U_t& adu){
			ctrl->adufunc(du, adu);
		}
		call_adufunc(controller_t* c):ctrl(c){}
	};

	void unew(double t, const x_t& x, const x_t& x1, u_t& u){
		// t + h
		ts = t + hdir;

		 //前進差分計算のためにxの時間微分xdが必要だが，実際の計算ではxdを求めないため，ここで差分で近似している
		 //xd = (x1 - x) / dt
		 //x1s = x + h xd = x + (x1 - x) * (h/dt) = (h/dt) x1 + (1 - h/dt) x
		x1s = hdirbht * x1 + x * onemhdir;

		// F(t, x, u)
		errfunc(t, x, utau, hutau);

		// F(t + h, x + h xd, u)
		errfunc(ts, x1s, utau, hutau1);

		////< x1s = x1 + h xd = x1 + (x1 - x) * (h/dt) = -(h/dt)x + (1 + h/dt)x1
		//x1s = -hdirbht * x + x1 * onephdir;

		//// F(t, x1, u)
		//errfunc(ts, x1, utau, hutau);

		//// F(t + h, x1 + h xd, u)
		//errfunc(t, x1s, utau, hutau1);

		// b on Eq.(11)
		// b = As F(U,x,t) - DhF(U,x,t:0,xd,1),
		// From DhF(U,x,t:0,xd,1) = (F(U, x + h xd, t+h) - F(U,x,t))/h,
		// b = [(1 + h As) F(U,x,t) - F(U, x + h xd, t+h)] / h
		bvec = (hutau * onemzetahdir - hutau1) / hdir;

		// Solve F_U Ud = b by GMRES
		// adufunc is a function that gives A*dU while A is F_U here.
		nfgmres<call_adufunc, U_t, kmax>(call_adufunc(this), bvec, duvec, errvec);

		// U += dU * ht;
		utau += ht * duvec;

		// Limit the control inputs with upper and lower bound
		for(int i = 0; i < N; i++)for(int j = 0; j < model->NCar; j++){
			if(utau.elem(i)[j] > model->umax){
					utau.elem(i)[j] = model->umax;
			}
			else if(utau.elem(i)[j] < model->umin){
					utau.elem(i)[j] = model->umin;
			}
			duvec = (utau - utautmp)/ht;
		}
		errfunc(t,x1,utau,hutau1);

		//double lim = 1000;
		//if(abs(hutau1.norm()) > lim){
		//for(int i = 0; i < N; i++)for(int j = 0; j < modeltmp.NCar; j++){
		//	if(xtau.elem(i)[3*j+0] <= modeltmp.Dmin+0.1){
		//		utau.elem(i)[j] *= utau.elem(i)[j]>0?0.1:1.5;
		//		//if(utau.elem(i)[j] < modeltmp.umin[j]) utau.elem(i)[j] = modeltmp.umin[j];
		//		//utau.elem(i)[j] = modeltmp.umin[j];
		//	}
		//	if(xtau.elem(i)[3*j+0] >= modeltmp.Dmin && abs(utau.elem(i)[2*modeltmp.NCar + j])>lim){
		//		utau.elem(i)[1*modeltmp.NCar+j] = sqrt(xtau.elem(i)[3*j+0]-modeltmp.Dmin);
		//		utau.elem(i)[2*modeltmp.NCar+j] = modeltmp.s1[j]/(2*modeltmp.sc[j]*utau.elem(i)[1*modeltmp.NCar+j]);
		//	}
		//}
		//duvec = (utau - utautmp)/ht;
		//errfunc(t,x1,utau,hutau1);
		//}

		//// もし更新したutauでのhutauが更新前より悪化した場合は戻す
		//if(hutau1.norm() > hutau.norm()){
		//	utau = utautmp;
		//	duvec = (utau-utautmp)/ht;
		//	errfunc(t,x1,utau,hutau1);
		//}

		// Apply the first elements of U as actual input u
		u = utau.elem(0);
		utautmp = utau;
		//dutmp = duvec;
	}

	private:
        const double tsim0 = 0.0;
};


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

// 就像以前的例子一样，我们必须包括几个文件，其中的含义已经讨论过了。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/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/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_out.h>

// 下面两个文件提供了多线程程序的类和信息。在第一个文件中，声明了我们需要做并行装配的类和函数（即
// WorkStream
// 命名空间）。第二个文件有一个类MultithreadInfo，可以用来查询系统中的处理器数量，这在决定启动多少个并行线程时通常很有用。

#include <deal.II/base/work_stream.h>
#include <deal.II/base/multithread_info.h>

// 下一个新的include文件声明了一个基类 TensorFunction ，与
// Function 类不一样，但不同的是 TensorFunction::value
// 返回一个张量而不是一个标量。

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

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

// 这是C++，因为我们想把一些输出写入磁盘。

#include <fstream>
#include <iostream>

// 最后一步和以前的程序一样。

namespace Step9
{
  using namespace dealii;
  // @sect3{Equation data declaration}

  // 接下来我们声明一个描述平流场的类。当然，这是一个矢量场，有多少分量就有多少空间维度。现在我们可以使用一个从
  // Function
  // 基类派生出来的类，就像我们在前面的例子中对边界值和系数所做的那样，但是在库中还有另一种可能性，即一个描述张量值函数的基类。这比重写
  // Function::value()
  // 知道多个函数成分的方法更方便：最后我们需要一个张量，所以我们不妨直接使用一个返回张量的类。

  template <int dim>
  class AdvectionField : public TensorFunction<1, dim>
  {
  public:
    virtual Tensor<1, dim> value(const Point<dim> &p) const override;

    // 在前面的例子中，我们已经在多个地方使用了抛出异常的断言。但是，我们还没有看到如何声明这种异常。这可以这样做。

    DeclException2(ExcDimensionMismatch,
                   unsigned int,
                   unsigned int,
                   << "The vector has size " << arg1 << " but should have "
                   << arg2 << " elements.");

    // 语法可能看起来有点奇怪，但很合理。其格式基本如下：使用其中一个宏的名称
    // DeclExceptionN, where N
    // 表示异常对象应采取的附加参数的数量。在本例中，由于我们想在两个向量的大小不同时抛出异常，我们需要两个参数，所以我们使用
    // DeclException2
    // 。第一个参数描述了异常的名称，而下面的参数则声明了参数的数据类型。最后一个参数是一连串的输出指令，这些指令将被输送到
    // std::cerr  对象中，因此出现了奇怪的格式，前面是
    // @<@<  操作符之类的。注意，我们可以通过使用名称
    // arg1 through argN  来访问在构造时（即在
    // Assert  调用中）传递给异常的参数，其中  N
    // 是通过使用各自的宏  DeclExceptionN  来定义的参数数。

    // 要了解预处理器如何将这个宏扩展为实际代码，请参考异常类的文档。简而言之，这个宏调用声明并定义了一个继承自
    // ExceptionBase 的类  ExcDimensionMismatch
    // ，它实现了所有必要的错误输出功能。
  };

  // 下面的两个函数实现了上述的接口。第一个简单地实现了介绍中所描述的函数，而第二个使用了同样的技巧来避免调用虚拟函数，在前面的例子程序中已经介绍过了。注意第二个函数中对参数的正确大小的检查，这种检查应该始终存在于这类函数中；根据我们的经验，许多甚至大多数编程错误都是由不正确的初始化数组、不兼容的函数参数等造成的；像本例中那样使用断言可以消除许多这样的问题。

  template <int dim>
  Tensor<1, dim> AdvectionField<dim>::value(const Point<dim> &p) const
  {
    Tensor<1, dim> value;
    value[0] = 2;
    for (unsigned int i = 1; i < dim; ++i)
      value[i] = 1 + 0.8 * std::sin(8. * numbers::PI * p[0]);

    return value;
  }

  // 除了平流场，我们还需要两个描述源项（  right hand side
  // ）和边界值的函数。如介绍中所述，源是一个源点附近的常数函数，我们用常数静态变量
  // center_point  表示。我们使用与我们在 step-7
  // 示例程序中所示相同的模板技巧来设置这个中心的值。剩下的就很简单了，之前已经展示过了。

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

  private:
    static const Point<dim> center_point;
  };

  template <>
  const Point<1> RightHandSide<1>::center_point = Point<1>(-0.75);

  template <>
  const Point<2> RightHandSide<2>::center_point = Point<2>(-0.75, -0.75);

  template <>
  const Point<3> RightHandSide<3>::center_point = Point<3>(-0.75, -0.75, -0.75);

  // 这里唯一的新东西是我们检查 component
  // 参数的值。由于这是一个标量函数，很明显，只有当所需分量的索引为0时才有意义，所以我们断言这确实是这样的。
  // ExcIndexRange
  // 是一个全局预定义的异常（可能是最经常使用的异常，因此我们让它成为全局的，而不是某个类的局部），它需要三个参数：超出允许范围的索引，有效范围的第一个元素和超过最后一个的元素（即又是C++标准库中经常使用的半开放区间）。

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    const double diameter = 0.1;
    return ((p - center_point).norm_square() < diameter * diameter ?
              0.1 / std::pow(diameter, dim) :
              0.0);
  }

  // 最后是边界值，这只是从 Function 基类派生的另一个类。

  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
  {
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    const double sine_term = std::sin(16. * numbers::PI * p.norm_square());
    const double weight    = std::exp(5. * (1. - p.norm_square()));
    return weight * sine_term;
  }
  // @sect3{AdvectionProblem class declaration}

  // 这里是这个程序的主类。它和前面的例子中的主类非常相似，所以我们再次只对其不同之处进行评论。

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

  private:
    void setup_system();

    // 下一组函数将被用来组装矩阵。然而，与前面的例子不同，
    // assemble_system()
    // 函数不会自己做这些工作，而是将实际的装配工作委托给辅助函数
    // assemble_local_system()  和
    // copy_local_to_global()
    // 。其原理是，矩阵组装可以很好地并行化，因为每个单元的局部贡献的计算完全独立于其他单元，我们只需要在将一个单元的贡献添加到全局矩阵中时进行同步。

    // 我们在这里选择的并行化策略是文档中 @ref threads 模块中详细提及的可能性之一。具体来说，我们将使用那里讨论的WorkStream方法。由于这个模块有很多文档，我们不会在这里重复设计选择的理由（例如，如果你读完上面提到的模块，你会明白 AssemblyScratchData 和 AssemblyCopyData 结构的目的是什么）。相反，我们将只讨论具体的实现。

    // 如果你阅读了上面提到的页面，你会发现为了使汇编并行化，我们需要两个数据结构--一个对应于我们在局部集成过程中需要的数据（"scratch
    // data"，即我们只需要作为临时存储的东西），另一个是将信息从局部集成携带到函数中，然后将局部贡献添加到全局矩阵的相应元素中。其中前者通常包含FEValues和FEFaceValues对象，而后者则有局部矩阵、局部右手边，以及关于哪些自由度生活在我们正在组装局部贡献的单元上的信息。有了这些信息，下面的内容应该是相对不言自明的。

    struct AssemblyScratchData
    {
      AssemblyScratchData(const FiniteElement<dim> &fe);
      AssemblyScratchData(const AssemblyScratchData &scratch_data);

      // FEValues和FEFaceValues是很昂贵的设置对象，所以我们把它们包含在scratch对象中，以便尽可能多的数据在单元格之间被重复使用。

      FEValues<dim>     fe_values;
      FEFaceValues<dim> fe_face_values;

      // 我们还存储了一些向量，我们将在每个单元格上填充数值。在通常情况下，设置这些对象是很便宜的；但是，它们需要内存分配，这在多线程应用程序中可能很昂贵。因此，我们把它们保存在这里，这样在一个单元格上的计算就不需要新的分配。

      std::vector<double>         rhs_values;
      std::vector<Tensor<1, dim>> advection_directions;
      std::vector<double>         face_boundary_values;
      std::vector<Tensor<1, dim>> face_advection_directions;

      // 最后，我们需要描述该问题数据的对象。

      AdvectionField<dim> advection_field;
      RightHandSide<dim>  right_hand_side;
      BoundaryValues<dim> boundary_values;
    };

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

    void assemble_system();
    void local_assemble_system(
      const typename DoFHandler<dim>::active_cell_iterator &cell,
      AssemblyScratchData &                                 scratch,
      AssemblyCopyData &                                    copy_data);
    void copy_local_to_global(const AssemblyCopyData &copy_data);

    // 下面的函数又和前面的例子一样，后面的变量也是一样的。

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

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

    FE_Q<dim> fe;

    AffineConstraints<double> hanging_node_constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

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

  //  @sect3{GradientEstimation class declaration}

  // 现在，最后，这里有一个类，它将计算每个单元上梯度的差分近似值，并以网格大小的幂数进行权衡，如介绍中所述。这个类是库中
  // DerivativeApproximation
  // 类的一个简单版本，它使用类似的技术来获得有限元场的梯度的有限差分近似值，或者更高导数。

  // 该类有一个公共静态函数 estimate
  // ，被调用来计算误差指标的向量，还有一些私有函数，在所有活动单元上做实际工作。在库的其他部分，我们遵循一个非正式的惯例，使用浮点数向量作为误差指标，而不是常见的双数向量，因为对于估计值来说，额外的精度是没有必要的。

  // 除了这两个函数，该类还声明了两个异常，当一个单元在每个空间方向上都没有邻居时（在这种情况下，介绍中描述的矩阵将是奇异的，不能被倒置），而另一个异常用于更常见的函数参数无效的情况，即一个大小错误的向量。

  // 还有两点意见：首先，这个类没有非静态成员函数或变量，所以这不是一个真正的类，而是起到了C++中
  // namespace
  // 的作用。我们选择类而不是命名空间的原因是，这种方式我们可以声明私有的函数。如果在命名空间的头文件中声明一些函数，并在实现文件中实现这些函数和其他函数，这也可以用命名空间来实现。没有在头文件中声明的函数仍然在名字空间中，但不能从外部调用。然而，由于我们这里只有一个文件，在目前的情况下不可能隐藏函数。

  // 第二个意见是，维度模板参数被附在函数上，而不是附在类本身。这样，你就不必像其他大多数情况下那样自己指定模板参数，而是编译器可以从作为第一个参数传递的DoFHandler对象的尺寸中自行计算出其值。

  // 在开始实施之前，让我们也来评论一下并行化策略。我们已经在上面这个程序的主类的声明中介绍了使用WorkStream概念的必要框架。我们将在这里再次使用它。在目前的情况下，这意味着我们必须定义
  //
  。
  //
 类，用于抓取和复制对象，
  。
  //
  一个在一个单元上进行局部计算的函数，以及

  //
  一个将本地结果复制到全局对象的函数。

  //

  // 鉴于这个总体框架，我们将稍微偏离它。特别是，WorkStream一般是为这样的情况而发明的，即每个单元上的局部计算adds到一个全局对象--例如，在组装线性系统时，我们将局部贡献添加到全局矩阵和右手边中。WorkStream的设计是为了处理多个线程试图同时进行这种添加的潜在冲突，因此必须提供一些方法来确保每次只有一个线程可以做这个。然而，这里的情况略有不同：我们单独计算每个单元的贡献，但随后我们需要做的是将它们放入每个单元独有的输出向量中的一个元素。因此，不存在来自两个单元的写操作可能发生冲突的风险，也没有必要使用WorkStream的复杂机制来避免冲突的写操作。因此，我们要做的就是这样。我们仍然需要一个持有例如
  // FEValues 对象的 scratch
  // 对象。但是，我们只创建一个假的、空的拷贝数据结构。同样，我们确实需要计算本地贡献的函数，但由于它已经可以把结果放到最终位置，我们不需要一个从本地到全球的拷贝函数，而是给
  // WorkStream::run() 函数一个空函数对象--相当于一个NULL函数指针。

  class GradientEstimation
  {
  public:
    template <int dim>
    static void estimate(const DoFHandler<dim> &dof,
                         const Vector<double> & solution,
                         Vector<float> &        error_per_cell);

    DeclException2(ExcInvalidVectorLength,
                   int,
                   int,
                   << "Vector has length " << arg1 << ", but should have "
                   << arg2);
    DeclException0(ExcInsufficientDirections);

  private:
    template <int dim>
    struct EstimateScratchData
    {
      EstimateScratchData(const FiniteElement<dim> &fe,
                          const Vector<double> &    solution,
                          Vector<float> &           error_per_cell);
      EstimateScratchData(const EstimateScratchData &data);

      FEValues<dim> fe_midpoint_value;
      std::vector<typename DoFHandler<dim>::active_cell_iterator>
        active_neighbors;

      const Vector<double> &solution;
      Vector<float> &       error_per_cell;

      std::vector<double> cell_midpoint_value;
      std::vector<double> neighbor_midpoint_value;
    };

    struct EstimateCopyData
    {};

    template <int dim>
    static void
    estimate_cell(const typename DoFHandler<dim>::active_cell_iterator &cell,
                  EstimateScratchData<dim> &scratch_data,
                  const EstimateCopyData &  copy_data);
  };

  //  @sect3{AdvectionProblem class implementation}

  // 现在是主类的实现。构造器、析构器和函数 setup_system
  // 遵循之前使用的模式，所以我们不需要对这三个函数进行评论。

  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem() /* 第一步，类构造函数 */
    : dof_handler(triangulation)
    , fe(5)
  {}

  template <int dim>
  void AdvectionProblem<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    hanging_node_constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler,
                                            hanging_node_constraints);
    hanging_node_constraints.close();

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

    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);

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

  // 在下面的函数中，矩阵和右手被组装起来。正如上面main类的文档所述，它本身并不做这个，而是委托给接下来的函数，利用 @ref threads 中讨论的WorkStream概念。

  // 如果你看了 @ref threads 模块，你会发现并行装配并不需要大量的额外代码，只要你认真地描述什么是从头开始和复制数据对象，如果你为本地装配和从本地贡献到全局对象的复制操作定义了合适的函数。完成这些工作后，下面将完成所有繁重的工作，使这些操作在多个线程上完成，只要你的系统有多少个内核。

  template <int dim>
  void AdvectionProblem<dim>::assemble_system()
  {
    WorkStream::run(dof_handler.begin_active(),
                    dof_handler.end(),
                    *this,
                    &AdvectionProblem::local_assemble_system,
                    &AdvectionProblem::copy_local_to_global,
                    AssemblyScratchData(fe),
                    AssemblyCopyData());
  }

  // 正如上面已经提到的，我们需要有抓取对象来进行局部贡献的并行计算。这些对象包含FEValues和FEFaceValues对象（以及一些数组），因此我们需要有构造函数和复制构造函数，以便我们能够创建它们。对于单元项，我们需要形状函数的值和梯度、正交点以确定给定点的源密度和平流场，以及正交点的权重乘以这些点的雅各布系数的行列式。相反，对于边界积分，我们不需要梯度，而是需要单元的法向量。这决定了我们必须将哪些更新标志传递给类的成员的构造函数。

  template <int dim>
  AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const FiniteElement<dim> &fe)
    : fe_values(fe,
                QGauss<dim>(fe.degree + 1),
                update_values | update_gradients | update_quadrature_points |
                  update_JxW_values)
    , fe_face_values(fe,
                     QGauss<dim - 1>(fe.degree + 1),
                     update_values | update_quadrature_points |
                       update_JxW_values | update_normal_vectors)
    , rhs_values(fe_values.get_quadrature().size())
    , advection_directions(fe_values.get_quadrature().size())
    , face_boundary_values(fe_face_values.get_quadrature().size())
    , face_advection_directions(fe_face_values.get_quadrature().size())
  {}

  template <int dim>
  AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(),
                scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points |
                  update_JxW_values)
    , fe_face_values(scratch_data.fe_face_values.get_fe(),
                     scratch_data.fe_face_values.get_quadrature(),
                     update_values | update_quadrature_points |
                       update_JxW_values | update_normal_vectors)
    , rhs_values(scratch_data.rhs_values.size())
    , advection_directions(scratch_data.advection_directions.size())
    , face_boundary_values(scratch_data.face_boundary_values.size())
    , face_advection_directions(scratch_data.face_advection_directions.size())
  {}

  // 现在，这就是做实际工作的函数。它与前面例子程序中的
  // assemble_system
  // 函数没有什么不同，所以我们将再次只对其不同之处进行评论。数学上的东西紧跟我们在介绍中所说的。

  // 不过，这里有一些值得一提的地方。首先，我们把FEValues和FEFaceValues对象移到了ScratchData对象中。我们这样做是因为我们每次进入这个函数时都要简单地创建一个，也就是在每个单元格上。现在发现，FEValues类的编写目标很明确，就是将所有从单元格到单元格保持不变的东西都移到对象的构造中，每当我们移到一个新单元格时，只在
  // FEValues::reinit()
  // 做尽可能少的工作。这意味着在这个函数中创建一个这样的新对象是非常昂贵的，因为我们必须为每一个单元格都这样做--这正是我们想通过FEValues类来避免的事情。相反，我们所做的是在抓取对象中只创建一次（或少数几次），然后尽可能多地重复使用它。

  // 这就引出了一个问题：我们在这个函数中创建的其他对象，与它的使用相比，其创建成本很高。事实上，在函数的顶部，我们声明了各种各样的对象。
  // AdvectionField  ,  RightHandSide and
  // BoundaryValues
  // 的创建成本并不高，所以这里没有什么危害。然而，在创建
  // rhs_values
  // 和下面类似的变量时，分配内存通常要花费大量的时间，而只是访问我们存储在其中的（临时）值。因此，这些将是移入
  // AssemblyScratchData 类的候选者。我们将把这作为一个练习。

  template <int dim>
  void AdvectionProblem<dim>::local_assemble_system(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    AssemblyScratchData &                                 scratch_data,
    AssemblyCopyData &                                    copy_data)
  {
    // 我们定义一些缩写，以避免不必要的长行。

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points =
      scratch_data.fe_values.get_quadrature().size();
    const unsigned int n_face_q_points =
      scratch_data.fe_face_values.get_quadrature().size();

    // 我们声明单元格矩阵和单元格右侧...

    copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
    copy_data.cell_rhs.reinit(dofs_per_cell);

    // ...一个数组，用于保存我们目前正在处理的单元格的自由度的全局索引...

    copy_data.local_dof_indices.resize(dofs_per_cell);

    // ...然后初始化 FEValues 对象...

    scratch_data.fe_values.reinit(cell);

    // ... 获得正交点的右手边和平流方向的数值...

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

    // ... 设置流线扩散参数的值，如介绍中所述...

    const double delta = 0.1 * cell->diameter();

    // ...... 并按照上面的讨论，集合对系统矩阵和右手边的局部贡献。

    for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        {
          // 别名AssemblyScratchData对象，以防止行数过长。

          const auto &sd = scratch_data;
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) +=
              ((sd.fe_values.shape_value(i, q_point) +           // (phi_i +
                delta * (sd.advection_directions[q_point] *      // delta beta
                         sd.fe_values.shape_grad(i, q_point))) * // grad phi_i)
               sd.advection_directions[q_point] *                // beta
               sd.fe_values.shape_grad(j, q_point)) *            // grad phi_j
              sd.fe_values.JxW(q_point);                         // dx

          copy_data.cell_rhs(i) +=
            (sd.fe_values.shape_value(i, q_point) +           // (phi_i +
             delta * (sd.advection_directions[q_point] *      // delta beta
                      sd.fe_values.shape_grad(i, q_point))) * // grad phi_i)
            sd.rhs_values[q_point] *                          // f
            sd.fe_values.JxW(q_point);                        // dx
        }

    // 除了我们现在建立的单元项，本问题的双线性形式还包含域的边界上的项。因此，我们必须检查这个单元的任何一个面是否在域的边界上，如果是的话，也要把这个面的贡献集合起来。当然，双线性形式只包含来自边界
    // inflow
    // 部分的贡献，但要找出本单元的某个面是否属于流入边界的一部分，我们必须有关于正交点的确切位置和该点的流动方向的信息；我们使用FEFaceValues对象获得这些信息，并只在主循环中决定某个正交点是否在流入边界上。

    for (const auto &face : cell->face_iterators())
      if (face->at_boundary())
        {
          // 好的，当前单元格的这个面是在域的边界上。就像我们在前面的例子和上面的例子中使用的通常的FEValues对象一样，我们必须重新初始化当前面的FEFaceValues对象。

          scratch_data.fe_face_values.reinit(cell, face);

          // 对于手头的正交点，我们要求提供流入函数的值和流动方向。

          scratch_data.boundary_values.value_list(
            scratch_data.fe_face_values.get_quadrature_points(),
            scratch_data.face_boundary_values);
          scratch_data.advection_field.value_list(
            scratch_data.fe_face_values.get_quadrature_points(),
            scratch_data.face_advection_directions);

          // 现在循环所有正交点，看看这个面是在边界的流入还是流出部分。法向量指向单元外：由于该面处于边界，法向量指向域外，所以如果平流方向指向域内，其与法向量的标量乘积一定是负的（要知道为什么会这样，请考虑使用余弦的标量乘积定义）。

          for (unsigned int q_point = 0; q_point < n_face_q_points; ++q_point)
            if (scratch_data.fe_face_values.normal_vector(q_point) *
                  scratch_data.face_advection_directions[q_point] <
                0.)

              // 如果该面是流入边界的一部分，则使用从FEFaceValues对象中获得的值和介绍中讨论的公式，计算该面对全局矩阵和右侧的贡献。

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                {
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    copy_data.cell_matrix(i, j) -=
                      (scratch_data.face_advection_directions[q_point] *
                       scratch_data.fe_face_values.normal_vector(q_point) *
                       scratch_data.fe_face_values.shape_value(i, q_point) *
                       scratch_data.fe_face_values.shape_value(j, q_point) *
                       scratch_data.fe_face_values.JxW(q_point));

                  copy_data.cell_rhs(i) -=
                    (scratch_data.face_advection_directions[q_point] *
                     scratch_data.fe_face_values.normal_vector(q_point) *
                     scratch_data.face_boundary_values[q_point] *
                     scratch_data.fe_face_values.shape_value(i, q_point) *
                     scratch_data.fe_face_values.JxW(q_point));
                }
        }

    // 复制程序需要的最后一条信息是这个单元上自由度的全局索引，所以我们最后把它们写到本地数组中。

    cell->get_dof_indices(copy_data.local_dof_indices);
  }

  // 我们需要写的第二个函数是将前一个函数计算出的本地贡献（并放入AssemblyCopyData对象）复制到全局矩阵和右侧向量对象。这基本上就是我们在每个单元上装配东西时，一直作为最后一块代码的内容。因此，下面的内容应该是很明显的。

  template <int dim>
  void
  AdvectionProblem<dim>::copy_local_to_global(const AssemblyCopyData &copy_data)
  {
    hanging_node_constraints.distribute_local_to_global(
      copy_data.cell_matrix,
      copy_data.cell_rhs,
      copy_data.local_dof_indices,
      system_matrix,
      system_rhs);
  }

  // 这里是线性求解程序。由于系统不再像以前的例子那样是对称正定的，我们不能再使用共轭梯度法。相反，我们使用一个更通用的，不依赖矩阵的任何特殊属性的求解器：GMRES方法。GMRES和共轭梯度法一样，需要一个合适的预处理程序：我们在这里使用一个雅可比预处理程序，它对这个问题来说足够好。

  template <int dim>
  void AdvectionProblem<dim>::solve()
  {
    SolverControl               solver_control(std::max<std::size_t>(1000,
                                                       system_rhs.size() / 10),
                                 1e-10 * system_rhs.l2_norm());
    SolverGMRES<Vector<double>> solver(solver_control);
    PreconditionJacobi<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.0);
    solver.solve(system_matrix, solution, system_rhs, preconditioner);

    Vector<double> residual(dof_handler.n_dofs());

    system_matrix.vmult(residual, solution);
    residual -= system_rhs;
    std::cout << "   Iterations required for convergence: "
              << solver_control.last_step() << '\n'
              << "   Max norm of residual:                "
              << residual.linfty_norm() << '\n';

    hanging_node_constraints.distribute(solution);
  }

  // 下面的函数根据介绍中描述的数量来细化网格。各自的计算是在类
  // GradientEstimation  中进行的。

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

    GradientEstimation::estimate(dof_handler,
                                 solution,
                                 estimated_error_per_cell);

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

    triangulation.execute_coarsening_and_refinement();
  }

  // 这个函数与第6步中的函数类似，但由于我们使用的是高阶有限元，所以我们以不同的方式保存解决方案。像VisIt和Paraview这样的可视化程序通常只能理解与节点相关的数据：它们不能绘制五度基函数，这导致我们计算的解的图片非常不准确。为了解决这个问题，我们为每个单元保存了多个
  //  补丁  ：在二维中，我们为每个单元在VTU文件中保存64个双线性
  // "单元"，在三维中，我们保存512个。最终的结果是，可视化程序将使用立方体基础函数的片状线性插值：这捕捉到了解决方案的细节，并且在大多数屏幕分辨率下，看起来很平滑。我们在一个单独的步骤中保存网格，没有额外的补丁，这样我们就有了细胞面的视觉表现。

  // 9.1版本的deal.II获得了编写更高程度多项式（即为我们的片状二项式解决方案编写片状二项式可视化数据）VTK和VTU输出的能力：然而，并非所有最新版本的ParaView和Viscit（截至2018年）都能读取这种格式，所以我们在这里使用更古老、更通用（但效率较低）的方法。

  template <int dim>
  void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
  {
    {
      GridOut       grid_out;
      std::ofstream output("grid-" + std::to_string(cycle) + ".vtu");
      grid_out.write_vtu(triangulation, output);
    }

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

      // VTU输出可能很昂贵，无论是计算还是写入磁盘。这里我们要求ZLib，一个压缩库，以最大限度地提高吞吐量的方式来压缩数据。

      DataOutBase::VtkFlags vtk_flags;
      vtk_flags.compression_level =
        DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
      data_out.set_flags(vtk_flags);

      std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
      data_out.write_vtu(output);
    }
  }

  // ... 如同主循环（设置-求解-细化）一样，除了循环次数和初始网格之外。

  template <int dim>
  void AdvectionProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 10; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, -1, 1);
            triangulation.refine_global(3);
          }
        else
          {
            refine_grid();
          }

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

        setup_system();

        std::cout << "   Number of degrees of freedom:        "
                  << dof_handler.n_dofs() << std::endl;

        assemble_system();
        solve();
        output_results(cycle);
      }
  }

  //  @sect3{GradientEstimation class implementation}

  // 现在是 GradientEstimation 类的实现。让我们先为
  // estimate_cell() 函数所使用的 EstimateScratchData
  // 类定义构造函数。

  template <int dim>
  GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const FiniteElement<dim> &fe,
    const Vector<double> &    solution,
    Vector<float> &           error_per_cell)
    : fe_midpoint_value(fe,
                        QMidpoint<dim>(),
                        update_values | update_quadrature_points)
    , solution(solution)
    , error_per_cell(error_per_cell)
    , cell_midpoint_value(1)
    , neighbor_midpoint_value(1)

  {
    // 我们分配一个向量来保存一个单元的所有活动邻居的迭代器。我们保留活动邻居的最大数量，以避免以后的重新分配。注意这个最大的活动邻居数是如何计算出来的。

    active_neighbors.reserve(GeometryInfo<dim>::faces_per_cell *
                             GeometryInfo<dim>::max_children_per_face);
  }

  template <int dim>
  GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const EstimateScratchData &scratch_data)
    : fe_midpoint_value(scratch_data.fe_midpoint_value.get_fe(),
                        scratch_data.fe_midpoint_value.get_quadrature(),
                        update_values | update_quadrature_points)
    , solution(scratch_data.solution)
    , error_per_cell(scratch_data.error_per_cell)
    , cell_midpoint_value(1)
    , neighbor_midpoint_value(1)
  {}

  // 接下来是对 GradientEstimation
  // 类的实现。第一个函数除了将工作委托给另一个函数外，并没有做什么，但在顶部有一点设置。

  // 在开始工作之前，我们要检查写入结果的向量是否有正确的大小。在编程中，忘记在调用处正确确定参数大小的错误是很常见的。因为没有发现这种错误所造成的损失往往是微妙的（例如，内存中某个地方的数据损坏，或者是无法重现的结果），所以非常值得努力去检查这些东西。

  template <int dim>
  void GradientEstimation::estimate(const DoFHandler<dim> &dof_handler,
                                    const Vector<double> & solution,
                                    Vector<float> &        error_per_cell)
  {
    Assert(
      error_per_cell.size() == dof_handler.get_triangulation().n_active_cells(),
      ExcInvalidVectorLength(error_per_cell.size(),
                             dof_handler.get_triangulation().n_active_cells()));

    WorkStream::run(dof_handler.begin_active(),
                    dof_handler.end(),
                    &GradientEstimation::template estimate_cell<dim>,
                    std::function<void(const EstimateCopyData &)>(),
                    EstimateScratchData<dim>(dof_handler.get_fe(),
                                             solution,
                                             error_per_cell),
                    EstimateCopyData());
  }

  // 这里是通过计算梯度的有限差分近似值来估计局部误差的函数。该函数首先计算当前单元的活动邻居列表，然后为每个邻居计算介绍中描述的数量。之所以有这样的顺序，是因为在局部细化网格的情况下，要找到一个给定的邻居并不是一蹴而就的事情。原则上，一个优化的实现可以在一个步骤中找到邻域和取决于它们的量，而不是先建立一个邻域列表，然后在第二步中找到它们的贡献，但是我们很乐意将此作为一个练习。正如之前所讨论的，传递给 WorkStream::run 的工作者函数是在保留所有临时对象的 "scratch "对象上工作。这样，我们就不需要在每次为给定单元调用工作的函数内创建和初始化那些昂贵的对象了。这样的参数被作为第二个参数传递。第三个参数是一个 "copy-data "对象（更多信息见 @ref threads ），但我们在这里实际上没有使用这些对象。由于 WorkStream::run() 坚持传递三个参数，我们声明这个函数有三个参数，但简单地忽略了最后一个参数。

  // （从美学角度看，这是不令人满意的。它可以通过使用一个匿名（lambda）函数来避免。如果你允许的话，让我们在这里展示一下如何做。首先，假设我们已经声明这个函数只接受两个参数，省略了未使用的最后一个参数。现在，
  // WorkStream::run 仍然想用三个参数来调用这个函数，所以我们需要找到一种方法来
  // "忘记 "调用中的第三个参数。简单地像上面那样把指针传给 WorkStream::run
  // 这个函数是做不到的--编译器会抱怨一个声明为有两个参数的函数在调用时有三个参数。然而，我们可以通过将以下内容作为第三个参数传递给
  // WorkStream::run(): 来做到这一点
  // @code
  //  [](const typename DoFHandler<dim>::active_cell_iterator &cell,
  //     EstimateScratchData<dim> &                            scratch_data,
  //     EstimateCopyData &)
  //  {
  //    GradientEstimation::estimate_cell<dim>(cell, scratch_data);
  //  }
  //  @endcode
  //  这并不比下面实现的解决方案好多少：要么例程本身必须带三个参数，要么它必须被带三个参数的东西包起来。我们不使用这种方法，因为在开始时添加未使用的参数更简单。

  // 现在来看看细节。

  template <int dim>
  void GradientEstimation::estimate_cell(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    EstimateScratchData<dim> &                            scratch_data,
    const EstimateCopyData &)
  {
    // 我们需要为张量 Y 提供空间，它是Y向量的外积之和。

    Tensor<2, dim> Y;

    // 首先初始化  FEValues  对象，以及  Y  张量。

    scratch_data.fe_midpoint_value.reinit(cell);

    // 现在，在我们继续之前，我们首先计算当前单元的所有活动邻居的列表。我们首先在所有面上进行循环，看那里的邻居是否处于活动状态，如果它与本单元在同一级别或更粗一级，就会出现这种情况（注意，一个邻居只能比本单元粗一次，因为我们在deal.II中只允许在一个面上有一个最大的细化差）。另外，邻居也可能在同一级别，并被进一步细化；那么我们必须找到它的哪些子单元与当前单元相邻，并选择这些子单元（注意，如果一个活动单元的邻居的一个子单元与这个活动单元相邻，那么它本身就必须是活动的，这是由于上面提到的一个细化规则）。

    // 在一个空间维度上，情况略有不同，因为在那里不存在单一细化规则：相邻的活动单元可以在任意多的细化级别上有所不同。在这种情况下，计算变得有点困难，但我们将在下面解释。

    // 在开始对当前单元的所有邻域进行循环之前，我们当然要清除存储活动邻域的迭代器的数组。

    scratch_data.active_neighbors.clear();
    for (const auto face_n : GeometryInfo<dim>::face_indices())
      if (!cell->at_boundary(face_n))
        {
          // 首先定义面的迭代器和邻居的缩写

          const auto face     = cell->face(face_n);
          const auto neighbor = cell->neighbor(face_n);

          // 然后检查邻居是否是活动的。如果是，那么它就在同一层或更粗的一层（如果我们不是在1D中），而且我们在任何情况下都会对它感兴趣。

          if (neighbor->is_active())
            scratch_data.active_neighbors.push_back(neighbor);
          else
            {
              // 如果邻居没有活动，则检查其子女。

              if (dim == 1)
                {
                  // 要找到与本单元相邻的子单元，如果我们在本单元的左边（n==0），则依次去找其右边的子单元，如果我们在右边（n==1），则依次去找左边的子单元，直到找到一个活动单元。

                  auto neighbor_child = neighbor;
                  while (neighbor_child->has_children())
                    neighbor_child = neighbor_child->child(face_n == 0 ? 1 : 0);

                  // 由于这使用了一些非微妙的几何直觉，我们可能想检查一下我们是否做对了，也就是说，检查我们找到的单元格的邻居是否确实是我们目前正在处理的单元。像这样的检查通常是有用的，并且经常发现像上面这一行的算法（不由自主地交换
                  // n==1 for n==0
                  // 或类似的算法是很简单的）和库中的错误（上面的算法所依据的假设可能是错误的，记录错误，或者由于库中的错误而被违反）。原则上，我们可以在程序运行一段时间后删除这样的检查，但是无论如何留下它来检查库中或上述算法中的变化可能是一件好事。
                  // 请注意，如果这个检查失败了，那么这肯定是一个无法恢复的错误，而且很可能被称为内部错误。因此我们在这里使用一个预定义的异常类来抛出。

                  Assert(neighbor_child->neighbor(face_n == 0 ? 1 : 0) == cell,
                         ExcInternalError());

                  // 如果检查成功，我们就把刚刚发现的活动邻居推到我们保留的堆栈中。

                  scratch_data.active_neighbors.push_back(neighbor_child);
                }
              else

                // 如果我们不在1d中，我们收集所有 "在
                // "当前面的子面后面的邻居孩子，然后继续前进。

                for (unsigned int subface_n = 0; subface_n < face->n_children();
                     ++subface_n)
                  scratch_data.active_neighbors.push_back(
                    cell->neighbor_child_on_subface(face_n, subface_n));
            }
        }

    // 好了，现在我们有了所有的邻居，让我们开始对他们每个人进行计算。首先，我们做一些预备工作：找出当前单元格的中心和该点的解决方案。后者是以正交点的函数值向量的形式得到的，当然，正交点只有一个。同样地，中心的位置是实空间中第一个（也是唯一的）正交点的位置。

    const Point<dim> this_center =
      scratch_data.fe_midpoint_value.quadrature_point(0);

    scratch_data.fe_midpoint_value.get_function_values(
      scratch_data.solution, scratch_data.cell_midpoint_value);

    // 现在在所有活动邻居上循环，收集我们需要的数据。

    Tensor<1, dim> projected_gradient;
    for (const auto &neighbor : scratch_data.active_neighbors)
      {
        // 然后得到邻近单元的中心和该点的有限元函数值。注意，为了获得这些信息，我们必须重新初始化相邻单元的
        // FEValues 对象。

        scratch_data.fe_midpoint_value.reinit(neighbor);
        const Point<dim> neighbor_center =
          scratch_data.fe_midpoint_value.quadrature_point(0);

        scratch_data.fe_midpoint_value.get_function_values(
          scratch_data.solution, scratch_data.neighbor_midpoint_value);

        // 计算连接两个单元格中心的向量 y
        // 。注意，与介绍不同，我们用 y
        // 表示归一化的差分向量，因为这是在计算中随处可见的数量。

        Tensor<1, dim> y        = neighbor_center - this_center;
        const double   distance = y.norm();
        y /= distance;

        // 然后把这个单元格对Y矩阵的贡献加起来...

        for (unsigned int i = 0; i < dim; ++i)
          for (unsigned int j = 0; j < dim; ++j)
            Y[i][j] += y[i] * y[j];

        // ...并更新差额商数之和。

        projected_gradient += (scratch_data.neighbor_midpoint_value[0] -
                               scratch_data.cell_midpoint_value[0]) /
                              distance * y;
      }

    // 如果现在，在收集了来自邻居的所有信息后，我们可以确定当前单元的梯度的近似值，那么我们需要经过跨越整个空间的向量
    // y
    // ，否则我们就不会有梯度的所有成分。这可以通过矩阵的可逆性来说明。

    // 如果矩阵不可逆，那么当前单元的活动邻居数量不足。与之前所有的情况（我们提出了异常）相比，这不是一个编程错误：这是一个运行时错误，即使在调试模式下运行良好，也可能在优化模式下发生，所以在优化模式下尝试捕捉这个错误是合理的。对于这种情况，有一个
    // AssertThrow 宏：它像 Assert
    // 宏一样检查条件，但不仅仅是在调试模式下；然后输出一个错误信息，但不是像
    // Assert 宏那样中止程序，而是使用C++的 throw
    // 命令抛出异常。这样，人们就有可能捕捉到这个错误，并采取合理的应对措施。其中一个措施是在全局范围内细化网格，因为如果初始网格的每个单元都至少被细化过一次，就不会出现方向不足的情况。

    AssertThrow(determinant(Y) != 0, ExcInsufficientDirections());

    // 如果另一方面，矩阵是可反转的，那么就反转它，用它乘以其他数量，然后用这个数量和正确的网格宽度的幂来计算估计误差。

    const Tensor<2, dim> Y_inverse = invert(Y);

    const Tensor<1, dim> gradient = Y_inverse * projected_gradient;

    // 这个函数的最后一部分是将我们刚刚计算出来的内容写入输出向量的元素中。这个向量的地址已经存储在Scratch数据对象中，我们所要做的就是知道如何在这个向量中获得正确的元素--但我们可以问一下我们所在的单元格是第多少个活动单元。

    scratch_data.error_per_cell(cell->active_cell_index()) =
      (std::pow(cell->diameter(), 1 + 1.0 * dim / 2) * gradient.norm());
  }
} // namespace Step9
// @sect3{Main function}

//  main 函数与前面的例子类似。主要区别是我们使用MultithreadInfo来设置最大的线程数（更多信息请参见文档模块  @ref threads  "多处理器访问共享内存的并行计算"）。使用的线程数是环境变量DEAL_II_NUM_THREADS和  set_thread_limit  的参数的最小值。如果没有给  set_thread_limit  的值，则使用英特尔线程构建块（TBB）库的默认值。如果省略了对  set_thread_limit  的调用，线程的数量将由 TBB 选择，与 DEAL_II_NUM_THREADS无关。

int main()
{
  using namespace dealii;
  try
    {
      MultithreadInfo::set_thread_limit();

      Step9::AdvectionProblem<2> advection_problem_2d;
      advection_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}

//


#pragma once

#include <iostream> // cerr
#include <random> // mt19937_64, uniform_x_distribution
#include <vector>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <iostream>

#include "inf.hpp"
#include "getInf.hpp"
#include "util.hpp"

using namespace boost;

template<typename Number> using Graph = adjacency_list<listS, vecS, directedS, no_property, property<edge_weight_t, Number>>;
template<typename Number> using Vertex = typename graph_traits<Graph<Number>>::vertex_descriptor;
typedef std::pair<int, int> Edge;

template<typename Number>
struct graph_t {
  int V;
  int E;
  Edge *edge_array;
  Number *weights;
};

#ifdef CUDA
template<typename Number>
struct graph_cuda_t {
  int V;
  int E;
  Edge *edge_array;
  Number *weights;
  int *starts;
};
#endif

template<typename Number> size_t init_random_adjacency_matrix(Number *adjacencyMatrix, const int n, const double p, const unsigned long seed) {
  static const Number inf = getInf<Number>();
  static std::uniform_real_distribution<double> flip(0, 1);
  static std::uniform_real_distribution<double> choose_weight(1, 100);

  std::mt19937_64 rand_engine(seed);

  size_t e = 0;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == j) {
        adjacencyMatrix[i * n + j] = 0;
      } else if (flip(rand_engine) < p) {
        adjacencyMatrix[i * n + j] = choose_weight(rand_engine);
        e++;
      } else {
        adjacencyMatrix[i * n + j] = inf;
      }
    }
  }
  return e;
}

template<typename Number> size_t count_edges(const Number *adjacencyMatrix, const int n) {
  static const Number inf = getInf<Number>();
  size_t e = 0;
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < n * n; i++) {
    Number weight = adjacencyMatrix[i];
    if (weight != 0 && weight != inf) {
#ifdef _OPENMP
#pragma omp atomic
#endif
      e++;
    }
  }
  return e;
}

template<typename Number> graph_t<Number> * init_graph(const Number *adjacencyMatrix, const int n) {
  static const Number inf = getInf<Number>();
  size_t e = count_edges<Number>(adjacencyMatrix, n);
  Edge *edge_array = new Edge[e];
  Number *weights = new Number[e];
  int ei = 0;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (adjacencyMatrix[i * n + j] != 0 && adjacencyMatrix[i * n + j] != inf) {
#ifdef _OPENMP
#pragma omp critical (init_graph)
#endif
        {
          edge_array[ei] = Edge(i, j);
          weights[ei] = adjacencyMatrix[i * n + j];
          ei++;
        }
      }
    }
  }
  graph_t<Number> *gr = new graph_t<Number>;
  gr->V = n;
  gr->E = e;
  gr->edge_array = edge_array;
  gr->weights = weights;
  return gr;
}

template<typename Number> graph_t<Number> * init_random_graph(const int n, const double p, const unsigned long seed) {
  static const Number inf = getInf<Number>();
  Number *adjacencyMatrix = new Number[n * n];
  size_t e = init_random_adjacency_matrix<Number>(adjacencyMatrix, n, p, seed);
  graph_t<Number> * gr = init_graph<Number>(adjacencyMatrix, n, e);
  delete[] adjacencyMatrix;
  return gr;
}

template<typename Number> void free_graph(const graph_t<Number> *g) {
  delete[] g->edge_array;
  delete[] g->weights;
  delete g;
}

#ifdef CUDA

template<typename Number> graph_cuda_t<Number> * init_graph_cuda(const Number *adjacencyMatrix, const int n) {
  static const Number inf = getInf<Number>();
  size_t e = count_edges<Number>(adjacencyMatrix, n);
  Edge *edge_array = new Edge[e];
  Number *weights = new Number[e];
  int* starts = new int[n + 1];  // Starting point for each edge
  int ei = 0;
  for (int i = 0; i < n; i++) {
    starts[i] = ei;
    for (int j = 0; j < n; j++) {
      if (adjacencyMatrix[i * n + j] != 0 && adjacencyMatrix[i * n + j] != inf) {
#ifdef _OPENMP
#pragma omp critical (init_graph_cuda)
#endif
        {
          edge_array[ei] = Edge(i, j);
          weights[ei] = adjacencyMatrix[i * n + j];
          ei++;
        }
      }
    }
  }

  starts[n] = ei; // One extra

  graph_cuda_t<Number> *gr = new graph_cuda_t<Number>;
  gr->V = n;
  gr->E = e;
  gr->edge_array = edge_array;
  gr->weights = weights;
  gr->starts = starts;
  return gr;
}

template<typename Number> graph_cuda_t<Number> * johnson_cuda_random_init(const int n, const double p, const unsigned long seed) {
  static const Number inf = getInf<Number>();
  Number* adjacencyMatrix = new Number[n * n];
  int e = init_random_adjacency_matrix<Number>(adjacencyMatrix, n, p, seed);
  graph_cuda_t<Number> *gr = init_graph_cuda<Number>(adjacencyMatrix, n);
  delete[] adjacencyMatrix;
  return gr;
}

template<typename Number> void free_graph_cuda(const graph_cuda_t<Number> * g) {
  delete[] g->edge_array;
  delete[] g->weights;
  delete[] g->starts;
  delete g;
}

template<typename Number> void johnson_cuda(graph_cuda_t<Number> *gr, Number *distanceMatrix);
template<typename Number> void johnson_successor_cuda(graph_cuda_t<Number> *gr, Number *distanceMatrix, int *successorMatrix);

#endif

template<typename Number> inline bool bellman_ford(const graph_t<Number> *gr, Number *dist, int src) {
  static const Number inf = getInf<Number>();
  int v = gr->V;
  int e = gr->E;
  Edge *edges = gr->edge_array;
  Number *weights = gr->weights;

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < v; i++) {
    dist[i] = inf;
  }
  dist[src] = 0;

  for (int i = 1; i <= v - 1; i++) {
#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int j = 0; j < e; j++) {
      int u = std::get<0>(edges[j]);
      int v = std::get<1>(edges[j]);
      Number new_dist = weights[j] + dist[u];
      if (dist[u] != inf && new_dist < dist[v])
        dist[v] = new_dist;
    }
  }

  bool no_neg_cycle = true;
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < e; i++) {
    int u = std::get<0>(edges[i]);
    int v = std::get<1>(edges[i]);
    Number weight = weights[i];
    if (dist[u] != inf && dist[u] + weight < dist[v])
      no_neg_cycle = false;
  }
  return no_neg_cycle;
}

template<typename Number> void johnson_parallel(const graph_t<Number> *gr, Number *distanceMatrix) {
  static const Number inf = getInf<Number>();
  int v = gr->V;

  // Make new graph for Bellman-Ford
  // First, a new node q is added to the graph, connected by zero-weight edges
  // to each of the other nodes.
  graph_t<Number> *bf_graph = new graph_t<Number>;
  bf_graph->V = v + 1;
  bf_graph->E = gr->E + v;
  bf_graph->edge_array = new Edge[bf_graph->E];
  bf_graph->weights = new Number[bf_graph->E];

  std::memcpy(bf_graph->edge_array, gr->edge_array, gr->E * sizeof(Edge));
  std::memcpy(bf_graph->weights, gr->weights, gr->E * sizeof(Number));
  std::memset(&bf_graph->weights[gr->E], 0, v * sizeof(Number));

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < v; e++) {
    bf_graph->edge_array[e + gr->E] = Edge(v, e);
  }

  // Second, the Bellman–Ford algorithm is used, starting from the new vertex q,
  // to find for each vertex v the minimum weight h(v) of a path from q to v. If
  // this step detects a negative cycle, the algorithm is terminated.
  // TODO Can run parallel version?
  Number *h = new Number[bf_graph->V];
  bool r = bellman_ford<Number>(bf_graph, h, v);
  if (!r) {
    std::cerr << "\nNegative Cycles Detected! Terminating Early\n";
    exit(1);
  }

  // Next the edges of the original graph are reweighted using the values computed
  // by the Bellman–Ford algorithm: an edge from u to v, having length
  // w(u,v), is given the new length w(u,v) + h(u) − h(v).
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < gr->E; e++) {
    int u = std::get<0>(gr->edge_array[e]);
    int v = std::get<1>(gr->edge_array[e]);
    gr->weights[e] = gr->weights[e] + h[u] - h[v];
  }

  Graph<Number> G(gr->edge_array, gr->edge_array + gr->E, gr->weights, v);

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
  for (int s = 0; s < v; s++) {
    std::vector <Vertex<Number>> p(num_vertices(G));
    std::vector<Number> d(num_vertices(G));
    dijkstra_shortest_paths(G, s, distance_map(&d[0]).distance_inf(inf));
    for (int vi = 0; vi < v; vi++) {
      int i = s * v + vi;
      distanceMatrix[i] = d[vi] + h[vi] - h[s];
    }
  }

  delete[] h;
  free_graph<Number>(bf_graph);
}

template<typename Number> void johnson_parallel(const graph_t<Number> *gr, Number *distanceMatrix, int *successorMatrix) {
  static const Number inf = getInf<Number>();
  int v = gr->V;

  // Make new graph for Bellman-Ford
  // First, a new node q is added to the graph, connected by zero-weight edges
  // to each of the other nodes.
  graph_t<Number> *bf_graph = new graph_t<Number>;
  bf_graph->V = v + 1;
  bf_graph->E = gr->E + v;
  bf_graph->edge_array = new Edge[bf_graph->E];
  bf_graph->weights = new Number[bf_graph->E];

  std::memcpy(bf_graph->edge_array, gr->edge_array, gr->E * sizeof(Edge));
  std::memcpy(bf_graph->weights, gr->weights, gr->E * sizeof(Number));
  std::memset(&bf_graph->weights[gr->E], 0, v * sizeof(Number));

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < v; e++) {
    bf_graph->edge_array[e + gr->E] = Edge(v, e);
  }

  // Second, the Bellman–Ford algorithm is used, starting from the new vertex q,
  // to find for each vertex v the minimum weight h(v) of a path from q to v. If
  // this step detects a negative cycle, the algorithm is terminated.
  // TODO Can run parallel version?
  Number *h = new Number[bf_graph->V];
  bool r = bellman_ford<Number>(bf_graph, h, v);
  if (!r) {
    std::cerr << "\nNegative Cycles Detected! Terminating Early\n";
    exit(1);
  }

  // Next the edges of the original graph are reweighted using the values computed
  // by the Bellman–Ford algorithm: an edge from u to v, having length
  // w(u,v), is given the new length w(u,v) + h(u) − h(v).
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < gr->E; e++) {
    int u = std::get<0>(gr->edge_array[e]);
    int v = std::get<1>(gr->edge_array[e]);
    gr->weights[e] = gr->weights[e] + h[u] - h[v];
  }

  Graph<Number> G(gr->edge_array, gr->edge_array + gr->E, gr->weights, v);

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
  for (int s = 0; s < v; s++) {
    std::vector <Vertex<Number>> p(num_vertices(G));
    std::vector<Number> d(num_vertices(G));
    dijkstra_shortest_paths(G, s, distance_map(&d[0]).predecessor_map(&p[0]).distance_inf(inf));
    for (int vi = 0; vi < v; vi++) {
      int i = s * v + vi;
      distanceMatrix[i] = d[vi] + h[vi] - h[s];
      successorMatrix[vi * v + s] = p[vi];
    }
  }

  delete[] h;
  free_graph<Number>(bf_graph);
}

template<typename Number> void johnson_parallel_matrix(const Number *adjacencyMatrix, Number **distanceMatrix, const int n) {
  *distanceMatrix = (Number *) malloc(sizeof(Number) * n * n);
  memcpy(*distanceMatrix, adjacencyMatrix, sizeof(Number) * n * n);
#ifdef CUDA
  graph_cuda_t<Number> *cuda_gr = init_graph_cuda<Number>(adjacencyMatrix, n);
  johnson_cuda<Number>(cuda_gr, *distanceMatrix);
  free_graph_cuda<Number>(cuda_gr);
#else
  const graph_t<Number> *gr = init_graph<Number>(adjacencyMatrix, n);
  johnson_parallel<Number>(gr, *distanceMatrix);
  delete gr;
#endif
}

template<typename Number> void johnson_parallel_matrix(const Number *adjacencyMatrix, Number **distanceMatrix, int **successorMatrix, const int n) {
  *distanceMatrix = (Number *) malloc(sizeof(Number) * n * n);
  *successorMatrix = (int *) malloc(sizeof(int) * n * n);
  memcpy(*distanceMatrix, adjacencyMatrix, sizeof(Number) * n * n);
#ifdef CUDA
  graph_cuda_t<Number> *cuda_gr = init_graph_cuda<Number>(adjacencyMatrix, n);
  johnson_successor_cuda<Number>(cuda_gr, *distanceMatrix, *successorMatrix);
  free_graph_cuda<Number>(cuda_gr);
#else
    const graph_t<Number> *gr = init_graph<Number>(adjacencyMatrix, n);
  johnson_parallel<Number>(gr, *distanceMatrix, *successorMatrix);
  delete gr;
#endif
}

template<typename Number> void free_johnson_parallel_matrix(Number **distanceMatrix) {
  free(*distanceMatrix);
}
template<typename Number> void free_johnson_parallel_matrix(Number **distanceMatrix, int **successorMatrix) {
  free(*distanceMatrix);
  free(*successorMatrix);
}

extern "C" void johnson_parallel_matrix_double(const double *adjacencyMatrix, double **distanceMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_double(double **distanceMatrix);
extern "C" void johnson_parallel_matrix_float(const float *adjacencyMatrix, float **distanceMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_float(float **distanceMatrix);
extern "C" void johnson_parallel_matrix_int(const int *adjacencyMatrix, int **distanceMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_int(int **distanceMatrix);

extern "C" void johnson_parallel_matrix_successor_double(const double *adjacencyMatrix, double **distanceMatrix, int **successorMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_successor_double(double **distanceMatrix, int **successorMatrix);
extern "C" void johnson_parallel_matrix_successor_float(const float *adjacencyMatrix, float **distanceMatrix, int **successorMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_successor_float(float **distanceMatrix, int **successorMatrix);
extern "C" void johnson_parallel_matrix_successor_int(const int *adjacencyMatrix, int **distanceMatrix, int **successorMatrix, const int n);
extern "C" void free_johnson_parallel_matrix_successor_int(int **distanceMatrix, int **successorMatrix);


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>

#include "dpOptTrans/pcHelpers.h"

#include <boost/program_options.hpp>
namespace po = boost::program_options;

void ComputeMoments(const pcl::PointCloud<pcl::PointXYZRGBNormal>& pc,
    Eigen::Matrix3d& S, Eigen::Vector3d& mu) {
  mu.fill(0.); S.fill(0.);
  double W = 0.;
  for (uint32_t i=0; i<pc.size(); ++i) {
    Eigen::Map<const Eigen::Vector3f> p(&(pc.at(i).x));
    double w = pc.at(i).curvature; // Stores the weights
    mu += p.cast<double>()*w;
    W+=w;
  }
  mu /= W;
  for (uint32_t i=0; i<pc.size(); ++i) {
    Eigen::Map<const Eigen::Vector3f> p(&(pc.at(i).x));
    double w = pc.at(i).curvature; // Stores the weights
    S += w*(p.cast<double>()-mu) * (p.cast<double>()-mu).transpose();
  }
  S /= W;
}

void DisplayPcs(const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcA,
  const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcB,
  const Eigen::Quaterniond& q_star, const Eigen::Vector3d& t_star,
  const Eigen::Vector3d& muA,
  const Eigen::Vector3d& muB,
  float scale) {

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcA_ptr = pcA.makeShared();
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcB_ptr = pcB.makeShared();
    // Construct transformed point cloud.
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcB_T_ptr(new
        pcl::PointCloud<pcl::PointXYZRGBNormal>(pcB));
    for (uint32_t i=0; i<pcB_T_ptr->size(); ++i) {
      Eigen::Map<Eigen::Vector3f> p(&(pcB_T_ptr->at(i).x));
      p = q_star.cast<float>().inverse()._transformVector( p - t_star.cast<float>());
      Eigen::Map<Eigen::Vector3f> n(pcB_T_ptr->at(i).normal);
      n = q_star.cast<float>().inverse()._transformVector(n);
      pcB_T_ptr->at(i).rgb = ((int)128) << 16 | ((int)255) << 8 | ((int)128);
    }
    for (uint32_t i=0; i<pcA_ptr->size(); ++i) {
      pcA_ptr->at(i).rgb = ((int)255) << 16 | ((int)128) << 8 | ((int)128);
    }
    // Construct surface normal point clouds.
    pcl::PointCloud<pcl::PointXYZ>::Ptr nA_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(pcA.size(),1));
    nA_ptr->getMatrixXfMap(3,4,0) = pcA.getMatrixXfMap(3,12,4);
    pcl::PointCloud<pcl::PointXYZ>::Ptr nB_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(pcB.size(),1));
    nB_ptr->getMatrixXfMap(3,4,0) = pcB.getMatrixXfMap(3,12,4);
    pcl::PointCloud<pcl::PointXYZ>::Ptr nB_T_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(*nB_ptr));
    for (uint32_t i=0; i<nA_ptr->size(); ++i) {
      nA_ptr->at(i).z -=1.1;
    }
    for (uint32_t i=0; i<nB_T_ptr->size(); ++i) {
      Eigen::Map<Eigen::Vector3f> n(&(nB_T_ptr->at(i).x));
      n = q_star.cast<float>().inverse()._transformVector(n);
      n(2) += 1.1;
    }

    // Otherwise the default is a identity rotation with scaling of
    // 0.5.
    pcA_ptr->sensor_orientation_.setIdentity();
    pcB_ptr->sensor_orientation_.setIdentity();
    pcB_T_ptr->sensor_orientation_.setIdentity();
    nA_ptr->sensor_orientation_.setIdentity();
    nB_ptr->sensor_orientation_.setIdentity();
    nB_T_ptr->sensor_orientation_.setIdentity();

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerPc (new
        pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewerPc->initCameraParameters ();
    viewerPc->setBackgroundColor (1., 1., 1.);
//    viewerPc->addCoordinateSystem (scale);

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbA(pcA_ptr);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbB(pcB_ptr);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbB_T(pcB_T_ptr);
    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcA_ptr, rgbA, "cloudA");
//    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_ptr, rgbB, "cloudB",v1);
    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_T_ptr, rgbB_T,
        "cloudB transformed");

    char label[10];
    pcl::PointXYZ p;
    p.x = muA(0); p.y = muA(1); p.z = muA(2);
    sprintf(label,"SA%d",0);
    viewerPc->addSphere(p, scale, 1,0,0, label);
    p.x = muB(0); p.y = muB(1); p.z = muB(2);
    sprintf(label,"SB%d",0);
    viewerPc->addSphere(p, scale, 0.5,1,0.5, label);
    Eigen::Vector3d mu =
      q_star.inverse()._transformVector(muB -t_star);
    p.x = mu(0); p.y = mu(1); p.z = mu(2);
    sprintf(label,"S%d",0);
    viewerPc->addSphere(p, scale, 0,1,0, label);

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerNc (new
        pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewerNc->initCameraParameters ();
    viewerNc->setBackgroundColor (0., 0., 0.);
    viewerNc->addCoordinateSystem (1.0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      nA_color(nA_ptr, 255, 128, 128);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      nB_color(nB_ptr, 128, 255, 128);
    viewerNc->addPointCloud<pcl::PointXYZ> (nA_ptr, nA_color,
        "normalsA");
//    viewerNc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_ptr, rgbB, "cloudB",v1);
    viewerNc->addPointCloud<pcl::PointXYZ> (nB_T_ptr, nB_color,
        "normals B transformed");

    while (!viewerPc->wasStopped () && !viewerNc->wasStopped()) {
      viewerPc->spinOnce (50);
      viewerNc->spinOnce (50);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

int main(int argc, char** argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("in_a,a", po::value<std::string>(), "path to first input file")
    ("in_b,b", po::value<std::string>(), "path to second input file")
    ("out,o", po::value<std::string>(), "path to output file")
    ("display,d", "display results")
    ;

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

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  std::string pathA = "";
  std::string pathB = "";
  std::string pathOut = "";
  //  std::string mode = "";
  //  if(vm.count("mode")) mode = vm["mode"].as<std::string>();
  if(vm.count("in_a")) pathA = vm["in_a"].as<std::string>();
  if(vm.count("in_b")) pathB = vm["in_b"].as<std::string>();
  if(vm.count("out")) pathOut = vm["out"].as<std::string>();

  // Load point clouds.
  pcl::PointCloud<pcl::PointXYZRGBNormal> pcA, pcB;
  pcl::PLYReader reader;
  if (reader.read(pathA, pcA))
    std::cout << "error reading " << pathA << std::endl;
  else
    std::cout << "loaded pc from " << pathA << ": " << pcA.width << "x"
      << pcA.height << std::endl;
  if (reader.read(pathB, pcB))
    std::cout << "error reading " << pathB << std::endl;
  else
    std::cout << "loaded pc from " << pathB << ": " << pcB.width << "x"
      << pcB.height << std::endl;

  ComputeAreaWeightsPc(pcA);
  ComputeAreaWeightsPc(pcB);

  Eigen::Vector3d muA, muB;
  Eigen::Matrix3d SA, SB;

  ComputeMoments(pcA, SA, muA);
  ComputeMoments(pcB, SB, muB);

  // Assume xB = R*xA + t
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigA(SA);
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigB(SB);
  Eigen::Matrix3d U = eigA.eigenvectors();
  Eigen::Matrix3d V = eigB.eigenvectors();

  // Because R apears in a quadratic form we are free to negate it.
  // Since R \in R^3 this negates the determinant and makes it +1. -- a
  // rotation instead of a reflection.
  double min_cost = 9999999.;
  Eigen::Matrix3d R_star;
  Eigen::Vector3d t_star;

  for (uint32_t x=0; x < 3; x++)
    for (uint32_t y=0; y < 3; y++)
      for (uint32_t z=0; z < 3; z++)
        for (float i=-1.; i<=1.; i+=2.)
          for (float j=-1.; j<=1.; j+=2.)
            for (float k=-1.; k<=1.; k+=2.) {
              Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
              if (x == y || x == z || y == z)
                continue;
              P(0, x) = 1;
              P(1, y) = 1;
              P(2, z) = 1;
              Eigen::Matrix3d R_ = V*P*U.inverse();
              R_.col(0) *= i;
              R_.col(1) *= j;
              R_.col(2) *= k;
              if (R_.determinant() > 0.) {
                Eigen::Quaterniond q_(R_);
                Eigen::Vector3d t_ = muB - R_*muA;
                double c = ComputeClosestPointEucledianCost(pcA, pcB, &R_, &t_);
                std::cout << "permutation " << P << "\t cost=" << c << std::endl;
                std::cout << q_.coeffs().transpose()
                  << "\tt: " << t_.transpose() << std::endl;
                if(min_cost > c) {
                  min_cost = c;
                  R_star = R_;
                  t_star = t_;
                }
              }
            }
  Eigen::Matrix3d R = R_star;
  Eigen::Quaterniond q(R);
  Eigen::Vector3d t = muB - R.transpose()*muA;
  std::cout << "R:\n"<< R << std::endl;
  std::cout << "det(R) = " << R.determinant() << std::endl;
  std::cout << "t: " << t.transpose() << std::endl;
  std::cout << "norm of difference in covariance matrixes after matching: "
    << (R*SA*R.transpose() -SB).norm() << std::endl;
  std::cout << "norm of the difference in the mean vectors after matching: "
    << (muA - R.transpose()*(muB - t)).norm() << std::endl;

  if (vm.count("display")) {
    std::cout << "Cov A" << std::endl << SA << std::endl;
    std::cout << "EV A" << std::endl << U << std::endl;
    std::cout << "E A" << std::endl << eigA.eigenvalues() << std::endl;
    std::cout << "mu A " << std::endl << muA << std::endl;
    std::cout << "Cov B" << std::endl << SB << std::endl;
    std::cout << "EV B" << std::endl << V << std::endl;
    std::cout << "E B" << std::endl << eigB.eigenvalues() << std::endl;
    std::cout << "mu B " << std::endl << muB << std::endl;
    std::cout << "Cov B Transformed" << std::endl
      << R.transpose()*SB*R << std::endl;
    std::cout << "mu B Transformed" << std::endl
      << R.transpose()*(muB - t) << std::endl;
  }

  if (false) {
    Eigen::Matrix<double,3,6> M;
    M << 1,-1,0,0,0,0,
      0,0,1,-1,0,0,
      0,0,0,0,1,-1;
    uint32_t k =0;
    double min_cost = 9999.;
    Eigen::Matrix3d R_star;
    Eigen::Vector3d t_star;
    for(uint32_t i=0; i<6; ++i)
      for(uint32_t j=0; j<6; ++j) {
        Eigen::Matrix3d Rperm;
        Rperm.col(0) = M.col(i);
        Rperm.col(1) = M.col(j);
        Rperm.col(2) = M.col(i).cross(M.col(j));
        if (fabs(Rperm.determinant() -1.) < 1e-6) {
          //        std::cout << Rperm << std::endl;
          Eigen::Matrix3d R_ = Rperm*R;
          Eigen::Quaterniond q_(R_);
          Eigen::Vector3d t_ = muB - R_*muA;
          double c = ComputeClosestPointEucledianCost(pcA, pcB, &R_, &t_);
          std::cout << "permutation k=" << k << "\t cost=" << c << std::endl;
          std::cout << q_.coeffs().transpose()
            << "\tt: " << t_.transpose() << std::endl;
          ++k;
          if(min_cost > c) {
            min_cost = c;
            R_star = R_;
            t_star = t_;
          }
        }
      }
    q = Eigen::Quaterniond(R_star);
    t = t_star;
  }
  if(pathOut.size() > 1) {
    std::ofstream out(pathOut + std::string(".csv"));
    out << "q_w q_x q_y q_z t_x t_y t_z" << std::endl;
    out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << t(0)
      << " " << t(1) << " " << t(2) << " " << std::endl;
    out.close();
  }

  if (vm.count("display")) {
    double scale = sqrt(eigA.eigenvalues().real().maxCoeff())/5.;
    DisplayPcs(pcA, pcB, q, t, muA, muB, scale);
  }
}


#include <armadillo>

#include "DenseMatrix.h"

struct DenseMatrix::Impl {
    arma::dmat RawData;

  public:
    Impl() = default;
};

struct DenseVector::SubspectrumDataImpl {
    arma::dvec eigenvalues;

  public:
    SubspectrumDataImpl() = default;
};

DenseMatrix::DenseMatrix() : pImpl {std::make_unique<DenseMatrix::Impl>()} {}

DenseVector::DenseVector() : pImpl {std::make_unique<DenseVector::SubspectrumDataImpl>()} {}

DenseMatrix::~DenseMatrix() = default;
DenseMatrix::DenseMatrix(DenseMatrix&&) noexcept = default;
DenseMatrix& DenseMatrix::operator=(DenseMatrix&&) noexcept = default;
DenseVector::~DenseVector() = default;
DenseVector::DenseVector(DenseVector&&) noexcept = default;
DenseVector& DenseVector::operator=(DenseVector&&) noexcept = default;

void DenseMatrix::add_to_position(double value, uint32_t i, uint32_t j) {
    pImpl->RawData(i, j) += value;
}

void DenseMatrix::assign_to_position(double value, uint32_t i, uint32_t j) {
    pImpl->RawData(i, j) = value;
}

std::ostream& operator<<(std::ostream& os, const DenseMatrix& decomposition) {
    os << decomposition.pImpl->RawData << std::endl;
    return os;
}

std::ostream& operator<<(std::ostream& os, const DenseVector& raw_data) {
    os << raw_data.pImpl->eigenvalues << std::endl;
    return os;
}

void DenseMatrix::resize(
    uint32_t matrix_in_space_basis_size_i,
    uint32_t matrix_in_space_basis_size_j) {
    // TODO: is it the fastest way to initialize pImpl->RawData?
    pImpl->RawData.resize(matrix_in_space_basis_size_i, matrix_in_space_basis_size_j);
    pImpl->RawData.fill(arma::fill::zeros);
}

void DenseMatrix::resize_with_nans(
    uint32_t matrix_in_space_basis_size_i,
    uint32_t matrix_in_space_basis_size_j) {
    // TODO: is it the fastest way to initialize pImpl->RawData?
    pImpl->RawData.resize(matrix_in_space_basis_size_i, matrix_in_space_basis_size_j);
    pImpl->RawData.fill(arma::datum::nan);
    //    for (size_t i = 0; i < pImpl->RawData.n_rows; ++i) {
    //        for (size_t j = 0; j < pImpl->RawData.n_rows; ++j) {
    //            pImpl->RawData(i, j) = NAN;
    //        }
    //    }
}

void DenseMatrix::diagonalize(DenseVector& values, DenseMatrix& vectors) const {
    arma::eig_sym(values.pImpl->eigenvalues, vectors.pImpl->RawData, pImpl->RawData);
}

void DenseMatrix::diagonalize(DenseVector& values) const {
    arma::eig_sym(values.pImpl->eigenvalues, pImpl->RawData);
}

DenseMatrix DenseMatrix::unitary_transform(const DenseMatrix& matrix_to_transform) const {
    DenseMatrix transformed_matrix;
    transformed_matrix.pImpl->RawData =
        pImpl->RawData.t() * matrix_to_transform.pImpl->RawData * pImpl->RawData;
    return std::move(transformed_matrix);
}

DenseVector DenseMatrix::return_main_diagonal() const {
    DenseVector main_diagonal;
    main_diagonal.pImpl->eigenvalues = pImpl->RawData.diag();
    return std::move(main_diagonal);
}

double DenseMatrix::operator()(uint32_t i, uint32_t j) const {
    return pImpl->RawData(i, j);
}

uint32_t DenseMatrix::size() const {
    return pImpl->RawData.n_rows;
}

uint32_t DenseMatrix::size_rows() const {
    return pImpl->RawData.n_rows;
}

uint32_t DenseMatrix::size_cols() const {
    return pImpl->RawData.n_rows;
}

uint32_t DenseVector::size() const {
    return pImpl->eigenvalues.size();
}

double DenseVector::operator()(uint32_t i) const {
    return pImpl->eigenvalues(i);
}

void DenseVector::assign_to_position(double value, uint32_t i) {
    pImpl->eigenvalues.at(i) = value;
}

void DenseVector::resize(uint32_t new_size) {
    pImpl->eigenvalues.resize(new_size);
}

std::vector<double> concatenate(const std::vector<DenseVector>& dense_vectors) {
    size_t size = 0;
    for (const auto& dense_vector : dense_vectors) {
        size += dense_vector.size();
    }
    std::vector<double> result_vector;
    result_vector.reserve(size);
    for (const auto& dense_vector : dense_vectors) {
        result_vector.insert(
            result_vector.end(),
            dense_vector.pImpl->eigenvalues.begin(),
            dense_vector.pImpl->eigenvalues.end());
    }
    return result_vector;
}

bool DenseVector::operator==(const DenseVector& rhs) const {
    return arma::approx_equal(pImpl->eigenvalues, rhs.pImpl->eigenvalues, "absdiff", 0);
}

bool DenseVector::operator!=(const DenseVector& rhs) const {
    return !(rhs == *this);
}

DenseVector DenseVector::divide_and_wise_exp(double denominator) const {
    DenseVector answer;
    answer.resize(size());
    answer.pImpl->eigenvalues = arma::exp(pImpl->eigenvalues / (denominator));
    return answer;
}

double DenseVector::dot(const DenseVector& rhs) const {
    return arma::dot(pImpl->eigenvalues, rhs.pImpl->eigenvalues);
}

DenseVector DenseVector::element_wise_multiplication(const DenseVector& rhs) const {
    DenseVector answer;
    answer.pImpl->eigenvalues = pImpl->eigenvalues % rhs.pImpl->eigenvalues;
    return answer;
}

void DenseVector::concatenate_with(const DenseVector& rhs) {
    arma::dvec tmp = arma::join_cols(pImpl->eigenvalues, rhs.pImpl->eigenvalues);
    pImpl->eigenvalues.reset();
    pImpl->eigenvalues = std::move(tmp);
}

void DenseVector::add_identical_values(size_t number, double value) {
    arma::dvec tmp;
    tmp.resize(number);
    tmp.fill(value);
    tmp = arma::join_cols(pImpl->eigenvalues, tmp);
    pImpl->eigenvalues.reset();
    pImpl->eigenvalues = std::move(tmp);
}

void DenseVector::subtract_minimum() {
    double minimum = arma::min(pImpl->eigenvalues);
    pImpl->eigenvalues -= minimum;
}


/**
 * @author     : Zhao Chonyyao (cyzhao@zju.edu.cn)
 * @date       : 2021-04-30
 * @description: simple mass spring problem
 * @version    : 1.0
 */
#include <memory>

#include <boost/property_tree/ptree.hpp>

#include "Common/error.h"

// TODO: possible bad idea of having dependence to model in problem module
#include "Model/fem/elas_energy.h"
#include "Model/fem/mass_matrix.h"
#include "Model/mass_spring/mass_spring_obj.h"
#include "Model/mass_spring/para.h"

#include "Problem/energy/basic_energy.h"
#include "Io/io.h"
#include "Geometry/extract_surface.imp"

#include "mass_spring_problem.h"

namespace PhysIKA {
using namespace std;
using namespace Eigen;

template <typename T>
using MAT = Eigen::Matrix<T, -1, -1>;
template <typename T>
using VEC = Eigen::Matrix<T, -1, 1>;

template <typename T>
ms_problem_builder<T>::ms_problem_builder(const T* x, const boost::property_tree::ptree& para_tree)
    : pt_(para_tree)
{
    auto blender            = para_tree.get_child("blender");
    auto simulation_para    = para_tree.get_child("simulation_para");
    auto common             = para_tree.get_child("common");
    para::dt                = common.get<double>("time_step", 0.01);
    para::line_search       = simulation_para.get<int>("line_search", true);  // todo
    para::density           = common.get<double>("density", 10);
    para::frame             = common.get<int>("frame", 100);
    para::newton_fastMS     = simulation_para.get<string>("newton_fastMS");
    para::stiffness         = simulation_para.get<double>("stiffness", 8000);
    para::gravity           = common.get<double>("gravity", 9.8);
    para::object_name       = blender.get<string>("surf");
    para::out_dir_simulator = common.get<string>("out_dir_simulator");
    para::simulation_type   = simulation_para.get<string>("simulation", "static");
    para::weight_line_search =
        simulation_para.get<double>("weight_line_search", 1e-5);
    para::input_object   = common.get<string>("input_object");
    para::force_function = simulation_para.get<string>("force_function");
    para::intensity      = simulation_para.get<double>("intensity");
    para::coll_z         = simulation_para.get<bool>("coll_z", false);
    //TODO: need to check exception

    const string      filename = para::input_object;
    Matrix<T, -1, -1> nods;
    MatrixXi          cells;
    IF_ERR(exit, mesh_read_from_vtk<T, 4>(filename.c_str(), nods, cells));

    const size_t num_nods = nods.cols();
    if (x != nullptr)
        nods = Map<const MAT<T>>(x, nods.rows(), nods.cols());

    REST_  = nods;
    cells_ = cells;

    //read fixed points
    vector<size_t> cons(0);
    if (para_tree.find("input_constraint") != para_tree.not_found())
    {
        const string cons_file_path = common.get<string>("input_constraint");
        /* IF_ERR(exit, read_fixed_verts_from_csv(cons_file_path.c_str(), cons));*/
    }
    cout << "constrint " << cons.size() << " points" << endl;

    //calc mass vector
    Matrix<T, -1, 1> mass_vec(num_nods);
    calc_mass_vector<T>(nods, cells, para::density, mass_vec);
    // mass_calculator<T, 3, 4, 1, 1, basis_func, quadrature>(nods, cells, para::density, mass_vec);

    cout << "build energy" << endl;
    int ELAS = 0;
    int GRAV = 1;
    int KIN  = 2;
    int POS  = 3;
    if (pt_.get<string>("solver_type") == "explicit")
        POS = 2;

    ebf_.resize(POS + 1);
    ebf_[ELAS] = make_shared<MassSpringObj<T>>(para::input_object.c_str(), para::stiffness);
    char axis  = common.get<char>("grav_axis", 'y') | 0x20;

    ebf_[GRAV] = make_shared<gravity_energy<T, 3>>(num_nods, 1, para::gravity, mass_vec, axis);
    kinetic_   = make_shared<momentum<T, 3>>(nods.data(), num_nods, mass_vec, para::dt);

    if (pt_.get<string>("solver_type") == "implicit")
        ebf_[KIN] = kinetic_;
    ebf_[POS] = make_shared<position_constraint<T, 3>>(nods.data(), num_nods, simulation_para.get<double>("w_pos", 1e6), cons);

    //set constraint
    enum constraint_type
    {
        COLL
    };
    cbf_.resize(COLL + 1);
    collider_  = nullptr;
    cbf_[COLL] = collider_;

    if (pt_.get<string>("solver_type") == "explicit")
    {
        Map<Matrix<T, -1, 1>> position(REST_.data(), REST_.size());
        semi_implicit_ = make_shared<semi_implicit<T>>(para::dt, mass_vec, position);
    }
}

template <typename T>
std::shared_ptr<Problem<T, 3>> ms_problem_builder<T>::build_problem() const
{
    cout << "assemble energy" << endl;
    shared_ptr<Functional<T, 3>> energy;
    try
    {
        energy = build_energy_t<T, 3>(ebf_);
    }
    catch (std::exception& e)
    {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    shared_ptr<Constraint<T>> constraint;
    cout << "assemble constraint" << endl;
    bool all_null = true;
    for (auto& c : cbf_)
        if (c != nullptr)
            all_null = false;
    if (all_null)
    {
        constraint = nullptr;
        cout << "WARNGING: No hard constraints." << endl;
    }
    else
    {
        try
        {
            constraint = build_constraint_t<T>(cbf_);
        }
        catch (std::exception& e)
        {
            cerr << e.what() << endl;
            exit(EXIT_FAILURE);
        }
    }

    exit_if(constraint != nullptr && energy->Nx() != constraint->Nx(), "energy and constraint has different dimension.");
    return make_shared<Problem<T, 3>>(energy, constraint);
}

template <typename T>
int ms_problem_builder<T>::update_problem(const T* x, const T* v)
{
    IF_ERR(return, kinetic_->update_location_and_velocity(x, v));
    if (collider_ != nullptr)
        IF_ERR(return, collider_->update(x));
    return 0;
}

template class ms_problem_builder<double>;

template class ms_problem_builder<float>;

}  // namespace PhysIKA


// Std includes
#include <cmath>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "Eigen/src/Core/Matrix.h"
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/ellipsoid.h"
// Simple includes
#include "flow.h"

// Types
using TypeScalar = double;
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
template<int Nx, int Ny>
using TypeMatrix = Eigen::Matrix<TypeScalar, Nx, Ny>;
// Space
constexpr unsigned int DIM = 3;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeSpaceMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<Eigen::Dynamic>, TypeView>;
// Flow
using TypeFlow = Flow<TypeSpaceVector, TypeSpaceMatrix, TypeRef>;

int main () {
	TypeSpaceVector x0;
	x0 << 0,
		  0,
		  0;
	TypeSpaceVector p0;
	p0 << 1,
		  0,
		  0;
	TypeSpaceVector p1;
	p1 << 0,
		  1,
		  0;
	double t0 = 0.0;
	double dt = 1e-2;
	double tEnd = 1.0;
	// Create ellipsoid
	sl0::Ellipsoid<TypeVector, TypeMatrix, DIM, TypeView, TypeFlow, TypeSolver> ellipsoid(std::make_shared<TypeFlow>(), std::vector<double>({1.0, 1.0, 1.0}));
	// Set initial state
	ellipsoid.sStep->x(ellipsoid.state.data()) = x0;
	ellipsoid.sStep->axis(ellipsoid.state.data(), 0) = p0;
	ellipsoid.sStep->axis(ellipsoid.state.data(), 1) = p1;
	ellipsoid.t = t0;
	// Compute
	std::cout << "Ellipsoid in a simple shear flow : \n";
	std::cout << "\n";
	std::cout << "Orientation after each step : \n";
	for(std::size_t i = 0; i < (tEnd - t0)/dt; i++) {
		ellipsoid.update(dt);
		std::cout << ellipsoid.sStep->cBasis(ellipsoid.state.data()) << "\n";
	}
	// out
	std::cout << "Ellipsoid in a simple shear flow : \n";
	std::cout << "\n";
	std::cout << "Final orientation : \n";
	std::cout << ellipsoid.sStep->cBasis(ellipsoid.state.data()) << "\n";
	std::cout << std::endl;
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_SPHERICAL_AREA_BOX_HPP
#define BOOST_GEOMETRY_STRATEGY_SPHERICAL_AREA_BOX_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/util/normalize_spheroidal_box_coordinates.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

// https://math.stackexchange.com/questions/131735/surface-element-in-spherical-coordinates
// http://www.cs.cmu.edu/afs/cs/academic/class/16823-s16/www/pdfs/appearance-modeling-3.pdf
// https://www.astronomyclub.xyz/celestial-sphere-2/solid-angle-on-the-celestial-sphere.html
// https://mathworld.wolfram.com/SolidAngle.html
// https://en.wikipedia.org/wiki/Spherical_coordinate_system
// Note that the equations used in the above articles are spherical polar coordinates.
// We use spherical equatorial, so the equation is different:
// assume(y_max > y_min);
// assume(x_max > x_min);
// /* because of polar to equatorial conversion */
// sin(%pi / 2 - y);
// O: r ^ 2 * cos(y);
// S: integrate(integrate(O, y, y_min, y_max), x, x_min, x_max);
template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical_box
{
    typedef typename strategy_detail::get_radius
        <
            RadiusTypeOrSphere
        >::type radius_type;

public:
    template <typename Box>
    struct result_type
        : strategy::area::detail::result_type
            <
                Box,
                CalculationType
            >
    {};

    // For consistency with other strategies the radius is set to 1
    inline spherical_box()
        : m_radius(1.0)
    {}

    template <typename RadiusOrSphere>
    explicit inline spherical_box(RadiusOrSphere const& radius_or_sphere)
        : m_radius(strategy_detail::get_radius
                    <
                        RadiusOrSphere
                    >::apply(radius_or_sphere))
    {}

    template <typename Box>
    inline auto apply(Box const& box) const
    {
        typedef typename result_type<Box>::type return_type;

        return_type x_min = get_as_radian<min_corner, 0>(box); // lon
        return_type y_min = get_as_radian<min_corner, 1>(box); // lat
        return_type x_max = get_as_radian<max_corner, 0>(box);
        return_type y_max = get_as_radian<max_corner, 1>(box);

        if (x_min == x_max || y_max == y_min)
        {
            return return_type(0);
        }

        math::normalize_spheroidal_box_coordinates<radian>(x_min, y_min, x_max, y_max);

        return (x_max - x_min)
             * (sin(y_max) - sin(y_min))
             * return_type(m_radius * m_radius);
    }

    srs::sphere<radius_type> model() const
    {
        return srs::sphere<radius_type>(m_radius);
    }

private:
    radius_type m_radius;
};

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_SPHERICAL_AREA_BOX_HPP


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_SURVEYOR_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_SURVEYOR_HPP

#include <boost/mpl/if.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Area calculation for cartesian points
\ingroup strategies
\details Calculates area using the Surveyor's formula, a well-known
    triangulation algorithm
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}

*/
template
<
    typename PointOfSegment,
    typename CalculationType = void
>
class surveyor
{
public :
    // If user specified a calculation type, use that type,
    //   whatever it is and whatever the point-type is.
    // Else, use the pointtype, but at least double
    typedef typename
        geofeatures_boost::mpl::if_c
        <
            geofeatures_boost::is_void<CalculationType>::type::value,
            typename select_most_precise
            <
                typename coordinate_type<PointOfSegment>::type,
                double
            >::type,
            CalculationType
        >::type return_type;

private :

    class summation
    {
        friend class surveyor;

        return_type sum;
    public :

        inline summation() : sum(return_type())
        {
            // Strategy supports only 2D areas
            assert_dimension<PointOfSegment, 2>();
        }
        inline return_type area() const
        {
            return_type result = sum;
            return_type const two = 2;
            result /= two;
            return result;
        }
    };

public :
    typedef summation state_type;
    typedef PointOfSegment segment_point_type;

    static inline void apply(PointOfSegment const& p1,
                PointOfSegment const& p2,
                summation& state)
    {
        // SUM += x2 * y1 - x1 * y2;
        state.sum += detail::determinant<return_type>(p2, p1);
    }

    static inline return_type result(summation const& state)
    {
        return state.area();
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{
    template <typename Point>
    struct default_strategy<cartesian_tag, Point>
    {
        typedef strategy::area::surveyor<Point> type;
    };

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::area

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_SURVEYOR_HPP


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <math.h>

#include <Eigen/Dense>
#include <unordered_map>
#include <vector>
#include <random>

namespace py = pybind11;

// POSTERIOR PVAL FOR T
// Generate posterior pvals selecting a new or existing table
Eigen::VectorXd posterior_t_cpp(Eigen::VectorXd t_j, Eigen::VectorXd n_jt,
                           Eigen::VectorXd k_jt, Eigen::VectorXd m_k, Eigen::VectorXd fk,
                           double gamma, double V, double alpha){

    // Get table specific topic and counts
    Eigen::VectorXd k_idx = k_jt(t_j);
    Eigen::VectorXd n_idx = n_jt(t_j);

    // If t is not new
    Eigen::VectorXd post_t = n_idx.cwiseProduct(fk(k_idx)); // element-wise product (scalar * vector)

    // If t is new
    double post_t_new = m_k.dot(fk) + gamma/V;
    post_t(0) = post_t_new * alpha / (gamma + m_k.sum());

    post_t /= post_t.sum(); // Normalize to generate pvals

    return post_t;

}

// Conditional density of x_ji given k and all data items except x_j
Eigen::VectorXd fk_cpp(int word, Eigen::MatrixXd  n_kv){

    Eigen::VectorXd lik = n_kv.row(word).cwiseQuotient(n_kv.colwise().sum()); // element-wise division(vec / scalar)
    lik[0] = 0; // First should always be zero

    return lik;
}

// POSTERIOR PVAL FOR K
// Compute explicit posterior multinomial-dirichlet posterior distribution
Eigen::VectorXd posterior_k_cpp(int tbl, Eigen::VectorXd k_jt,
                                Eigen::VectorXd n_jt, Eigen::VectorXd topic_idx, Eigen::MatrixXd n_kv,
                                Eigen::VectorXd m_k, const std::unordered_map<int, int> n_jtw,
                                double gamma, double V, double beta){

    // Topic k of table t
    int k = k_jt(tbl);
    double cnt_t = n_jt(tbl);
    double vbeta = V*beta;

    // Remove all counts associated with topic k in table t, from overall topic counts (n_k)
    Eigen::VectorXd n_k = n_kv.colwise().sum();
    n_k(k) -= cnt_t;
    n_k = n_k(topic_idx);

    // Initialized k posterior in log-form for simplicity, this computes f_k^{-X_ji}
    // has Dirichlet-Multinomial form
    double log_post_k_new = std::log(gamma) + std::lgamma(vbeta) - std::lgamma(vbeta + cnt_t); // for new k

    // for old ks, needs to loop since it's a vector
    m_k = m_k(topic_idx);
    Eigen::VectorXd log_post_k = m_k.array().log();
    for (int i = 0; i < log_post_k.size() ; i++){
        log_post_k(i) += std::lgammaf(n_k(i)) - std::lgammaf(n_k(i) + cnt_t);
    };

    // Remove individual word counts associated with topic k
    // add their contributions to k posterior
    for (auto item : n_jtw){

        if (item.second == 0) continue; //if word count is 0 skip

        //For word w, get counts across topics
        Eigen::VectorXd w_cnt_k = n_kv.row(item.first);

        // For specific topic k, remove count from associated table t
        w_cnt_k(k) -= item.second;
        w_cnt_k = w_cnt_k(topic_idx);
        w_cnt_k(0) = 1;

        // Add contributions of individual observations (words) for k new and k old
        log_post_k_new += std::lgammaf(item.second + beta) - std::lgammaf(beta);

        for (int j = 0; j < w_cnt_k.size(); j++){
            log_post_k(j) += std::lgammaf(w_cnt_k(j) + item.second) - std::lgammaf(w_cnt_k(j));

        };
    };

    // p-val for new k
    log_post_k(0) = log_post_k_new;

    // Bring back to non-log realm
    double pk_max = log_post_k.maxCoeff();

    for (int s = 0; s < log_post_k.size(); s++){
        log_post_k(s) -= pk_max;
    };

    log_post_k = log_post_k.array().exp();

    log_post_k /= log_post_k.sum(); // Normalize

    return log_post_k;

}

// Random Multinomial Sampling in Eigen w/ argmax selection
// Given a set of pvals and N, gives back index most likely category
// Ref:
//   C.S. David, The computer generation of multinomial random variates,
//   Comp. Stat. Data Anal. 16 (1993) 205-217
//

int argmax_multinomial_cpp(int N, Eigen::VectorXd pvals){

    // Init random number generated
    std::random_device rd;
    std::mt19937 re(rd());

    // Allocate memory
    double sum_p = 0.0;
    double norm = 0.0;
    int sum_n = 0;
    int K = pvals.size(); // size of vector (i.e. # of components)
    Eigen::MatrixXd n_vec(1,K);
    Eigen::MatrixXd::Index maxRow, maxCol;

    norm += pvals.sum(); // = 1 if pvals are input (sum of components)

    // For each component (except last one) sample from binomial
    for (int i=0; i < K-1; i++){

        // Binomial sampler varies across loop, pvals and remaining N get adjusted
        std::binomial_distribution<int> binomial(N-sum_n, pvals(i)/(norm - sum_p) );

        if (pvals(i) > 0.0){
            n_vec(0,i) = binomial(re);
        }
        else {
            n_vec(i) = 0;
        };

        sum_p += pvals(i); // adjustment for pvals and remaining n < N
        sum_n += n_vec(0,i);

    };

    // For last component allocate remaining N
    n_vec(0,K-1) = N - sum_n;

    int max = n_vec.maxCoeff(&maxRow, &maxCol); // Get argmax of vector

    return maxCol;

}

// Pybind11 module referencing
PYBIND11_MODULE(hdp_funcs, m) {
    m.doc() = "pybind11 cpp functions used in hierarchical dirichlet processes topic modeling",
    m.def("posterior_t_cpp", &posterior_t_cpp, "Generate posterior pvals to allocate word to table"),
    m.def("fk_cpp", &fk_cpp, "Conditional distribution necessary for posterior sampling"),
    m.def("posterior_k_cpp", &posterior_k_cpp, "Generate posterior pvals to allocate word to topic"),
    m.def("argmax_multinomial_cpp", &argmax_multinomial_cpp, "Vectorized multinomial random sampling");
}