//  Copyright (c) 2018-2019 Cem Bassoy
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//
//  And we acknowledge the support from all contributors.

#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <boost/test/unit_test.hpp>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_tensor_functions, * boost::unit_test::depends_on("test_tensor_contraction") )

using test_types = zip<int,long,float,double,std::complex<float>>::with_t<boost::numeric::ublas::first_order, boost::numeric::ublas::last_order>;

//using test_types = zip<int>::with_t<boost::numeric::ublas::first_order>;

struct fixture
{
	using extents_type = boost::numeric::ublas::shape;
	fixture()
	  : extents {
	      extents_type{1,1}, // 1
	      extents_type{1,2}, // 2
	      extents_type{2,1}, // 3
	      extents_type{2,3}, // 4
	      extents_type{2,3,1}, // 5
	      extents_type{4,1,3}, // 6
	      extents_type{1,2,3}, // 7
	      extents_type{4,2,3}, // 8
	      extents_type{4,2,3,5}} // 9
	{
	}
	std::vector<extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_vector, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;
	using vector_type  = typename tensor_type::vector_type;

	for(auto const& n : extents){

		auto a = tensor_type(n, value_type{2});

		for(auto m = 0u; m < n.size(); ++m){

			auto b = vector_type  (n[m], value_type{1} );

			auto c = ublas::prod(a, b, m+1);

			for(auto i = 0u; i < c.size(); ++i)
				BOOST_CHECK_EQUAL( c[i] , value_type(n[m]) * a[i] );

		}
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_matrix, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;
	using matrix_type  = typename tensor_type::matrix_type;

	for(auto const& n : extents) {

		auto a = tensor_type(n, value_type{2});

		for(auto m = 0u; m < n.size(); ++m){

			auto b  = matrix_type  ( n[m], n[m], value_type{1} );

			auto c = ublas::prod(a, b, m+1);

			for(auto i = 0u; i < c.size(); ++i)
				BOOST_CHECK_EQUAL( c[i] , value_type(n[m]) * a[i] );

		}
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_tensor_1, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	// left-hand and right-hand side have the
	// the same number of elements

	for(auto const& na : extents) {

		auto a  = tensor_type( na, value_type{2} );
		auto b  = tensor_type( na, value_type{3} );

		auto const pa = a.rank();

		// the number of contractions is changed.
		for( auto q = 0ul; q <= pa; ++q) { // pa

			auto phi = std::vector<std::size_t> ( q );

			std::iota(phi.begin(), phi.end(), 1ul);

			auto c = ublas::prod(a, b, phi);

			auto acc = value_type(1);
			for(auto i = 0ul; i < q; ++i)
				acc *= a.extents().at(phi.at(i)-1);

			for(auto i = 0ul; i < c.size(); ++i)
				BOOST_CHECK_EQUAL( c[i] , acc * a[0] * b[0] );

		}
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_tensor_2, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	auto compute_factorial = [](auto const& p){
		auto f = 1ul;
		for(auto i = 1u; i <= p; ++i)
			f *= i;
		return f;
	};

	auto permute_extents = [](auto const& pi, auto const& na){
		auto nb = na;
		assert(pi.size() == na.size());
		for(auto j = 0u; j < pi.size(); ++j)
			nb[pi[j]-1] = na[j];
		return nb;
	};

	// left-hand and right-hand side have the
	// the same number of elements

	for(auto const& na : extents) {

		auto a  = tensor_type( na, value_type{2} );
		auto const pa = a.rank();

		auto pi   = std::vector<std::size_t>(pa);
		auto fac = compute_factorial(pa);
		std::iota( pi.begin(), pi.end(), 1 );

		for(auto f = 0ul; f < fac; ++f)
		{
			auto nb = permute_extents( pi, na  );
			auto b  = tensor_type( nb, value_type{3} );

			// the number of contractions is changed.
			for( auto q = 0ul; q <= pa; ++q) { // pa

				auto phia = std::vector<std::size_t> ( q );  // concatenation for a
				auto phib = std::vector<std::size_t> ( q );  // concatenation for b

				std::iota(phia.begin(), phia.end(), 1ul);
				std::transform(  phia.begin(), phia.end(), phib.begin(),
				                 [&pi] ( std::size_t i ) { return pi.at(i-1); } );

				auto c = ublas::prod(a, b, phia, phib);

				auto acc = value_type(1);
				for(auto i = 0ul; i < q; ++i)
					acc *= a.extents().at(phia.at(i)-1);

				for(auto i = 0ul; i < c.size(); ++i)
					BOOST_CHECK_EQUAL( c[i] , acc * a[0] * b[0] );

			}

			std::next_permutation(pi.begin(), pi.end());
		}
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_inner_prod, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	for(auto const& n : extents) {

		auto a  = tensor_type(n, value_type(2));
		auto b  = tensor_type(n, value_type(1));

		auto c = ublas::inner_prod(a, b);
		auto r = std::inner_product(a.begin(),a.end(), b.begin(),value_type(0));

		BOOST_CHECK_EQUAL( c , r );

	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_norm, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	for(auto const& n : extents) {

		auto a  = tensor_type(n);

		auto one = value_type(1);
		auto v = one;
		for(auto& aa: a)
			aa = v, v += one;

		auto c = ublas::inner_prod(a, a);
		auto r = std::inner_product(a.begin(),a.end(), a.begin(),value_type(0));

		auto r2 = ublas::norm( (a+a) / 2  );

		BOOST_CHECK_EQUAL( c , r );
		BOOST_CHECK_EQUAL( std::sqrt( c ) , r2 );

	}
}

BOOST_FIXTURE_TEST_CASE( test_tensor_real_imag_conj, fixture )
{
	using namespace boost::numeric;
	using value_type   = float;
	using complex_type = std::complex<value_type>;
	using layout_type  = ublas::first_order;

	using tensor_complex_type  = ublas::tensor<complex_type,layout_type>;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	for(auto const& n : extents) {

		auto a   = tensor_type(n);
		auto r0  = tensor_type(n);
		auto r00 = tensor_complex_type(n);

		auto one = value_type(1);
		auto v = one;
		for(auto& aa: a)
			aa = v, v += one;

		tensor_type b = (a+a) / value_type( 2 );
		tensor_type r1 = ublas::real( (a+a) / value_type( 2 )  );
		std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::real( l );  }   );
		BOOST_CHECK( r0 == r1 );

		tensor_type r2 = ublas::imag( (a+a) / value_type( 2 )  );
		std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::imag( l );  }   );
		BOOST_CHECK( r0 == r2 );

		tensor_complex_type r3 = ublas::conj( (a+a) / value_type( 2 )  );
		std::transform(  b.begin(), b.end(), r00.begin(), [](auto const& l){ return std::conj( l );  }   );
		BOOST_CHECK( r00 == r3 );

	}

	for(auto const& n : extents) {

		auto a   = tensor_complex_type(n);

		auto r00 = tensor_complex_type(n);
		auto r0  = tensor_type(n);

		auto one = complex_type(1,1);
		auto v = one;
		for(auto& aa: a)
			aa = v, v = v + one;

		tensor_complex_type b = (a+a) / complex_type( 2,2 );

		tensor_type r1 = ublas::real( (a+a) / complex_type( 2,2 )  );
		std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::real( l );  }   );
		BOOST_CHECK( r0 == r1 );

		tensor_type r2 = ublas::imag( (a+a) / complex_type( 2,2 )  );
		std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::imag( l );  }   );
		BOOST_CHECK( r0 == r2 );

		tensor_complex_type r3 = ublas::conj( (a+a) / complex_type( 2,2 )  );
		std::transform(  b.begin(), b.end(), r00.begin(), [](auto const& l){ return std::conj( l );  }   );
		BOOST_CHECK( r00 == r3 );

	}

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_outer_prod, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	for(auto const& n1 : extents) {
		auto a  = tensor_type(n1, value_type(2));
		for(auto const& n2 : extents) {

			auto b  = tensor_type(n2, value_type(1));
			auto c  = ublas::outer_prod(a, b);

			for(auto const& cc : c)
				BOOST_CHECK_EQUAL( cc , a[0]*b[0] );
		}
	}
}

template<class V>
void init(std::vector<V>& a)
{
	auto v = V(1);
	for(auto i = 0u; i < a.size(); ++i, ++v){
		a[i] = v;
	}
}

template<class V>
void init(std::vector<std::complex<V>>& a)
{
	auto v = std::complex<V>(1,1);
	for(auto i = 0u; i < a.size(); ++i){
		a[i] = v;
		v.real(v.real()+1);
		v.imag(v.imag()+1);
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_trans, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = ublas::tensor<value_type,layout_type>;

	auto fak = [](auto const& p){
		auto f = 1ul;
		for(auto i = 1u; i <= p; ++i)
			f *= i;
		return f;
	};

	auto inverse = [](auto const& pi){
		auto pi_inv = pi;
		for(auto j = 0u; j < pi.size(); ++j)
			pi_inv[pi[j]-1] = j+1;
		return pi_inv;
	};

	for(auto const& n : extents)
	{
		auto const p = n.size();
		auto const s = n.product();
		auto aref = tensor_type(n);
		auto v    = value_type{};
		for(auto i = 0u; i < s; ++i, v+=1)
			aref[i] = v;
		auto a    = aref;

		auto pi = std::vector<std::size_t>(p);
		std::iota(pi.begin(), pi.end(), 1);
		a = ublas::trans( a, pi );
		BOOST_CHECK( a == aref  );

		auto const pfak = fak(p);
		auto i = 0u;
		for(; i < pfak-1; ++i) {
			std::next_permutation(pi.begin(), pi.end());
			a = ublas::trans( a, pi );
		}
		std::next_permutation(pi.begin(), pi.end());
		for(; i > 0; --i) {
			std::prev_permutation(pi.begin(), pi.end());
			auto pi_inv = inverse(pi);
			a = ublas::trans( a, pi_inv );
		}

		BOOST_CHECK( a == aref  );

	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

#include "geometrycentral/surface/halfedge_mesh.h"
#include "geometrycentral/surface/heat_method_distance.h"
#include "geometrycentral/surface/halfedge_factories.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/surface_centers.h"
#include "geometrycentral/surface/vector_heat_method.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <sstream>
#include <chrono>

using namespace geometrycentral;
using namespace geometrycentral::surface;
namespace py = pybind11;

// Geometry-central data
std::unique_ptr<HalfedgeMesh> mesh;
std::unique_ptr<VertexPositionGeometry> geometry;

// Algorithm parameters for Vector Heat method
float tCoef = 1.0;
std::unique_ptr<VectorHeatMethodSolver> solver;

// HELPER FUNCTIONS ------------------------------------------------------------

// Loads a mesh from a NumPy array
std::tuple<std::unique_ptr<HalfedgeMesh>, std::unique_ptr<VertexPositionGeometry>>
loadMesh_np(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Set vertex positions
  std::vector<Vector3> vertexPositions(pos.rows());
  for (size_t i = 0; i < pos.rows(); i++) {
    vertexPositions[i][0] = pos(i, 0);
    vertexPositions[i][1] = pos(i, 1);
    vertexPositions[i][2] = pos(i, 2);
  }

  // Get face list
  std::vector<std::vector<size_t>> faceIndices(faces.rows());
  for (size_t i = 0; i < faces.rows(); i++) {
    faceIndices[i] = {faces(i, 0), faces(i, 1), faces(i, 2)};
  }

  return makeHalfedgeAndGeometry(faceIndices, vertexPositions);
}

// Precompute parallel transport and logarithmic map for a given neighborhood.
Eigen::MatrixXd precomputeHarmonic(Vertex& sourceV, Eigen::Matrix<size_t, Eigen::Dynamic, 1> targetVs,
                                  Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points) {
  if (solver == nullptr) {
    solver.reset(new VectorHeatMethodSolver(*geometry, tCoef));
  }

  // Coordinate systems are aligned to smoothed principal curvature directions
  Vector2 sourcePrincipalCurvature = geometry->vertexPrincipalCurvatureDirections[sourceV].normalize();

  // To compute parallel transport from point i (targetV) to point j (sourceV),
  // we transport the x-axis (the principal curvature direction) from sourceV to targetV.

  // First, set the source vectors to the principal curvature directions.
  std::vector<std::tuple<SurfacePoint, Vector2>> points;
  points.emplace_back(sourceV, sourcePrincipalCurvature);

  // Then, compute parallel transport of source vectors.
  VertexData<Vector2> connection = solver->transportTangentVectors(points);

  // And compute the logarithmic map from point i to j
  VertexData<Vector2> logmap = solver->computeLogMap(sourceV);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(targetVs.rows(), 4);

  // For every target point
  for (size_t i = 0; i < targetVs.rows(); i++) {
    size_t v = sample_points(targetVs(i));

    // The original logarithmic map is computed with a coordinate system aligned to the first edge.
    // To align the logarithmic map to the principal curvature directions,
    // we rotate the logmap by the principal curvature direction at the source point.
    Vector2 targetCoords = logmap[v] / sourcePrincipalCurvature;

    // Likewise for parallel transport, but we rotate by the principal curvature direction at the target point.
    Vector2 targetPrincipalCurvature = geometry->vertexPrincipalCurvatureDirections[v].normalize();
    Vector2 targetConnection = connection[v] / targetPrincipalCurvature;

    // Store the parallel transport (connection) and logarithmic map.
    res(i, 0) = targetConnection.x;
    res(i, 1) = targetConnection.y;
    res(i, 2) = targetCoords.x;
    res(i, 3) = targetCoords.y;
  }

  return res;
}

// PRECOMPUTATION for HSN ------------------------------------------------------------

// Precomputes the logarithmic map and parallel transport, given a mesh.
// The mesh should be provides as a NumPy array of vertex positions and a NumPy array of face indices.
// Additionally, one should provide a NumPy array of edge indices (source, target),
// a NumPy array with the degree of every source vertex, and indices of sampled points to return values for.
Eigen::MatrixXd precompute(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 2>& edge_index, Eigen::Matrix<size_t, Eigen::Dynamic, 1> degree,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();
  geometry->requireVertexLumpedMassMatrix();
  geometry->requireVertexPrincipalCurvatureDirections();

  // Setup solver for Vector Heat Method.
  solver.reset(new VectorHeatMethodSolver(*geometry, tCoef));

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(edge_index.rows(), 4);
  size_t index = 0;
  // For each sampled point:
  for (size_t row = 0; row < sample_points.rows(); row++) {
    Vertex v = mesh->vertex(sample_points(row));

    // Compute parallel transport and logarithmic map for neighborhood.
    res.block(index, 0, degree(row), 4) = precomputeHarmonic(v, edge_index.block(index, 1, degree(row), 1), sample_points);
    index += degree(row);
  }

  geometry->unrequireVertexPrincipalCurvatureDirections();
  geometry->unrequireVertexLumpedMassMatrix();
  geometry->unrequireVertexIndices();
  return res;
}

// Computes the vertex lumped mass matrix for each sampled vertex,
// automatically adding the weights from nearest geodesic neighbors.
Eigen::MatrixXd weights(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& sample_points, Eigen::Matrix<size_t, Eigen::Dynamic, 1>& labels) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();
  geometry->requireVertexLumpedMassMatrix();

  // We use short-time heat diffusion to retrieve geodesic nearest neighbors.
  VectorHeatMethodSolver vhmSolver(*geometry, 0.0001);

  // Set up indices of sampled points to diffuse.
  std::vector<std::tuple<SurfacePoint, double>> points;
  for (size_t row = 0; row < sample_points.rows(); row++) {
    points.emplace_back(SurfacePoint(mesh->vertex(sample_points(row))), labels(row));
  }

  // Solve heat diffusion.
  VertexData<double> scalarExtension = vhmSolver.extendScalar(points);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res = Eigen::MatrixXd::Zero(sample_points.rows(), 1);
  // For each vertex:
  for (size_t row = 0; row < pos.rows(); row++) {
    size_t to_idx = std::lround(scalarExtension[mesh->vertex(row)]);

    // Clamp nearest neighbor index from heat diffusion to range [0, n_vertices]
    if (to_idx >= sample_points.rows()) {
      to_idx = sample_points.rows() - 1;
    } else if (to_idx < 0) {
      to_idx = 0;
    }

    // Add vertex lumped mass to nearest sampled vertex.
    res(to_idx) += geometry->vertexLumpedMassMatrix.coeff(row, row);
  }

  geometry->unrequireVertexLumpedMassMatrix();
  geometry->unrequireVertexIndices();

  return res;
}

// UTILITIES for HSN ------------------------------------------------------------

// Compute the surface area of a mesh.
double surface_area(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  float surfaceArea = 0.0f;
  for (Face f : mesh->faces()) {
    surfaceArea += geometry->faceArea(f);
  }

  return surfaceArea;
}

// Compute geodesic nearest neighbors
Eigen::MatrixXd nearest(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces,
        Eigen::Matrix<size_t, Eigen::Dynamic, 1>& selected_points, Eigen::Matrix<size_t, Eigen::Dynamic, 1>& labels) {

  // Load mesh
  std::tie(mesh, geometry) = loadMesh_np(pos, faces);

  geometry->requireVertexIndices();

  // We use short-time heat diffusion to retrieve geodesic nearest neighbors.
  VectorHeatMethodSolver vhmSolver(*geometry, 0.0001);

  // Set up indices of sampled points to diffuse.
  std::vector<std::tuple<SurfacePoint, double>> points;
  for (size_t row = 0; row < selected_points.rows(); row++) {
    points.emplace_back(SurfacePoint(mesh->vertex(selected_points(row))), labels(row));
  }

  // Solve heat diffusion
  VertexData<double> scalarExtension = vhmSolver.extendScalar(points);

  // Store the results in an Eigen matrix, which can be accessed as a NumPy array.
  Eigen::MatrixXd res(pos.rows(), 1);
  for (size_t row = 0; row < pos.rows(); row++) {
    res(row) = scalarExtension[mesh->vertex(row)];
  }

  geometry->unrequireVertexIndices();

  return res;
}

PYBIND11_MODULE(vectorheat, m) {
    m.doc() = R"pbdoc(
        Harmonic Surface Networks precomputation module.
        -----------------------

        .. currentmodule:: precomputation

        .. autosummary::
           :toctree: _generate

           add
           precompute
           diameter
    )pbdoc";

    m.def("precompute", &precompute, py::return_value_policy::copy, R"pbdoc(
        Precompute parallel transport and logarithmic map for meshes given by pos, face, edges and degree.
    )pbdoc");

    m.def("surface_area", &surface_area, py::return_value_policy::copy, R"pbdoc(
        Computes surface area of the given mesh.
    )pbdoc");

    m.def("weights", &weights, py::return_value_policy::copy, R"pbdoc(
        Computes vertex lumped mass matrix for sampled points.
    )pbdoc");

    m.def("nearest", &nearest, py::return_value_policy::copy, R"pbdoc(
        Returns a mapping from all vertices to the nearest sampled points.
    )pbdoc");

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


#include <gtest/gtest.h>
#include "../include/model.h"
#include "../include/KalmanFilter.h"
#include "../include/filterModel.h"
#include "../include/mathWrapper/double.h"
#include "../include/mathWrapper/eigen.h"
#include "../include/mathWrapper/boost.h"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <math.h>
#include "../include/Eigen.h"
#include <iostream>

TEST(discreteDiscreteKalmanFilter, test){

	int stateCount = 1;
	int sensorCount = 1;
	int initialTime = 0;
	vectorDouble initialEstimate(1);
	matrixDouble initialCovariance(1);

	Eigen::VectorXd x(1);x<<1;
	vectorEigen initialEstimateEigen(x);
	Eigen::MatrixXd y(1,1);y<<1;
	matrixEigen initialCovarianceEigen(y);

	boost::numeric::ublas::matrix<double> initialCovarianceBoost(1,1);
	initialCovarianceBoost(0,0)=1;
	boost::numeric::ublas::vector<double> initialEstimateBoost(1);
	initialEstimateBoost(0) = 1;

	Eigen::VectorXd initialEstimateEigenSimple(1);initialEstimateEigenSimple<<1;
	Eigen::MatrixXd initialCovarianceEigenSimple(1,1);initialCovarianceEigenSimple<<1;

	class stateModel: public discreteModel<vectorDouble>{
		public:
			vectorDouble function(const vectorDouble & val, const int time) const override{
				vectorDouble result(val.getSystemValue() + 0.1);
				return result;
			}
	};
	class stateModelEigen: public discreteModel<vectorEigen>{
		Eigen::VectorXd velocity;
		public:
			stateModelEigen(){
				Eigen::VectorXd vel(1);vel<<0.1;
				velocity = vel;
			}
			vectorEigen function(const vectorEigen & val, const int time) const override{
				vectorEigen result(val.getSystemValue() + velocity);
				return result;
			}
	};
	class stateModelBoost: public discreteModel<vectorBoost>{
		boost::numeric::ublas::vector<double> velocity;
		public:
			stateModelBoost(){
				boost::numeric::ublas::vector<double> vel(1);vel(0) = 0.1;
				velocity = vel;
			}
			vectorBoost function(const vectorBoost & val, const int time) const override{
				vectorBoost result(val.getSystemValue() + velocity);
				return result;
			}
	};
	class stateModelEigenSimple: public discreteModel<Eigen::VectorXd>{
		Eigen::VectorXd velocity;
		public:
			stateModelEigenSimple(){
				Eigen::VectorXd vel(1);vel<< 0.1;
				velocity = vel;
			}
			Eigen::VectorXd function(const Eigen::VectorXd & val, const int time) const override{
				return val + velocity;
			}
	};

	class measurementModel: public discreteModel<vectorDouble>{
		public:
			vectorDouble function(const vectorDouble & val, const int time) const override{
				vectorDouble result(val.getSystemValue());
				return result;
			}
	};
	class measurementModelEigen: public discreteModel<vectorEigen>{
		public:
			vectorEigen function(const vectorEigen & val, const int time) const override{
				vectorEigen result(val.getSystemValue());
				return result;
			}
	};
	class measurementModelBoost: public discreteModel<vectorBoost>{
		public:
			vectorBoost function(const vectorBoost & val, const int time) const override{
				vectorBoost result(val.getSystemValue());
				return result;
			}
	};
	class measurementModelEigenSimple: public discreteModel<Eigen::VectorXd>{
		public:
			Eigen::VectorXd function(const Eigen::VectorXd & val, const int time) const override{
				return val;
			}
	};

	class transitionJac: public jacobianDiscrete<vectorDouble,matrixDouble>{
		public:
			matrixDouble function(const vectorDouble & val, int t){
				return matrixDouble(1);
			}
	};
	class measurementJac: public jacobianDiscrete<vectorDouble,matrixDouble>{
		public:
			matrixDouble function(const vectorDouble & val, int t){
				return matrixDouble(1);
			}
	};
	class transitionJacEigen: public jacobianDiscrete<vectorEigen,matrixEigen>{
		Eigen::MatrixXd v;
		public:
			transitionJacEigen(){
				Eigen::MatrixXd tmp(1,1);tmp<<1;
				v = tmp;
			}

			matrixEigen function(const vectorEigen & val, int t){
				return matrixEigen(v);
			}
	};
	class measurementJacEigen: public jacobianDiscrete<vectorEigen,matrixEigen>{
		Eigen::MatrixXd v;
		public:
			measurementJacEigen(){
				Eigen::MatrixXd tmp(1,1);tmp<<1;
				v = tmp;
			}

			matrixEigen function(const vectorEigen & val, int t){
				return matrixEigen(v);
			}
	};
	class transitionJacBoost: public jacobianDiscrete<vectorBoost,matrixBoost>{
		boost::numeric::ublas::matrix<double> v;
		public:
			transitionJacBoost(){
				boost::numeric::ublas::matrix<double> tmp(1,1);tmp(0,0) = 1;
				v = tmp;

			}

			matrixBoost function(const vectorBoost & val, int t){
				return matrixBoost(v);
			}
	};
	class measurementJacBoost: public jacobianDiscrete<vectorBoost,matrixBoost>{
		boost::numeric::ublas::matrix<double> v;
		public:
			measurementJacBoost(){
				boost::numeric::ublas::matrix<double> tmp(1,1);tmp(0,0) = 1;
				v = tmp;
			}

			matrixBoost function(const vectorBoost & val, int t){
				return matrixBoost(v);
			}
	};
	class transitionJacEigenSimple: public jacobianDiscrete<Eigen::VectorXd,Eigen::MatrixXd>{
		Eigen::MatrixXd v;
		public:
			transitionJacEigenSimple(){
				Eigen::MatrixXd tmp(1,1);tmp << 1;
				v = tmp;

			}

			Eigen::MatrixXd function(const Eigen::VectorXd & val, int t){
				return v;
			}
	};
	class measurementJacEigenSimple: public jacobianDiscrete<Eigen::VectorXd,Eigen::MatrixXd>{
		Eigen::MatrixXd v;
		public:
			measurementJacEigenSimple(){
				Eigen::MatrixXd tmp(1,1);tmp<< 1;
				v = tmp;
			}

			Eigen::MatrixXd function(const Eigen::VectorXd & val, int t){
				return v;
			}
	};

	class processNoise: public discreteNoiseCovariance<vectorDouble,matrixDouble>{
		matrixDouble function(const vectorDouble &est,int t) override{
			return matrixDouble(0.01);
		}
		matrixDouble sqrt(const vectorDouble &est, int t) override{
			return matrixDouble(0.1);
		}
	};
	class sensorNoise: public discreteNoiseCovariance<vectorDouble,matrixDouble>{
		matrixDouble function(const vectorDouble &est,int t) override{
			return matrixDouble(0.01);
		}
		matrixDouble sqrt(const vectorDouble &est, int t) override{
			return matrixDouble(0.1);
		}
	};
	class processNoiseEigen: public discreteNoiseCovariance<vectorEigen,matrixEigen>{
		Eigen::MatrixXd v;
		Eigen::MatrixXd v2;
		public:
		processNoiseEigen(){
			Eigen::MatrixXd tmp(1,1);tmp<<0.01;
			Eigen::MatrixXd tmp2(1,1);tmp2<<0.1;
			v = tmp;
			v2 = tmp2;
		}
		matrixEigen function(const vectorEigen &est,int t) override{
			return matrixEigen(v);
		}
		matrixEigen sqrt(const vectorEigen &est, int t) override{
			return matrixEigen(v2);
		}
	};
	class sensorNoiseEigen: public discreteNoiseCovariance<vectorEigen,matrixEigen>{
		Eigen::MatrixXd v;
		Eigen::MatrixXd v2;
		public:
		sensorNoiseEigen(){
			Eigen::MatrixXd tmp(1,1);tmp<<0.01;
			Eigen::MatrixXd tmp2(1,1);tmp2<<0.1;
			v = tmp;
			v2 = tmp2;
		}
		matrixEigen function(const vectorEigen &est, int t) override{
			return matrixEigen(v);
		}
		matrixEigen sqrt(const vectorEigen &est, int t) override{
			return matrixEigen(v2);
		}
	};
	class processNoiseBoost: public discreteNoiseCovariance<vectorBoost,matrixBoost>{
		boost::numeric::ublas::matrix<double> v;
		boost::numeric::ublas::matrix<double> v2;
		public:
		processNoiseBoost(){
			boost::numeric::ublas::matrix<double> tmp(1,1);tmp(0,0) = 0.01;
			boost::numeric::ublas::matrix<double> tmp2(1,1);tmp2(0,0) = 0.01;
			v = tmp;
			v2 = tmp2;
		}
		matrixBoost function(const vectorBoost &est,int t) override{
			return matrixBoost(v);
		}
		matrixBoost sqrt(const vectorBoost &est, int t) override{
			return matrixBoost(v2);
		}
	};
	class sensorNoiseBoost: public discreteNoiseCovariance<vectorBoost,matrixBoost>{
		boost::numeric::ublas::matrix<double> v;
		boost::numeric::ublas::matrix<double> v2;
		public:
		sensorNoiseBoost(){
			boost::numeric::ublas::matrix<double> tmp(1,1);tmp(0,0) = 0.01;
			boost::numeric::ublas::matrix<double> tmp2(1,1);tmp2(0,0) = 0.01;
			v = tmp;
			v2 = tmp2;
		}
		matrixBoost function(const vectorBoost &est,int t) override{
			return matrixBoost(v);
		}
		matrixBoost sqrt(const vectorBoost &est, int t) override{
			return matrixBoost(v2);
		}
	};
	class processNoiseEigenSimple: public discreteNoiseCovariance<Eigen::VectorXd,Eigen::MatrixXd>{
		Eigen::MatrixXd v;
		Eigen::MatrixXd v2;
		public:
		processNoiseEigenSimple(){
			Eigen::MatrixXd tmp(1,1);tmp<< 0.01;
			Eigen::MatrixXd tmp2(1,1);tmp2<<0.1;
			v = tmp;
			v2 = tmp2;
		}
		Eigen::MatrixXd function(const Eigen::VectorXd &est,int t) override{
			return v;
		}
		Eigen::MatrixXd sqrt(const Eigen::VectorXd &est, int t) override{
			return v2;
		}
	};
	class sensorNoiseEigenSimple: public discreteNoiseCovariance<Eigen::VectorXd,Eigen::MatrixXd>{
		Eigen::MatrixXd v;
		Eigen::MatrixXd v2;
		public:
		sensorNoiseEigenSimple(){
			Eigen::MatrixXd tmp(1,1);tmp<< 0.01;
			Eigen::MatrixXd tmp2(1,1);tmp2<<0.1;
			v = tmp;
			v2 = tmp2;
		}
		Eigen::MatrixXd function(const Eigen::VectorXd &est,int t) override{
			return v;
		}
		Eigen::MatrixXd sqrt(const Eigen::VectorXd &est, int t) override{
			return v2;
		}
	};

	stateModel tm;
	measurementModel mm;
	processNoise pn;
	sensorNoise sn;
	transitionJac tj;
	measurementJac mj;
	discreteDiscreteFilterModel<vectorDouble,matrixDouble> ddfm(&tm,&mm,&pn,&sn,&tj,&mj,stateCount,sensorCount);

	stateModelEigen tmEigen;
	measurementModelEigen mmEigen;
	processNoiseEigen pnEigen;
	sensorNoiseEigen snEigen;
	transitionJacEigen tjEigen;
	measurementJacEigen mjEigen;
	discreteDiscreteFilterModel<vectorEigen,matrixEigen> ddfmEigen(&tmEigen,&mmEigen,&pnEigen,&snEigen,&tjEigen,&mjEigen,
		stateCount,sensorCount);

	stateModelBoost tmBoost;
	measurementModelBoost mmBoost;
	processNoiseBoost pnBoost;
	sensorNoiseBoost snBoost;
	transitionJacBoost tjBoost;
	measurementJacBoost mjBoost;
	discreteDiscreteFilterModel<vectorBoost,matrixBoost> ddfmBoost(&tmBoost,&mmBoost,&pnBoost,&snBoost,&tjBoost,&mjBoost,
		stateCount,sensorCount);

	stateModelEigenSimple tmEigenSimple;
	measurementModelEigenSimple mmEigenSimple;
	processNoiseEigenSimple pnEigenSimple;
	sensorNoiseEigenSimple snEigenSimple;
	transitionJacEigenSimple tjEigenSimple;
	measurementJacEigenSimple mjEigenSimple;
	discreteDiscreteFilterModel<Eigen::VectorXd,Eigen::MatrixXd> ddfmEigenSimple(&tmEigenSimple,&mmEigenSimple,
		&pnEigenSimple,&snEigenSimple,&tjEigenSimple,&mjEigenSimple,stateCount,sensorCount);

	discreteDiscreteKalmanFilter<vectorDouble,matrixDouble>
		filter(initialTime, initialEstimate, initialCovariance, &ddfm);

	discreteDiscreteKalmanFilter<vectorEigen,matrixEigen>
		filterEigen(initialTime, initialEstimateEigen, initialCovarianceEigen, &ddfmEigen);

	discreteDiscreteKalmanFilter<vectorBoost,matrixBoost>
		filterBoost(initialTime, initialEstimateBoost, initialCovarianceBoost, &ddfmBoost);

	discreteDiscreteKalmanFilter<Eigen::VectorXd,Eigen::MatrixXd>
		filterEigenSimple( initialTime, initialEstimateEigenSimple, initialCovarianceEigenSimple,
			&ddfmEigenSimple);

	for(int i = 0; i < 10; i++){
		ASSERT_NEAR(filter.getCurrentEstimate().getSystemValue(),filterEigen.getCurrentEstimate().getSystemValue()[0],1e-9);
		ASSERT_NEAR(filter.getCurrentCovariance().getSystemValue(),
			filterEigen.getCurrentCovariance().getSystemValue().coeff(0,0),1e-9);
		ASSERT_NEAR(filter.getCurrentTime(),filterEigen.getCurrentTime(),1e-9);

		ASSERT_NEAR(filter.getCurrentEstimate().getSystemValue(),filterBoost.getCurrentEstimate().getSystemValue()[0],1e-9);
		boost::numeric::ublas::matrix<double> tmp = filterBoost.getCurrentCovariance().getSystemValue();
		ASSERT_NEAR(filter.getCurrentCovariance().getSystemValue(),tmp(0,0),1e-9);
		ASSERT_NEAR(filter.getCurrentTime(),filterBoost.getCurrentTime(),1e-9);

		filter.predict(1);
		filterEigen.predict(1);
		filterBoost.predict(1);
		filterEigenSimple.predict(1);
	}

	for(int i = 0; i < 10; i++){
		vectorDouble measurement(i);
		Eigen::VectorXd tmp(1);tmp<<i;
		vectorEigen tmp2(tmp);
		boost::numeric::ublas::vector<double> tmpBoost(1);tmpBoost(0)=i;
		vectorBoost tmp2Boost(tmpBoost);

		double sk = 1*filter.getCurrentCovariance().getSystemValue()*1 + 0.01;
		double kk = filter.getCurrentCovariance().getSystemValue()*(1.0/sk);
		double curEst = filter.getCurrentEstimate().getSystemValue();

		filter.update(measurement);
		filterEigen.update(tmp2);
		filterBoost.update(tmp2Boost);
		filterEigenSimple.update(tmp);

		ASSERT_NEAR(filter.getCurrentEstimate().getSystemValue(),filterEigen.getCurrentEstimate().getSystemValue()[0],1e-9);
		ASSERT_NEAR(filter.getCurrentEstimate().getSystemValue(),filterEigenSimple.getCurrentEstimate().coeff(0,0),1e-9);
		ASSERT_NEAR(filter.getCurrentCovariance().getSystemValue(),
			filterEigen.getCurrentCovariance().getSystemValue().coeff(0,0),1e-9);
		boost::numeric::ublas::matrix<double> t = filterBoost.getCurrentCovariance().getSystemValue();
		ASSERT_NEAR(filter.getCurrentCovariance().getSystemValue(),t(0,0),1e-9);
		ASSERT_NEAR(filter.getCurrentEstimate().getSystemValue(),filterBoost.getCurrentEstimate().getSystemValue()[0],1e-9);
	}
}

int main(int argc, char **argv){
	testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cosine: \f$(e^{x}+e^{-x})/2\f$.

    @see sinh, tanh, sech, csch, sinhcosh

    @par Header <boost/simd/function/cosh.hpp>

    @par Example:

      @snippet cosh.cpp cosh

    @par Possible output:

      @snippet cosh.txt cosh
  **/
  IEEEValue cosh(IEEEValue const& x);
} }
#endif

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

#endif


#pragma once
#include "utils.hpp"

#include <Eigen/Dense>
#include <cmath>

namespace khmot {

const int STATE_SIZE = 6;
const int OBSERVATION_SIZE = 3;
const double EPSILON = 1e-9;
const double PI = M_PI;
constexpr double TAU = 2 * M_PI;

// clang-format off
// TODO: noise covariance matrix should be configurable
const auto defaultNoiseCov =
    (Eigen::MatrixXd(STATE_SIZE, STATE_SIZE) << 0.5, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.5, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, .05, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, .01)
        .finished();
const auto defaultObsMatrix = // observe x, y, yaw
    (Eigen::MatrixXd(STATE_SIZE, STATE_SIZE) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        .finished();
// clang-format on

using State = Eigen::Matrix<double, STATE_SIZE, 1>;
using Covariance = Eigen::Matrix<double, STATE_SIZE, STATE_SIZE>;

enum StateMembers {
  StateMemberX = 0,
  StateMemberY,
  StateMemberYaw,
  StateMemberVx,
  StateMemberVy,
  StateMemberVyaw
};

struct KalmanObservation {
  State state = Eigen::VectorXd::Zero(STATE_SIZE);
  Covariance covariance =
      Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE) * EPSILON;
  double timestamp = 0.0;
};

class Kalman {
 public:
  Kalman(bool omnidirectional = true,
         Eigen::MatrixXd noiseCov = defaultNoiseCov);
  const Covariance& covariance() const { return P_; };
  const State& state() const { return state_; };

  void correct(KalmanObservation obs);
  void predict(const double timestamp);
  double lastObsTime() const { return lastObsTime_; };
  void reset();
  void wrapYaw();

 private:
  bool initialized_;
  bool omnidirectional_;  // Restrict motion sideways for non-omnidirectional
                          // robots
  double lastPredTime_;
  double lastObsTime_;
  Eigen::MatrixXd H_;  // KalmanObservation matrix
  Eigen::MatrixXd F_;  // State transition matrix (system dynamics)
  Eigen::MatrixXd Q_;  // Process noise covariance matrix
  State state_;        // Estimated state vector
  Covariance P_;       // Estimated error covariance matrix
};

double clampRotation(double rotation);
void preprocessObs(KalmanObservation& obs);

}  // namespace khmot


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschrÃ¤nkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

static const int DIM =3;
static const int NB  =4;
typedef dense2D<double, mat::parameters<tag::row_major,
					   mtl::index::c_index, mtl::fixed::dimensions< DIM, DIM> > > Mat3;
typedef mat::block_diagonal2D<Mat3> MatB;

int main()
{
    double array[][3]={{1.0, 0.0, 5.5}, {0.0, 2.0, 4.0}, {0.0, 0.0, 3.0}};
    Mat3 E3(array);

    MatB Eb(NB*DIM, NB*DIM);

    for(int i=0; i<NB; i++)
	Eb.insert(i*DIM, (i+1)*DIM, E3) ;

    mtl::io::tout << "E3 = \n" << E3 << "\n";
    mtl::io::tout << "Eb = \n" << Eb << "\n";

    MTL_THROW_IF((Eb(9, 9) != 1.0), mtl::unexpected_result());
    MTL_THROW_IF((Eb(9, 10) != 0.0), mtl::unexpected_result());

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::data::record.hpp                              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_DATA_RECORDER_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_DATA_RECORDER_HPP_ER_2009
#include <ostream>
#include <limits>
#include <boost/operators.hpp>
#include <boost/format.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //isinf
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/statistics/survival/constant.hpp>
#include <boost/statistics/survival/data/data/event.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

// Abstraction for entry time (t) and the failure time (rt).
//
// If not set during initialization, the failure time can be set later.
template<typename T>
class record : less_than_comparable<record<T> >{
protected:
    typedef constant<T> const_;
public:
    typedef T value_type;
    record(); //inf,inf
    record(value_type t);
    record(value_type t, value_type rt);
    // default copy/assign
    void set_failure_time(value_type rt);
    value_type entry_time()const;
    value_type failure_time()const;

    template<typename Archive>
    void serialize(Archive & ar, const unsigned int version);

    bool operator<(const record& other)const;

protected:
    value_type entry_time_;     //t
    value_type failure_time_;   //rt
};

template<typename T>
std::ostream& operator<<(std::ostream& out,const record<T>& r);

template<typename T>
typename record<T>::value_type
time_since_entry(const record<T>& r, typename record<T>::value_type t);

// TODO is_at_risk

template<typename T>
event<T>  make_event(const record<T>& r, typename record<T>::value_type t);

    // Implementation //

// Construct
template<typename T>
record<T>::record():entry_time_(const_::inf_),failure_time_(const_::inf_){}

template<typename T>
record<T>::record(value_type t):entry_time_(t),failure_time_(const_::inf_){}

template<typename T>
record<T>::record(value_type t,value_type rt)
:entry_time_(t),failure_time_(rt){}

// Assign
template<typename T>
void record<T>::set_failure_time(value_type rt){
    static const char* method = "statistics::survival::record::set_failure_time(%1%)";
    if(math::isinf(this->failure_time_)){
        this->failure_time_ = rt;
    }else{
        static const char* err = " error : overriding failure time";
        throw exception(
            format(method,rt).str(),
            err,
            *this
        );
    }
}

template<typename T>
template<typename Archive>
void record<T>::serialize(Archive & ar, const unsigned int version){
    ar & entry_time_;
    ar & failure_time_;
}

// Access

template<typename T>
typename record<T>::value_type
record<T>::entry_time()const{ return entry_time_; }

template<typename T>
typename record<T>::value_type
record<T>::failure_time()const{ return failure_time_; }

template<typename T>
std::ostream& operator<<(std::ostream& out,const record<T>& r){
    out << '(' <<  r.entry_time() << ',' << r.failure_time() << ')';
    return out;
}

template<typename T>
bool record<T>::operator<(const record<T>& other)const{
    return ( (this->entry_time()) < other.entry_time() );
}

template<typename T>
typename record<T>::value_type
time_since_entry(const record<T>& r, typename record<T>::value_type t)
{
    //Warning may be <0
    return t-r.entry_time();
}

// TODO is_at_risk

template<typename T>
event<T>  make_event(const record<T>& r, typename record<T>::value_type t){
    static const char* fun = "statistics::survival::make_event(%1%,%2)";
    typedef typename record<T>::value_type value_t;
    value_t eps = math::tools::epsilon<T>();
    value_t dt = time_since_entry(r,t);
    if(dt>eps){
        typedef event<T> result_type;
        bool b = ( r.failure_time() <= dt );
        value_t rt = b ? r.failure_time() : dt;
        return result_type(b, rt);
    }else{
        static const char* err = "error: time_since_entry = %1%";
        format f(fun); f%r%t;
        std::string str = f.str();
        f = format(err); f%dt; str+= f.str();
        throw std::runtime_error( str );
    }
}

}// data
}// survival
}// statistics
}// boost

#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 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: Abner Salgado, Texas A&M University 2009
 */

// @sect3{Include files}

// 我们首先包括所有必要的deal.II头文件和一些C++相关的文件。它们中的每一个都已经在以前的教程程序中讨论过了，所以我们在这里就不做详细介绍了。

#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/point.h>
#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/base/parallel.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/conditional_ostream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/sparse_ilu.h>
#include <deal.II/lac/sparse_direct.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/grid/grid_in.h>

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

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

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

#include <fstream>
#include <cmath>
#include <iostream>

// 最后这和以前的所有程序一样。

namespace Step35
{
  using namespace dealii;
// @sect3{Run time parameters}

// 由于我们的方法有几个可以微调的参数，我们把它们放到一个外部文件中，这样就可以在运行时确定它们。

// 这尤其包括辅助变量  $\phi$  的方程表述，为此我们声明一个  enum  。接下来，我们声明一个类，它将读取和存储我们的程序运行所需的所有参数。

  namespace RunTimeParameters
  {
    enum class Method
    {
      standard,
      rotational
    };

    class Data_Storage
    {
    public:
      Data_Storage();

      void read_data(const std::string &filename);

      Method form;

      double dt;
      double initial_time;
      double final_time;

      double Reynolds;

      unsigned int n_global_refines;

      unsigned int pressure_degree;

      unsigned int vel_max_iterations;
      unsigned int vel_Krylov_size;
      unsigned int vel_off_diagonals;
      unsigned int vel_update_prec;
      double       vel_eps;
      double       vel_diag_strength;

      bool         verbose;
      unsigned int output_interval;

    protected:
      ParameterHandler prm;
    };

// 在这个类的构造函数中，我们声明所有的参数。这方面的细节已经在其他地方讨论过了，例如在  step-29  。

    Data_Storage::Data_Storage()
      : form(Method::rotational)
      , dt(5e-4)
      , initial_time(0.)
      , final_time(1.)
      , Reynolds(1.)
      , n_global_refines(0)
      , pressure_degree(1)
      , vel_max_iterations(1000)
      , vel_Krylov_size(30)
      , vel_off_diagonals(60)
      , vel_update_prec(15)
      , vel_eps(1e-12)
      , vel_diag_strength(0.01)
      , verbose(true)
      , output_interval(15)
    {
      prm.declare_entry("Method_Form",
                        "rotational",
                        Patterns::Selection("rotational|standard"),
                        " Used to select the type of method that we are going "
                        "to use. ");
      prm.enter_subsection("Physical data");
      {
        prm.declare_entry("initial_time",
                          "0.",
                          Patterns::Double(0.),
                          " The initial time of the simulation. ");
        prm.declare_entry("final_time",
                          "1.",
                          Patterns::Double(0.),
                          " The final time of the simulation. ");
        prm.declare_entry("Reynolds",
                          "1.",
                          Patterns::Double(0.),
                          " The Reynolds number. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Time step data");
      {
        prm.declare_entry("dt",
                          "5e-4",
                          Patterns::Double(0.),
                          " The time step size. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Space discretization");
      {
        prm.declare_entry("n_of_refines",
                          "0",
                          Patterns::Integer(0, 15),
                          " The number of global refines we do on the mesh. ");
        prm.declare_entry("pressure_fe_degree",
                          "1",
                          Patterns::Integer(1, 5),
                          " The polynomial degree for the pressure space. ");
      }
      prm.leave_subsection();

      prm.enter_subsection("Data solve velocity");
      {
        prm.declare_entry(
          "max_iterations",
          "1000",
          Patterns::Integer(1, 1000),
          " The maximal number of iterations GMRES must make. ");
        prm.declare_entry("eps",
                          "1e-12",
                          Patterns::Double(0.),
                          " The stopping criterion. ");
        prm.declare_entry("Krylov_size",
                          "30",
                          Patterns::Integer(1),
                          " The size of the Krylov subspace to be used. ");
        prm.declare_entry("off_diagonals",
                          "60",
                          Patterns::Integer(0),
                          " The number of off-diagonal elements ILU must "
                          "compute. ");
        prm.declare_entry("diag_strength",
                          "0.01",
                          Patterns::Double(0.),
                          " Diagonal strengthening coefficient. ");
        prm.declare_entry("update_prec",
                          "15",
                          Patterns::Integer(1),
                          " This number indicates how often we need to "
                          "update the preconditioner");
      }
      prm.leave_subsection();

      prm.declare_entry("verbose",
                        "true",
                        Patterns::Bool(),
                        " This indicates whether the output of the solution "
                        "process should be verbose. ");

      prm.declare_entry("output_interval",
                        "1",
                        Patterns::Integer(1),
                        " This indicates between how many time steps we print "
                        "the solution. ");
    }

    void Data_Storage::read_data(const std::string &filename)
    {
      std::ifstream file(filename);
      AssertThrow(file, ExcFileNotOpen(filename));

      prm.parse_input(file);

      if (prm.get("Method_Form") == std::string("rotational"))
        form = Method::rotational;
      else
        form = Method::standard;

      prm.enter_subsection("Physical data");
      {
        initial_time = prm.get_double("initial_time");
        final_time   = prm.get_double("final_time");
        Reynolds     = prm.get_double("Reynolds");
      }
      prm.leave_subsection();

      prm.enter_subsection("Time step data");
      {
        dt = prm.get_double("dt");
      }
      prm.leave_subsection();

      prm.enter_subsection("Space discretization");
      {
        n_global_refines = prm.get_integer("n_of_refines");
        pressure_degree  = prm.get_integer("pressure_fe_degree");
      }
      prm.leave_subsection();

      prm.enter_subsection("Data solve velocity");
      {
        vel_max_iterations = prm.get_integer("max_iterations");
        vel_eps            = prm.get_double("eps");
        vel_Krylov_size    = prm.get_integer("Krylov_size");
        vel_off_diagonals  = prm.get_integer("off_diagonals");
        vel_diag_strength  = prm.get_double("diag_strength");
        vel_update_prec    = prm.get_integer("update_prec");
      }
      prm.leave_subsection();

      verbose = prm.get_bool("verbose");

      output_interval = prm.get_integer("output_interval");
    }
  } // namespace RunTimeParameters

//  @sect3{Equation data}

// 在下一个命名空间中，我们声明初始和边界条件。

  namespace EquationData
  {

// 由于我们选择了一个完全解耦的公式，我们将不利用deal.II处理矢量值问题的能力。然而，我们确实希望为方程数据使用一个独立于维度的接口。为了做到这一点，我们的函数应该能够知道我们目前在哪个空间分量上工作，而且我们应该能够有一个通用的接口来做到这一点。下面的类是在这个方向上的一个尝试。

    template <int dim>
    class MultiComponentFunction : public Function<dim>
    {
    public:
      MultiComponentFunction(const double initial_time = 0.);
      void set_component(const unsigned int d);

    protected:
      unsigned int comp;
    };

    template <int dim>
    MultiComponentFunction<dim>::MultiComponentFunction(
      const double initial_time)
      : Function<dim>(1, initial_time)
      , comp(0)
    {}

    template <int dim>
    void MultiComponentFunction<dim>::set_component(const unsigned int d)
    {
      Assert(d < dim, ExcIndexRange(d, 0, dim));
      comp = d;
    }

// 有了这个类的定义，我们声明描述速度和压力的边界条件的类。

    template <int dim>
    class Velocity : public MultiComponentFunction<dim>
    {
    public:
      Velocity(const double initial_time = 0.0);

      virtual double value(const Point<dim> & p,
                           const unsigned int component = 0) const override;

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

    template <int dim>
    Velocity<dim>::Velocity(const double initial_time)
      : MultiComponentFunction<dim>(initial_time)
    {}

    template <int dim>
    void Velocity<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<double> &          values,
                                   const unsigned int) const
    {
      const unsigned int n_points = points.size();
      Assert(values.size() == n_points,
             ExcDimensionMismatch(values.size(), n_points));
      for (unsigned int i = 0; i < n_points; ++i)
        values[i] = Velocity<dim>::value(points[i]);
    }

    template <int dim>
    double Velocity<dim>::value(const Point<dim> &p, const unsigned int) const
    {
      if (this->comp == 0)
        {
          const double Um = 1.5;
          const double H  = 4.1;
          return 4. * Um * p(1) * (H - p(1)) / (H * H);
        }
      else
        return 0.;
    }

    template <int dim>
    class Pressure : public Function<dim>
    {
    public:
      Pressure(const double initial_time = 0.0);

      virtual double value(const Point<dim> & p,
                           const unsigned int component = 0) const override;

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

    template <int dim>
    Pressure<dim>::Pressure(const double initial_time)
      : Function<dim>(1, initial_time)
    {}

    template <int dim>
    double Pressure<dim>::value(const Point<dim> & p,
                                const unsigned int component) const
    {
      (void)component;
      AssertIndexRange(component, 1);
      return 25. - p(0);
    }

    template <int dim>
    void Pressure<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<double> &          values,
                                   const unsigned int component) const
    {
      (void)component;
      AssertIndexRange(component, 1);
      const unsigned int n_points = points.size();
      Assert(values.size() == n_points,
             ExcDimensionMismatch(values.size(), n_points));
      for (unsigned int i = 0; i < n_points; ++i)
        values[i] = Pressure<dim>::value(points[i]);
    }
  } // namespace EquationData

//  @sect3{The NavierStokesProjection class}

// 现在是程序的主类。它实现了纳维-斯托克斯方程的各种版本的投影方法。考虑到介绍中给出的实现细节，所有方法和成员变量的名称应该是不言自明的。

  template <int dim>
  class NavierStokesProjection
  {
  public:
    NavierStokesProjection(const RunTimeParameters::Data_Storage &data);

    void run(const bool verbose = false, const unsigned int n_plots = 10);

  protected:
    RunTimeParameters::Method type;

    const unsigned int deg;
    const double       dt;
    const double       t_0;
    const double       T;
    const double       Re;

    EquationData::Velocity<dim>               vel_exact;
    std::map<types::global_dof_index, double> boundary_values;
    std::vector<types::boundary_id>           boundary_ids;

    Triangulation<dim> triangulation;

    FE_Q<dim> fe_velocity;
    FE_Q<dim> fe_pressure;

    DoFHandler<dim> dof_handler_velocity;
    DoFHandler<dim> dof_handler_pressure;

    QGauss<dim> quadrature_pressure;
    QGauss<dim> quadrature_velocity;

    SparsityPattern sparsity_pattern_velocity;
    SparsityPattern sparsity_pattern_pressure;
    SparsityPattern sparsity_pattern_pres_vel;

    SparseMatrix<double> vel_Laplace_plus_Mass;
    SparseMatrix<double> vel_it_matrix[dim];
    SparseMatrix<double> vel_Mass;
    SparseMatrix<double> vel_Laplace;
    SparseMatrix<double> vel_Advection;
    SparseMatrix<double> pres_Laplace;
    SparseMatrix<double> pres_Mass;
    SparseMatrix<double> pres_Diff[dim];
    SparseMatrix<double> pres_iterative;

    Vector<double> pres_n;
    Vector<double> pres_n_minus_1;
    Vector<double> phi_n;
    Vector<double> phi_n_minus_1;
    Vector<double> u_n[dim];
    Vector<double> u_n_minus_1[dim];
    Vector<double> u_star[dim];
    Vector<double> force[dim];
    Vector<double> v_tmp;
    Vector<double> pres_tmp;
    Vector<double> rot_u;

    SparseILU<double>   prec_velocity[dim];
    SparseILU<double>   prec_pres_Laplace;
    SparseDirectUMFPACK prec_mass;
    SparseDirectUMFPACK prec_vel_mass;

    DeclException2(ExcInvalidTimeStep,
                   double,
                   double,
                   << " The time step " << arg1 << " is out of range."
                   << std::endl
                   << " The permitted range is (0," << arg2 << "]");

    void create_triangulation_and_dofs(const unsigned int n_refines);

    void initialize();

    void interpolate_velocity();

    void diffusion_step(const bool reinit_prec);

    void projection_step(const bool reinit_prec);

    void update_pressure(const bool reinit_prec);

  private:
    unsigned int vel_max_its;
    unsigned int vel_Krylov_size;
    unsigned int vel_off_diagonals;
    unsigned int vel_update_prec;
    double       vel_eps;
    double       vel_diag_strength;

    void initialize_velocity_matrices();

    void initialize_pressure_matrices();

// 接下来的几个结构和函数是用来做各种并行的事情。它们遵循 @ref threads 中规定的方案，使用WorkStream类。正如那里所解释的，这需要我们为每个装配器声明两个结构，一个是每个任务的数据，一个是scratch数据结构。然后，这些结构被移交给组装本地贡献的函数，并将这些本地贡献复制到全局对象上。

// 这个程序的一个特殊之处在于，我们并不是只有一个DoFHandler对象来代表速度和压力，而是为这两种变量使用单独的DoFHandler对象。当我们想把涉及这两个变量的条款，如速度的发散和压力的梯度，乘以各自的测试函数时，我们要为这种优化付费。在这样做的时候，我们不能只使用一个FEValues对象，而是需要两个，而且需要用单元格迭代器来初始化它们，这些单元格迭代器指向三角形中的同一个单元格，但不同的DoFHandlers。

// 为了在实践中做到这一点，我们声明一个 "同步 "迭代器--一个内部由几个（在我们的例子中是两个）迭代器组成的对象，每次同步迭代器向前移动一步，内部存储的每个迭代器也向前移动一步，从而始终保持同步。碰巧的是，有一个deal.II类可以促进这种事情。这里重要的是要知道，建立在同一个三角形上的两个DoFHandler对象将以相同的顺序走过三角形的单元。

    using IteratorTuple =
      std::tuple<typename DoFHandler<dim>::active_cell_iterator,
                 typename DoFHandler<dim>::active_cell_iterator>;

    using IteratorPair = SynchronousIterators<IteratorTuple>;

    void initialize_gradient_operator();

    struct InitGradPerTaskData
    {
      unsigned int                         d;
      unsigned int                         vel_dpc;
      unsigned int                         pres_dpc;
      FullMatrix<double>                   local_grad;
      std::vector<types::global_dof_index> vel_local_dof_indices;
      std::vector<types::global_dof_index> pres_local_dof_indices;

      InitGradPerTaskData(const unsigned int dd,
                          const unsigned int vdpc,
                          const unsigned int pdpc)
        : d(dd)
        , vel_dpc(vdpc)
        , pres_dpc(pdpc)
        , local_grad(vdpc, pdpc)
        , vel_local_dof_indices(vdpc)
        , pres_local_dof_indices(pdpc)
      {}
    };

    struct InitGradScratchData
    {
      unsigned int  nqp;
      FEValues<dim> fe_val_vel;
      FEValues<dim> fe_val_pres;
      InitGradScratchData(const FE_Q<dim> &  fe_v,
                          const FE_Q<dim> &  fe_p,
                          const QGauss<dim> &quad,
                          const UpdateFlags  flags_v,
                          const UpdateFlags  flags_p)
        : nqp(quad.size())
        , fe_val_vel(fe_v, quad, flags_v)
        , fe_val_pres(fe_p, quad, flags_p)
      {}
      InitGradScratchData(const InitGradScratchData &data)
        : nqp(data.nqp)
        , fe_val_vel(data.fe_val_vel.get_fe(),
                     data.fe_val_vel.get_quadrature(),
                     data.fe_val_vel.get_update_flags())
        , fe_val_pres(data.fe_val_pres.get_fe(),
                      data.fe_val_pres.get_quadrature(),
                      data.fe_val_pres.get_update_flags())
      {}
    };

    void assemble_one_cell_of_gradient(const IteratorPair & SI,
                                       InitGradScratchData &scratch,
                                       InitGradPerTaskData &data);

    void copy_gradient_local_to_global(const InitGradPerTaskData &data);

// 同样的一般布局也适用于以下实现平流项组装的类和函数。

    void assemble_advection_term();

    struct AdvectionPerTaskData
    {
      FullMatrix<double>                   local_advection;
      std::vector<types::global_dof_index> local_dof_indices;
      AdvectionPerTaskData(const unsigned int dpc)
        : local_advection(dpc, dpc)
        , local_dof_indices(dpc)
      {}
    };

    struct AdvectionScratchData
    {
      unsigned int                nqp;
      unsigned int                dpc;
      std::vector<Point<dim>>     u_star_local;
      std::vector<Tensor<1, dim>> grad_u_star;
      std::vector<double>         u_star_tmp;
      FEValues<dim>               fe_val;
      AdvectionScratchData(const FE_Q<dim> &  fe,
                           const QGauss<dim> &quad,
                           const UpdateFlags  flags)
        : nqp(quad.size())
        , dpc(fe.n_dofs_per_cell())
        , u_star_local(nqp)
        , grad_u_star(nqp)
        , u_star_tmp(nqp)
        , fe_val(fe, quad, flags)
      {}

      AdvectionScratchData(const AdvectionScratchData &data)
        : nqp(data.nqp)
        , dpc(data.dpc)
        , u_star_local(nqp)
        , grad_u_star(nqp)
        , u_star_tmp(nqp)
        , fe_val(data.fe_val.get_fe(),
                 data.fe_val.get_quadrature(),
                 data.fe_val.get_update_flags())
      {}
    };

    void assemble_one_cell_of_advection(
      const typename DoFHandler<dim>::active_cell_iterator &cell,
      AdvectionScratchData &                                scratch,
      AdvectionPerTaskData &                                data);

    void copy_advection_local_to_global(const AdvectionPerTaskData &data);

// 最后几个函数实现了扩散解以及输出的后处理，包括计算速度的曲线。

    void diffusion_component_solve(const unsigned int d);

    void output_results(const unsigned int step);

    void assemble_vorticity(const bool reinit_prec);
  };

//  @sect4{ NavierStokesProjection::NavierStokesProjection }

// 在构造函数中，我们只是从作为参数传递的 Data_Storage 对象中读取所有数据，验证我们读取的数据是否合理，最后，创建三角形并加载初始数据。

  template <int dim>
  NavierStokesProjection<dim>::NavierStokesProjection(
    const RunTimeParameters::Data_Storage &data)
    : type(data.form)
    , deg(data.pressure_degree)
    , dt(data.dt)
    , t_0(data.initial_time)
    , T(data.final_time)
    , Re(data.Reynolds)
    , vel_exact(data.initial_time)
    , fe_velocity(deg + 1)
    , fe_pressure(deg)
    , dof_handler_velocity(triangulation)
    , dof_handler_pressure(triangulation)
    , quadrature_pressure(deg + 1)
    , quadrature_velocity(deg + 2)
    , vel_max_its(data.vel_max_iterations)
    , vel_Krylov_size(data.vel_Krylov_size)
    , vel_off_diagonals(data.vel_off_diagonals)
    , vel_update_prec(data.vel_update_prec)
    , vel_eps(data.vel_eps)
    , vel_diag_strength(data.vel_diag_strength)
  {
    if (deg < 1)
      std::cout
        << " WARNING: The chosen pair of finite element spaces is not stable."
        << std::endl
        << " The obtained results will be nonsense" << std::endl;

    AssertThrow(!((dt <= 0.) || (dt > .5 * T)), ExcInvalidTimeStep(dt, .5 * T));

    create_triangulation_and_dofs(data.n_global_refines);
    initialize();
  }
// @sect4{NavierStokesProjection::create_triangulation_and_dofs}

// 创建三角形的方法，并将其细化到所需的次数。在创建三角形之后，它创建了与网格相关的数据，即分配自由度和重新编号，并初始化我们将使用的矩阵和向量。

  template <int dim>
  void NavierStokesProjection<dim>::create_triangulation_and_dofs(
    const unsigned int n_refines)
  {
    GridIn<dim> grid_in;
    grid_in.attach_triangulation(triangulation);

    {
      std::string   filename = "nsbench2.inp";
      std::ifstream file(filename);
      Assert(file, ExcFileNotOpen(filename.c_str()));
      grid_in.read_ucd(file);
    }

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

    boundary_ids = triangulation.get_boundary_ids();

    DoFRenumbering::boost::Cuthill_McKee(dof_handler_velocity);
    dof_handler_pressure.distribute_dofs(fe_pressure);
    DoFRenumbering::boost::Cuthill_McKee(dof_handler_pressure);

    initialize_velocity_matrices();
    initialize_pressure_matrices();
    initialize_gradient_operator();

    pres_n.reinit(dof_handler_pressure.n_dofs());
    pres_n_minus_1.reinit(dof_handler_pressure.n_dofs());
    phi_n.reinit(dof_handler_pressure.n_dofs());
    phi_n_minus_1.reinit(dof_handler_pressure.n_dofs());
    pres_tmp.reinit(dof_handler_pressure.n_dofs());
    for (unsigned int d = 0; d < dim; ++d)
      {
        u_n[d].reinit(dof_handler_velocity.n_dofs());
        u_n_minus_1[d].reinit(dof_handler_velocity.n_dofs());
        u_star[d].reinit(dof_handler_velocity.n_dofs());
        force[d].reinit(dof_handler_velocity.n_dofs());
      }
    v_tmp.reinit(dof_handler_velocity.n_dofs());
    rot_u.reinit(dof_handler_velocity.n_dofs());

    std::cout << "dim (X_h) = " << (dof_handler_velocity.n_dofs() * dim) //
              << std::endl                                               //
              << "dim (M_h) = " << dof_handler_pressure.n_dofs()         //
              << std::endl                                               //
              << "Re        = " << Re << std::endl                       //
              << std::endl;
  }
// @sect4{ NavierStokesProjection::initialize }

// 该方法创建常数矩阵并加载初始数据。

  template <int dim>
  void NavierStokesProjection<dim>::initialize()
  {
    vel_Laplace_plus_Mass = 0.;
    vel_Laplace_plus_Mass.add(1. / Re, vel_Laplace);
    vel_Laplace_plus_Mass.add(1.5 / dt, vel_Mass);

    EquationData::Pressure<dim> pres(t_0);
    VectorTools::interpolate(dof_handler_pressure, pres, pres_n_minus_1);
    pres.advance_time(dt);
    VectorTools::interpolate(dof_handler_pressure, pres, pres_n);
    phi_n         = 0.;
    phi_n_minus_1 = 0.;
    for (unsigned int d = 0; d < dim; ++d)
      {
        vel_exact.set_time(t_0);
        vel_exact.set_component(d);
        VectorTools::interpolate(dof_handler_velocity,
                                 vel_exact,
                                 u_n_minus_1[d]);
        vel_exact.advance_time(dt);
        VectorTools::interpolate(dof_handler_velocity, vel_exact, u_n[d]);
      }
  }
// @sect4{ NavierStokesProjection::initialize_*_matrices }

// 在这组方法中，我们初始化了稀疏模式、约束条件（如果有的话）并组装了不依赖于时间步长的矩阵  dt  。请注意，对于拉普拉斯矩阵和质量矩阵，我们可以使用库中的函数来做这件事。因为这个函数的昂贵操作--创建两个矩阵--是完全独立的，我们原则上可以把它们标记为可以使用 Threads::new_task 函数进行%并行工作的任务。我们不会在这里这样做，因为这些函数在内部已经被并行化了，特别是由于当前的函数在每个程序运行中只被调用一次，所以在每个时间步长中不会产生费用。然而，必要的修改将是非常直接的。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_velocity_matrices()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_velocity.n_dofs(),
                                 dof_handler_velocity.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_velocity, dsp);
      sparsity_pattern_velocity.copy_from(dsp);
    }
    vel_Laplace_plus_Mass.reinit(sparsity_pattern_velocity);
    for (unsigned int d = 0; d < dim; ++d)
      vel_it_matrix[d].reinit(sparsity_pattern_velocity);
    vel_Mass.reinit(sparsity_pattern_velocity);
    vel_Laplace.reinit(sparsity_pattern_velocity);
    vel_Advection.reinit(sparsity_pattern_velocity);

    MatrixCreator::create_mass_matrix(dof_handler_velocity,
                                      quadrature_velocity,
                                      vel_Mass);
    MatrixCreator::create_laplace_matrix(dof_handler_velocity,
                                         quadrature_velocity,
                                         vel_Laplace);
  }

//作用于压力空间的矩阵的初始化与作用于速度空间的矩阵相似。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_pressure_matrices()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_pressure.n_dofs(),
                                 dof_handler_pressure.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_pressure, dsp);
      sparsity_pattern_pressure.copy_from(dsp);
    }

    pres_Laplace.reinit(sparsity_pattern_pressure);
    pres_iterative.reinit(sparsity_pattern_pressure);
    pres_Mass.reinit(sparsity_pattern_pressure);

    MatrixCreator::create_laplace_matrix(dof_handler_pressure,
                                         quadrature_pressure,
                                         pres_Laplace);
    MatrixCreator::create_mass_matrix(dof_handler_pressure,
                                      quadrature_pressure,
                                      pres_Mass);
  }

// 对于梯度算子，我们从初始化稀疏模式和压缩它开始。这里需要注意的是，梯度算子从压力空间作用到速度空间，所以我们必须处理两个不同的有限元空间。为了保持循环的同步，我们使用之前定义的别名，即 PairedIterators and IteratorPair  。

  template <int dim>
  void NavierStokesProjection<dim>::initialize_gradient_operator()
  {
    {
      DynamicSparsityPattern dsp(dof_handler_velocity.n_dofs(),
                                 dof_handler_pressure.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler_velocity,
                                      dof_handler_pressure,
                                      dsp);
      sparsity_pattern_pres_vel.copy_from(dsp);
    }

    InitGradPerTaskData per_task_data(0,
                                      fe_velocity.n_dofs_per_cell(),
                                      fe_pressure.n_dofs_per_cell());
    InitGradScratchData scratch_data(fe_velocity,
                                     fe_pressure,
                                     quadrature_velocity,
                                     update_gradients | update_JxW_values,
                                     update_values);

    for (unsigned int d = 0; d < dim; ++d)
      {
        pres_Diff[d].reinit(sparsity_pattern_pres_vel);
        per_task_data.d = d;
        WorkStream::run(
          IteratorPair(IteratorTuple(dof_handler_velocity.begin_active(),
                                     dof_handler_pressure.begin_active())),
          IteratorPair(IteratorTuple(dof_handler_velocity.end(),
                                     dof_handler_pressure.end())),
          *this,
          &NavierStokesProjection<dim>::assemble_one_cell_of_gradient,
          &NavierStokesProjection<dim>::copy_gradient_local_to_global,
          scratch_data,
          per_task_data);
      }
  }

  template <int dim>
  void NavierStokesProjection<dim>::assemble_one_cell_of_gradient(
    const IteratorPair & SI,
    InitGradScratchData &scratch,
    InitGradPerTaskData &data)
  {
    scratch.fe_val_vel.reinit(std::get<0>(*SI));
    scratch.fe_val_pres.reinit(std::get<1>(*SI));

    std::get<0>(*SI)->get_dof_indices(data.vel_local_dof_indices);
    std::get<1>(*SI)->get_dof_indices(data.pres_local_dof_indices);

    data.local_grad = 0.;
    for (unsigned int q = 0; q < scratch.nqp; ++q)
      {
        for (unsigned int i = 0; i < data.vel_dpc; ++i)
          for (unsigned int j = 0; j < data.pres_dpc; ++j)
            data.local_grad(i, j) +=
              -scratch.fe_val_vel.JxW(q) *
              scratch.fe_val_vel.shape_grad(i, q)[data.d] *
              scratch.fe_val_pres.shape_value(j, q);
      }
  }

  template <int dim>
  void NavierStokesProjection<dim>::copy_gradient_local_to_global(
    const InitGradPerTaskData &data)
  {
    for (unsigned int i = 0; i < data.vel_dpc; ++i)
      for (unsigned int j = 0; j < data.pres_dpc; ++j)
        pres_Diff[data.d].add(data.vel_local_dof_indices[i],
                              data.pres_local_dof_indices[j],
                              data.local_grad(i, j));
  }
// @sect4{ NavierStokesProjection::run }

// 这是时间行进函数，从 t_0 开始，使用时间步长 dt 的投影法在时间上前进，直到 T  。

// 它的第二个参数 verbose 表示该函数是否应该输出它在任何特定时刻正在做什么的信息：例如，它将说明我们是否正在进行扩散、投影子步骤；更新前置条件器等等。我们没有使用像
// @code
//    if (verbose) std::cout << "something";
//  @endcode
//  那样的代码来实现这种输出，而是使用ConditionalOStream类来为我们做这个。该类接受一个输出流和一个条件，该条件表明你传递给它的东西是否应该被传递到给定的输出流，或者应该被忽略。这样，上面的代码就变成了
//  @code
//    verbose_cout << "something";
//  @endcode，并且在任何情况下都会做正确的事情。

  template <int dim>
  void NavierStokesProjection<dim>::run(const bool         verbose,
                                        const unsigned int output_interval)
  {
    ConditionalOStream verbose_cout(std::cout, verbose);

    const auto n_steps = static_cast<unsigned int>((T - t_0) / dt);
    vel_exact.set_time(2. * dt);
    output_results(1);
    for (unsigned int n = 2; n <= n_steps; ++n)
      {
        if (n % output_interval == 0)
          {
            verbose_cout << "Plotting Solution" << std::endl;
            output_results(n);
          }
        std::cout << "Step = " << n << " Time = " << (n * dt) << std::endl;
        verbose_cout << "  Interpolating the velocity " << std::endl;

        interpolate_velocity();
        verbose_cout << "  Diffusion Step" << std::endl;
        if (n % vel_update_prec == 0)
          verbose_cout << "    With reinitialization of the preconditioner"
                       << std::endl;
        diffusion_step((n % vel_update_prec == 0) || (n == 2));
        verbose_cout << "  Projection Step" << std::endl;
        projection_step((n == 2));
        verbose_cout << "  Updating the Pressure" << std::endl;
        update_pressure((n == 2));
        vel_exact.advance_time(dt);
      }
    output_results(n_steps);
  }

  template <int dim>
  void NavierStokesProjection<dim>::interpolate_velocity()
  {
    for (unsigned int d = 0; d < dim; ++d)
      {
        u_star[d].equ(2., u_n[d]);
        u_star[d] -= u_n_minus_1[d];
      }
  }
// @sect4{NavierStokesProjection::diffusion_step}

// 扩散步骤的实现。请注意，昂贵的操作是函数末尾的扩散解，我们必须为每个速度分量做一次。为了加快进度，我们允许以%并行方式进行，使用 Threads::new_task 函数，确保 dim 的求解都得到处理，并被安排到可用的处理器上：如果你的机器有一个以上的处理器核心，并且这个程序的其他部分目前没有使用资源，那么扩散求解将以%并行方式运行。另一方面，如果你的系统只有一个处理器核心，那么以%并行方式运行将是低效的（因为它导致了，例如，缓存拥堵），事情将被顺序地执行。

  template <int dim>
  void NavierStokesProjection<dim>::diffusion_step(const bool reinit_prec)
  {
    pres_tmp.equ(-1., pres_n);
    pres_tmp.add(-4. / 3., phi_n, 1. / 3., phi_n_minus_1);

    assemble_advection_term();

    for (unsigned int d = 0; d < dim; ++d)
      {
        force[d] = 0.;
        v_tmp.equ(2. / dt, u_n[d]);
        v_tmp.add(-.5 / dt, u_n_minus_1[d]);
        vel_Mass.vmult_add(force[d], v_tmp);

        pres_Diff[d].vmult_add(force[d], pres_tmp);
        u_n_minus_1[d] = u_n[d];

        vel_it_matrix[d].copy_from(vel_Laplace_plus_Mass);
        vel_it_matrix[d].add(1., vel_Advection);

        vel_exact.set_component(d);
        boundary_values.clear();
        for (const auto &boundary_id : boundary_ids)
          {
            switch (boundary_id)
              {
                case 1:
                  VectorTools::interpolate_boundary_values(
                    dof_handler_velocity,
                    boundary_id,
                    Functions::ZeroFunction<dim>(),
                    boundary_values);
                  break;
                case 2:
                  VectorTools::interpolate_boundary_values(dof_handler_velocity,
                                                           boundary_id,
                                                           vel_exact,
                                                           boundary_values);
                  break;
                case 3:
                  if (d != 0)
                    VectorTools::interpolate_boundary_values(
                      dof_handler_velocity,
                      boundary_id,
                      Functions::ZeroFunction<dim>(),
                      boundary_values);
                  break;
                case 4:
                  VectorTools::interpolate_boundary_values(
                    dof_handler_velocity,
                    boundary_id,
                    Functions::ZeroFunction<dim>(),
                    boundary_values);
                  break;
                default:
                  Assert(false, ExcNotImplemented());
              }
          }
        MatrixTools::apply_boundary_values(boundary_values,
                                           vel_it_matrix[d],
                                           u_n[d],
                                           force[d]);
      }

    Threads::TaskGroup<void> tasks;
    for (unsigned int d = 0; d < dim; ++d)
      {
        if (reinit_prec)
          prec_velocity[d].initialize(vel_it_matrix[d],
                                      SparseILU<double>::AdditionalData(
                                        vel_diag_strength, vel_off_diagonals));
        tasks += Threads::new_task(
          &NavierStokesProjection<dim>::diffusion_component_solve, *this, d);
      }
    tasks.join_all();
  }

  template <int dim>
  void
  NavierStokesProjection<dim>::diffusion_component_solve(const unsigned int d)
  {
    SolverControl solver_control(vel_max_its, vel_eps * force[d].l2_norm());
    SolverGMRES<Vector<double>> gmres(
      solver_control,
      SolverGMRES<Vector<double>>::AdditionalData(vel_Krylov_size));
    gmres.solve(vel_it_matrix[d], u_n[d], force[d], prec_velocity[d]);
  }
// @sect4{ NavierStokesProjection::assemble_advection_term }

// 下面的几个函数是关于集合平流项的，平流项是扩散步骤的系统矩阵的一部分，在每个时间步骤中都会发生变化。如上所述，我们将使用WorkStream类和文件模块 @ref threads 中描述的其他设施，在所有单元上平行运行装配循环。

  template <int dim>
  void NavierStokesProjection<dim>::assemble_advection_term()
  {
    vel_Advection = 0.;
    AdvectionPerTaskData data(fe_velocity.n_dofs_per_cell());
    AdvectionScratchData scratch(fe_velocity,
                                 quadrature_velocity,
                                 update_values | update_JxW_values |
                                   update_gradients);
    WorkStream::run(
      dof_handler_velocity.begin_active(),
      dof_handler_velocity.end(),
      *this,
      &NavierStokesProjection<dim>::assemble_one_cell_of_advection,
      &NavierStokesProjection<dim>::copy_advection_local_to_global,
      scratch,
      data);
  }

  template <int dim>
  void NavierStokesProjection<dim>::assemble_one_cell_of_advection(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    AdvectionScratchData &                                scratch,
    AdvectionPerTaskData &                                data)
  {
    scratch.fe_val.reinit(cell);
    cell->get_dof_indices(data.local_dof_indices);
    for (unsigned int d = 0; d < dim; ++d)
      {
        scratch.fe_val.get_function_values(u_star[d], scratch.u_star_tmp);
        for (unsigned int q = 0; q < scratch.nqp; ++q)
          scratch.u_star_local[q](d) = scratch.u_star_tmp[q];
      }

    for (unsigned int d = 0; d < dim; ++d)
      {
        scratch.fe_val.get_function_gradients(u_star[d], scratch.grad_u_star);
        for (unsigned int q = 0; q < scratch.nqp; ++q)
          {
            if (d == 0)
              scratch.u_star_tmp[q] = 0.;
            scratch.u_star_tmp[q] += scratch.grad_u_star[q][d];
          }
      }

    data.local_advection = 0.;
    for (unsigned int q = 0; q < scratch.nqp; ++q)
      for (unsigned int i = 0; i < scratch.dpc; ++i)
        for (unsigned int j = 0; j < scratch.dpc; ++j)
          data.local_advection(i, j) += (scratch.u_star_local[q] *            //
                                           scratch.fe_val.shape_grad(j, q) *  //
                                           scratch.fe_val.shape_value(i, q)   //
                                         +                                    //
                                         0.5 *                                //
                                           scratch.u_star_tmp[q] *            //
                                           scratch.fe_val.shape_value(i, q) * //
                                           scratch.fe_val.shape_value(j, q))  //
                                        * scratch.fe_val.JxW(q);
  }

  template <int dim>
  void NavierStokesProjection<dim>::copy_advection_local_to_global(
    const AdvectionPerTaskData &data)
  {
    for (unsigned int i = 0; i < fe_velocity.n_dofs_per_cell(); ++i)
      for (unsigned int j = 0; j < fe_velocity.n_dofs_per_cell(); ++j)
        vel_Advection.add(data.local_dof_indices[i],
                          data.local_dof_indices[j],
                          data.local_advection(i, j));
  }

//  @sect4{NavierStokesProjection::projection_step}

// 这实现了投影的步骤。

  template <int dim>
  void NavierStokesProjection<dim>::projection_step(const bool reinit_prec)
  {
    pres_iterative.copy_from(pres_Laplace);

    pres_tmp = 0.;
    for (unsigned d = 0; d < dim; ++d)
      pres_Diff[d].Tvmult_add(pres_tmp, u_n[d]);

    phi_n_minus_1 = phi_n;

    static std::map<types::global_dof_index, double> bval;
    if (reinit_prec)
      VectorTools::interpolate_boundary_values(dof_handler_pressure,
                                               3,
                                               Functions::ZeroFunction<dim>(),
                                               bval);

    MatrixTools::apply_boundary_values(bval, pres_iterative, phi_n, pres_tmp);

    if (reinit_prec)
      prec_pres_Laplace.initialize(pres_iterative,
                                   SparseILU<double>::AdditionalData(
                                     vel_diag_strength, vel_off_diagonals));

    SolverControl solvercontrol(vel_max_its, vel_eps * pres_tmp.l2_norm());
    SolverCG<Vector<double>> cg(solvercontrol);
    cg.solve(pres_iterative, phi_n, pres_tmp, prec_pres_Laplace);

    phi_n *= 1.5 / dt;
  }
// @sect4{ NavierStokesProjection::update_pressure }

// 这是投影法的压力更新步骤。它实现了该方法的标准表述，即
//  @f[ p^{n+1} = p^n +
//  \phi^{n+1},
//  @f]
//  或旋转形式，即
//  @f[ p^{n+1} = p^n +
//  \phi^{n+1} - \frac{1}{Re} \nabla\cdot u^{n+1}.
//  @f]

  template <int dim>
  void NavierStokesProjection<dim>::update_pressure(const bool reinit_prec)
  {
    pres_n_minus_1 = pres_n;
    switch (type)
      {
        case RunTimeParameters::Method::standard:
          pres_n += phi_n;
          break;
        case RunTimeParameters::Method::rotational:
          if (reinit_prec)
            prec_mass.initialize(pres_Mass);
          pres_n = pres_tmp;
          prec_mass.solve(pres_n);
          pres_n.sadd(1. / Re, 1., pres_n_minus_1);
          pres_n += phi_n;
          break;
        default:
          Assert(false, ExcNotImplemented());
      };
  }
// @sect4{ NavierStokesProjection::output_results }

// 该方法绘制了当前的解决方案。主要的困难是，我们想创建一个单一的输出文件，其中包含所有的速度分量、压力以及流动的涡度的数据。另一方面，速度和压力存在于不同的DoFHandler对象中，因此不能用一个DataOut对象写入同一个文件。因此，我们必须更努力地把各种数据放到一个DoFHandler对象中，然后用它来驱动图形输出。

// 我们不会在这里详细说明这个过程，而是参考  step-32  ，那里使用了一个类似的程序（并有记录），为所有变量创建一个联合的 DoFHandler 对象。

// 我们还注意到，我们在这里将涡度作为一个单独的函数中的标量来计算，使用 $L^2$ 量的投影 $\text{curl} u$ 到用于速度成分的有限元空间。但原则上，我们也可以从速度中计算出一个点状量，并通过 step-29 和 step-33 中讨论的DataPostprocessor机制实现。

  template <int dim>
  void NavierStokesProjection<dim>::output_results(const unsigned int step)
  {
    assemble_vorticity((step == 1));
    const FESystem<dim> joint_fe(
      fe_velocity, dim, fe_pressure, 1, fe_velocity, 1);
    DoFHandler<dim> joint_dof_handler(triangulation);
    joint_dof_handler.distribute_dofs(joint_fe);
    Assert(joint_dof_handler.n_dofs() ==
             ((dim + 1) * dof_handler_velocity.n_dofs() +
              dof_handler_pressure.n_dofs()),
           ExcInternalError());
    Vector<double> joint_solution(joint_dof_handler.n_dofs());
    std::vector<types::global_dof_index> loc_joint_dof_indices(
      joint_fe.n_dofs_per_cell()),
      loc_vel_dof_indices(fe_velocity.n_dofs_per_cell()),
      loc_pres_dof_indices(fe_pressure.n_dofs_per_cell());
    typename DoFHandler<dim>::active_cell_iterator
      joint_cell = joint_dof_handler.begin_active(),
      joint_endc = joint_dof_handler.end(),
      vel_cell   = dof_handler_velocity.begin_active(),
      pres_cell  = dof_handler_pressure.begin_active();
    for (; joint_cell != joint_endc; ++joint_cell, ++vel_cell, ++pres_cell)
      {
        joint_cell->get_dof_indices(loc_joint_dof_indices);
        vel_cell->get_dof_indices(loc_vel_dof_indices);
        pres_cell->get_dof_indices(loc_pres_dof_indices);
        for (unsigned int i = 0; i < joint_fe.n_dofs_per_cell(); ++i)
          switch (joint_fe.system_to_base_index(i).first.first)
            {
              case 0:
                Assert(joint_fe.system_to_base_index(i).first.second < dim,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) =
                  u_n[joint_fe.system_to_base_index(i).first.second](
                    loc_vel_dof_indices[joint_fe.system_to_base_index(i)
                                          .second]);
                break;
              case 1:
                Assert(joint_fe.system_to_base_index(i).first.second == 0,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) =
                  pres_n(loc_pres_dof_indices[joint_fe.system_to_base_index(i)
                                                .second]);
                break;
              case 2:
                Assert(joint_fe.system_to_base_index(i).first.second == 0,
                       ExcInternalError());
                joint_solution(loc_joint_dof_indices[i]) = rot_u(
                  loc_vel_dof_indices[joint_fe.system_to_base_index(i).second]);
                break;
              default:
                Assert(false, ExcInternalError());
            }
      }
    std::vector<std::string> joint_solution_names(dim, "v");
    joint_solution_names.emplace_back("p");
    joint_solution_names.emplace_back("rot_u");
    DataOut<dim> data_out;
    data_out.attach_dof_handler(joint_dof_handler);
    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      component_interpretation(
        dim + 2, DataComponentInterpretation::component_is_part_of_vector);
    component_interpretation[dim] =
      DataComponentInterpretation::component_is_scalar;
    component_interpretation[dim + 1] =
      DataComponentInterpretation::component_is_scalar;
    data_out.add_data_vector(joint_solution,
                             joint_solution_names,
                             DataOut<dim>::type_dof_data,
                             component_interpretation);
    data_out.build_patches(deg + 1);
    std::ofstream output("solution-" + Utilities::int_to_string(step, 5) +
                         ".vtk");
    data_out.write_vtk(output);
  }

// 下面是一个辅助函数，通过将 $\text{curl} u$ 项投影到用于速度分量的有限元空间来计算涡度。这个函数只有在我们生成图形输出时才会被调用，所以不是很频繁，因此我们没有像对待其他装配函数那样，麻烦地使用WorkStream概念来并行化它。不过，如果需要的话，这应该不会太复杂。此外，我们在这里的实现只适用于2D，所以如果不是这种情况，我们就放弃了。

  template <int dim>
  void NavierStokesProjection<dim>::assemble_vorticity(const bool reinit_prec)
  {
    Assert(dim == 2, ExcNotImplemented());
    if (reinit_prec)
      prec_vel_mass.initialize(vel_Mass);

    FEValues<dim>      fe_val_vel(fe_velocity,
                             quadrature_velocity,
                             update_gradients | update_JxW_values |
                               update_values);
    const unsigned int dpc = fe_velocity.n_dofs_per_cell(),
                       nqp = quadrature_velocity.size();
    std::vector<types::global_dof_index> ldi(dpc);
    Vector<double>                       loc_rot(dpc);

    std::vector<Tensor<1, dim>> grad_u1(nqp), grad_u2(nqp);
    rot_u = 0.;

    for (const auto &cell : dof_handler_velocity.active_cell_iterators())
      {
        fe_val_vel.reinit(cell);
        cell->get_dof_indices(ldi);
        fe_val_vel.get_function_gradients(u_n[0], grad_u1);
        fe_val_vel.get_function_gradients(u_n[1], grad_u2);
        loc_rot = 0.;
        for (unsigned int q = 0; q < nqp; ++q)
          for (unsigned int i = 0; i < dpc; ++i)
            loc_rot(i) += (grad_u2[q][0] - grad_u1[q][1]) * //
                          fe_val_vel.shape_value(i, q) *    //
                          fe_val_vel.JxW(q);

        for (unsigned int i = 0; i < dpc; ++i)
          rot_u(ldi[i]) += loc_rot(i);
      }

    prec_vel_mass.solve(rot_u);
  }
} // namespace Step35
// @sect3{ The main function }

// 主函数看起来和其他所有的教程程序非常相似，所以这里没有什么可评论的。

int main()
{
  try
    {
      using namespace Step35;

      RunTimeParameters::Data_Storage data;
      data.read_data("parameter-file.prm");

      deallog.depth_console(data.verbose ? 2 : 0);

      NavierStokesProjection<2> test(data);
      test.run(data.verbose, data.output_interval);
    }
  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;
    }
  std::cout << "----------------------------------------------------"
            << std::endl
            << "Apparently everything went fine!" << std::endl
            << "Don't forget to brush your teeth :-)" << std::endl
            << std::endl;
  return 0;
}


#pragma once
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <Eigen/Dense>
#include <memory>
#include <vector>
#include <iostream>
#include <cmath>

namespace py = pybind11;

namespace dummyml{

using EigenMatrix = Eigen::Matrix<
    double,
    Eigen::Dynamic,
    Eigen::Dynamic,
    Eigen::RowMajor
>;
using EigenVector = Eigen::VectorXd;

class mean_variance
{
private:
    std::vector<double> _mean;
    std::vector<double> _variance;
public:
    mean_variance() = default;
    mean_variance(size_t size): _mean(size), _variance(size){}
    inline size_t size() const {
        return _mean.size();
    }
    inline const    double& mean    (size_t index) const {
        return _mean.at(index);
    }
    inline          double& mean    (size_t index) {
        return _mean.at(index);
    }
    inline const    double& variance(size_t index) const {
        return _variance.at(index);
    }
    inline          double& variance(size_t index) {
        return _variance.at(index);
    }
    inline static double normalDistribution(double m, double v, double x, bool ignore_const = false) {
        return exp(-pow(x-m,2.0) / (ignore_const ? v : 2.0*v)) / sqrt(ignore_const ? v : v*M_PI_2);
    }
    inline static double logNormalDistribution(double m, double v, double x, bool ignore_const = false) {
        return -pow(x-m,2.0) / (ignore_const ? v : 2.0*v) - 0.5*log(ignore_const ? v : v*M_PI_2);
    }
    double* mean_data(){
        return _mean.data();
    }
    double* variance_data(){
        return _variance.data();
    }
};

class kernel{
public:
    enum type{
        NoneKernel = 0,
        LinearKernel,
        RadialBasisFunctionKernel
    } _T;
    kernel() = default;
    virtual double operator()(const double &x1, const double &x2) = 0;
    virtual double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2) = 0;
};

class linear_kernel: public kernel{
public:
    linear_kernel(): kernel(){
        _T = LinearKernel;
    }
    double operator()(const double &x1, const double &x2){
        return x1*x2;
    }
    double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2){
        return x1.dot(x2);
    }
};

class radial_basis_function_kernel: public kernel{
private:
    double gamma;
public:
    radial_basis_function_kernel(double g = 0.1): kernel(), gamma(g){
        _T = RadialBasisFunctionKernel;
    }
    void set_gamma(double g){
        gamma = g;
    }
    double operator()(const double &x1, const double &x2){
        return exp(-gamma*(x1-x2)*(x1-x2));
    }
    double operator()(
        Eigen::Ref<EigenVector> x1,
        Eigen::Ref<EigenVector> x2){
        return exp(-gamma*((x1-x2).squaredNorm()));
    }
};

std::unique_ptr<kernel> get_kernel(kernel::type);

std::vector<double> softmax(const std::vector<double>&);

template<typename To, typename From>
To dummy_cast(From ptr){
    return static_cast<To>(static_cast<void*>(ptr));
}

} // namespace dummyml

void export_utils(py::module_ &m);


#include <Eigen/SVD>

#include "beta.h"
#include "input_validation.h"

using namespace Eigen;

/*
beta = [clus_1, clus_2, ..., clus_c, loc_1, loc_2, ..., loc_r]
*/

VectorXd get_beta_extended(const VectorXd& beta) {
  size_t m = beta.size();
  VectorXd beta_ext(m + 1);
  beta_ext[m] = 0;
  for (size_t idx = 0; idx < m; idx++) {
    beta_ext[idx] = beta[idx];
  }

  return beta_ext;
}

void beta_description(const MatrixXi& x, const MatrixXi& y, const VectorXd& beta) {
  check_x_y(x, y);

  size_t clusters = y.rows();
  size_t loci = x.cols();

  for (size_t j = 0; j < clusters; j++) {
    OUTPUT << "clus_" << (j+1) << " ";
  }

  for (size_t k = 0; k < (loci - 1); k++) {
    if (k > 0) {
      OUTPUT << " ";
    }

    OUTPUT << "loc_" << (k+1);
  }
}

MatrixXd beta_to_discrete_laplace_parameters(const MatrixXi& x, const MatrixXi& y, const VectorXd& beta) {
  check_x_y(x, y);

  size_t clusters = y.rows();
  size_t loci = x.cols();

  VectorXd beta_ext = get_beta_extended(beta);
  MatrixXd pars(clusters, loci);

  for (size_t j = 0; j < clusters; j++) {
    double b_j = beta_ext[j];

    for (size_t k = 0; k < loci; k++) {
      double b_k = beta_ext[clusters + k];
      double p = std::exp(b_j + b_k);
      pars(j, k) = p;
    }
  }

  return pars;
}


#include <boost/math/distributions/hypergeometric.hpp>
#include <algorithm> // for min and max
#include <Rcpp.h>

using namespace Rcpp;
using namespace boost::math;
using namespace std;

double fisher_exact_test(double a, double b, double c, double d) {
  double N = a + b + c + d;
  double r = a + c;
  double n = c + d;
  double max_for_k = min(r, n);
  double min_for_k = (double)max(0.0, double(r + n - N));
  hypergeometric_distribution<> hgd(r, n, N);
  double cutoff = pdf(hgd, c);
  double tmp_p = 0.0;
  for(int k = min_for_k; k < max_for_k+1; k++) {
    double p = pdf(hgd, k);
    if(p <= cutoff) {
      tmp_p += p;
    }
  }
  return tmp_p;
}

// [[Rcpp::export]]
SEXP fisherExactTest(SEXP a, SEXP b, SEXP c, SEXP d) {

  Rcpp::IntegerVector a_(a), b_(b), c_(c), d_(d);
  Rcpp::NumericVector pval(a);
  int n = a_.size();

  for(int i = 0; i < n; i++) {
    pval(i) = fisher_exact_test(a_(i), b_(i), c_(i), d_(i));
  }

  return wrap(pval);
}