/*
 * Copyright (c) 2018 Jonas Deyson
 *
 * This software is released under the MIT License.
 *
 * You should have received a copy of the MIT License
 * along with this program. If not, see <https://opensource.org/licenses/MIT>
 */

#include "SceneReader.h"
#include "exprtk.hpp"
#include <QTextStream>
#include <QDebug>
#include <QFile>
#include <tuple>
#include <iostream>
#include <boost/algorithm/string.hpp>

using ExpSymbolTable = exprtk::symbol_table<float>;
using ExpExpression = exprtk::expression<float>;
using ExpParser = exprtk::parser<float>;
using ExpSymbol = ExpParser::dependent_entity_collector::symbol_t;
using ExpError = exprtk::parser_error::type;
using ExpPtr = std::unique_ptr<ExpExpression>;

//=====================================================================
//                        AUXILIARY FUNCTIONS
//=====================================================================
namespace
{

void extractInfo(const std::string infoStr, int& width, int& height, int& spp)
{
    ExpSymbolTable symbol_table;
    float w = 0.f, h = 0.f;
    float spp_f = 0.f;
    symbol_table.add_variable("width", w);
    symbol_table.add_variable("height", h);
    symbol_table.add_variable("spp", spp_f);

    ExpExpression expression;
    expression.register_symbol_table(symbol_table);

    ExpParser parser;

    if (!parser.compile(infoStr, expression))
    {
        printf("Error: %s\tExpression: %s\n",
               parser.error().c_str(),
               infoStr.c_str());

        for (std::size_t i = 0; i < parser.error_count(); ++i)
        {
            ExpError error = parser.get_error(i);
            printf("Error: %02d Position: %02d Type: [%s] Msg: %s Expr: %s\n",
                   static_cast<int>(i),
                   static_cast<int>(error.token.position),
                   exprtk::parser_error::to_str(error.mode).c_str(),
                   error.diagnostic.c_str(),
                   infoStr.c_str());
        }

        fflush(stdout);
    }

    expression.value();
    width = std::round(w);
    height = std::round(h);
    spp = std::round(spp_f);
}

std::tuple< std::unique_ptr<ExpExpression>,
            std::vector<RandomParameter>,
            std::vector<Feature> >
compileExpression(const std::string expStr, int width, int height, int spp, float* params, float* features)
{
    ExpSymbolTable symbol_table;
    symbol_table.add_constants();

    symbol_table.add_constant("width", width);
    symbol_table.add_constant("height", height);
    symbol_table.add_constant("spp", spp);

    symbol_table.add_variable("IMAGE_X", params[0]);
    symbol_table.add_variable("IMAGE_Y", params[1]);
    symbol_table.add_variable("LENS_U", params[2]);
    symbol_table.add_variable("LENS_V", params[3]);
    symbol_table.add_variable("TIME", params[4]);
    symbol_table.add_variable("LIGHT_X", params[5]);
    symbol_table.add_variable("LIGHT_Y", params[6]);

    symbol_table.add_variable("COLOR_R", features[COLOR_R]);
    symbol_table.add_variable("COLOR_G", features[COLOR_G]);
    symbol_table.add_variable("COLOR_B", features[COLOR_B]);
    symbol_table.add_variable("DEPTH", features[DEPTH]);
    symbol_table.add_variable("DIRECT_LIGHT_R", features[DIRECT_LIGHT_R]);
    symbol_table.add_variable("DIRECT_LIGHT_G", features[DIRECT_LIGHT_G]);
    symbol_table.add_variable("DIRECT_LIGHT_B", features[DIRECT_LIGHT_B]);
    symbol_table.add_variable("WORLD_X", features[WORLD_X]);
    symbol_table.add_variable("WORLD_Y", features[WORLD_Y]);
    symbol_table.add_variable("WORLD_Z", features[WORLD_Z]);
    symbol_table.add_variable("NORMAL_X", features[NORMAL_X]);
    symbol_table.add_variable("NORMAL_Y", features[NORMAL_Y]);
    symbol_table.add_variable("NORMAL_Z", features[NORMAL_Z]);
    symbol_table.add_variable("TEXTURE_COLOR_R", features[TEXTURE_COLOR_R]);
    symbol_table.add_variable("TEXTURE_COLOR_G", features[TEXTURE_COLOR_G]);
    symbol_table.add_variable("TEXTURE_COLOR_B", features[TEXTURE_COLOR_B]);
    symbol_table.add_variable("WORLD_X_1", features[WORLD_X_1]);
    symbol_table.add_variable("WORLD_Y_1", features[WORLD_Y_1]);
    symbol_table.add_variable("WORLD_Z_1", features[WORLD_Z_1]);
    symbol_table.add_variable("NORMAL_X_1", features[NORMAL_X_1]);
    symbol_table.add_variable("NORMAL_Y_1", features[NORMAL_Y_1]);
    symbol_table.add_variable("NORMAL_Z_1", features[NORMAL_Z_1]);
    symbol_table.add_variable("TEXTURE_COLOR_R_1", features[TEXTURE_COLOR_R_1]);
    symbol_table.add_variable("TEXTURE_COLOR_G_1", features[TEXTURE_COLOR_G_1]);
    symbol_table.add_variable("TEXTURE_COLOR_B_1", features[TEXTURE_COLOR_B_1]);
    symbol_table.add_variable("WORLD_X_NS", features[WORLD_X_NS]);
    symbol_table.add_variable("WORLD_Y_NS", features[WORLD_Y_NS]);
    symbol_table.add_variable("WORLD_Z_NS", features[WORLD_Z_NS]);
    symbol_table.add_variable("NORMAL_X_NS", features[NORMAL_X_NS]);
    symbol_table.add_variable("NORMAL_Y_NS", features[NORMAL_Y_NS]);
    symbol_table.add_variable("NORMAL_Z_NS", features[NORMAL_Z_NS]);
    symbol_table.add_variable("TEXTURE_COLOR_R_NS", features[TEXTURE_COLOR_R_NS]);
    symbol_table.add_variable("TEXTURE_COLOR_G_NS", features[TEXTURE_COLOR_G_NS]);
    symbol_table.add_variable("TEXTURE_COLOR_B_NS", features[TEXTURE_COLOR_B_NS]);

    std::unique_ptr<ExpExpression> expression(new ExpExpression);
    expression->register_symbol_table(symbol_table);

    ExpParser parser;
    parser.dec().collect_variables() = true;
    parser.dec().collect_assignments() = true;

    if (!parser.compile(expStr, *expression))
    {
        printf("Error: %s\tExpression: %s\n",
               parser.error().c_str(),
               expStr.c_str());

        for (std::size_t i = 0; i < parser.error_count(); ++i)
        {
            ExpError error = parser.get_error(i);
            printf("Error: %02d Position: %02d Type: [%s] Msg: %s Expr: %s\n",
                   static_cast<int>(i),
                   static_cast<int>(error.token.position),
                   exprtk::parser_error::to_str(error.mode).c_str(),
                   error.diagnostic.c_str(),
                   expStr.c_str());
        }

        fflush(stdout);
    }

    // Find list of used variables
    std::deque<ExpSymbol> symbol_list;
    parser.dec().symbols(symbol_list);
    auto newEnd = std::remove_if(symbol_list.begin(), symbol_list.end(),
        [](ExpSymbol& symbol)
        { return symbol.second != ExpParser::e_st_variable; }
    );
    symbol_list.erase(newEnd, symbol_list.end());
    // Create list of used random parameters from the list of used variables
    std::vector<RandomParameter> usedParameters;
    for (std::size_t i = 0; i < symbol_list.size(); ++i)
    {
        const ExpSymbol& symbol = symbol_list[i];
        RandomParameter p;
        if(stringToRandomParameter(boost::to_upper_copy(symbol.first), &p))
            usedParameters.push_back(p);
    }

    // Find list of variables assigned to
    symbol_list = std::deque<ExpSymbol>();
    parser.dec().assignment_symbols(symbol_list);
    // Create list of used features from the list of variables assigned to
    std::vector<Feature> usedFeatures;
    for (std::size_t i = 0; i < symbol_list.size(); ++i)
    {
        const ExpSymbol& symbol = symbol_list[i];
        Feature f;
        if(stringToFeature(boost::to_upper_copy(symbol.first), &f))
            usedFeatures.push_back(f);
    }

    return std::make_tuple(std::move(expression), usedParameters, usedFeatures);
}

}

//=====================================================================
//                            SCENE
//=====================================================================
Scene::Scene(int w, int h, int spp, ExpPtr exp, const std::vector<RandomParameter>& parameters, const std::vector<Feature>& features):
    width(w),
    height(h),
    spp(spp),
    randomParameters(parameters),
    features(features),
    exp(std::move(exp))
{}

void Scene::evaluate()
{
    exp->value();
}

Scene::~Scene() = default;

//=====================================================================
//                            READ SCENE
//=====================================================================
std::unique_ptr<Scene> readScene(const QString& filename, float* params, float* features)
{
    QString info;
    QString exp;

    QString all;
    QFile file(filename);
    if(file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        QTextStream stream(&file);
        all = stream.readAll();

        QStringList sections = all.split("#");
        if(sections.size() != 2)
        {
            qDebug() << "Scene file should contain two sections separated by a #.";
            return nullptr;
        }

        info = sections.at(0);
        exp = sections.at(1);
    }
    else
    {
        qDebug() << "Couldn't open file " << filename;
        qDebug() << file.errorString();
        return nullptr;
    }

    int w = 0, h = 0, spp = 0;
    extractInfo(info.toStdString(), w, h, spp);

    ExpPtr expPtr;
    std::vector<RandomParameter> usedParameters;
    std::vector<Feature> usedFeatures;
    std::tie(expPtr, usedParameters, usedFeatures) = compileExpression(exp.toStdString(), w, h, spp, params, features);
    std::unique_ptr<Scene> scene(new Scene(w, h, spp, std::move(expPtr), usedParameters, usedFeatures));

    return scene;
}


#include "sbs/physics/collision/intersections.h"

#include <Eigen/Geometry>

namespace sbs {
namespace physics {
namespace collision {

line_segment_t::line_segment_t(point_t const& p, point_t const& q) : p(p), q(q) {}

triangle_t::triangle_t(point_t const& a, point_t const& b, point_t const& c) : a(a), b(b), c(c) {}

normal_t triangle_t::normal() const
{
    Eigen::Vector3d const ab = b - a;
    Eigen::Vector3d const ac = c - a;
    return ab.cross(ac).normalized();
}

ray_t::ray_t(point_t const& p, direction_t const& v, double t) : p(p), v(v), t(t) {}

std::optional<point_t> intersect(line_segment_t const& segment, triangle_t const& triangle)
{
    Eigen::Vector3d const ab = triangle.b - triangle.a;
    Eigen::Vector3d const ac = triangle.c - triangle.a;
    Eigen::Vector3d const qp = segment.p - segment.q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = segment.p - triangle.a;
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};
    if (t > d)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a + v * triangle.b + w * triangle.c;
    return intersection;
}

std::optional<point_t> intersect_twoway(line_segment_t const& segment, triangle_t const& triangle)
{
    auto const intersection = intersect(segment, triangle);
    if (intersection.has_value())
        return intersection;

    line_segment_t const flipped_segment{segment.q, segment.p};
    return intersect(flipped_segment, triangle);
}

} // namespace collision
} // namespace physics
} // namespace sbs


#ifndef Area_hpp
#define Area_hpp

#include <shapes/Circle.hpp>
#include <shapes/Rectangle.hpp>

#include <boost/math/constants/constants.hpp>
namespace bmc = boost::math::constants;

float
area( Rectangle const & r )
{
  return r.width * r.height;
}

float
area( Circle const & c )
{
  return bmc::pi<float>() * c.radius * c.radius;
}

#endif // Area_hpp


#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <string>
#include <sstream>
#include <random>
#include "ProbDistributions.h"
#include "DataSet.h"
#include "Cluster.h"
using namespace std;

int dim = 0;

int pickCandidateCluster(vector<int> *bincount,double d = 0.0)
{
	vector<double> prob_list;
	const double alpha = 0.8;
	int n = accumulate(bincount->begin(),bincount->end(),0.0);
	int n_k = 0;
	int i = 0;
	for(auto bin : *bincount){
		n_k += bin;
		prob_list.push_back(((double)n_k - d*(i+1))/(n + alpha));
	}

	double r = pd.uniformRand(0.0,1.0);
	int candidate_cluster = 0;
	for(auto p : prob_list){
		if(r < p)
			break;
		candidate_cluster++;
	}

	return candidate_cluster;
}

double densityMultiNormal(Data &d, Cluster &c)
{
	Eigen::MatrixXd info = c.cov.inverse();
	Eigen::VectorXd mu = c.mean;//[0],c.mean[1]);
	Eigen::VectorXd diff = d.normalized_data - mu;

	double det = c.cov.determinant();
	double a = 1.0/ (pow(2 * 3.151592,c.dimension*0.5) *  sqrt(det));
	double exp_part = -0.5 * diff.transpose() * info * diff;

	return a*exp(exp_part);
}

bool resampling(DataSet *ds, Clusters *cs, Data *d, vector<int> &bin)
{
	int org_c_num = (int)cs->c.size();
	bin[d->cluster_id]--;
	int candidate_cluster = pickCandidateCluster(&bin);
	bin[d->cluster_id]++;

	if(candidate_cluster == d->cluster_id){
		return false;
	}

	Cluster *old_cluster = &(cs->c[d->cluster_id]);
	double eval_new = 0.0;
	double eval_old = densityMultiNormal(*d,*old_cluster);
	Cluster c(dim);
	if(candidate_cluster == org_c_num){//新しいクラスタ
		eval_new = densityMultiNormal(*d,c);
	}else{//既存のクラスタ
		eval_new = densityMultiNormal(*d,cs->c[candidate_cluster]);
	}

	double acceptance = eval_new/eval_old;
	if(pd.uniformRand(0.0,1.0) >= acceptance)
		return false;

	bin[d->cluster_id]--;
	d->cluster_id = candidate_cluster;
	if(candidate_cluster == org_c_num){
		cs->c.push_back(c);
		bin.push_back(1);
	}else
		bin[candidate_cluster]++;
	return true;
}

void sweep(DataSet *ds,Clusters *cs)
{
	vector<int> bincount(cs->c.size(),0);
	for(auto x : ds->x){
		bincount[x.cluster_id]++;
	}

	int chance = 3;
	for(auto &target : ds->x){
		for(int i=0;i<chance;i++){
			resampling(ds,cs,&target,bincount);
		}
	}
	//どのクラスタに標本が幾つかる数える
	for(auto &c : cs->c){
		c.clear();
	}
	for(auto &d : ds->x){
		cs->c[d.cluster_id].regData(&d);
	}
	cs->calcParams();
	cerr << "----" << endl;
}

int main(int argc, char const* argv[])
{
	Clusters cs;
	DataSet ds;
	dim = ds.read();
	if(dim <= 0)
		exit(1);

	int sweep_num = 50;

	//最初のクラスタを作る。平均値は1軸ごとにガウス分布からサンプリング
	cs.c.push_back(Cluster(dim));
	for(auto &d : ds.x){
		cs.c[d.cluster_id].regData(&d);
	}
	cs.calcParams();
	cerr << "----" << endl;

	for(int k=0;k<sweep_num;k++){
		cerr << "sweep " << k << endl;
		sweep(&ds,&cs);
	}
	ds.print();

	exit(0);
}


//
// Created by yue on 29.11.19.
//

#include <iostream>
#include <time.h>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

using namespace std;
using namespace cv;
using namespace Eigen;

int main()
{
    Mat myImage = imread("../../images/piece01_1200x900.jpg");
    Mat myImageGrey;
    cvtColor(myImage, myImageGrey, CV_BGR2GRAY);
    Mat dst_blur, dst_blur_median, dst_filter2d, dst_laplacian, dst_sobel, dst_diffusion;
    blur(myImageGrey, dst_blur, Size(5, 5));
    medianBlur(myImageGrey, dst_blur_median, 5);
    Laplacian(myImageGrey, dst_laplacian, -1, 3);
    Sobel(myImageGrey, dst_sobel, -1, 1, 0, 3);
    ximgproc::anisotropicDiffusion(myImage, dst_diffusion, 0.15, 100, 10);

    Matrix<char, 3, 3> m;
    m << 0, 1, 0, 1, -4, 1, 0, 1, 0; // Laplacian filter
    m << -1, -1, -1, -1, 9, -1, -1, -1, -1; // Laplacian filter
    m << -1, -2, -1, 0, 0, 0, 1, 2, 1; // Sobel filter x_dir
//    m.fill(1.0/9);
    Mat kernel;
    eigen2cv(m, kernel);
    cout << typeToString(kernel.type());
    filter2D(myImageGrey, dst_filter2d, -1, kernel);

    imshow("myImageGrey", myImageGrey);
//    imshow("dst_blur", dst_blur);
//    imshow("dst_blur_median", dst_blur_median);
    imshow("dst_laplacian", dst_laplacian);
    imshow("dst_sobel", dst_sobel);
    imshow("dst_filter2d", dst_filter2d);
    imshow("dst_diffusion", dst_diffusion);
    waitKey(0);
    return 0;
}


#include <armadillo>
#include <json/json.h>

#include "ema/Ema.h"

#include "settings.h"

int main(int argc, char* argv[]) {

  Settings settings(argc, argv);

  Ema ema;
  ema.read().from(settings.input);

  const EmaInfo& info = ema.info();

  arma::vec mean = arma::zeros(3);
  const int pointAmount = settings.coils.size() * ema.info().sample_amount();

  for(const std::string& currentCoil: settings.coils) {

    for(int i = 0; i < info.sample_amount(); ++i) {

      mean += ema.coil(currentCoil).access().position(i);

    }

  }

  mean /= pointAmount;

  Json::Value root(Json::objectValue);

  root["x"] = mean(0);
  root["y"] = mean(1);
  root["z"] = mean(2);

  std::ofstream outFile(settings.output);

  outFile << root << std::endl;

  outFile.close();

  return 0;

}


#include <math.h>
#include <vector>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> MapM3Xd;
typedef Eigen::Ref<Matrix3Xd> RefM3Xd;

// Pass the number of points in point cloud and address of the 0th point coords
static double_t pointCloudVolume( std::size_t N, double_t* p ){

    // We will extract 3*N doubles representing particle positions
    MapM3Xd Xt( p, 3, N );
    Matrix3Xd X0(3,N);

    // Project the points to a unit sphere
    X0 = Xt.colwise().normalized();

    // Rotate all points of the shell so that the 0th point is along z-axis
    Vector3d c = X0.col(0);
    double_t cos_t = c(2);
    double_t sin_t = std::sin( std::acos( cos_t ) );
    Vector3d axis;
    axis << c(1), -c(0), 0.;
    axis.normalize();
    Matrix3d rotMat, axis_cross, outer;
    axis_cross << 0. , -axis(2), axis(1),
               axis(2), 0., -axis(0),
               -axis(1), axis(0), 0.;
    outer.noalias() = axis*axis.transpose();
    rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1 - cos_t)*outer;
    Matrix3Xd rPts(3,N);
    rPts = rotMat*X0;

    // Calculate the stereographic projections
    Vector3d p0;
    p0 << 0,0,-1.0; // Point on the plane of projection
    c = rPts.col(0); // The point from which we are projecting

    MapM3Xd l0( &(rPts(0,1)), 3, N-1 );
    Matrix3Xd l(3,N-1), proj(3,N-1);
    l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
    for( std::size_t j=0; j < N-1; ++j ){
        proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
    }

    // Insert the projected points in a CGAL vertex_with_info vector
    std::vector< std::pair< Point, unsigned> > verts;
    for( std::size_t j=0; j < N-1; ++j ){
        verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
    }

    // Triangulate
    Delaunay dt( verts.begin(), verts.end() );

    // Iterate over the triangles to calculate volume
    double_t volume = 0.0;
    for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
            ++ffi){
        auto i = ffi->vertex(0)->info();
        auto j = ffi->vertex(2)->info();
        auto k = ffi->vertex(1)->info();
        volume += 0.166666667*(Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
    }

    // Iterate over infinite faces
    Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
    if (fc != 0) {
        do{
            auto i = dt.is_infinite(fc->vertex(0))?0:fc->vertex(0)->info();
            auto j = dt.is_infinite(fc->vertex(2))?0:fc->vertex(2)->info();
            auto k = dt.is_infinite(fc->vertex(1))?0:fc->vertex(1)->info();
            volume += 0.166666667*
                (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
        }while(++fc != done);
    }

    return volume;
}


#include <cmath>
#include <string>
#include <stdexcept>
#include <boost/math/special_functions/bessel.hpp>
#include "spida/grid/besselR.h"
#include <iostream>

namespace spida{

BesselRootGridR::BesselRootGridR(int nr,double maxr) : GridR(nr,maxr),
    m_r(nr), m_sr(nr)
{
    //OutputIterator cyl_bessel_j_zero(
    //                 T v,                       // Floating-point value for Jv.
    //                 int start_index,           // 1-based index of first zero.
    //                 unsigned number_of_roots,  // How many roots to generate.
    //                 OutputIterator out_it);

    // Want J0 -> v = 0, starting with first root -> start_index=1
    boost::math::cyl_bessel_j_zero<double>(0.0,1,nr,std::back_inserter(m_roots));
    // 1-based index of zero (use nr+1 for m_jN rather than nr)
    m_jN = boost::math::cyl_bessel_j_zero<double>(0.0,nr+1);
    // Set physical grid
    for(auto i = 0; i < nr; i++)
        m_r[i] = m_roots[i]*GridR::getMaxR()/m_jN;
    // Set spectral grid
    for(auto i = 0; i < nr; i++)
        m_sr[i] = m_roots[i]*getMaxSR()/m_jN;
}

}


#pragma once
#include "kissfft.hh"
#include <boost/math/constants/constants.hpp>
#include <boost/optional.hpp>
#include <cstddef>

namespace vv
{

	template <class T, class U>
	auto lerp(T x0, T x1, U ratio)
	{
		return x0 + (x1 - x0) * ratio;
	}

	template <class T>
	auto invlerp(T x0, T x1, T x)
	{
		return (x - x0) / (x1 - x0);
	}

	template <class T>
	auto squared(T x)
	{
		return x * x;
	}

	class processor
	{
	public:

		static const std::size_t buffer_size = 4096;
		static const std::size_t nsdf_size = buffer_size / 2;

		explicit processor(double sampleRate)
			: sampleRate_(sampleRate)
			, v1_(buffer_size)
			, v2_(buffer_size + nsdf_size)
			, v3_(buffer_size + nsdf_size)
			, v4_(buffer_size + nsdf_size)
			, v5_(buffer_size + nsdf_size)
			, v6_(buffer_size)
			, v7_(buffer_size / 2)
		{
		}

		void operator ()(const float* input, float* output, double pitch_shift, double formant_shift)
		{
			for (std::size_t i = 0; i < buffer_size; ++i)
				v1_[i] = std::complex<float>(input[i], 0.0f);

			for (std::size_t i = 0; i < buffer_size; ++i)
			{
				auto r = static_cast<double>(i) / static_cast<double>(buffer_size);
				auto w = 0.5 - 0.5 * std::cos(boost::math::constants::two_pi<double>() * r);
				v2_[i] = v1_[i] * static_cast<float>(w);
			}

			fft_.transform(v2_.data(), v3_.data());

			auto cutoff_hz = 800.0;
			auto cutoff_index = static_cast<std::size_t>(std::round(cutoff_hz * static_cast<double>(buffer_size) / sampleRate_));

			for (std::size_t i = 0; i < cutoff_index; ++i)
			{
				v4_[i + 1] = std::norm(v3_[i + 1]);
				v4_[buffer_size - i - 1] = std::norm(v3_[buffer_size - i - 1]);
			}

			ifft_.transform(v4_.data(), v5_.data());

			for (std::size_t i = 0; i < buffer_size + nsdf_size; ++i)
				v5_[i] /= static_cast<float>(buffer_size + nsdf_size);

			for (std::size_t i = 1; i < buffer_size; ++i)
			{
				auto j = buffer_size - i - 1;
				v6_[j] = v6_[j + 1] + squared(v2_[i].real()) + squared(v2_[j].real());
			}

			for (std::size_t i = 0; i < buffer_size / 2; ++i)
			{
				if (v6_[i] < std::numeric_limits<double>::min())
					v7_[i] = 0.0f;
				else
					v7_[i] = 2.0f * v5_[i].real() / v6_[i];
			}

			auto minimum_hz = 50.0;
			auto maximum_hz = 300.0;

			auto minimum_index = static_cast<std::size_t>(std::round(sampleRate_ / maximum_hz));
			auto maximum_index = static_cast<std::size_t>(std::round(sampleRate_ / minimum_hz));

			minimum_index = std::max<std::size_t>(minimum_index, 1);
			minimum_index = std::min<std::size_t>(minimum_index, buffer_size / 2 - 2);

			maximum_index = std::max<std::size_t>(maximum_index, 1);
			maximum_index = std::min<std::size_t>(maximum_index, buffer_size / 2 - 2);

			double maximum_value = 0.0;

			for (std::size_t i = minimum_index; i < maximum_index; ++i)
			{
				auto p1 = v7_[i - 1];
				auto p2 = v7_[i];
				auto p3 = v7_[i + 1];

				if (p1 < p2 && p2 > p3 && p2 > maximum_value)
					maximum_value = p2;
			}

			boost::optional<std::size_t> peak_index;
			double peak_value = 0.0;

			for (std::size_t i = minimum_index; i < maximum_index; ++i)
			{
				auto p1 = v7_[i - 1];
				auto p2 = v7_[i];
				auto p3 = v7_[i + 1];

				if (p1 < p2 && p2 > p3 && p2 > maximum_value * 0.9)
				{
					peak_index = i;
					peak_value = p2;
					break;
				}
			}

			if (!peak_index && last_peak_index_)
				peak_index = last_peak_index_;

			last_peak_index_ = peak_index;

			bool enable = false;

			if (peak_index)
			{
				std::size_t last_dst = 0;
				std::size_t last_src1 = 0;
				std::size_t last_src2 = 0;
				double last_src_ratio = 0.0;

				auto easing = [&](double x)
				{
					return (1.0 - std::cos(boost::math::constants::pi<double>() * x)) / 2.0;
				};

				auto interpolate = [&](double x1, double x2, double ratio)
				{
					return lerp(x1, x2, easing(ratio));
				};

				auto get_value = [&](double indexf) -> double
				{
					if (indexf < 0.0)
						return input[0];

					auto index = static_cast<std::size_t>(std::floor(indexf));
					if (index >= buffer_size - 1)
						return input[buffer_size - 1];

					auto ratio = indexf - std::floor(indexf);

					return interpolate(input[index], input[index + 1], ratio);
				};

				auto overlap = [&](std::size_t dst, std::size_t src1, std::size_t src2, double src_ratio)
				{
					for (std::size_t i = last_dst; i < dst; ++i)
					{
						auto ratio = static_cast<double>(i - last_dst) / static_cast<double>(dst - last_dst);

						auto p1_1 = get_value(static_cast<double>(last_src1) + static_cast<double>(i - last_dst) * formant_shift);
						auto p1_2 = get_value(static_cast<double>(last_src2) + static_cast<double>(i - last_dst) * formant_shift);
						auto p1 = interpolate(p1_1, p1_2, last_src_ratio);

						auto p2_1 = get_value(static_cast<double>(src1) - static_cast<double>(dst - i) * formant_shift);
						auto p2_2 = get_value(static_cast<double>(src2) - static_cast<double>(dst - i) * formant_shift);
						auto p2 = interpolate(p2_1, p2_2, src_ratio);

						output[i] = static_cast<float>(interpolate(p1, p2, ratio));
					}

					last_dst = dst;
					last_src1 = src1;
					last_src2 = src2;
					last_src_ratio = src_ratio;
				};

				auto q = buffer_size / *peak_index;
				auto r = buffer_size % *peak_index;

				auto nf = (static_cast<double>(buffer_size) * pitch_shift - static_cast<double>(r)) / static_cast<double>(*peak_index);
				auto n = static_cast<std::size_t>(std::max(0.0, std::round(nf)));

				if (q != 0 && n != 0)
				{
					auto actual_pitch_shift = static_cast<double>(n * *peak_index + r) / static_cast<double>(buffer_size);

					for (std::size_t i = 1; i <= n; ++i)
					{
						double frame_indexf = 1.0;

						if (n != 1)
							frame_indexf = static_cast<double>((i - 1) * (q - 1)) / static_cast<double>(n - 1) + 1;

						auto frame_index = static_cast<std::size_t>(std::floor(frame_indexf));

						auto dst = static_cast<std::size_t>(std::floor(static_cast<double>(i * *peak_index) / actual_pitch_shift));
						auto src = frame_index * *peak_index;

						if (frame_index == q)
						{
							overlap(dst, src, src, 0.0);
						}
						else
						{
							auto src_ratio = frame_indexf - std::floor(frame_indexf);
							overlap(dst, src, src + *peak_index, src_ratio);
						}
					}

					overlap(buffer_size, buffer_size, buffer_size, 0.0);

					enable = true;
				}
			}

			if (!enable)
				std::copy(input, input + buffer_size, output);
		}

	private:

		double sampleRate_;

		kissfft<float> fft_{ buffer_size + nsdf_size, false };
		kissfft<float> ifft_{ buffer_size + nsdf_size, true };

		std::vector<std::complex<float>> v1_;
		std::vector<std::complex<float>> v2_;
		std::vector<std::complex<float>> v3_;
		std::vector<std::complex<float>> v4_;
		std::vector<std::complex<float>> v5_;
		std::vector<float> v6_;
		std::vector<float> v7_;

		boost::optional<std::size_t> last_peak_index_;

	};

}


// test_inverse_gamma.cpp

// Copyright Paul A. Bristow 2010.
// Copyright John Maddock 2010.

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

#ifdef _MSC_VER
#  pragma warning (disable : 4224) // nonstandard extension used : formal parameter 'type' was previously defined as a type
// in Boost.test and lexical_cast
#  pragma warning (disable : 4310) // cast truncates constant value
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept
using ::boost::math::concepts::real_concept;

//#include <boost/math/tools/test.hpp>
#include <boost/test/test_exec_monitor.hpp> // for test_main
#include <boost/test/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE_FRACTION
#include "test_out_of_range.hpp"

#include <boost/math/distributions/inverse_gamma.hpp> // for inverse_gamma_distribution
using boost::math::inverse_gamma_distribution;
using  ::boost::math::inverse_gamma;
//  using  ::boost::math::cdf;
//  using  ::boost::math::pdf;

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma; // for naive pdf.

#include <iostream>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits;

template <class RealType>
RealType naive_pdf(RealType shape, RealType scale, RealType x)
{ // Formula from Wikipedia
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType result = (pow(scale, shape) * pow(x, (-shape -1)) * exp(-scale/x) ) / tgamma(shape);
   return result;
}

// Test using a spot value from some other reference source,
// in this case test values from output from R provided by Thomas Mang.

template <class RealType>
void test_spot(
     RealType shape, // shape,
     RealType scale, // scale,
     RealType x, // random variate x,
     RealType pd, // expected pdf,
     RealType P, // expected CDF,
     RealType Q, // expected complement of CDF,
     RealType tol) // test tolerance.
{
   boost::math::inverse_gamma_distribution<RealType> dist(shape, scale);

   BOOST_CHECK_CLOSE_FRACTION
      ( // Compare to expected PDF.
      pdf(dist, x), // calculated.
      pd, // expected
      tol);

    BOOST_CHECK_CLOSE_FRACTION( // Compare to naive formula (might be less accurate).
      pdf(dist, x), naive_pdf(dist.shape(), dist.scale(), x), tol);

   BOOST_CHECK_CLOSE_FRACTION( // Compare to expected CDF.
      cdf(dist, x), P, tol);

   if((P < 0.999) && (Q < 0.999))
   {  // We can only check this if P is not too close to 1,
      // so that we can guarantee Q is accurate:
      BOOST_CHECK_CLOSE_FRACTION(
        cdf(complement(dist, x)), Q, tol);
      BOOST_CHECK_CLOSE_FRACTION(
        quantile(dist, P), x, tol); // quantile(pdf) = x
      BOOST_CHECK_CLOSE_FRACTION(
        quantile(complement(dist, Q)), x, tol);
   }
} // test_spot

// Test using a spot value from some other reference source.

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
  // Basic sanity checks, test data is to six decimal places only
  // so set tolerance to 0.000001 expressed as a percentage = 0.0001%.

  RealType tolerance = 0.000001f; // as fraction.
  cout << "Tolerance = " << tolerance * 100 << "%." << endl;

// This test values from output from R provided by Thomas Mang.
  test_spot(static_cast<RealType>(2), static_cast<RealType>(1), // shape, scale
  static_cast<RealType>(2.L), // x
  static_cast<RealType>(0.075816332464079136L), // pdf
  static_cast<RealType>(0.90979598956895047L), // cdf
  static_cast<RealType>(1 - 0.90979598956895047L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(1.593), static_cast<RealType>( 0.5), // shape, scale
  static_cast<RealType>( 0.5), // x
  static_cast<RealType>(0.82415241749687074L), // pdf
  static_cast<RealType>(0.60648042700409865L), // cdf
  static_cast<RealType>(1 - 0.60648042700409865L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(13.319), static_cast<RealType>(0.5), // shape, scale
  static_cast<RealType>(0.5), // x
  static_cast<RealType>(0.00000000068343206235379223), // pdf
  static_cast<RealType>(0.99999999997242739L), // cdf
  static_cast<RealType>(1 - 0.99999999997242739L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(1.593), static_cast<RealType>(1), // shape, scale
  static_cast<RealType>(1.977), // x
  static_cast<RealType>(0.11535946773398653L), // pdf
  static_cast<RealType>(0.82449794420341549L), // cdf
  static_cast<RealType>(1 - 0.82449794420341549L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(6.666), static_cast<RealType>(1.411), // shape, scale
  static_cast<RealType>(5), // x
  static_cast<RealType>(0.000000084415758206386872), // pdf
  static_cast<RealType>(0.99999993427280998L), // cdf
  static_cast<RealType>(1 - 0.99999993427280998L), // cdf complement
  tolerance  // tol
  );

  // Check some bad parameters to the distribution,
  BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> igbad1(-1, 0), std::domain_error); // negative shape.
  BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> igbad2(0, -1), std::domain_error); // negative scale.
  BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> igbad2(-1, -1), std::domain_error); // negative scale and shape.

  inverse_gamma_distribution<RealType> ig21(2, 1);

  if(std::numeric_limits<RealType>::has_infinity)
  {
    BOOST_CHECK_THROW(pdf(ig21, +std::numeric_limits<RealType>::infinity()), std::domain_error); // x = + infinity, pdf = 0
    BOOST_CHECK_THROW(pdf(ig21, -std::numeric_limits<RealType>::infinity()),  std::domain_error); // x = - infinity, pdf = 0
    BOOST_CHECK_THROW(cdf(ig21, +std::numeric_limits<RealType>::infinity()),std::domain_error ); // x = + infinity, cdf = 1
    BOOST_CHECK_THROW(cdf(ig21, -std::numeric_limits<RealType>::infinity()), std::domain_error); // x = - infinity, cdf = 0
    BOOST_CHECK_THROW(cdf(complement(ig21, +std::numeric_limits<RealType>::infinity())), std::domain_error); // x = + infinity, c cdf = 0
    BOOST_CHECK_THROW(cdf(complement(ig21, -std::numeric_limits<RealType>::infinity())), std::domain_error); // x = - infinity, c cdf = 1
    BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> nbad1(std::numeric_limits<RealType>::infinity(), static_cast<RealType>(1)), std::domain_error); // +infinite mean
    BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> nbad1(-std::numeric_limits<RealType>::infinity(),  static_cast<RealType>(1)), std::domain_error); // -infinite mean
    BOOST_CHECK_THROW(boost::math::inverse_gamma_distribution<RealType> nbad1(static_cast<RealType>(0), std::numeric_limits<RealType>::infinity()), std::domain_error); // infinite sd
  }

  if (std::numeric_limits<RealType>::has_quiet_NaN)
  {
    // No longer allow x to be NaN, then these tests should throw.
    BOOST_CHECK_THROW(pdf(ig21, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
    BOOST_CHECK_THROW(cdf(ig21, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
    BOOST_CHECK_THROW(cdf(complement(ig21, +std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // x = + infinity
    BOOST_CHECK_THROW(quantile(ig21, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // p = + infinity
    BOOST_CHECK_THROW(quantile(complement(ig21, +std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // p = + infinity
  }
    // Spot check for pdf using 'naive pdf' function
  for(RealType x = 0.5; x < 5; x += 0.5)
  {
    BOOST_CHECK_CLOSE_FRACTION(
      pdf(inverse_gamma_distribution<RealType>(5, 6), x),
      naive_pdf(RealType(5), RealType(6), x),
      tolerance);
  }   // Spot checks for parameters:

  RealType tol_few_eps = boost::math::tools::epsilon<RealType>() * 5; // 5 eps as a fraction.
  inverse_gamma_distribution<RealType> dist51(5, 1);
  inverse_gamma_distribution<RealType> dist52(5, 2);
  inverse_gamma_distribution<RealType> dist31(3, 1);
  inverse_gamma_distribution<RealType> dist111(11, 1);
  // 11 mean 0.10000000000000001, variance  0.0011111111111111111, sd 0.033333333333333333

  RealType x = static_cast<RealType>(0.125);
  using namespace std; // ADL of std names.
  using namespace boost::math;

  //  mean, variance etc
  BOOST_CHECK_CLOSE_FRACTION(mean(dist52), static_cast<RealType>(0.5), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(mean(dist111), static_cast<RealType>(0.1L), tol_few_eps);
  inverse_gamma_distribution<RealType> igamma41(static_cast<RealType>(4.), static_cast<RealType>(1.) );
  BOOST_CHECK_CLOSE_FRACTION(mean(igamma41), static_cast<RealType>(0.3333333333333333333333333333333333333333333333333333333L), tol_few_eps);
  // variance:
  BOOST_CHECK_CLOSE_FRACTION(variance(dist51), static_cast<RealType>(0.0208333333333333333333333333333333333333333333333333L), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(dist31), static_cast<RealType>(0.25), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(dist111), static_cast<RealType>(0.001111111111111111111111111111111111111111111111111L), tol_few_eps);
  // std deviation:
  BOOST_CHECK_CLOSE_FRACTION(standard_deviation(dist31), static_cast<RealType>(0.5), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(standard_deviation(dist111), static_cast<RealType>(0.0333333333333333333333333333333333333333333333333L), tol_few_eps);
  // hazard:
  BOOST_CHECK_CLOSE_FRACTION(hazard(dist51, x), pdf(dist51, x) / cdf(complement(dist51, x)), tol_few_eps);
 //  cumulative hazard:
  BOOST_CHECK_CLOSE_FRACTION(chf(dist51, x), -log(cdf(complement(dist51, x))), tol_few_eps);
  // coefficient_of_variation:
  BOOST_CHECK_CLOSE_FRACTION(coefficient_of_variation(dist51), standard_deviation(dist51) / mean(dist51), tol_few_eps);
  // mode:
  BOOST_CHECK_CLOSE_FRACTION(mode(dist51), static_cast<RealType>(0.166666666666666666666666666666666666666666666666666L), tol_few_eps);
  // median
  //BOOST_CHECK_CLOSE_FRACTION(median(dist52), static_cast<RealType>(0), tol_few_eps);
  // Useful to have an exact median?  Failing that use a loop back test.
   BOOST_CHECK_CLOSE_FRACTION(cdf(dist111, median(dist111)), 0.5, tol_few_eps);
  // skewness:
  BOOST_CHECK_CLOSE_FRACTION(skewness(dist111), static_cast<RealType>(1.5), tol_few_eps);
   //kurtosis:
  BOOST_CHECK_CLOSE_FRACTION(kurtosis(dist51), static_cast<RealType>(42 + 3), tol_few_eps);
  // kurtosis excess:
  BOOST_CHECK_CLOSE_FRACTION(kurtosis_excess(dist51), static_cast<RealType>(42), tol_few_eps);

  tol_few_eps = boost::math::tools::epsilon<RealType>() * 3; // 3 eps as a percentage.

  // Special and limit cases:

  if(std::numeric_limits<RealType>::is_specialized)
  {
    RealType mx = (std::numeric_limits<RealType>::max)();
    RealType mi = (std::numeric_limits<RealType>::min)();

     BOOST_CHECK_EQUAL(
     pdf(inverse_gamma_distribution<RealType>(1),
       static_cast<RealType>(mx)), // max()
       static_cast<RealType>(0)
       );

     BOOST_CHECK_EQUAL(
     pdf(inverse_gamma_distribution<RealType>(1),
       static_cast<RealType>(mi)), // min()
       static_cast<RealType>(0)
       );

  }

  BOOST_CHECK_EQUAL(
    pdf(inverse_gamma_distribution<RealType>(1), static_cast<RealType>(0)), static_cast<RealType>(0));
  BOOST_CHECK_EQUAL(
    pdf(inverse_gamma_distribution<RealType>(3), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_gamma_distribution<RealType>(1), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_gamma_distribution<RealType>(2), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_gamma_distribution<RealType>(3), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_gamma_distribution<RealType>(1), static_cast<RealType>(0)))
    , static_cast<RealType>(1));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_gamma_distribution<RealType>(2), static_cast<RealType>(0)))
    , static_cast<RealType>(1));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_gamma_distribution<RealType>(3), static_cast<RealType>(0)))
    , static_cast<RealType>(1));

  BOOST_CHECK_THROW(
    pdf(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(-1)), // shape negative.
    static_cast<RealType>(1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    pdf(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    cdf(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    cdf(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    cdf(complement(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(1))), std::domain_error
    );
  BOOST_CHECK_THROW(
    cdf(complement(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1))), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(0.5)), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(1.1)), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(complement(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(0.5))), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(complement(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1))), std::domain_error
    );
  BOOST_CHECK_THROW(
    quantile(complement(
    inverse_gamma_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(1.1))), std::domain_error
    );
   check_out_of_range<inverse_gamma_distribution<RealType> >(1, 1);
} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{
  BOOST_MATH_CONTROL_FP;

  // Check that can generate inverse_gamma distribution using the two convenience methods:
  // inverse_gamma_distribution; // with default parameters, shape = 1, scale - 1
  using boost::math::inverse_gamma;
  inverse_gamma ig2(2.); // Using typedef and shape parameter (and default scale = 1).
  BOOST_CHECK_EQUAL(ig2.shape(), 2.); // scale  == 2.
  BOOST_CHECK_EQUAL(ig2.scale(), 1.); // scale  == 1 (default).
  inverse_gamma ig; // Using typedef, type double and default values, shape = 1 and scale = 1
  // check default is (1, 1)
  BOOST_CHECK_EQUAL(ig.shape(), 1.); // shape == 1
  BOOST_CHECK_EQUAL(ig.scale(), 1.); // scale  == 1
  BOOST_CHECK_EQUAL(mode(ig), 0.5); // mode = 1/2

  // Used to find some 'exact' values for testing mean, variance ...
 //for (int shape = 4; shape < 30; shape++)
 // {
 //   inverse_gamma ig(shape, 1);
 //   cout.precision(17);
 //   cout << shape << ' ' << mean(ig) << ' ' << variance(ig) << ' ' << standard_deviation(ig)
 //     << ' ' << median(ig) << endl;
 // }

  // and "using boost::math::inverse_gamma_distribution;".
  inverse_gamma_distribution<> ig23(2., 3.); // Using default RealType double.
  BOOST_CHECK_EQUAL(ig23.shape(), 2.); //
  BOOST_CHECK_EQUAL(ig23.scale(), 3.); //

  inverse_gamma_distribution<float> igf23(1.f, 2.f); // Using explicit RealType float.
  BOOST_CHECK_EQUAL(igf23.shape(), 1.f); //
  BOOST_CHECK_EQUAL(igf23.scale(), 2.f); //
  // Some tests using default double.
  double tol5eps = boost::math::tools::epsilon<double>() * 5; // 5 eps as a fraction.
  inverse_gamma_distribution<double> ig102(10., 2.); //
  BOOST_CHECK_EQUAL(ig102.shape(), 10.); //
  BOOST_CHECK_EQUAL(ig102.scale(), 2.); //
  // formatC(SuppDists::dinvGauss(10, 1, 0.5), digits=17)[1] "0.0011774669940754754"
  BOOST_CHECK_CLOSE_FRACTION(pdf(ig102, 0.5), 0.1058495335284024, tol5eps);
  // formatC(SuppDists::pinvGauss(10, 1, 0.5), digits=17) [1] "0.99681494462166653"
  BOOST_CHECK_CLOSE_FRACTION(cdf(ig102, 0.5), 0.99186775720306608, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(quantile(ig102, 0.05), 0.12734622346137681, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(quantile(ig102, 0.5), 0.20685272858879727, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(quantile(ig102, 0.95),  0.36863602680851204, tol5eps);
  // Check mean, etc spot values.
  inverse_gamma_distribution<double> ig51(5., 1.); // shape = 5, scale = 1
  BOOST_CHECK_CLOSE_FRACTION(mean(ig51), 0.25, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(ig51), 0.0208333333333333333333333333333333333333333, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(skewness(ig51), 2 * std::sqrt(3.), tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(kurtosis_excess(ig51), 42, tol5eps);
  // mode and median
  inverse_gamma_distribution<double> ig21(1., 2.);
  BOOST_CHECK_CLOSE_FRACTION(mode(ig21), 1, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(median(ig21), 2.8853900817779268, tol5eps);

  BOOST_CHECK_CLOSE_FRACTION(quantile(ig21, 0.5), 2.8853900817779268, tol5eps);
  BOOST_CHECK_CLOSE_FRACTION(cdf(ig21, median(ig21)), 0.5, tol5eps);

    // Check throws from bad parameters.
  inverse_gamma ig051(0.5, 1.); // shape < 1, so wrong for mean.
  BOOST_CHECK_THROW(mean(ig051), std::domain_error);
  inverse_gamma ig191(1.9999, 1.); // shape < 2, so wrong for variance.
  BOOST_CHECK_THROW(variance(ig191), std::domain_error);
  inverse_gamma ig291(2.9999, 1.); // shape < 3, so wrong for skewness.
  BOOST_CHECK_THROW(skewness(ig291), std::domain_error);
  inverse_gamma ig391(3.9999, 1.); // shape < 1, so wrong for kurtosis and kurtosis_excess.
  BOOST_CHECK_THROW(kurtosis(ig391), std::domain_error);
  BOOST_CHECK_THROW(kurtosis_excess(ig391), std::domain_error);

  // Basic sanity-check spot values.
  // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
  std::cout << "<note>The long double tests have been disabled on this platform "
    "either because the long double overloads of the usual math functions are "
    "not available at all, or because they are too inaccurate for these tests "
    "to pass.</note>" << std::cout;
#endif
  return 0;
} // int test_main(int, char* [])

/*

Output:

------ Build started: Project: test_inverse_gamma_distribution, Configuration: Release Win32 ------
  test_inverse_gamma_distribution.cpp
  Generating code
  Finished generating code
  test_inverse_gamma_distribution.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\test_inverse_gamma_distribution.exe
  Running 1 test case...
  Tolerance = 0.0001%.
  Tolerance = 0.0001%.
  Tolerance = 0.0001%.
  Tolerance = 0.0001%.

  *** No errors detected
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


#include <algorithm>
#include <iostream>

#include <boost/format.hpp>
#include <gtest/gtest.h>

#include <sort.hpp>

static void print(my::container &v) {
    for (auto &i : v) {
        std::cout << i << " ";
    }
    std::cout << std::endl;
}

class sort_test : public ::testing::Test {
  public:
    void test(std::function<void(my::container::iterator,
                                 my::container::iterator)> &&func) {
        for (auto &v : datas) {
            print(v);
            func(v.begin(), v.end());
            print(v);
            EXPECT_TRUE(std::is_sorted(v.begin(), v.end()));
        }
    }

  protected:
    void SetUp() override {
        datas = {{3, 2, 1, 5, 4}, {0, 0, 0}, {0, 1, 2}, {2, 0, 0}, {1, 2, 1}};
    }
    std::vector<std::vector<int>> datas;
};

TEST_F(sort_test, insert_sort) { test(my::insert_sort::sort); }
TEST_F(sort_test, merge_sort) { test(my::merge_sort::sort); }
TEST_F(sort_test, heap_sort) { test(my::heap_sort::sort); }
TEST_F(sort_test, quick_sort) { test(my::quick_sort::sort); }


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

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

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

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

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

//[relation
//` Shows how to calculate the spatial relation between a point and a polygon

#include <iostream>
#include <string>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
/*<-*/ #include "create_svg_two.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    polygon_type poly;
    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", poly);

    point_type p(4, 1);

    boost::geometry::de9im::matrix matrix = boost::geometry::relation(p, poly);
    std::string code = matrix.str();

    std::cout << "relation: " << code << std::endl;
    /*<-*/ create_svg("relation.svg", poly, p); /*->*/
    return 0;
}

//]

//[relation_output
/*`
Output:
[pre
relation: 0FFFFF212

[$img/algorithms/within.png]
]

*/
//]


#pragma once

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

template <int NumArguments, typename StepRNG = std::uniform_real_distribution<>,
          class Generator = std::mt19937>
class MetropolisAlgorithm {
 public:
  using Argument = Eigen::Array<double, NumArguments, 1>;
  using FunctionType = std::function<double(const Argument&)>;
  using GeneratorResType = Generator::result_type;

 private:
  FunctionType pdf;
  StepRNG step;
  std::uniform_real_distribution<> uniform =
      std::uniform_real_distribution<>(0, 1);
  std::mt19937 gen;

  // saving the old probability minimizes the function calls
  double p_old;
  std::size_t argument_index = 0;

 public:
  Argument argument;

  MetropolisAlgorithm(FunctionType pdf, Argument& argstart, StepRNG step,
                      GeneratorResType seed,
                      std::size_t warm_up = 100 * NumArguments)
      : pdf(pdf), argument(argstart), step(step) {
    gen = Generator(seed);  // Standard mersenne_twister_engine seeded with seed

    p_old = pdf(argument);

    for (std::size_t i = 0; i < warm_up; i++) do_step();
  };

  MetropolisAlgorithm(FunctionType pdf, Argument& argstart, StepRNG step,
                      std::size_t warm_up = 100 * NumArguments)
      : pdf(pdf), step(step), argument(argstart) {
    std::random_device rd;
    gen = Generator(rd());  // Standard mersenne_twister_engine seeded with seed

    p_old = pdf(argument);

    for (std::size_t i = 0; i < warm_up; i++) do_step();
  };

  void do_step() {
    double x_old = argument[argument_index];
    double x_new = x_old + step(gen);

    argument[argument_index] = x_new;
    double p_new = pdf(argument);
    double p = p_new / p_old;

    if (p < 1) {
      double s = uniform(gen);

      // reject new values
      if (s > p) {
        x_new = x_old;
        p_new = p_old;
      }
    }

    argument[argument_index] = x_new;
    p_old = p_new;

    argument_index++;
    if (argument_index >= NumArguments) {
      argument_index = 0;
    }
  }

  Argument& next() {
    do_step();
    return argument;
  }

  /**
   * @brief Calculates the expectation value and the standard deviation of the
   * given funtion (under the pdf)
   *
   * Using the Welfords online algorithm to calculate the mean and standard
   * deviation.
   *
   * @param function function to average
   * @param samples number of samples to collect
   * @return std::tuple<double, double, double> Mean, std of mean, variance
   */
  std::tuple<double, double, double> average(
      std::function<double(const Argument&)> function, std::size_t samples) {
    std::size_t n;
    double mean = 0;
    // double mean2 = 0;
    double S = 0;

    for (n = 1; n <= samples; n++) {
      do_step();
      double x = function(argument);
      double old_mean = mean;
      mean += (x - mean) / double(n);
      // mean2 += (x * x - mean2) / double(n);
      S += (x - mean) * (x - old_mean);
    }

    // return {mean, std::sqrt(mean2 - mean * mean)};
    return {mean, std::sqrt(S / (samples - 1) / samples), S / (samples - 1)};
  }

  Eigen::Array<double, Eigen::Dynamic, NumArguments> get_sample(
      Eigen::Index N) {
    using namespace Eigen;
    typedef Array<double, Eigen::Dynamic, NumArguments> ReturnType;

    ReturnType returner(N, NumArguments);

    for (Index i = 0; i < N; i++) {
      do_step();
      returner.row(i) = argument;
    }

    return returner;
  }
};


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <math/rev/scal/fun/nan_util.hpp>
#include <math/rev/scal/util.hpp>

TEST(AgradRev, gamma_q_var_var) {
  AVAR a = 0.5;
  AVAR b = 1.0;
  AVAR f = gamma_q(a, b);
  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), f.val());

  AVEC x = createAVEC(a, b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[1]);

  a = -0.5;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);
}
TEST(AgradRevGammaQ, infLoopInVersion2_0_1_var_var) {
  AVAR a = 8.01006;
  AVAR b = 2.47579e+215;
  AVEC x = createAVEC(a, b);

  AVAR f = gamma_q(a, b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(0, g[0]);
}
TEST(AgradRevGammaQ, infLoopInVersion2_0_1_var_double) {
  AVAR a = 8.01006;
  double b = 2.47579e+215;
  AVEC x = createAVEC(a);

  AVAR f = gamma_q(a, b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(0, g[0]);
}
TEST(AgradRev, gamma_q_double_var) {
  double a = 0.5;
  AVAR b = 1.0;
  AVAR f = gamma_q(a, b);
  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), f.val());

  AVEC x = createAVEC(b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[0]);

  a = -0.5;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);
}
TEST(AgradRev, gamma_q_var_double) {
  AVAR a = 0.5;
  double b = 1.0;
  AVAR f = gamma_q(a, b);
  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), f.val());

  AVEC x = createAVEC(a);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);

  a = -0.5;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_q(a, b), std::domain_error);
}

namespace {
struct gamma_q_fun {
  template <typename T0, typename T1>
  inline typename stan::return_type<T0, T1>::type operator()(
      const T0& arg1, const T1& arg2) const {
    return gamma_q(arg1, arg2);
  }
};
}  // namespace

TEST(AgradRev, gamma_q_nan) {
  gamma_q_fun gamma_q_;
  test_nan(gamma_q_, 3.0, 5.0, false, true);
}

TEST(AgradRev, check_varis_on_stack_24) {
  AVAR a = 0.5;
  AVAR b = 1.0;
  test::check_varis_on_stack(stan::math::gamma_q(a, b));
  test::check_varis_on_stack(stan::math::gamma_q(a, 1.0));
  test::check_varis_on_stack(stan::math::gamma_q(0.5, b));
}


#include <iostream>

#include <boost/graph/grid_graph.hpp>

#include "NearestNeighbor/metric_space_search.hpp"

int main(int argc, char *argv[])
{
  typedef boost::grid_graph<2> GraphType;

  //const unsigned int dimension = 5;
  const unsigned int gridDimension = 300;
  boost::array<std::size_t, 2> lengths = { { gridDimension, gridDimension } };
  GraphType graph(lengths);

  typedef boost::graph_traits<GraphType>::vertex_descriptor VertexDescriptor;

  VertexDescriptor v = { { 0, 1 } };

  const unsigned int descriptorDimension = 100;
  typedef boost::hypercube_topology<descriptorDimension, boost::minstd_rand> TopologyType;
  TopologyType myTopology;
  typedef TopologyType::point_type PointType;

  std::vector<PointType> vertexData(gridDimension * gridDimension);

  // This is an "exterior property" of the grid_graph
  typedef boost::property_map<GraphType, boost::vertex_index_t>::const_type IndexMapType;

  IndexMapType indexMap(get(boost::vertex_index, graph));

  typedef boost::iterator_property_map<std::vector<PointType>::iterator, IndexMapType> MapType;
  MapType myMap(vertexData.begin(), indexMap);

  // Add vertices to the graph and corresponding points increasing integer points to the tree.
  // The experiment here is to query the nearest neighbor of a point like (5.2, 5.2, 5.1, 5.3, 5.2, 5.1)
  // and ensure we get back (5,5,5,5,5,5)
  unsigned int counter = 0;
//   for(unsigned int vertexId = 0; vertexId < num_vertices(graph); ++vertexId)
//   {
//     PointType p;
//     for(unsigned int dim = 0; dim < descriptorDimension; ++dim)
//       {
//       p[dim] = vertexId;
//       }
//     boost::put(myMap, v, p);
//   };
  typedef typename boost::graph_traits<GraphType>::vertex_iterator VertexIter;
  VertexIter ui,ui_end; tie(ui,ui_end) = vertices(graph);
  for(VertexIter iter = ui; iter != ui_end; ++iter)
    {
    PointType p;
    for(unsigned int dim = 0; dim < descriptorDimension; ++dim)
      {
      p[dim] = counter;
      }
    counter++;
    boost::put(myMap, *iter, p);
    }

  // Prefer to initialize the DVP-tree with a filled graph, this way,
  // the entire DVP-tree will be initialized at once (gets best results).
  typedef dvp_tree<VertexDescriptor, TopologyType, MapType> TreeType;
  TreeType tree(graph, myTopology, myMap);

  multi_dvp_tree_search<GraphType, TreeType> nearestNeighborFinder;
  nearestNeighborFinder.graph_tree_map[&graph] = &tree;

  PointType queryPoint;
  for(unsigned int dim = 0; dim < descriptorDimension; ++dim)
    {
    queryPoint[dim] = 5.2;
    }

  VertexDescriptor nearestNeighbor = nearestNeighborFinder(queryPoint, graph, myTopology, myMap);
  std::cout << "nearestNeighbor[0]: " << nearestNeighbor[0] << std::endl;

  return 0;
}


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

using namespace std;
using namespace Eigen;

void program_body()
{
  Matrix<double, 2, 1> input;
  input << 9, 2;

  Matrix<double, 3, 2> layer1;
  layer1 << 1, 2, 3, 4, 5, 6;

  auto output = layer1 * input;

  Matrix<double, 3, 1> expected_output;
  expected_output << 13, 35, 57;

  if ( output != expected_output ) {
    throw runtime_error( "test failure" );
  }
}

int main()
{
  try {
    program_body();
    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_SIGNEDSVD_HPP
#define MCL_SIGNEDSVD_HPP 1

#include <Eigen/Dense>
#include <Eigen/SVD>

namespace mcl
{

template <typename T, int DIM>
static inline void signed_svd(
	const Eigen::Matrix<T,DIM,DIM> &F,
	Eigen::Matrix<T,DIM,1> &S,
	Eigen::Matrix<T,DIM,DIM> &U,
	Eigen::Matrix<T,DIM,DIM> &V)
{
	using namespace Eigen;
	int dim = DIM == Eigen::Dynamic ? F.rows() : DIM;
	typedef Matrix<T,DIM,DIM> MatX;

	JacobiSVD<MatX> svd(F, ComputeFullU | ComputeFullV);
	S = svd.singularValues();
	U = svd.matrixU();
	V = svd.matrixV();

	MatX J = MatX::Identity(dim,dim);
	J(dim-1,dim-1) = -1.0;

	if (U.determinant() < 0.0)
	{
		U = U * J;
		S[dim-1] *= -1.0;
	}

	if (V.determinant() < 0.0)
	{
		V = (J * V.transpose()).transpose();
		S[dim-1] *= -1.0;
	}

	// Degenerate case
	if (!S.allFinite())
	{
		S.setZero();
		U.setIdentity();
		V.setIdentity();
	}

} // end signed svd

} // end mcl

#endif


#ifndef TEST_UNIT_MATH_PRIM_SCAL_PROB_HPP
#define TEST_UNIT_MATH_PRIM_SCAL_PROB_HPP

#include <boost/math/distributions.hpp>
#include <algorithm>
#include <vector>

/**
 * Uses a chi-squared test to assert that a vector of observed counts
 * is consistent with a vector of expected counts. Useful for testing RNGs.
 */
void assert_chi_squared(const std::vector<int>& counts,
                        const std::vector<double>& expected, double tolerance) {
  int bins = counts.size();
  EXPECT_EQ(bins, expected.size());

  double chi = 0;
  for (int i = 0; i < bins; ++i) {
    double discrepancy = expected[i] - counts[i];
    chi += discrepancy * discrepancy / expected[i];
  }
  boost::math::chi_squared dist(bins - 1);
  double chi_threshold = quantile(complement(dist, tolerance));

  EXPECT_TRUE(chi < chi_threshold);
}

/**
 * From a collection of samples and a list of quantiles, assumed ordered,
 * assert that the samples resemble draws from a distribution with those
 * quantiles, using a chi_squared goodness of fit test.
 */
void assert_matches_quantiles(const std::vector<double>& samples,
                              const std::vector<double>& quantiles,
                              double tolerance) {
  int N = samples.size();
  std::vector<double> mysamples = samples;
  std::sort(mysamples.begin(), mysamples.end());

  int K = quantiles.size();
  double expected_count = static_cast<double>(N) / K;

  std::vector<double> expected;
  for (int i = 0; i < K; i++) {
    expected.push_back(expected_count);
  }

  std::vector<int> counts(K);
  size_t current_index = 0;
  for (int i = 0; i < N; ++i) {
    while (mysamples[i] >= quantiles[current_index]) {
      ++current_index;
      EXPECT_TRUE(current_index < quantiles.size());
    }
    ++counts[current_index];
  }
  assert_chi_squared(counts, expected, tolerance);
}
#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2008 - 2020 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: Liang Zhao and Timo Heister, Clemson University, 2016
 */

// @sect3{Include files}

// 像往常一样，我们从包括一些著名的文件开始。

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

#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

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

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

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

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

// 为了在网格之间传输解决方案，包括这个文件。

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

// 这个文件包括UMFPACK：直接求解器。

#include <deal.II/lac/sparse_direct.h>

// 还有一个ILU预处理程序。

#include <deal.II/lac/sparse_ilu.h>

#include <fstream>
#include <iostream>

namespace Step57
{
  using namespace dealii;
// @sect3{The NavierStokesProblem class template}

// 该类管理介绍中描述的矩阵和向量：特别是，我们为当前的解决方案、当前的牛顿更新和直线搜索更新存储了一个BlockVector。 我们还存储了两个AffineConstraints对象：一个是强制执行Dirichlet边界条件的对象，另一个是将所有边界值设为0的对象。第一个约束解向量，第二个约束更新（也就是说，我们从不更新边界值，所以我们强制相关的更新向量值为零）。

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

  private:
    void setup_dofs();

    void initialize_system();

    void assemble(const bool initial_step, const bool assemble_matrix);

    void assemble_system(const bool initial_step);

    void assemble_rhs(const bool initial_step);

    void solve(const bool initial_step);

    void refine_mesh();

    void process_solution(unsigned int refinement);

    void output_results(const unsigned int refinement_cycle) const;

    void newton_iteration(const double       tolerance,
                          const unsigned int max_n_line_searches,
                          const unsigned int max_n_refinements,
                          const bool         is_initial_step,
                          const bool         output_result);

    void compute_initial_guess(double step_size);

    double                               viscosity;
    double                               gamma;
    const unsigned int                   degree;
    std::vector<types::global_dof_index> dofs_per_block;

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

    AffineConstraints<double> zero_constraints;
    AffineConstraints<double> nonzero_constraints;

    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;
    SparseMatrix<double>      pressure_mass_matrix;

    BlockVector<double> present_solution;
    BlockVector<double> newton_update;
    BlockVector<double> system_rhs;
    BlockVector<double> evaluation_point;
  };
// @sect3{Boundary values and right hand side}

// 在这个问题中，我们设定沿空腔上表面的速度为1，其他三面墙的速度为0。右边的函数为零，所以我们在本教程中不需要设置右边的函数。边界函数的分量数为  dim+1  。我们最终将使用 VectorTools::interpolate_boundary_values 来设置边界值，这就要求边界值函数的分量数与解相同，即使没有全部使用。换个说法：为了让这个函数高兴，我们为压力定义了边界值，尽管我们实际上永远不会用到它们。

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

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> & p,
                                    const unsigned int component) const
  {
    Assert(component < this->n_components,
           ExcIndexRange(component, 0, this->n_components));
    if (component == 0 && std::abs(p[dim - 1] - 1.0) < 1e-10)
      return 1.0;

    return 0;
  }
// @sect3{BlockSchurPreconditioner for Navier Stokes equations}

// 正如介绍中所讨论的，Krylov迭代方法中的预处理器是作为一个矩阵-向量乘积算子实现的。在实践中，舒尔补码预处理器被分解为三个矩阵的乘积（如第一节所述）。第一个因素中的 $\tilde{A}^{-1}$ 涉及到对线性系统 $\tilde{A}x=b$ 的求解。在这里，为了简单起见，我们通过一个直接求解器来解决这个系统。第二个因素中涉及的计算是一个简单的矩阵-向量乘法。舒尔补码 $\tilde{S}$ 可以被压力质量矩阵很好地近似，其逆值可以通过不精确求解器得到。因为压力质量矩阵是对称和正定的，我们可以用CG来解决相应的线性系统。

  template <class PreconditionerMp>
  class BlockSchurPreconditioner : public Subscriptor
  {
  public:
    BlockSchurPreconditioner(double                           gamma,
                             double                           viscosity,
                             const BlockSparseMatrix<double> &S,
                             const SparseMatrix<double> &     P,
                             const PreconditionerMp &         Mppreconditioner);

    void vmult(BlockVector<double> &dst, const BlockVector<double> &src) const;

  private:
    const double                     gamma;
    const double                     viscosity;
    const BlockSparseMatrix<double> &stokes_matrix;
    const SparseMatrix<double> &     pressure_mass_matrix;
    const PreconditionerMp &         mp_preconditioner;
    SparseDirectUMFPACK              A_inverse;
  };

// 我们可以注意到，左上角的矩阵逆的初始化是在构造函数中完成的。如果是这样，那么预处理程序的每一次应用就不再需要计算矩阵因子了。

  template <class PreconditionerMp>
  BlockSchurPreconditioner<PreconditionerMp>::BlockSchurPreconditioner(
    double                           gamma,
    double                           viscosity,
    const BlockSparseMatrix<double> &S,
    const SparseMatrix<double> &     P,
    const PreconditionerMp &         Mppreconditioner)
    : gamma(gamma)
    , viscosity(viscosity)
    , stokes_matrix(S)
    , pressure_mass_matrix(P)
    , mp_preconditioner(Mppreconditioner)
  {
    A_inverse.initialize(stokes_matrix.block(0, 0));
  }

  template <class PreconditionerMp>
  void BlockSchurPreconditioner<PreconditionerMp>::vmult(
    BlockVector<double> &      dst,
    const BlockVector<double> &src) const
  {
    Vector<double> utmp(src.block(0));

    {
      SolverControl solver_control(1000, 1e-6 * src.block(1).l2_norm());
      SolverCG<Vector<double>> cg(solver_control);

      dst.block(1) = 0.0;
      cg.solve(pressure_mass_matrix,
               dst.block(1),
               src.block(1),
               mp_preconditioner);
      dst.block(1) *= -(viscosity + gamma);
    }

    {
      stokes_matrix.block(0, 1).vmult(utmp, dst.block(1));
      utmp *= -1.0;
      utmp += src.block(0);
    }

    A_inverse.vmult(dst.block(0), utmp);
  }
// @sect3{StationaryNavierStokes class implementation}
// @sect4{StationaryNavierStokes::StationaryNavierStokes}

// 该类的构造函数看起来与  step-22  中的构造函数非常相似。唯一的区别是粘度和增强的拉格朗日系数  gamma  。

  template <int dim>
  StationaryNavierStokes<dim>::StationaryNavierStokes(const unsigned int degree)
    : viscosity(1.0 / 7500.0)
    , gamma(1.0)
    , degree(degree)
    , triangulation(Triangulation<dim>::maximum_smoothing)
    , fe(FE_Q<dim>(degree + 1), dim, FE_Q<dim>(degree), 1)
    , dof_handler(triangulation)
  {}
// @sect4{StationaryNavierStokes::setup_dofs}

// 这个函数初始化DoFHandler，列举当前网格上的自由度和约束。

  template <int dim>
  void StationaryNavierStokes<dim>::setup_dofs()
  {
    system_matrix.clear();
    pressure_mass_matrix.clear();

// 第一步是将DoFs与给定的网格联系起来。

    dof_handler.distribute_dofs(fe);

// 我们对组件重新编号，使所有的速度DoF在压力DoF之前，以便能够将解向量分成两个块，在块预处理程序中分别访问。

    std::vector<unsigned int> block_component(dim + 1, 0);
    block_component[dim] = 1;
    DoFRenumbering::component_wise(dof_handler, block_component);

    dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
    unsigned int dof_u = dofs_per_block[0];
    unsigned int dof_p = dofs_per_block[1];

// 在牛顿方案中，我们首先将边界条件应用于从初始步骤得到的解。为了确保边界条件在牛顿迭代过程中保持满足，在更新时使用零边界条件  $\delta u^k$  。因此我们设置了两个不同的约束对象。

    FEValuesExtractors::Vector velocities(0);
    {
      nonzero_constraints.clear();

      DoFTools::make_hanging_node_constraints(dof_handler, nonzero_constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               BoundaryValues<dim>(),
                                               nonzero_constraints,
                                               fe.component_mask(velocities));
    }
    nonzero_constraints.close();

    {
      zero_constraints.clear();

      DoFTools::make_hanging_node_constraints(dof_handler, zero_constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               Functions::ZeroFunction<dim>(
                                                 dim + 1),
                                               zero_constraints,
                                               fe.component_mask(velocities));
    }
    zero_constraints.close();

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << dof_u << " + " << dof_p << ')' << std::endl;
  }
// @sect4{StationaryNavierStokes::initialize_system}

// 在每个网格上，SparsityPattern和线性系统的大小是不同的。这个函数在网格细化后初始化它们。

  template <int dim>
  void StationaryNavierStokes<dim>::initialize_system()
  {
    {
      BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);
      DoFTools::make_sparsity_pattern(dof_handler, dsp, nonzero_constraints);
      sparsity_pattern.copy_from(dsp);
    }

    system_matrix.reinit(sparsity_pattern);

    present_solution.reinit(dofs_per_block);
    newton_update.reinit(dofs_per_block);
    system_rhs.reinit(dofs_per_block);
  }
// @sect4{StationaryNavierStokes::assemble}

// 这个函数建立了我们目前工作的系统矩阵和右手边。 @p initial_step 参数用于确定我们应用哪一组约束（初始步骤为非零，其他为零）。 @p assemble_matrix 参数分别决定了是组装整个系统还是只组装右手边的向量。

  template <int dim>
  void StationaryNavierStokes<dim>::assemble(const bool initial_step,
                                             const bool assemble_matrix)
  {
    if (assemble_matrix)
      system_matrix = 0;

    system_rhs = 0;

    QGauss<dim> quadrature_formula(degree + 2);

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

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

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);

    FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     local_rhs(dofs_per_cell);

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

// 对于线性化系统，我们为当前速度和梯度以及当前压力创建临时存储。在实践中，它们都是通过正交点的形状函数获得的。

    std::vector<Tensor<1, dim>> present_velocity_values(n_q_points);
    std::vector<Tensor<2, dim>> present_velocity_gradients(n_q_points);
    std::vector<double>         present_pressure_values(n_q_points);

    std::vector<double>         div_phi_u(dofs_per_cell);
    std::vector<Tensor<1, dim>> phi_u(dofs_per_cell);
    std::vector<Tensor<2, dim>> grad_phi_u(dofs_per_cell);
    std::vector<double>         phi_p(dofs_per_cell);

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

        local_matrix = 0;
        local_rhs    = 0;

        fe_values[velocities].get_function_values(evaluation_point,
                                                  present_velocity_values);

        fe_values[velocities].get_function_gradients(
          evaluation_point, present_velocity_gradients);

        fe_values[pressure].get_function_values(evaluation_point,
                                                present_pressure_values);

//装配类似于  step-22  。一个以gamma为系数的附加项是增强拉格朗日（AL），它是通过grad-div稳定化组装的。 正如我们在介绍中所讨论的，系统矩阵的右下块应该为零。由于压力质量矩阵是在创建预处理程序时使用的，所以我们在这里组装它，然后在最后把它移到一个单独的SparseMatrix中（与 step-22 相同）。

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              {
                div_phi_u[k]  = fe_values[velocities].divergence(k, q);
                grad_phi_u[k] = fe_values[velocities].gradient(k, q);
                phi_u[k]      = fe_values[velocities].value(k, q);
                phi_p[k]      = fe_values[pressure].value(k, q);
              }

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                if (assemble_matrix)
                  {
                    for (unsigned int j = 0; j < dofs_per_cell; ++j)
                      {
                        local_matrix(i, j) +=
                          (viscosity *
                             scalar_product(grad_phi_u[j], grad_phi_u[i]) +
                           present_velocity_gradients[q] * phi_u[j] * phi_u[i] +
                           grad_phi_u[j] * present_velocity_values[q] *
                             phi_u[i] -
                           div_phi_u[i] * phi_p[j] - phi_p[i] * div_phi_u[j] +
                           gamma * div_phi_u[j] * div_phi_u[i] +
                           phi_p[i] * phi_p[j]) *
                          fe_values.JxW(q);
                      }
                  }

                double present_velocity_divergence =
                  trace(present_velocity_gradients[q]);
                local_rhs(i) +=
                  (-viscosity * scalar_product(present_velocity_gradients[q],
                                               grad_phi_u[i]) -
                   present_velocity_gradients[q] * present_velocity_values[q] *
                     phi_u[i] +
                   present_pressure_values[q] * div_phi_u[i] +
                   present_velocity_divergence * phi_p[i] -
                   gamma * present_velocity_divergence * div_phi_u[i]) *
                  fe_values.JxW(q);
              }
          }

        cell->get_dof_indices(local_dof_indices);

        const AffineConstraints<double> &constraints_used =
          initial_step ? nonzero_constraints : zero_constraints;

        if (assemble_matrix)
          {
            constraints_used.distribute_local_to_global(local_matrix,
                                                        local_rhs,
                                                        local_dof_indices,
                                                        system_matrix,
                                                        system_rhs);
          }
        else
          {
            constraints_used.distribute_local_to_global(local_rhs,
                                                        local_dof_indices,
                                                        system_rhs);
          }
      }

    if (assemble_matrix)
      {

// 最后我们把压力质量矩阵移到一个单独的矩阵中。

        pressure_mass_matrix.reinit(sparsity_pattern.block(1, 1));
        pressure_mass_matrix.copy_from(system_matrix.block(1, 1));

// 注意，将这个压力块设置为零并不等同于不在这个块中装配任何东西，因为这里的操作将（错误地）删除从压力作用力的悬挂节点约束中进来的对角线条目。这意味着，我们的整个系统矩阵将有完全为零的行。幸运的是，FGMRES处理这些行没有任何问题。

        system_matrix.block(1, 1) = 0;
      }
  }

  template <int dim>
  void StationaryNavierStokes<dim>::assemble_system(const bool initial_step)
  {
    assemble(initial_step, true);
  }

  template <int dim>
  void StationaryNavierStokes<dim>::assemble_rhs(const bool initial_step)
  {
    assemble(initial_step, false);
  }
// @sect4{StationaryNavierStokes::solve}

// 在这个函数中，我们使用FGMRES和程序开始时定义的块状预处理程序来解决线性系统。我们在这一步得到的是解向量。如果这是初始步骤，解向量为我们提供了纳维尔-斯托克斯方程的初始猜测。对于初始步骤，非零约束被应用，以确保边界条件得到满足。在下面的步骤中，我们将求解牛顿更新，所以使用零约束。

  template <int dim>
  void StationaryNavierStokes<dim>::solve(const bool initial_step)
  {
    const AffineConstraints<double> &constraints_used =
      initial_step ? nonzero_constraints : zero_constraints;

    SolverControl solver_control(system_matrix.m(),
                                 1e-4 * system_rhs.l2_norm(),
                                 true);

    SolverFGMRES<BlockVector<double>> gmres(solver_control);
    SparseILU<double>                 pmass_preconditioner;
    pmass_preconditioner.initialize(pressure_mass_matrix,
                                    SparseILU<double>::AdditionalData());

    const BlockSchurPreconditioner<SparseILU<double>> preconditioner(
      gamma,
      viscosity,
      system_matrix,
      pressure_mass_matrix,
      pmass_preconditioner);

    gmres.solve(system_matrix, newton_update, system_rhs, preconditioner);
    std::cout << "FGMRES steps: " << solver_control.last_step() << std::endl;

    constraints_used.distribute(newton_update);
  }
// @sect4{StationaryNavierStokes::refine_mesh}

// 在粗略的网格上找到一个好的初始猜测后，我们希望通过细化网格来减少误差。这里我们做了类似于 step-15 的自适应细化，只是我们只使用了速度上的Kelly估计器。我们还需要使用SolutionTransfer类将当前的解转移到下一个网格。

  template <int dim>
  void StationaryNavierStokes<dim>::refine_mesh()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    FEValuesExtractors::Vector velocity(0);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      present_solution,
      estimated_error_per_cell,
      fe.component_mask(velocity));

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

    triangulation.prepare_coarsening_and_refinement();
    SolutionTransfer<dim, BlockVector<double>> solution_transfer(dof_handler);
    solution_transfer.prepare_for_coarsening_and_refinement(present_solution);
    triangulation.execute_coarsening_and_refinement();

// 首先，DoFHandler被设置，约束被生成。然后我们创建一个临时的BlockVector  tmp  ，其大小与新网格上的解决方案一致。

    setup_dofs();

    BlockVector<double> tmp(dofs_per_block);

// 将解决方案从粗网格转移到细网格，并对新转移的解决方案应用边界值约束。注意，present_solution仍然是对应于旧网格的一个向量。

    solution_transfer.interpolate(present_solution, tmp);
    nonzero_constraints.distribute(tmp);

// 最后设置矩阵和向量，并将present_solution设置为插值后的数据。

    initialize_system();
    present_solution = tmp;
  }
// @sect4{StationaryNavierStokes<dim>::newton_iteration}

// 这个函数实现了牛顿迭代，给定了公差、最大迭代次数和要做的网格细化次数。

// 参数 is_initial_step 告诉我们是否需要 setup_system ，以及应该装配哪一部分，系统矩阵或右手边的矢量。如果我们做直线搜索，在最后一次迭代中检查残差准则时，右手边已经被组装起来了。因此，我们只需要在当前迭代中装配系统矩阵。最后一个参数 output_result 决定了是否应该产生图形输出。

  template <int dim>
  void StationaryNavierStokes<dim>::newton_iteration(
    const double       tolerance,
    const unsigned int max_n_line_searches,
    const unsigned int max_n_refinements,
    const bool         is_initial_step,
    const bool         output_result)
  {
    bool first_step = is_initial_step;

    for (unsigned int refinement_n = 0; refinement_n < max_n_refinements + 1;
         ++refinement_n)
      {
        unsigned int line_search_n = 0;
        double       last_res      = 1.0;
        double       current_res   = 1.0;
        std::cout << "grid refinements: " << refinement_n << std::endl
                  << "viscosity: " << viscosity << std::endl;

        while ((first_step || (current_res > tolerance)) &&
               line_search_n < max_n_line_searches)
          {
            if (first_step)
              {
                setup_dofs();
                initialize_system();
                evaluation_point = present_solution;
                assemble_system(first_step);
                solve(first_step);
                present_solution = newton_update;
                nonzero_constraints.distribute(present_solution);
                first_step       = false;
                evaluation_point = present_solution;
                assemble_rhs(first_step);
                current_res = system_rhs.l2_norm();

                evaluation_point = present_solution;
                assemble_system(first_step);
                solve(first_step);

// 为了确保我们的解决方案越来越接近精确的解决方案，我们让解决方案用权重 alpha 更新，使新的残差小于上一步的残差，这是在下面的循环中完成。这与  step-15  中使用的线搜索算法相同。

                for (double alpha = 1.0; alpha > 1e-5; alpha *= 0.5)
                  {
                    evaluation_point = present_solution;
                    evaluation_point.add(alpha, newton_update);
                    nonzero_constraints.distribute(evaluation_point);
                    assemble_rhs(first_step);
                    current_res = system_rhs.l2_norm();
                    std::cout << "  alpha: " << std::setw(10) << alpha
                              << std::setw(0) << "  residual: " << current_res
                              << std::endl;
                    if (current_res < last_res)
                      break;
                  }
                {
                  present_solution = evaluation_point;
                  std::cout << "  number of line searches: " << line_search_n
                            << "  residual: " << current_res << std::endl;
                  last_res = current_res;
                }
                ++line_search_n;
              }

            if (output_result)
              {
                output_results(max_n_line_searches * refinement_n +
                               line_search_n);

                if (current_res <= tolerance)
                  process_solution(refinement_n);
              }
          }

        if (refinement_n < max_n_refinements)
          {
            refine_mesh();
          }
      }
  }
// @sect4{StationaryNavierStokes::compute_initial_guess}

// 这个函数将通过使用延续法为我们提供一个初始猜测，正如我们在介绍中讨论的那样。雷诺数被逐级增加 step- ，直到我们达到目标值。通过实验，斯托克斯的解足以成为雷诺数为1000的NSE的初始猜测，所以我们从这里开始。 为了确保前一个问题的解决方案与下一个问题足够接近，步长必须足够小。

  template <int dim>
  void StationaryNavierStokes<dim>::compute_initial_guess(double step_size)
  {
    const double target_Re = 1.0 / viscosity;

    bool is_initial_step = true;

    for (double Re = 1000.0; Re < target_Re;
         Re        = std::min(Re + step_size, target_Re))
      {
        viscosity = 1.0 / Re;
        std::cout << "Searching for initial guess with Re = " << Re
                  << std::endl;
        newton_iteration(1e-12, 50, 0, is_initial_step, false);
        is_initial_step = false;
      }
  }
// @sect4{StationaryNavierStokes::output_results}

// 这个函数与 step-22 中的函数相同，只是我们为输出文件选择了一个同时包含雷诺数（即当前环境下的粘度的倒数）的名称。

  template <int dim>
  void StationaryNavierStokes<dim>::output_results(
    const unsigned int output_index) const
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(present_solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches();

    std::ofstream output(std::to_string(1.0 / viscosity) + "-solution-" +
                         Utilities::int_to_string(output_index, 4) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{StationaryNavierStokes::process_solution}

// 在我们的测试案例中，我们不知道分析解。该函数输出沿 $x=0.5$ 和 $0 \leq y \leq 1$ 的速度分量，以便与文献中的数据进行比较。

  template <int dim>
  void StationaryNavierStokes<dim>::process_solution(unsigned int refinement)
  {
    std::ofstream f(std::to_string(1.0 / viscosity) + "-line-" +
                    std::to_string(refinement) + ".txt");
    f << "# y u_x u_y" << std::endl;

    Point<dim> p;
    p(0) = 0.5;
    p(1) = 0.5;

    f << std::scientific;

    for (unsigned int i = 0; i <= 100; ++i)
      {
        p(dim - 1) = i / 100.0;

        Vector<double> tmp_vector(dim + 1);
        VectorTools::point_value(dof_handler, present_solution, p, tmp_vector);
        f << p(dim - 1);

        for (int j = 0; j < dim; j++)
          f << " " << tmp_vector(j);
        f << std::endl;
      }
  }
// @sect4{StationaryNavierStokes::run}

// 这是本程序的最后一步。在这一部分，我们分别生成网格和运行其他函数。最大细化度可以通过参数来设置。

  template <int dim>
  void StationaryNavierStokes<dim>::run(const unsigned int refinement)
  {
    GridGenerator::hyper_cube(triangulation);
    triangulation.refine_global(5);

    const double Re = 1.0 / viscosity;

// 如果粘度小于 $1/1000$ ，我们必须首先通过延续法搜索初始猜测。我们应该注意的是，搜索总是在初始网格上进行的，也就是这个程序中的 $8 \times 8$ 网格。之后，我们只需做与粘度大于 $1/1000$ 时相同的工作：运行牛顿迭代，细化网格，转移解决方案，并重复。

    if (Re > 1000.0)
      {
        std::cout << "Searching for initial guess ..." << std::endl;
        const double step_size = 2000.0;
        compute_initial_guess(step_size);
        std::cout << "Found initial guess." << std::endl;
        std::cout << "Computing solution with target Re = " << Re << std::endl;
        viscosity = 1.0 / Re;
        newton_iteration(1e-12, 50, refinement, false, true);
      }
    else
      {

// 当粘度大于1/1000时，斯托克斯方程的解作为初始猜测已经足够好。如果是这样，我们就不需要用延续法来搜索初始猜测了。牛顿迭代可以直接开始。

        newton_iteration(1e-12, 50, refinement, true, true);
      }
  }
} // namespace Step57

int main()
{
  try
    {
      using namespace Step57;

      StationaryNavierStokes<2> flow(/* degree = */

1);
      flow.run(4);
    }
  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
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/utilities/mesh_data.h"
#include "geometrycentral/surface/edge_length_geometry.h"
#include "geometrycentral/surface/heat_method_distance.h"
#include <Eigen/Core>
#include <stdio.h>
#include <iostream>
#include <math.h>

using namespace geometrycentral;
using namespace geometrycentral::surface;

extern "C" {

  int main(){
    // create quad with two triangles in CCW order
    //
    //   0--3
    //   | /|
    //   |/ |
    //   1--2
    //

    const size_t F = 2;
    Eigen::MatrixX3i faces(F, 3);
    faces << 0, 1, 3,
             1, 2, 3;

    Eigen::MatrixX3d edges(F, 3);
    edges << 1, M_SQRT2, 1,
             1, 1, M_SQRT2;

    std::cout << "Faces:\n" << faces << "\n";
    std::cout << "Edges:\n" << edges << "\n";

    // create surface mesh
    std::unique_ptr<ManifoldSurfaceMesh> mesh;
    mesh.reset(new ManifoldSurfaceMesh(faces));
    mesh->compress();
    mesh->printStatistics();

    // create geometry data
    EdgeData<double> edgeLengths(*mesh);
    for(size_t i = 0; i < faces.rows(); ++i){
      Face f = mesh->face(i);
      Halfedge he = f.halfedge(); edgeLengths[he.edge()] = edges(i, 0);
      he = he.next(); edgeLengths[he.edge()] = edges(i, 1);
      he = he.next(); edgeLengths[he.edge()] = edges(i, 2);
    }
    std::cout << "Edges:\n" << edgeLengths.raw() << "\n";
    for(Edge e : mesh->edges()){
      printf("Edge #%zu = %g\n", e.getIndex(), edgeLengths[e]);
    }
    std::unique_ptr<EdgeLengthGeometry> geometry;
    geometry.reset(new EdgeLengthGeometry(*mesh, edgeLengths));

    std::cout << "Precomputation\n";

    // create heat method distance solver (precomputation happens here)
    std::unique_ptr<HeatMethodDistanceSolver> heatSolver;
    heatSolver.reset(new HeatMethodDistanceSolver(*geometry));

    std::cout << "Solve\n";

    // solve for distance from first vertex
    const Vertex v = mesh->vertex(0);
    VertexData<double> distToSource = heatSolver->computeDistance(v);
    std::cout << "Distances:\n" << distToSource.raw() << "\n";

    return 0;
  }

}


#include <tdp/testing/testing.h>
#include <iostream>
#include <Eigen/Dense>
#include <tdp/manifold/SO3.h>
#include <tdp/manifold/rotation.h>
#include <tdp/manifold/SO3.h>
#include <tdp/eigen/dense.h>

using namespace tdp;

TEST(SO3, deriv) {
  float eps = 1e-3;

  for (size_t i=0; i<100; ++i) {
    Eigen::Vector3f p_c = Eigen::Vector3f::Random();
    tdp::SO3f T_wc = tdp::SO3f::Random();
    for (size_t j=0; j<3; ++j) {
      Eigen::Matrix<float,3,1> delta = Eigen::Matrix<float,3,1>::Zero();
      delta(j) = eps;
      tdp::SO3f T_wcDelta = T_wc.Exp(delta);
      Eigen::Vector3f diffGt = T_wcDelta*p_c - T_wc*p_c ;
      Eigen::Matrix<float,3,3> J = -T_wc.matrix()*tdp::SO3f::invVee(p_c);
      Eigen::Vector3f diffJ = J*delta;
      std::cout << j << ": " << (diffGt-diffJ).norm() << std::endl;
//        << ";\t" << diffGt.transpose() << " " << diffJ.transpose() << std::endl;
    }
  }
}

TEST(SO3, derivofInverse) {
  float eps = 1e-3;

  for (size_t i=0; i<100; ++i) {
    Eigen::Vector3f p_w = Eigen::Vector3f::Random();
    tdp::SO3f T_wc = tdp::SO3f::Random();
    for (size_t j=0; j<3; ++j) {
      Eigen::Matrix<float,3,1> delta = Eigen::Matrix<float,3,1>::Zero();
      delta(j) = eps;
      tdp::SO3f T_wcDelta = T_wc.Exp(delta);
      Eigen::Vector3f diffGt = T_wcDelta.Inverse()*p_w- T_wc.Inverse()*p_w;
      Eigen::Matrix<float,3,3> J = tdp::SO3f::invVee(T_wc.Inverse()*p_w);
      Eigen::Vector3f diffJ = J*delta;
      std::cout << j << ": " << (diffGt-diffJ).norm()
        << ";\t" << diffGt.transpose() << " " << diffJ.transpose() << std::endl;
    }
  }
}

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


//
//  cal_BTDB.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/6/18.
//
//

#ifndef cal_BTDB_hpp
#define cal_BTDB_hpp

#include <stdio.h>
#include <Eigen/Eigen>
using namespace Eigen;
void cal_BTDB (MatrixXd coord,double E, double nu,MatrixXd &B_T_D_B);

#endif /* cal_BTDB_hpp */


/**
 * @file    sfm.hpp
 * @brief   This provides the implementation of structure-from-motion algorithm
 * @author  Shubham Shrivastava
 */

#ifndef SFM_H_
#define SFM_H_

#include <Eigen/Dense>
#include "tapl/common/common.hpp"
#include "tapl/optim/gaussNewton.hpp"
#include "tapl/viz/visualization.hpp"

namespace tapl {
    namespace cve {

        /**< Structure-from-Motion Pipeline */
        class StructureFromMotion {
        private:
            // vector of images
            std::vector<cv::Mat> images;
            // camera intrinsic matrix
            cv::Mat K = cv::Mat::eye(3, 3, CV_32FC1);
            // camera distortion matrix
            cv::Mat dist_coeff = cv::Mat::zeros(4, 1, CV_32FC1);
            // min and max XYZ
            std::vector<float> minXYZ;
            std::vector<float> maxXYZ;
            // verbose
            bool verbose;
            // triangulated 3D points in each image local coordinate systems
            std::vector<std::vector<tapl::Point3d>> points3d_local;
            // triangulated 3D points in the origin's (first image) coordinate systems
            std::vector<tapl::Point3d> points3d_global;
            // bundle size
            uint16_t bundle_size=2;
            // Gauss-Newton Optimizer
            tapl::optim::GaussNewtonOptimizer gnOptim;

            /**
             * @brief Linear estimate of the 3d point
             *
             * @param[in] point2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras
             *
             * @return estimated 3d point
             */
            tapl::Point3d linearEstimate3dPt( const std::vector<tapl::Point2d> &point2d,
                                              const std::vector<Eigen::MatrixXd> &projectionMatrices );

            /**
             * @brief Non-linear estimate of the 3d point
             *
             * @param[in] point2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras
             * @param[out] reprojectionErrors pair of pre-optimization and post-optimization
             *                                  reprojection errors
             * @param[in] nIterations maximum number of iterations for non-linear optimization
             * @param[in] reprErrorThresh reprojection error threshold for non-linear optimization
             *
             * @return estimated 3d point
             */
            tapl::Point3d nonLinearEstimate3dPt( const std::vector<tapl::Point2d> &point2d,
                                                 const std::vector<Eigen::MatrixXd> &projectionMatrices,
                                                 std::pair<std::vector<float>,std::vector<float>> &reprojectionErrors,
                                                 const uint16_t nIterations=1000,
                                                 const float reprErrorThresh=2.0 );

            /**
             * @brief Estimate R, T, and triengulated points
             *
             * @param[in] E essential matrix relating the first and the second camera
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMat1 projection matrices of the first camera
             * @param[in] maxXYZ maximum values of x, y, and z to be considered
             * @param[in] maxReprojectionErr maximum absolute reprojection error in pixel
             *
             * @return pair of RT and triangulated points
             */
             std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>> computeSFM(
                                        const Eigen::MatrixXd &E,
                                        const std::vector<std::vector<tapl::Point2d>> &points2d,
                                        const Eigen::MatrixXd &projectionMat1,
                                        const float &maxReprojectionErr=50.0 ) ;

        public:
            /**
            * @brief This function initializes the structure-from-motion module
            *
            * @param[in] imgs images from which structure-from-motion is to be computed
            * @param[in] K camera intrinsic matrix
            */
            StructureFromMotion( const std::vector<cv::Mat> &images,
                                 const cv::Mat &K,
                                 const std::vector<float> &minXYZ={0.0, 0.0, 0.0},
                                 const std::vector<float> &maxXYZ={30.0, 30.0, 30.0},
                                 const bool verbose=true );

            /**
            * @brief This function performs structure-from-motion given a set of camera frames
            *
            * @param[out] points point-cloud corresponding to keypoints in the first camera's coordinate frame
            * @param[out] poses poses of each camera frame
            * @param[out] framePairs camera pairs with associated info such as triangulated points
            *
            * @return tapl::SUCCESS if success
            * @return tapl::FAILURE if failure
            */
            tapl::ResultCode process( std::vector<tapl::Point3dColor> &points,
                                      std::vector<tapl::Pose6dof> &poses,
                                      std::vector<tapl::CameraPairs> &framePairs);

        };
    }
}

#endif /* SFM_H_ */