/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/sigma_point.h>
#include <BayesFilters/directional_statistics.h>

#include <Eigen/SVD>

using namespace bfl;
using namespace bfl::directional_statistics;
using namespace bfl::sigma_point;
using namespace Eigen;

bfl::sigma_point::UTWeight::UTWeight
(
    std::size_t n,
    const double alpha,
    const double beta,
    const double kappa
) :
    mean((2 * n) + 1),
    covariance((2 * n) + 1)
{
    unscented_weights(n, alpha, beta, kappa, mean, covariance, c);
}

void bfl::sigma_point::unscented_weights
(
    const std::size_t n,
    const double alpha,
    const double beta,
    const double kappa,
    Ref<VectorXd> weight_mean,
    Ref<VectorXd> weight_covariance,
    double& c
)
{
    double lambda = std::pow(alpha, 2.0) * (n + kappa) - n;

    for (int j = 0; j < ((2 * n) + 1); ++j)
    {
        if (j == 0)
        {
            weight_mean(j)       = lambda / (n + lambda);
            weight_covariance(j) = lambda / (n + lambda) + (1 - std::pow(alpha, 2.0) + beta);
        }
        else
        {
            weight_mean(j)       = 1 / (2 * (n + lambda));
            weight_covariance(j) = weight_mean(j);
        }
    }

    c = n + lambda;
}

MatrixXd bfl::sigma_point::sigma_point(const GaussianMixture& state, const double c)
{
    MatrixXd sigma_points(state.dim, ((state.dim * 2) + 1) * state.components);

    for (std::size_t i = 0; i < state.components; i++)
    {
        JacobiSVD<MatrixXd> svd = state.covariance(i).jacobiSvd(ComputeThinU);

        MatrixXd A = svd.matrixU() * svd.singularValues().cwiseSqrt().asDiagonal();

        Ref<MatrixXd> sp = sigma_points.middleCols(((state.dim * 2) + 1) * i, ((state.dim * 2) + 1));

        sp << VectorXd::Zero(state.dim), std::sqrt(c) * A, -std::sqrt(c) * A;

        if (state.dim_linear > 0)
            sp.topRows(state.dim_linear).colwise() += state.mean(i).topRows(state.dim_linear);

        if (state.dim_circular > 0)
            sp.middleRows(state.dim_linear, state.dim_circular) = directional_add(sp.middleRows(state.dim_linear, state.dim_circular), state.mean(i).middleRows(state.dim_linear, state.dim_circular));

        if (state.dim_noise > 0)
            sp.bottomRows(state.dim_noise).colwise() += state.mean(i).bottomRows(state.dim_noise);
    }

    return sigma_points;
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& input,
    const UTWeight& weight,
    FunctionEvaluation function
)
{
    /* Sample sigma points. */
    MatrixXd input_sigma_points = sigma_point::sigma_point(input, weight.c);

    /* Propagate sigma points */
    Data fun_data;
    bool valid_fun_data;
    bfl::sigma_point::OutputSize output_size;
    std::tie(valid_fun_data, fun_data, output_size) = function(input_sigma_points);

    /* Stop here if function evaluation failed. */
    if (!valid_fun_data)
        return std::make_tuple(false, GaussianMixture(), MatrixXd(0, 0));

    /* For now casting Data to MatrixXd. */
    MatrixXd prop_sigma_points = bfl::any::any_cast<MatrixXd&&>(std::move(fun_data));

    /* Initialize transformed gaussian. */
    GaussianMixture output(input.components, output_size.first, output_size.second);

    /* Initialize cross covariance matrix. */
    MatrixXd cross_covariance(input.dim, output.dim * output.components);

    /* Process all the components of the mixture. */
    std::size_t base = ((input.dim * 2) + 1);
    for (std::size_t i = 0; i < input.components; i++)
    {
        Ref<MatrixXd> input_sigma_points_i = input_sigma_points.middleCols(base * i, base);
        Ref<MatrixXd> prop_sigma_points_i = prop_sigma_points.middleCols(base * i, base);

        /* Evaluate the mean. */
        output.mean(i).topRows(output_size.first).noalias() = prop_sigma_points_i.topRows(output_size.first) * weight.mean;
        output.mean(i).bottomRows(output_size.second) = directional_mean(prop_sigma_points_i.bottomRows(output_size.second), weight.mean);

        /* Evaluate the covariance. */
        prop_sigma_points_i.topRows(output_size.first).colwise() -= output.mean(i).topRows(output_size.first);
        prop_sigma_points_i.bottomRows(output_size.second) = directional_sub(prop_sigma_points_i.bottomRows(output_size.second), output.mean(i).bottomRows(output_size.second));
        output.covariance(i).noalias() = prop_sigma_points_i * weight.covariance.asDiagonal() * prop_sigma_points_i.transpose();

        /* Evaluate the input-output cross covariance matrix
           (noise components in the input are not considered). */
        Ref<MatrixXd> cross_covariance_i = cross_covariance.middleCols(output.dim * i, output.dim);
        input_sigma_points_i.topRows(input.dim_linear).colwise() -= input.mean(i).topRows(input.dim_linear);
        input_sigma_points_i.middleRows(input.dim_linear, input.dim_circular) = directional_sub(input_sigma_points_i.middleRows(input.dim_linear, input.dim_circular), input.mean(i).middleRows(input.dim_linear, input.dim_circular));
        cross_covariance_i.noalias() = input_sigma_points_i.topRows(input.dim_linear + input.dim_circular) * weight.covariance.asDiagonal() * prop_sigma_points_i.transpose();
    }

    return std::make_tuple(true, output, cross_covariance);
}

std::pair<GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    StateModel& state_model
)
{
    FunctionEvaluation f = [&state_model](const Ref<const MatrixXd>& state)
                           {
                               MatrixXd tmp(state.rows(), state.cols());

                               state_model.motion(state, tmp);

                               return std::make_tuple(true, std::move(tmp), state_model.getOutputSize());
                           };
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(std::ignore, output, cross_covariance) = unscented_transform(state, weight, f);

    return std::make_pair(output, cross_covariance);
}

std::pair<GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    AdditiveStateModel& state_model
)
{
    FunctionEvaluation f = [&state_model](const Ref<const MatrixXd>& state)
                           {
                               MatrixXd tmp(state.rows(), state.cols());

                               state_model.propagate(state, tmp);

                               return std::make_tuple(true, std::move(tmp), state_model.getOutputSize());
                           };

    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(std::ignore, output, cross_covariance) = unscented_transform(state, weight, f);

    /* In the additive case the covariance matrix is augmented with the noise
       covariance matrix. */
    for(std::size_t i = 0; i < state.components; i++)
        output.covariance(i) += state_model.getNoiseCovarianceMatrix();

    return std::make_pair(output, cross_covariance);
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    MeasurementModel& meas_model
)
{
    FunctionEvaluation f = [&meas_model](const Ref<const MatrixXd>& state)
                           {
                               bool valid_prediction;
                               bfl::Data prediction;

                               std::tie(valid_prediction, prediction) = meas_model.predictedMeasure(state);

                               return std::make_tuple(valid_prediction, std::move(prediction), meas_model.getOutputSize());
                           };

    bool valid;
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(valid, output, cross_covariance) = unscented_transform(state, weight, f);

    return std::make_tuple(valid, output, cross_covariance);
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    AdditiveMeasurementModel& meas_model
)
{
    FunctionEvaluation f = [&meas_model](const Ref<const MatrixXd>& state)
                           {
                               bool valid_prediction;
                               bfl::Data prediction;

                               std::tie(valid_prediction, prediction) = meas_model.predictedMeasure(state);

                               return std::make_tuple(valid_prediction, std::move(prediction), meas_model.getOutputSize());
                           };

    bool valid;
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(valid, output, cross_covariance) = unscented_transform(state, weight, f);

    /* In the additive case the covariance matrix is augmented with the noise
       covariance matrix. */
    MatrixXd noise_cov;
    std::tie(std::ignore, noise_cov) = meas_model.getNoiseCovarianceMatrix();
    for (std::size_t i = 0; i < state.components; i++)
        output.covariance(i) += noise_cov;

    return std::make_tuple(valid, output, cross_covariance);
}


#include<iostream>
#include<stdio.h>
#include<vector>
#include<cmath>
#include "ray.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <fstream>
using namespace std;
using namespace Eigen;

Vector3d operator*(double s, const Vector3d &v) {
	return Vector3d(s*v(0), s*v(1), s*v(2));
}

//Triangle with verticies pos, pos + p and pos + q.
class triangle: public shape {
	Vector3d p, q, normal;
	public:
	Vector3d pos;
	triangle(Vector3d np, Vector3d nq, Vector3d npos, colour &ncol, surface &s):
		p(np), q(nq), pos(npos) {
		col = ncol;
		surf = s;
		normal = p.cross(q);
	}

	Vector3d get_normal(const Vector3d ignore) const {
		return normal;
	}

	//Moller-Trumbore intersection algorithm
	double intersect(ray r) {
		Vector3d vec, T;
		double det, u, v, t;
		vec = r.dir.cross(q);
		det = p.dot(vec);
		//std::cout << "det = " << det << std::endl;
		//If det is zero then ray in plane of triangle
		if(det < INTERSECT_EPSILON && det > -INTERSECT_EPSILON) return 0.0;
		//Get ray from pos to ray origin
		T = r.origin - pos;
		u = T.dot(vec)/det;
		//Test u
		if(u < 0 || u > 1.0) return 0.0;
		//and v (+u)
		vec = T.cross(p);
		v = r.dir.dot(vec)/det;
		if(v < 0 || v + u > 1.0) return 0.0;
		//Intersect at origin + t*dir
		t = q.dot(vec)/det;
		//Check we're not behind the ray origin
		if(t < INTERSECT_EPSILON) return 0.0;
		//return ray intersect parameter: point = Origin + t*dir
		return t;
	}

};

class sphere: public shape {
	public:
	Vector3d pos;
	double radius;
	sphere(double nr, Vector3d np, colour &ncol, surface &s):
		pos(np), radius(nr){
		col = ncol;
		surf = s;
	}

	Vector3d get_normal(const Vector3d p) const {
		return p - pos;
	}

	double intersect(ray r) {
		double a, b, c, d, dsq, s1, s2;
		c = r.origin.dot(r.origin) + pos.dot(pos) - 2*r.origin.dot(pos) - radius*radius;
		b = 2 * r.dir.dot(r.origin - pos);
		a = r.dir.dot(r.dir);
		if(a < INTERSECT_EPSILON) {
			fprintf(stderr, "Warn: sphere::intersect: a < ITERSECT_EPSILON\n");
			return 0.0;
		}
		dsq = b*b - 4*a*c;
		if(dsq <= 0) {
			return 0;
		}
		d = std::sqrt(dsq);
		s1 = (-b+d)/(2*a);
		s2 = (-b-d)/(2*a);
		if(s1 < s2) {
			if(s1 > INTERSECT_EPSILON) {
				return s1;
			}
		}
		if(s2 > INTERSECT_EPSILON) {
			return s2;
		} else {
			return s1 > INTERSECT_EPSILON ? s1 : 0.0;
		}
	}

};

std::vector<shape*>* getMeshWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	surface shiny;
	shiny.reflection = 0.8;
	shiny.specular = 1.0;
	shiny.diffusion = 0.9;

	// w->push_back((shape*) (new sphere(2, Vector3d(0, 3, 15), red, shiny)));

		ifstream fin ;
					    // fin.open("/Users/Rachit/Documents/iqra_cg/tracer3/bumpy_cube.off");
					    fin.open("/Users/Rachit/Documents/iqra_cg/tracer3/bunny.off");

					    int nrows,nrows2;
					    string output;
					    if(fin.is_open())
					    {
					        fin >> output;
					        fin >> output;
					        nrows=std::stoi(output);
					        fin >> output;
					        nrows2 = std::stoi(output);
					        fin >> output;

					    }

					    Eigen::ArrayXXf X = Eigen::ArrayXXf::Zero(nrows,3);
					    Eigen::ArrayXXd Y = Eigen::ArrayXXd::Zero(nrows2,4);

					    if (fin.is_open())
					    {
					        for (int row = 0; row < nrows; row++)
					            for (int col = 0; col < 3; col++)
					            {
					                float item = 0.0;
					                fin >> item;
					                X(row, col) = item;
					            }

					    }
					   // cerr << "X = " << endl << X << endl;
					    if (fin.is_open())
					    {
					        for (int row = 0; row < nrows2; row++)
					            for (int col = 0; col < 4; col++)
					            {
					                float item = 0.0;
					                fin >> item;
					                Y(row, col) = item;
					            }
					        fin.close();
					    }
					    // cerr<<"Y ="<< endl << Y <<endl;

					    for(unsigned k=0 ; k < Y.rows();k++) {
					    	// if(k%100==0)
					    	// cerr<<"at row-->"<<k<<endl;
					    	w->push_back((shape*) (new triangle(
					    		   Vector3d(X(Y(k, 1), 0), X(Y(k, 1), 1), X(Y(k, 1), 2)),
                Vector3d(X(Y(k, 2), 0), X(Y(k, 2), 1), X(Y(k, 2), 2)),
                Vector3d(X(Y(k, 3), 0), X(Y(k, 3), 1), X(Y(k, 3), 2)),
                red,shiny
					    		)));

}

	//One sphere
	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}

std::vector<shape*>* partAWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;

	w->push_back((shape*) (new sphere(5, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), red, lambert)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partBWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;
	//Diffuse+specular
	surface specular;
	specular.reflection = 0.0;
	specular.specular = 1.0;
	specular.diffusion = 0.9;

	w->push_back((shape*) (new sphere(10, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), blue, specular)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partCWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;
	//Diffuse+specular
	surface specular;
	specular.reflection = 0.0;
	specular.specular = 1.0;
	specular.diffusion = 0.9;

	w->push_back((shape*) (new sphere(10, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), blue, specular)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partEWorld(int n_spheres) {

	surface shiny;
	shiny.reflection = 0.4;
	shiny.specular = 1.0;
	shiny.diffusion = 0.9;
	std::vector<shape*> *w = new std::vector<shape*>();
	int i, j, k;
	for(i=0; i<3; i++) {
	for(j=0; j<3; j++) {
	for(k=0; k<3; k++) {
		if(n_spheres > 0) {
			colour c = colour(i*127.5, 255-j*127.5, ((int) (127.5+(k*127.5)))%382);
			w->push_back((shape*) (new sphere(1, Vector3d(i*4-5.5, j*4-6, k*4+14), c, shiny)));
			n_spheres -= 1;
		}
	}
	}
	}
	return w;

}

void freeWorld(std::vector<shape*> *w) {
	while(w->size() > 0) {
		delete (w->back());
		w->pop_back();
	}
	delete w;
}

std::vector<light*>* getLights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	return l;
}
std::vector<light*>* partALights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	return l;
}
std::vector<light*>* partBLights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	l->push_back(new light(Vector3d(-15, -15, 5), colour(255, 255, 255)));

	return l;
}
std::vector<light*>* partELights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(-5, 3, 2), colour(255, 255, 255)));
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));

	return l;
}

void freeLights(std::vector<light*> *l) {
	while(l->size() > 0) {
		delete (l->back());
		l->pop_back();
	}
	delete l;
}


#ifndef CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_
#define CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_

#include <Eigen/SVD>

namespace cppmath
{
    /**
     * Calculates a SVD-based pseudo inverse matrix.
     * See:
     * - http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
     * - http://eigen.tuxfamily.org/index.php?title=FAQ#Is_there_a_method_to_compute_the_.28Moore-Penrose.29_pseudo_inverse_.3F
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    template< typename T >
    class PseudoInverseSVD: private Eigen::JacobiSVD< T >
    {
    public:
        /**
         * Constructor.
         *
         * \param matrix Matrix to compute the pseudo inverse from.
         * \param pinvThreshold Threshold to keep value as zero/nonzero (default: 1.0e-6).
         */
        PseudoInverseSVD( const T& matrix, float threshold = 1.0e-6 );

        virtual ~PseudoInverseSVD();

        /**
         * Computes the pseudo inverse.
         * The pseudo inverse is internally stored for further calculations.
         *
         * \return Reference to the internally stored pseudo inverse matrix.
         */
        const T& compute();

        /**
         * Computes the pseudo inverse.
         *
         * \param pinvmat Holds the pseudo inverse matrix after computation.
         */
        void compute( T* const pinvmat ) const;

        /**
         * Multiplies the pseudo inverse with a matrix.
         * The pseudo inverse is internally stored for further calculations.
         *
         * \param m Matrix
         * \return Result of pinv*m
         */
        T operator*( const T& m );

        /**
         *  Multiplies the pseudo inverse with a matrix.
         *
         * \param m Matrix
         * \return Result of pinv*m
         */
        T operator*( const T& m ) const;

    private:
        bool m_hasInverse; /**< Indicates if the internal inverse matrix is available. */

        T m_inverse; /**< Stores a computed inverse matrix. */

        const float m_threshold;
    };
} /* namespace cppmath */

// Load the implementation
#include "PseudoInverseSVD-impl.hpp"

#endif  // CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_


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

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

// 下面这块出场代码与 step-40 相同，可以在PETSc和Trilinos之间切换。

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

/* #define FORCE_USE_OF_TRILINOS */

namespace LA
{
#if defined(DEAL_II_WITH_PETSC) && !defined(DEAL_II_PETSC_WITH_COMPLEX) && \
  !(defined(DEAL_II_WITH_TRILINOS) && defined(FORCE_USE_OF_TRILINOS))
  using namespace dealii::LinearAlgebraPETSc;
#  define USE_PETSC_LA
#elif defined(DEAL_II_WITH_TRILINOS)
  using namespace dealii::LinearAlgebraTrilinos;
#else
#  error DEAL_II_WITH_PETSC or DEAL_II_WITH_TRILINOS required
#endif
} // namespace LA

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/solver_minres.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>

#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.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_values.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

#include <deal.II/base/utilities.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/index_set.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/distributed/grid_refinement.h>

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

namespace Step55
{
  using namespace dealii;
// @sect3{Linear solvers and preconditioners}

// 我们需要一些辅助类来表示我们在介绍中描述的求解器策略。

  namespace LinearSolvers
  {

// 这个类暴露了通过函数 InverseMatrix::vmult(). 应用给定矩阵的逆的动作，在内部，逆不是显式形成的。相反，一个带有CG的线性求解器被执行。这个类扩展了 step-22 中的InverseMatrix类，增加了一个指定预处理程序的选项，并允许在vmult函数中使用不同的矢量类型。

    template <class Matrix, class Preconditioner>
    class InverseMatrix : public Subscriptor
    {
    public:
      InverseMatrix(const Matrix &m, const Preconditioner &preconditioner);

      template <typename VectorType>
      void vmult(VectorType &dst, const VectorType &src) const;

    private:
      const SmartPointer<const Matrix> matrix;
      const Preconditioner &           preconditioner;
    };

    template <class Matrix, class Preconditioner>
    InverseMatrix<Matrix, Preconditioner>::InverseMatrix(
      const Matrix &        m,
      const Preconditioner &preconditioner)
      : matrix(&m)
      , preconditioner(preconditioner)
    {}

    template <class Matrix, class Preconditioner>
    template <typename VectorType>
    void
    InverseMatrix<Matrix, Preconditioner>::vmult(VectorType &      dst,
                                                 const VectorType &src) const
    {
      SolverControl solver_control(src.size(), 1e-8 * src.l2_norm());
      SolverCG<LA::MPI::Vector> cg(solver_control);
      dst = 0;

      try
        {
          cg.solve(*matrix, dst, src, preconditioner);
        }
      catch (std::exception &e)
        {
          Assert(false, ExcMessage(e.what()));
        }
    }

// 该类是一个简单的2x2矩阵的块状对角线预处理器的模板类。

    template <class PreconditionerA, class PreconditionerS>
    class BlockDiagonalPreconditioner : public Subscriptor
    {
    public:
      BlockDiagonalPreconditioner(const PreconditionerA &preconditioner_A,
                                  const PreconditionerS &preconditioner_S);

      void vmult(LA::MPI::BlockVector &      dst,
                 const LA::MPI::BlockVector &src) const;

    private:
      const PreconditionerA &preconditioner_A;
      const PreconditionerS &preconditioner_S;
    };

    template <class PreconditionerA, class PreconditionerS>
    BlockDiagonalPreconditioner<PreconditionerA, PreconditionerS>::
      BlockDiagonalPreconditioner(const PreconditionerA &preconditioner_A,
                                  const PreconditionerS &preconditioner_S)
      : preconditioner_A(preconditioner_A)
      , preconditioner_S(preconditioner_S)
    {}

    template <class PreconditionerA, class PreconditionerS>
    void BlockDiagonalPreconditioner<PreconditionerA, PreconditionerS>::vmult(
      LA::MPI::BlockVector &      dst,
      const LA::MPI::BlockVector &src) const
    {
      preconditioner_A.vmult(dst.block(0), src.block(0));
      preconditioner_S.vmult(dst.block(1), src.block(1));
    }

// @sect3{Problem setup}

// 下面的类代表测试问题的右手边和精确解。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide()
      : Function<dim>(dim + 1)
    {}

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  void RightHandSide<dim>::vector_value(const Point<dim> &p,
                                        Vector<double> &  values) const
  {
    const double R_x = p[0];
    const double R_y = p[1];

    const double pi  = numbers::PI;
    const double pi2 = pi * pi;
    values[0] =
      -1.0L / 2.0L * (-2 * sqrt(25.0 + 4 * pi2) + 10.0) *
        exp(R_x * (-2 * sqrt(25.0 + 4 * pi2) + 10.0)) -
      0.4 * pi2 * exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * cos(2 * R_y * pi) +
      0.1 * pow(-sqrt(25.0 + 4 * pi2) + 5.0, 2) *
        exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * cos(2 * R_y * pi);
    values[1] = 0.2 * pi * (-sqrt(25.0 + 4 * pi2) + 5.0) *
                  exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * sin(2 * R_y * pi) -
                0.05 * pow(-sqrt(25.0 + 4 * pi2) + 5.0, 3) *
                  exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * sin(2 * R_y * pi) /
                  pi;
    values[2] = 0;
  }

  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution()
      : Function<dim>(dim + 1)
    {}

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  void ExactSolution<dim>::vector_value(const Point<dim> &p,
                                        Vector<double> &  values) const
  {
    const double R_x = p[0];
    const double R_y = p[1];

    const double pi  = numbers::PI;
    const double pi2 = pi * pi;
    values[0] =
      -exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * cos(2 * R_y * pi) + 1;
    values[1] = (1.0L / 2.0L) * (-sqrt(25.0 + 4 * pi2) + 5.0) *
                exp(R_x * (-sqrt(25.0 + 4 * pi2) + 5.0)) * sin(2 * R_y * pi) /
                pi;
    values[2] =
      -1.0L / 2.0L * exp(R_x * (-2 * sqrt(25.0 + 4 * pi2) + 10.0)) -
      2.0 *
        (-6538034.74494422 +
         0.0134758939981709 * exp(4 * sqrt(25.0 + 4 * pi2))) /
        (-80.0 * exp(3 * sqrt(25.0 + 4 * pi2)) +
         16.0 * sqrt(25.0 + 4 * pi2) * exp(3 * sqrt(25.0 + 4 * pi2))) -
      1634508.68623606 * exp(-3.0 * sqrt(25.0 + 4 * pi2)) /
        (-10.0 + 2.0 * sqrt(25.0 + 4 * pi2)) +
      (-0.00673794699908547 * exp(sqrt(25.0 + 4 * pi2)) +
       3269017.37247211 * exp(-3 * sqrt(25.0 + 4 * pi2))) /
        (-8 * sqrt(25.0 + 4 * pi2) + 40.0) +
      0.00336897349954273 * exp(1.0 * sqrt(25.0 + 4 * pi2)) /
        (-10.0 + 2.0 * sqrt(25.0 + 4 * pi2));
  }

//  @sect3{The main program}

// 主类与  step-40  非常相似，只是矩阵和向量现在是块状的，而且我们为拥有的和相关的DoF存储一个  std::vector<IndexSet>  ，而不是一个IndexSet。我们正好有两个IndexSets，一个用于所有速度未知数，一个用于所有压力未知数。

  template <int dim>
  class StokesProblem
  {
  public:
    StokesProblem(unsigned int velocity_degree);

    void run();

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

    unsigned int velocity_degree;
    double       viscosity;
    MPI_Comm     mpi_communicator;

    FESystem<dim>                             fe;
    parallel::distributed::Triangulation<dim> triangulation;
    DoFHandler<dim>                           dof_handler;

    std::vector<IndexSet> owned_partitioning;
    std::vector<IndexSet> relevant_partitioning;

    AffineConstraints<double> constraints;

    LA::MPI::BlockSparseMatrix system_matrix;
    LA::MPI::BlockSparseMatrix preconditioner_matrix;
    LA::MPI::BlockVector       locally_relevant_solution;
    LA::MPI::BlockVector       system_rhs;

    ConditionalOStream pcout;
    TimerOutput        computing_timer;
  };

  template <int dim>
  StokesProblem<dim>::StokesProblem(unsigned int velocity_degree)
    : velocity_degree(velocity_degree)
    , viscosity(0.1)
    , mpi_communicator(MPI_COMM_WORLD)
    , fe(FE_Q<dim>(velocity_degree), dim, FE_Q<dim>(velocity_degree - 1), 1)
    , triangulation(mpi_communicator,
                    typename Triangulation<dim>::MeshSmoothing(
                      Triangulation<dim>::smoothing_on_refinement |
                      Triangulation<dim>::smoothing_on_coarsening))
    , dof_handler(triangulation)
    , pcout(std::cout,
            (Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
    , computing_timer(mpi_communicator,
                      pcout,
                      TimerOutput::summary,
                      TimerOutput::wall_times)
  {}

// Kovasnay流定义在域[-0.5, 1.5]^2上，我们通过将最小和最大值传递给 GridGenerator::hyper_cube. 来创建这个域。
  template <int dim>
  void StokesProblem<dim>::make_grid()
  {
    GridGenerator::hyper_cube(triangulation, -0.5, 1.5);
    triangulation.refine_global(3);
  }
// @sect3{System Setup}

// 与 step-40 相比，块矩阵和向量的构造是新的，与 step-22 这样的串行代码相比也是不同的，因为我们需要提供属于我们处理器的行的集合。

  template <int dim>
  void StokesProblem<dim>::setup_system()
  {
    TimerOutput::Scope t(computing_timer, "setup");

    dof_handler.distribute_dofs(fe);

// 将所有的昏暗速度放入0区块，压力放入1区块，然后按区块重新排列未知数。最后计算每块有多少个未知数。

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

    const std::vector<types::global_dof_index> dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, stokes_sub_blocks);

    const unsigned int n_u = dofs_per_block[0];
    const unsigned int n_p = dofs_per_block[1];

    pcout << "   Number of degrees of freedom: " << dof_handler.n_dofs() << " ("
          << n_u << '+' << n_p << ')' << std::endl;

// 我们根据我们想要创建块状矩阵和向量的方式，将本地拥有的和本地相关的DoF的IndexSet分割成两个IndexSets。

    owned_partitioning.resize(2);
    owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_u);
    owned_partitioning[1] =
      dof_handler.locally_owned_dofs().get_view(n_u, n_u + n_p);

    IndexSet locally_relevant_dofs;
    DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
    relevant_partitioning.resize(2);
    relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_u);
    relevant_partitioning[1] = locally_relevant_dofs.get_view(n_u, n_u + n_p);

// 设置边界条件和悬挂节点的约束与  step-40  相同。尽管我们没有任何悬空节点，因为我们只进行全局细化，但把这个函数调用放进去仍然是个好主意，以备以后引入自适应细化。

    {
      constraints.reinit(locally_relevant_dofs);

      FEValuesExtractors::Vector velocities(0);
      DoFTools::make_hanging_node_constraints(dof_handler, constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               ExactSolution<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));
      constraints.close();
    }

// 现在我们根据BlockDynamicSparsityPattern来创建系统矩阵。我们知道我们不会有不同速度分量之间的耦合（因为我们使用的是拉普拉斯而不是变形张量），也不会有压力与其测试函数之间的耦合，所以我们使用一个表来将这个耦合信息传达给  DoFTools::make_sparsity_pattern.  。
    {
      system_matrix.clear();

      Table<2, DoFTools::Coupling> coupling(dim + 1, dim + 1);
      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if (c == dim && d == dim)
            coupling[c][d] = DoFTools::none;
          else if (c == dim || d == dim || c == d)
            coupling[c][d] = DoFTools::always;
          else
            coupling[c][d] = DoFTools::none;

      BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);

      DoFTools::make_sparsity_pattern(
        dof_handler, coupling, dsp, constraints, false);

      SparsityTools::distribute_sparsity_pattern(
        dsp,
        dof_handler.locally_owned_dofs(),
        mpi_communicator,
        locally_relevant_dofs);

      system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
    }

// 先决条件矩阵有不同的耦合（我们只在1,1块中填入质量矩阵），否则这段代码与上面的system_matrix的构造是相同的。

    {
      preconditioner_matrix.clear();

      Table<2, DoFTools::Coupling> coupling(dim + 1, dim + 1);
      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if (c == dim && d == dim)
            coupling[c][d] = DoFTools::always;
          else
            coupling[c][d] = DoFTools::none;

      BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);

      DoFTools::make_sparsity_pattern(
        dof_handler, coupling, dsp, constraints, false);
      SparsityTools::distribute_sparsity_pattern(
        dsp,
        Utilities::MPI::all_gather(mpi_communicator,
                                   dof_handler.locally_owned_dofs()),
        mpi_communicator,
        locally_relevant_dofs);
      preconditioner_matrix.reinit(owned_partitioning,

// owned_partitioning。

                                   dsp,
                                   mpi_communicator);
    }

// 最后，我们以正确的尺寸构建块状向量。带有两个 std::vector<IndexSet> 的函数调用将创建一个重影向量。

    locally_relevant_solution.reinit(owned_partitioning,
                                     relevant_partitioning,
                                     mpi_communicator);
    system_rhs.reinit(owned_partitioning, mpi_communicator);
  }

//  @sect3{Assembly}

// 这个函数将系统矩阵、预处理矩阵和右手边集合起来。其代码非常标准。

  template <int dim>
  void StokesProblem<dim>::assemble_system()
  {
    TimerOutput::Scope t(computing_timer, "assembly");

    system_matrix         = 0;
    preconditioner_matrix = 0;
    system_rhs            = 0;

    const QGauss<dim> quadrature_formula(velocity_degree + 1);

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

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

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

    const RightHandSide<dim>    right_hand_side;
    std::vector<Vector<double>> rhs_values(n_q_points, Vector<double>(dim + 1));

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

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    const FEValuesExtractors::Vector     velocities(0);
    const FEValuesExtractors::Scalar     pressure(dim);

    for (const auto &cell : dof_handler.active_cell_iterators())
      if (cell->is_locally_owned())
        {
          cell_matrix  = 0;
          cell_matrix2 = 0;
          cell_rhs     = 0;

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

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                {
                  for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    {
                      cell_matrix(i, j) +=
                        (viscosity *
                           scalar_product(grad_phi_u[i], grad_phi_u[j]) -
                         div_phi_u[i] * phi_p[j] - phi_p[i] * div_phi_u[j]) *
                        fe_values.JxW(q);

                      cell_matrix2(i, j) += 1.0 / viscosity * phi_p[i] *
                                            phi_p[j] * fe_values.JxW(q);
                    }

                  const unsigned int component_i =
                    fe.system_to_component_index(i).first;
                  cell_rhs(i) += fe_values.shape_value(i, q) *
                                 rhs_values[q](component_i) * fe_values.JxW(q);
                }
            }

          cell->get_dof_indices(local_dof_indices);
          constraints.distribute_local_to_global(cell_matrix,
                                                 cell_rhs,
                                                 local_dof_indices,
                                                 system_matrix,
                                                 system_rhs);

          constraints.distribute_local_to_global(cell_matrix2,
                                                 local_dof_indices,
                                                 preconditioner_matrix);
        }

    system_matrix.compress(VectorOperation::add);
    preconditioner_matrix.compress(VectorOperation::add);
    system_rhs.compress(VectorOperation::add);
  }

//  @sect3{Solving}

// 这个函数用MINRES求解线性系统，如介绍中所述，对两个对角线块使用块状对角线预处理和AMG。预处理程序对0,0块应用v循环，对1,1块应用质量矩阵的CG（Schur补充）。

  template <int dim>
  void StokesProblem<dim>::solve()
  {
    TimerOutput::Scope t(computing_timer, "solve");

    LA::MPI::PreconditionAMG prec_A;
    {
      LA::MPI::PreconditionAMG::AdditionalData data;

#ifdef USE_PETSC_LA
      data.symmetric_operator = true;
#endif
      prec_A.initialize(system_matrix.block(0, 0), data);
    }

    LA::MPI::PreconditionAMG prec_S;
    {
      LA::MPI::PreconditionAMG::AdditionalData data;

#ifdef USE_PETSC_LA
      data.symmetric_operator = true;
#endif
      prec_S.initialize(preconditioner_matrix.block(1, 1), data);
    }

// InverseMatrix用于解决质量矩阵的问题。

    using mp_inverse_t = LinearSolvers::InverseMatrix<LA::MPI::SparseMatrix,
                                                      LA::MPI::PreconditionAMG>;
    const mp_inverse_t mp_inverse(preconditioner_matrix.block(1, 1), prec_S);

// 这是在上面定义的各个块的预处理的基础上构造的块预处理。

    const LinearSolvers::BlockDiagonalPreconditioner<LA::MPI::PreconditionAMG,
                                                     mp_inverse_t>
      preconditioner(prec_A, mp_inverse);

// 有了这些，我们终于可以设置线性求解器并求解该系统。

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

    SolverMinRes<LA::MPI::BlockVector> solver(solver_control);

    LA::MPI::BlockVector distributed_solution(owned_partitioning,
                                              mpi_communicator);

    constraints.set_zero(distributed_solution);

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

    pcout << "   Solved in " << solver_control.last_step() << " iterations."
          << std::endl;

    constraints.distribute(distributed_solution);

// 像在  step-56  中一样，我们减去平均压力，以便与我们的参考解决方案进行误差计算，该解决方案的平均值为零。

    locally_relevant_solution = distributed_solution;
    const double mean_pressure =
      VectorTools::compute_mean_value(dof_handler,
                                      QGauss<dim>(velocity_degree + 2),
                                      locally_relevant_solution,
                                      dim);
    distributed_solution.block(1).add(-mean_pressure);
    locally_relevant_solution.block(1) = distributed_solution.block(1);
  }

//  @sect3{The rest}

// 其余处理网格细化、输出和主循环的代码非常标准。

  template <int dim>
  void StokesProblem<dim>::refine_grid()
  {
    TimerOutput::Scope t(computing_timer, "refine");

    triangulation.refine_global();
  }

  template <int dim>
  void StokesProblem<dim>::output_results(const unsigned int cycle) const
  {
    {
      const ComponentSelectFunction<dim> pressure_mask(dim, dim + 1);
      const ComponentSelectFunction<dim> velocity_mask(std::make_pair(0, dim),
                                                       dim + 1);

      Vector<double> cellwise_errors(triangulation.n_active_cells());
      QGauss<dim>    quadrature(velocity_degree + 2);

      VectorTools::integrate_difference(dof_handler,
                                        locally_relevant_solution,
                                        ExactSolution<dim>(),
                                        cellwise_errors,
                                        quadrature,
                                        VectorTools::L2_norm,
                                        &velocity_mask);

      const double error_u_l2 =
        VectorTools::compute_global_error(triangulation,
                                          cellwise_errors,
                                          VectorTools::L2_norm);

      VectorTools::integrate_difference(dof_handler,
                                        locally_relevant_solution,
                                        ExactSolution<dim>(),
                                        cellwise_errors,
                                        quadrature,
                                        VectorTools::L2_norm,
                                        &pressure_mask);

      const double error_p_l2 =
        VectorTools::compute_global_error(triangulation,
                                          cellwise_errors,
                                          VectorTools::L2_norm);

      pcout << "error: u_0: " << error_u_l2 << " p_0: " << error_p_l2
            << std::endl;
    }

    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(locally_relevant_solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);

    LA::MPI::BlockVector interpolated;
    interpolated.reinit(owned_partitioning, MPI_COMM_WORLD);
    VectorTools::interpolate(dof_handler, ExactSolution<dim>(), interpolated);

    LA::MPI::BlockVector interpolated_relevant(owned_partitioning,
                                               relevant_partitioning,
                                               MPI_COMM_WORLD);
    interpolated_relevant = interpolated;
    {
      std::vector<std::string> solution_names(dim, "ref_u");
      solution_names.emplace_back("ref_p");
      data_out.add_data_vector(interpolated_relevant,
                               solution_names,
                               DataOut<dim>::type_dof_data,
                               data_component_interpretation);
    }

    Vector<float> subdomain(triangulation.n_active_cells());
    for (unsigned int i = 0; i < subdomain.size(); ++i)
      subdomain(i) = triangulation.locally_owned_subdomain();
    data_out.add_data_vector(subdomain, "subdomain");

    data_out.build_patches();

    data_out.write_vtu_with_pvtu_record(
      "./", "solution", cycle, mpi_communicator, 2);
  }

  template <int dim>
  void StokesProblem<dim>::run()
  {
#ifdef USE_PETSC_LA
    pcout << "Running using PETSc." << std::endl;
#else
    pcout << "Running using Trilinos." << std::endl;
#endif
    const unsigned int n_cycles = 5;
    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        pcout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          make_grid();
        else
          refine_grid();

        setup_system();

        assemble_system();
        solve();

        if (Utilities::MPI::n_mpi_processes(mpi_communicator) <= 32)
          {
            TimerOutput::Scope t(computing_timer, "output");
            output_results(cycle);
          }

        computing_timer.print_summary();
        computing_timer.reset();

        pcout << std::endl;
      }
  }
} // namespace Step55

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

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

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

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

  return 0;
}


/*
 * Copyright 2017 Mahdi Khanalizadeh
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef HEADER_EXT_VECTOR3_HPP_INCLUDED
#define HEADER_EXT_VECTOR3_HPP_INCLUDED

#include <cmath>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Vector3 :
		boost::equality_comparable<Vector3<T>,
		boost::additive<Vector3<T>,
		boost::multiplicative<Vector3<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x;
		Type y;
		Type z;

		Vector3() : x{0}, y{0}, z{0} {}
		Vector3(Type x_, Type y_, Type z_) : x{x_}, y{y_}, z{z_} {}
		template <typename U>
		Vector3(Vector3<U> const& v) : x{v.x}, y{v.y}, z{v.z} {}
	};

	using Vector3f = Vector3<float>;
	using Vector3d = Vector3<double>;
	using Vector3ld = Vector3<long double>;

	template <typename T>
	bool operator==(Vector3<T> const& v1, Vector3<T> const& v2)
	{
		return v1.x == v2.x && v1.y == v2.y && v1.z == v2.z;
	}

	template <typename T>
	Vector3<T>& operator+=(Vector3<T>& v1, Vector3<T> const& v2)
	{
		v1.x += v2.x;
		v1.y += v2.y;
		v1.z += v2.z;
		return v1;
	}

	template <typename T>
	Vector3<T>& operator-=(Vector3<T>& v1, Vector3<T> const& v2)
	{
		v1.x -= v2.x;
		v1.y -= v2.y;
		v1.z -= v2.z;
		return v1;
	}

	template <typename T>
	Vector3<T>& operator*=(Vector3<T>& v, T t)
	{
		v.x *= t;
		v.y *= t;
		v.z *= t;
		return v;
	}

	template <typename T>
	Vector3<T>& operator/=(Vector3<T>& v, T t)
	{
		v.x /= t;
		v.y /= t;
		v.z /= t;
		return v;
	}

	template <typename T>
	Vector3<T> operator+(Vector3<T> const& v)
	{
		return v;
	}

	template <typename T>
	Vector3<T> operator-(Vector3<T> const& v)
	{
		return {-v.x, -v.y, -v.z};
	}

	template <typename T>
	T dot(Vector3<T> const& v1, Vector3<T> const& v2)
	{
		return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
	}

	template <typename T>
	T norm(Vector3<T> const& v)
	{
		return std::sqrt(dot(v, v));
	}

	template <typename T>
	Vector3<T> normalize(Vector3<T> const& v)
	{
		return v / norm(v);
	}

} // namespace ext

#endif // !HEADER_EXT_VECTOR3_HPP_INCLUDED


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <fft/fft2.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

using namespace std;

const char* fname = "test_rt.h5";

// typedef FFTr2c<PlannerR2COD> fft_t;
typedef FFT fft_t;
typedef RT<std::complex<double>, RidgeletFrame, fft_t> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt)
{
  const char* fname = "f_rc.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

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

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("save", "save coefficients");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

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

  cout << setw(20) << "Jx: " << Jx << "\n"
       << setw(20) << "Jy: " << Jy << "\n"
       << setw(20) << "rho_x: " << rho_x << "\n"
       << setw(20) << "rho_y: " << rho_y << "\n"
       << setw(20) << "Nx: " << std::pow(2, Jx + 2) * rho_x << "\n"
       << setw(20) << "Ny: " << std::pow(2, Jy + 2) * rho_y << "\n";
  RDTSCTimer timer;

  timer.start();
  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  double time_frame_constructor = timer.stop();
  timer.print(cout, time_frame_constructor, "RidgeletFrame init");

  const unsigned int ncols = frame.Nx();  // #cols
  const unsigned int nrows = frame.Ny();  // #rows

  RT_t rt(frame);
  array_t F(nrows / 2, ncols / 2);
  {
    double pi = boost::math::constants::pi<double>();
    Eigen::ArrayXd x = pi * Eigen::ArrayXd::LinSpaced(ncols / 2, 0, 1);
    Eigen::ArrayXd y = pi * Eigen::ArrayXd::LinSpaced(nrows / 2, 0, 1);
    F = y.replicate(1, x.rows()).sin() * x.transpose().replicate(y.rows(), 1).sin();

    Eigen::MatrixXd tmp = F;
    tmp = tmp.triangularView<Eigen::Lower>();
    F = tmp.array();
  }

  fft_t fft;
  // debug
  complex_array_t Fhh(nrows / 2, ncols / 2);
  fft.ft(Fhh, F, false);
  hid_t file;
  if (vm.count("save")) {
    file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "Fhh", Fhh);  // debug
  }
  // ----------------------------------------
  complex_array_t Fh(nrows, ncols);
  Fh.setZero();
  ftcut(Fh, nrows / 2, ncols / 2) = Fhh;
  std::vector<rt_coeff_t> rt_coeffs(frame.size());
  timer.start();
  cout << "rt.rt(...)"
       << "\n";
  rt.rt(rt_coeffs, Fh);

  int ncoeffs = 0;
  for (unsigned int i = 0; i < rt_coeffs.size(); ++i) {
    ncoeffs += rt_coeffs[i].rows() * rt_coeffs[i].cols();
  }
  cout << "dim(rt_coeffs): " << ncoeffs << "\n";
  auto time_rt = timer.stop();
  timer.print(cout, time_rt, "rt.rt");

  // -------------------- Inverse transform --------------------
  complex_array_t Fh2(nrows, ncols);
  timer.start();
  cout << "rt.irt(...)"
       << "\n";
  rt.irt(Fh2, rt_coeffs);
  auto time_irt = timer.stop();
  timer.print(cout, time_irt, "rt.irt");

  array_t F2(nrows / 2, ncols / 2);
  complex_array_t Fh2_cut(nrows / 2, ncols / 2);
  Fh2_cut.setZero();
  Fh2_cut = ftcut(Fh2, nrows / 2, ncols / 2);
  fft.ift(F2, Fh2_cut);
  auto diff = (F - F2).abs();
  cout << "(F-F2).abs().sum(): " << diff.sum() << "\n";

  if (vm.count("save")) {
    eigen2hdf::save(file, "Fhl", Fhh);
    eigen2hdf::save(file, "Fh", Fh);
    eigen2hdf::save(file, "R", F);
    eigen2hdf::save(file, "Fh2", Fh2);
    eigen2hdf::save(file, "R2", F2);
    H5Fclose(file);
    cout << "written results to " << fname << "\n";
  }

  return 0;
}


#include "Geometry.h"

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

#include "glm/glm.hpp"
#include "glm/gtc/matrix_transform.hpp"
#include "glm/gtc/matrix_access.hpp"

#include <iostream>
#include <limits>

#define MIN_VAL -FLT_MAX
#define MAX_VAL FLT_MAX

using namespace Eigen;

void ComputeSpatialProperties(std::vector<float>* verts) {
	const int numVerts = ((int)verts->size()) / 3;
	glm::vec3 minPoint(MIN_VAL, MIN_VAL, MIN_VAL);
	glm::vec3 maxPoint(MAX_VAL, MAX_VAL, MAX_VAL);
	glm::vec3 centroid(0.0f);

	MatrixXf A(numVerts, 3);

	for (int i = 0; i < numVerts; i++) {

		float x = (*verts)[i * 3];
		float y = (*verts)[i * 3 + 1];
		float z = (*verts)[i * 3 + 2];
		if (x > minPoint.x) { minPoint.x = x; }
		if (y > minPoint.y) { minPoint.y = y; }
		if (z > minPoint.z) { minPoint.z = z; }

		centroid.x += x;
		centroid.y += y;
		centroid.z += z;

		A.row(i) << x, y, z;
	}

	//https://stats.stackexchange.com/questions/134282/relationship-between-svd-and-pca-how-to-use-svd-to-perform-pca
	//std::cout << A << std::endl;

	JacobiSVD<MatrixXf> svd(A, ComputeThinV);
	std::cout << "Its singular values are:" << std::endl << svd.singularValues() << std::endl;
	std::cout << "Its right singular vectors are the columns of the thin V matrix:" << std::endl << svd.matrixV() << std::endl;
	int ti = 1;

}


/**
 * @file finitevolumerobin_main.cc
 * @brief NPDE homework FiniteVolumeRobin code
 * @author Philippe Peter
 * @date February 2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <iostream>
#include <memory>
#include <string>
#include <utility>

#include "finitevolumerobin.h"

int main() {
  // coefficient functions
  auto g = [](const Eigen::Vector2d & /*x*/) { return 1.0; };
  auto gamma = [](const Eigen::Vector2d &x) { return 1.0 + x(0) * x(0); };

  // The equation is solved on  the four test meshes
  // disk1.msh, disk2.msh, disk3.msh and disk4.msh
  for (int i = 1; i <= 4; ++i) {
    // read mesh
    auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    lf::io::GmshReader reader(
        std::move(mesh_factory),
        CURRENT_SOURCE_DIR "/../meshes/disk" + std::to_string(i) + ".msh");
    auto mesh_p = reader.mesh();

    // Construct dofhanlder for linear finite elements on the current mesh.
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
    const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

    // Create a dataset of boolean flags indicating edges on the boundary of the
    // mesh
    auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 1)};

    // ASSEMBLE GALERKIN MATRIX
    // Matrix in triplet format holding Galerkin matrix, zero initially.
    lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());

    // First the part corresponding to piecewise Lagrangian finite elements
    lf::uscalfe::LinearFELaplaceElementMatrix elmat_provider;
    lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_provider, A);

    // Next the part corresponding to the modifications of the Galerkin matrix
    // on the boundary.
    FiniteVolumeRobin::EdgeMatrixProvider edmat_provider(gamma, bd_flags);
    lf::assemble::AssembleMatrixLocally(1, dofh, dofh, edmat_provider, A);

    // RIGHT-HAND SIDE VECTOR
    Eigen::VectorXd phi(dofh.NumDofs());
    phi.setZero();

    // Contributions on the boundary to the rhs vector
    FiniteVolumeRobin::EdgeVectorProvider edvec_provider(g, bd_flags);
    lf::assemble::AssembleVectorLocally(1, dofh, edvec_provider, phi);

    // SOLVE LINEAR SYSTEM
    Eigen::SparseMatrix<double> A_crs = A.makeSparse();
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    solver.compute(A_crs);
    Eigen::VectorXd sol_vec = solver.solve(phi);

    // OUTPUT RESULTS TO VTK FILE
    // construct mesh function representing the finite element solution
    lf::uscalfe::MeshFunctionFE mf_sol(fe_space, sol_vec);
    // construct vtk writer
    lf::io::VtkWriter vtk_writer(mesh_p, CURRENT_BINARY_DIR
                                             "/finite_volume_robin_solution_" +
                                             std::to_string(i) + ".vtk");
    // output data
    vtk_writer.WritePointData(
        "finite_volume_robin_solution_" + std::to_string(i), mf_sol);
  }
  return 0;
}


#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#include "plane3d.hpp"

using namespace Eigen;
using namespace std;

Plane3d::Plane3d(int _id, double alpha, double theta, Vector3d _target){
    id = _id;
    Vector3d u0(1, 0, 0);
    Vector3d v0(0, cos(alpha), sin(alpha));
    Vector3d w0(0, -sin(alpha), cos(alpha));

    Matrix3f rotationX;
    rotationX << u0[0], u0[1], u0[2],
		 v0[0], v0[1], v0[2],
		 w0[0], w0[1], w0[2];

    Vector3d u1(cos(theta), 0, sin(theta));
    Vector3d v1(0, 1, 0);
    Vector3d w1(-sin(theta), 0, cos(theta));

    Matrix3f rotationY;
    rotationY << u1[0], u1[1], u1[2],
		 v1[0], v1[1], v1[2],
		 w1[0], w1[1], w1[2];

    Matrix3f rotation = rotationX * rotationY;

    axisX = Vector3d(rotation.row(0)[0], rotation.row(0)[1], rotation.row(0)[2]);
    axisY = Vector3d(rotation.row(1)[0], rotation.row(1)[1], rotation.row(1)[2]);
    normal = Vector3d(rotation.row(2)[0], rotation.row(2)[1], rotation.row(2)[2]);

    Matrix3d A;
    A << axisX[0], axisX[1], axisX[2],
	 axisY[0], axisY[1], axisY[2],
	 normal[0], normal[1], normal[2];
    inverse = A.inverse();

    target = _target;
}

bool Plane3d::intersectsEdge(Vector3d v0, Vector3d v1){
    Vector3d diffVec0 = v0 - target;
    double dotProduct0 = diffVec0.dot(normal);

    Vector3d diffVec1 = v1 - target;
    double dotProduct1 = diffVec1.dot(normal);

    return dotProduct0 * dotProduct1 < 0;
}

bool Plane3d::containsPoint(Vector3d v0){
    Vector3d diffVec0 = v0 - target;
    double dotProduct0 = diffVec0.dot(normal);
    return dotProduct0 == 0;
}

array<double, 3> Plane3d::findIntersection(Vector3d v0, Vector3d v1){
    Vector3d w = v0 - target;
    Vector3d u = v1 - v0;

    double N = -(normal.dot(w));
    double D = normal.dot(u);

    array<double, 3> coords3d = {v0[0] + (N/D) * u[0], v0[1] + (N/D) * u[1], v0[2] + (N/D) * u[2]};

    return coords3d;
}

int Plane3d::Id(){
    return id;
}

Vector2d Plane3d::Rotate(array<double, 3> _vec){
    Vector3d vec(_vec[0], _vec[1], _vec[2]);
    double xCoord = axisX.dot(vec - target);
    double yCoord = axisY.dot(vec - target);

    return Vector2d(xCoord, yCoord);
}

Vector3d Plane3d::Get3dPoint(Vector3d pt2d){
    double x=pt2d[0]*inverse.row(0)[0] + pt2d[1]*inverse.row(0)[1];
    double y=pt2d[0]*inverse.row(1)[0] + pt2d[1]*inverse.row(1)[1];
    double z=pt2d[0]*inverse.row(2)[0] + pt2d[1]*inverse.row(2)[1];
    Vector3d pt(x + target[0], y + target[1], z + target[2]);
    return pt;
}

Shape3d::Shape3d(unsigned long int _tetId, vector<array<double, 3>> _vertices, double _weight, int _label){
    tetId = _tetId;
    vertices = _vertices;
    weight = _weight;
    label = _label;
}

unsigned long int Shape3d::TetId(){
    return tetId;
}

vector<array<double, 3>> Shape3d::Vertices(){
    //return Vector3d(vertices[0], vertices[1], vertices[2]);
    //vector<Vector3d> verticesAsVecs;

    //for(int i=0; i<
    return vertices;
}

double Shape3d::Weight(){
    return weight;
}

int Shape3d::Label(){
    return label;
}


/*
Copyright (c) 2015, Tianwei Shen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of libvot nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/** \file euclidean_matrix.cpp
 *	\brief euclidean matrix completion (exe)
 */
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <string>
#include <sstream>
#include <stdio.h>
#include <cmath>
#include <vector>

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

using namespace std;

/**
 * @brief An ad-hoc simple 2d point struct
 */
namespace {
struct point2d
{
	float x,y;
};
}	// end of namespace vot

float EuclideanDistance(point2d x1, point2d x2)
{
	return (x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y);
}

int main(int argc, char **argv)
{
	int input_size;
	cout << "the size of the matrix: \n";
	cin >> input_size;
	const int MATRIX_SIZE = input_size;
	string matrix_filename = "euclidean_matrix";
	stringstream ss;
	ss << matrix_filename << "_" << MATRIX_SIZE;
	ss >> matrix_filename;
	FILE *matrix_file = fopen(matrix_filename.c_str(), "w");

	std::vector<point2d> points(MATRIX_SIZE);
	for (int i = 0; i < MATRIX_SIZE; i++) {
		points[i].x = rand() % 1000;
		points[i].x /= 1000;
		points[i].y = rand() % 1000;
		points[i].y /= 1000;
	}

	Eigen::MatrixXf distance_matrix(MATRIX_SIZE, MATRIX_SIZE);
	for (int i = 0; i < MATRIX_SIZE; i++) {
		distance_matrix(i, i) = 0.0;
		for (int j = i+1; j < MATRIX_SIZE; j++) {
			distance_matrix(i, j) = EuclideanDistance(points[i], points[j]);
			distance_matrix(j, i) = distance_matrix(i, j);
		}
	}

	// output to the file
	for (int i = 0; i < MATRIX_SIZE; i++) {
		for (int j = 0; j < MATRIX_SIZE; j++) {
			cout << distance_matrix(i, j) << " ";
			fprintf(matrix_file, "%f ", distance_matrix(i, j));
		}
		cout << endl;
		fprintf(matrix_file, "\n");
	}

	Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(distance_matrix);
	//lu_decomp.setThreshold(1e-5);
	cout << "rank of distance matrix: " << lu_decomp.rank() << endl;

	//Eigen::JacobiSVD<Eigen::MatrixXf> svd(distance_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
	//cout << svd.singularValues() << endl;

	fclose(matrix_file);
	return 0;
}


#pragma once

#include "Combinations.hpp"
#include "DyckPaths.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////////////////////////////
/// \brief Class for iterating through all motzkin paths.
/// \param IntType must be a SIGNED integer type.
///
/// Motzkin paths are paths that go from \f$(0,0)\f$ to \f$(0,2n)\f$,
/// which never go below the \f$ y=0\f$ line, in which each step is from
/// \f$(x,y)\f$ to either \f$(x+1,y+1)\f$ or \f$(x+1,y-1)\f$ or \f$(x+1,y)\f$
/// #Example Usage:
///
///		motzkin_paths X(4)
///		for (auto&& x : X)
///			cout << x << endl;
/// Prints out:
///		[ 0 0 0 0 ]
///		[ 1 -1 0 0 ]
///		[ 1 0 -1 0 ]
///		[ 0 1 -1 0 ]
///		[ 1 0 0 -1 ]
///		[ 0 1 0 -1 ]
///		[ 0 0 1 -1 ]
///		[ 1 1 -1 -1 ]
///		[ 1 -1 1 -1 ]
///
///
/// # Example: Parenthesis
///
/// 	motzkin_paths X(4)
/// 	for (auto&& x : X)
/// 		cout << motzkin_paths::to_string(x, "(-)") << endl;
///
/// Prints out:
///		----
///		()--
///		(-)-
///		-()-
///		(--)
///		-(-)
///		--()
///		(())
///		()()
///
/////////////////////////////////////////////////////////////////////////////////////

template <class IntType = int, class RAContainerInt = std::vector<IntType>>
class MotzkinPaths
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = RAContainerInt;
    using motzkin_path = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    using comb_i = typename Combinations<IntType, RAContainerInt>::iterator;
    using dyck_i = typename DyckPaths<IntType, RAContainerInt>::iterator;
    class iterator;
    using const_iterator = iterator;

    static std::string to_string(const motzkin_path& data,
                                 const std::string& delim = "(-)")
    {
        std::string toReturn;

        for (auto i : data)
        {
            auto j = 1 - i;
            toReturn.push_back(delim[j]);
        }

        return toReturn;
    }

    // **************** End static functions

public:
    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    ///
    ////////////////////////////////////////////////////////////
    explicit MotzkinPaths(IntType n) : n_(n) {}

    ////////////////////////////////////////////////////////////
    /// \brief The total number of motzkin_paths
    ///
    /// \return M_n
    ///
    ////////////////////////////////////////////////////////////
    size_type size() const { return motzkin(n_); }

    IntType get_n() const { return n_; }

    iterator begin() const { return iterator(n_); }

    iterator end() const { return iterator::make_invalid_with_id(size()); }

    ////////////////////////////////////////////////////////////
    /// \brief Forward iterator class.
    ////////////////////////////////////////////////////////////
    class iterator
        : public boost::iterator_facade<iterator, const motzkin_path&, boost::forward_traversal_tag>
    {
    public:
        iterator() : data_(), comb_(), dyck_() {} // empty initializer

        explicit iterator(IntType n) : data_(n, 0), comb_(n, 0), dyck_(0) {}

        size_type ID() const { return ID_; }

        static iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;
            auto n = data_.size();

            if (ID_ == motzkin(n))
                return;

            ++comb_;
            if (comb_.is_at_end(n))
            {
                ++dyck_;

                if (dyck_.is_at_end(num_nonzero_halved_))
                {
                    num_nonzero_halved_ += 1;

                    dyck_.reset(num_nonzero_halved_);
                }

                comb_.reset(n, 2*num_nonzero_halved_);
            }

            ConvertToMotzkin(); // TODO(mraggi): do this laziliy
        }

        const motzkin_path& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

    private:
        size_type ID_{0};
        motzkin_path data_;
        comb_i comb_;
        dyck_i dyck_;
        IntType num_nonzero_halved_{0};

        void ConvertToMotzkin()
        {
            // 				cout << "Converting: " << *comb_ << " and " <<
            // *dyck_
            // << endl;
            for (size_t i = 0; i < data_.size(); ++i)
            {
                data_[i] = 0;
            }

            size_t count = 0;

            for (auto x : (*comb_))
            {
                data_[x] = (*dyck_)[count];
                ++count;
            }
        }

        friend class boost::iterator_core_access;
    }; // end class iterator

private:
    IntType n_;
}; // end class MotzkinPaths

using boost::container::static_vector;

using motzkin_paths = MotzkinPaths<int>;
using motzkin_paths_stack = MotzkinPaths<int, static_vector<int, 48>>;

} // namespace discreture


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <iomanip>
#include "RBF.h"
#include "MLS.h"
#include "HermiteRBF.h"
#include "GeneralizedMLS.h"
#include <boost/mpl/list.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/int.hpp>
using namespace std;
using namespace kt84;
namespace mpl = boost::mpl;

// utility for interpolation algorithms |
//--------------------------------------+
template <template <int, int, class, int> class FuncT>
struct FuncUtil;
template <> struct FuncUtil<RBF> {
    static const char* getName() { return "RBF"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v); }
    template <class Func> static void preprocess(Func& f) { f.factorize_and_solve(); }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return f.gradient(p); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].first) - f.constraints[i].second).norm();
            result.first = max<double>(result.first, error_v);
        }
        return result;
    }
};
template <> struct FuncUtil<MLS> {
    static const char* getName() { return "MLS"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v); }
    template <class Func> static void preprocess(Func& f) { }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return typename Func::Gradient::Zero(); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].first) - f.constraints[i].second).norm();
            result.first = max<double>(result.first, error_v);
        }
        return result;
    }
};
template <> struct FuncUtil<HermiteRBF> {
    static const char* getName() { return "HRBF"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v, g); }
    template <class Func> static void preprocess(Func& f) { f.factorize_and_solve(); }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return f.gradient(p); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].get<0>()) - f.constraints[i].get<1>()).norm();
            double error_g = (f.gradient_fd(f.constraints[i].get<0>(), epsilon) - f.constraints[i].get<2>()).norm();
            result.first  = max<double>(result.first , error_v);
            result.second = max<double>(result.second, error_g);
        }
        return result;
    }
};
template <> struct FuncUtil<GeneralizedMLS> {
    static const char* getName() { return "GMLS"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v, g); }
    template <class Func> static void preprocess(Func& f) { }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return typename Func::Gradient::Zero(); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].get<0>()) - f.constraints[i].get<1>()).norm();
            double error_g = (f.gradient_fd(f.constraints[i].get<0>(), epsilon) - f.constraints[i].get<2>()).norm();
            result.first  = max<double>(result.first , error_v);
            result.second = max<double>(result.second, error_g);
        }
        return result;
    }
};

// utility for RBF kernels |
//-------------------------+
template <class RBFKernel_Core>
struct KernelUtil;
template <> struct KernelUtil<RBFKernel_Gaussian      > { static const char* getName() { return "Gauss"; } static RBFKernel_Gaussian      ::Param getParam() { return RBFKernel_Gaussian      ::Param(10); } };
template <> struct KernelUtil<RBFKernel_SquaredInverse> { static const char* getName() { return "SqInv"; } static RBFKernel_SquaredInverse::Param getParam() { return RBFKernel_SquaredInverse::Param(20); } };
template <> struct KernelUtil<RBFKernel_Wendland      > { static const char* getName() { return "Wendl"; } static RBFKernel_Wendland      ::Param getParam() { return RBFKernel_Wendland      ::Param(1.5); } };
template <> struct KernelUtil<RBFKernel_Cubed         > { static const char* getName() { return "Cubed"; } static RBFKernel_Cubed         ::Param getParam() { return RBFKernel_Cubed         ::Param()   ; } };
template <> struct KernelUtil<RBFKernel_Identity      > { static const char* getName() { return "Ident"; } static RBFKernel_Identity      ::Param getParam() { return RBFKernel_Identity      ::Param()   ; } };
template <> struct KernelUtil<RBFKernel_SquaredLog    > { static const char* getName() { return "SqLog"; } static RBFKernel_SquaredLog    ::Param getParam() { return RBFKernel_SquaredLog    ::Param()   ; } };

// test functions |
//----------------+
template <int Dim, template <int, int, class, int> class FuncT>
struct Tester;

// 1D test
template <template <int, int, class, int> class FuncT>
struct Tester<1, FuncT> {
    template <class RBFKernel_Core, int DegreePolynomial>
    static void go(const typename RBFKernel_Core::Param& param) {
        typedef FuncT<1, 1, RBFKernel_Core, DegreePolynomial> Func;
        typedef Func::Point Point;
        typedef Func::Value Value;
        typedef Func::Gradient Gradient;

        Func f;
        f.kernel.param() = param;

        ifstream fin("input1d.txt");
        if (!fin)
            throw exception("input1d.txt not found!");
        while (!fin.eof()) {
            Point p;
            Value v;
            Gradient g;

            fin >> p[0] >> v[0] >> g[0];
            if (!fin.eof())
                FuncUtil<FuncT>::add_constraint(f, p, v, g);
        }

        FuncUtil<FuncT>::preprocess(f);

        stringstream fname;
        fname << "test1d_" << FuncUtil<FuncT>::getName() << "_" << KernelUtil<RBFKernel_Core>::getName() << "_p" << DegreePolynomial << ".txt";
        ofstream fout(fname.str().c_str());
        // report error first
        pair<double, double> maxError = FuncUtil<FuncT>::getMaxError(f, 0.00001);
        fout << "# max error (value, gradient): (" << maxError.first << ", " << maxError.second << ")" << endl << endl;
        // plot data
        const int N = 100;
        for (double i = 0; i <= N; ++i) {
            Func::Point point = Func::Point::Constant(i / N);
            Func::Value value = f(point);
            Func::Gradient gradient = FuncUtil<FuncT>::gradient(f, point);
            Func::Gradient gradient_fd = f.gradient_fd(point, 0.00001);
            fout
                << setw(12) << point[0] << " "
                << setw(12) << value[0] << " "
                << setw(12) << gradient[0] << " "
                << setw(12) << gradient_fd[0] << " "
                << endl;
        }
    }
    // generate gnuplot commands |
    //---------------------------+
    template <class RBFKernel_Core>
    static void genCmd(int DegreePolynomial) {
        stringstream fname;
        const char* funcName = FuncUtil<FuncT>::getName();
        const char* kernelName = KernelUtil<RBFKernel_Core>::getName();
        fname << "cmd1d_" << funcName << "_" << kernelName << ".txt";
        ofstream fout(fname.str().c_str());
        fout << "plot\\" << endl;
        for (int i = 0; i <= DegreePolynomial; ++i) {
            fout << "    \"test1d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" w linespoints";
            if (i < DegreePolynomial)
                fout << ",\\";
            fout << endl;
        }
        fout << "pause -1" << endl;
        fout.close();
        fname.str("");
        fname << "cmd1d_" << funcName << "_" << kernelName << "_g.txt";
        fout.open(fname.str().c_str());
        fout << "plot\\" << endl;
        for (int i = 0; i <= DegreePolynomial; ++i) {
            fout << "    \"test1d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" u 1:4 w linespoints";
            if (i < DegreePolynomial)
                fout << ",\\";
            fout << endl;
        }
        fout << "pause -1" << endl;
    }
};

// 2D test
template <template <int, int, class, int> class FuncT>
struct Tester<2, FuncT> {
    template <class RBFKernel_Core, int DegreePolynomial>
    static void go(const typename RBFKernel_Core::Param& param) {
        typedef FuncT<2, 1, RBFKernel_Core, DegreePolynomial> Func;
        typedef Func::Point Point;
        typedef Func::Value Value;
        typedef Func::Gradient Gradient;

        Func f;
        f.kernel.param() = param;

        ifstream fin("input2d.txt");
        if (!fin)
            throw exception("input2d.txt not found!");
        while (!fin.eof()) {
            Point p;
            Value v;
            Gradient g;

            fin >> p[0] >> p[1] >> v[0] >> g[0] >> g[1];
            if (!fin.eof())
                FuncUtil<FuncT>::add_constraint(f, p, v, g);
        }

        FuncUtil<FuncT>::preprocess(f);

        stringstream fname;
        fname << "test2d_" << FuncUtil<FuncT>::getName() << "_" << KernelUtil<RBFKernel_Core>::getName() << "_p" << DegreePolynomial << ".txt";
        ofstream fout(fname.str().c_str());
        // report error first
        pair<double, double> maxError = FuncUtil<FuncT>::getMaxError(f, 0.00001);
        fout << "# max error (value, gradient): (" << maxError.first << ", " << maxError.second << ")" << endl << endl;
        // plot data
        const int N = 32;
        for (double j = 0; j <= N; ++j) {
            for (double i = 0; i <= N; ++i) {
                Func::Point point = Func::Point(i / N, j / N);
                Func::Value value = f(point);
                Func::Gradient gradient = FuncUtil<FuncT>::gradient(f, point);
                Func::Gradient gradient_fd = f.gradient_fd(point, 0.00001);
                fout
                    << setw(12) << point[0] << " "
                    << setw(12) << point[1] << " "
                    << setw(12) << value[0] << " "
                    << setw(12) << gradient[0] << " "
                    << setw(12) << gradient[1] << " "
                    << setw(12) << gradient_fd[0] << " "
                    << setw(12) << gradient_fd[1] << " "
                    << endl;
            }
            fout << endl;
        }
    }
    // generate gnuplot commands |
    //---------------------------+
    template <class RBFKernel_Core>
    static void genCmd(int DegreePolynomial) {
        const char* funcName = FuncUtil<FuncT>::getName();
        const char* kernelName = KernelUtil<RBFKernel_Core>::getName();
        for (int i = 0; i <= DegreePolynomial; ++i) {
            // value
            stringstream fname;
            fname << "cmd2d_" << funcName << "_" << kernelName << "_p" << i << ".txt";
            ofstream fout(fname.str().c_str());
            fout << "set pm3d; unset surface; set pm3d hidden3d 100;set view 60, 320\n";
            fout << "splot \"test2d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" u 1:2:3\n";
            fout << "pause -1\n";
            fout.close();
            // gradient
            fname.str("");
            fname << "cmd2d_" << funcName << "_" << kernelName << "_p" << i << "_g.txt";
            fout.open(fname.str().c_str());
            fout << "set pm3d; unset surface; set pm3d hidden3d 100;set view 60, 320;\n";
            fout << "splot \"test2d_" << funcName << "_" << kernelName << "_p" << i << "_g.txt\" u 1:2:6\n";
            fout << "pause -1\n";
        }
    }
};

// list of valid kernels for each algorithm |
//------------------------------------------+
template <template <int, int, class, int> class FuncT>
struct KernelList;
template <> struct KernelList<RBF> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland      ,
        RBFKernel_Cubed         ,
        RBFKernel_Identity      ,
        RBFKernel_SquaredLog
    > Value;
};
template <> struct KernelList<HermiteRBF> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Cubed
    > Value;
};
template <> struct KernelList<MLS> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland
    > Value;
};
template <> struct KernelList<GeneralizedMLS> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland
    > Value;
};

// utility for looping over parameters |
//-------------------------------------+
template <template <int, int, class, int> class FuncT>
struct TestLooper {
    static void go() {
        typedef mpl::list<
            mpl::int_<1>,
            mpl::int_<2>
        > DimList;
        mpl::for_each<DimList>(LoopDim());
    }
    struct LoopDim {
        template <class IntDim>
        void operator()(const IntDim&) const {
            mpl::for_each<KernelList<FuncT>::Value>(LoopKernel<IntDim::value>());
        }
        template <int Dim>
        struct LoopKernel {
            template <class RBFKernel>
            void operator()(const RBFKernel&) const {
                typedef mpl::list<
                    mpl::int_<0>,
                    mpl::int_<1>,
                    mpl::int_<2>
                > DegreeList;
                mpl::for_each<DegreeList>(LoopDegree<RBFKernel>());
                Tester<Dim, FuncT>::genCmd<RBFKernel>(2);
            }
            template <class RBFKernel>
            struct LoopDegree {
                template <class IntDegree>
                void operator()(const IntDegree&) const {
                    Tester<Dim, FuncT>::go<RBFKernel, IntDegree::value>(KernelUtil<RBFKernel>::getParam());
                }
            };
        };
    };
};

int main() {
    TestLooper<RBF           >::go();       // list of templates cannot be handled by Boost.MPL
    TestLooper<MLS           >::go();
    TestLooper<HermiteRBF    >::go();
    TestLooper<GeneralizedMLS>::go();
}


#include "writer.hpp"
#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>
#include <nlohmann/json.hpp>
#include <filesystem>

//! Initial condition: grid-aligned rectangle
//! @param[in] (x,y) point to evaulate the initial condition
//! @param[out] evaluation of the function at (x,y)
double ic(double x, double y) {
    if (x > -0.3 && x < 0.3 && y > -0.3 && y < 0.3)
        return 1.0;
    else
        return 0.0;
}

//! Load configuration values from a JSON file
//! @param[in] jsonFilename file (including path if needed) of config.json
//! @param[out] json object with parameters: T, {x,y}{min,max}, Nx, Ny, cfl
nlohmann::json loadConfig(std::string jsonFilename) {
    std::ifstream i(jsonFilename);
    assert(i.good() && "config.json not found in current or parent directory");

    nlohmann::json j;
    i >> j;
    return j;
}

//! Apply periodic boundary conditions to matrix u.
//! u has relevant values in the Nx x Ny submatrix at the center
//! @param[in] u  (Nx+2)x(Ny+2) matrix
void applyBoundaryConditions(Eigen::MatrixXd &u) {
// (write your solution here)
}

//! An implementation of the 1D upwind numerical flux
//! @param[in] uM  value to the left of the interface
//! @param[in] uR  value to the right of the interface
//! @param[in] a   velocity at the interface
double F(double uM, double uP, double a) {
    return std::max(a, 0.)*uM + std::min(a, 0.)*uP;
}

//! Compute one step of the upwind method in 2D
//! @param[in] u           matrix of size (Nx+2)x(Ny+2) which will contain U^{n+1}
//! @param[in] u_old       matrix of size (Nx+2)x(Ny+2) of values U^{n}
//! @param[in] dx, dy, dt  meshsteps and timestep
//! @param[in] a           velocity as a function of R^2 to R^2
void updateUpwind(Eigen::MatrixXd &u, Eigen::MatrixXd &u_old, double dx,
                  double dy, double dt, double xmin, double ymin,
                  const std::function<Eigen::Vector2d(double, double)> &a) {
// (write your solution here)
}

//! Clear the contents of the output file before starting
//! Useful because we write in append mode
//! @param[in] outfile  name of the file to be wiped
void wipeFile(std::string outfile) {
    std::ofstream outstrm;
    outstrm.open(outfile, std::ofstream::out | std::ofstream::trunc);
    outstrm.close();
}

int main() {
    // Path to the config file relative to the binary. Try a couple of likely locations.
    std::string config_file = std::filesystem::exists("../config.json") ? "../config.json" : "config.json";

    auto j = loadConfig(config_file);
    double T = j["T"];
    double xmin = j["xmin"], ymin = j["ymin"], xmax = j["xmax"], ymax = j["ymax"];
    int Nx = j["Nx"], Ny = j["Ny"];
    double cfl = j["cfl"];

    // Derived data
    double dx = (xmax-xmin)/Nx;
    double dy = (ymax-ymin)/Ny;
    auto a = [](double x, double y) { return Eigen::Vector2d(y, -x); };
    double max_ax = ymax; // maximum of a_1 in domain
    double max_ay = -xmin; // maximum of a_2 in domain
    double dt_max = cfl / (max_ax/dx + max_ay/dy);
    std::string outfile = "u.txt";
    Eigen::MatrixXd u(Nx+2, Ny+2); // include ghost cells

    // apply initial condition to (1..Nx)x(1..Ny)
// (write your solution here)
    applyBoundaryConditions(u); // Complete u with BCs
    Eigen::MatrixXd u_old = u;

    wipeFile(outfile); // clear contents of output file

    double t = 0;
    std::vector<double> times;
    times.push_back(t);
    appendMatrixToFile(outfile, u.block(1,1,Nx,Ny));

    // Iterate over time
    while(t < T) {
        double dt = std::min(dt_max, T-t); // make sure we don't go beyond T
        t += dt;

        // Call updateUpwind. Don't forget the boundary conditions!
// (write your solution here)

        appendMatrixToFile(outfile, u.block(1,1,Nx,Ny));
        u_old = u;
        times.push_back(t);
    }
    writeToFile("time.txt", times);
}


#pragma once

#include "MerweScaledSigmaPoints.hpp"
#include "GaussianDistribution.hpp"

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

namespace icarus
{
    template<typename T, size_t N>
    struct UnscentedKalmanFilter
    {
        explicit UnscentedKalmanFilter() :
            mSigmaPoints(0.5f)
        {
            reset();
        }

        template<typename ProcessModel, typename MeasurementModel, size_t S>
        void filter(ProcessModel const & processModel, MeasurementModel const & measurementModel, GaussianDistribution<T, S> const & measurement, T timeStep)
        {
            auto points = mSigmaPoints(mState);

            for (auto & point : points) {
                point = processModel(point, timeStep);
            }

            mState = mSigmaPoints.unscentedTransform(points);
            mState.covariance.template triangularView<Eigen::Lower>() += processModel.noise();

            std::array<Eigen::Matrix<T, S, 1>, 2 * N + 1> measurementPoints;

            for (int i = 0; i < 2 * N + 1; ++i) {
                measurementPoints[i] = measurementModel(points[i]);
            }

            auto measurementDistribution = mSigmaPoints.unscentedTransform(measurementPoints);
            measurementDistribution.covariance.template triangularView<Eigen::Lower>() += measurement.covariance;
            measurementDistribution.covariance.template triangularView<Eigen::Upper>() = measurementDistribution.covariance.transpose();

            Eigen::Matrix<T, N, S> gain;
            gain.setZero();

            for (int i = 0; i < mSigmaPoints.size(); ++i) {
                auto weight = mSigmaPoints.covarianceWeight(i);
                auto stateDifference = points[i] - mState.mean;
                auto measurementDifference = measurementPoints[i] - measurementDistribution.mean;

                gain += weight * stateDifference * measurementDifference.transpose();
            }

            gain *= measurementDistribution.covariance.inverse();

            mState.mean += gain * (measurement.mean - measurementDistribution.mean);
            mState.covariance.template triangularView<Eigen::Lower>() -= gain * measurementDistribution.covariance * gain.transpose();
        }

        void reset()
        {
            mState.mean.setZero();
            mState.covariance.setZero();
        }

        Eigen::Matrix<T, N, 1> & stateVector()
        {
            return mState.mean;
        }
    private:
        MerweScaledSigmaPoints<T, N> mSigmaPoints;
        GaussianDistribution<T, N> mState;
    };
}


//
//  ambisonicDecoder.h
//  AmbisonicDecoder
//
//  Created by David Poirier-Quinot on 21/06/2017.
//  Copyright © 2017 ICL. All rights reserved.
//

#ifndef ambisonicDecoder_hpp
#define ambisonicDecoder_hpp

#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>

double deg2rad( const double degrees ){
    return degrees * 4.0 * atan (1.0) / 180.0;
}

unsigned int getNumAmbiCh( const unsigned int order )
{
    return pow(order + 1, 2);
}

class AmbisonicDecoder
{

//==========================================================================
// ATTRIBUTES

public:

private:

//==========================================================================
// METHODS

public:

    AmbisonicDecoder() {}

    ~AmbisonicDecoder() {}

    Eigen::MatrixXf getDecodingMatrix( const Eigen::MatrixXf & spkAzimElev, const unsigned int order, const bool useEpad )
    {
        // init
        unsigned long numSpk = spkAzimElev.cols();
        unsigned int numCh = getNumAmbiCh( order );
        Eigen::MatrixXf ambiGains ( numSpk, numCh ) ;
        Eigen::VectorXf gains( numCh );
        float azim; float elev;

        // loop over speaker positions
        for( int i = 0; i < numSpk; i++ ){
            // get spk azim elev
            azim = spkAzimElev(0, i);
            elev = spkAzimElev(1, i);

            // get spherical harmonic coefficients
            getRSH( order, azim, elev, gains );

            // fill output
            for( int j = 0; j < numCh; j++ ){
                ambiGains(i,j) = gains[j];
            }
        }

        if( useEpad ){
            // singular value decomposition
            Eigen::JacobiSVD<Eigen::MatrixXf> svd(ambiGains.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
            // get ambiGains out of left / right matrices
            ambiGains = svd.matrixV() * svd.matrixU().transpose();
        }

        // normalization (only step required for 'SAD')
        float norm = 4 * M_PI / numSpk;
        ambiGains *= norm;

        return ambiGains;
    }

    // get real spherical harmonics
    void getRSH( const unsigned int n, const float azim, const float elev, Eigen::VectorXf & gains )
    {
        // init
        float r;
        float ri;

        // convert from polarch coord. system to boost's
        float theta = deg2rad( 90 - elev );
        float phi = deg2rad( azim );

        // order 0
        gains[0] = 1.0 / sqrt(4*M_PI);

        // loop over spherical harmonic indices
        int index = 1;
        for( int nn = 1; nn <= n; nn += 1){
            for( int m = -nn; m <= abs(nn); m += 1){
                r = boost::math::spherical_harmonic_r(nn, m, theta, phi);
                ri = boost::math::spherical_harmonic_i(nn, m, theta, phi);

                if( m != 0 ){ r = pow(-1, m) * sqrt(2) * r; }
                ri = - sqrt(2) * ri;

                if( m < 0 ){ gains[index] = ri; }
                else{ gains[index] = r; }
                index++;
            }
        }
    }

};

#endif /* ambisonicDecoder_hpp */


#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <string>
#include <fstream>
#include <istream>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include "options.hpp"
#include "options_parser.hpp"
#include "LagrangianState.h"
#include "ParticlePhysics.h"
#include "TimeIntegration.h"
#include "Inputfile.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char* argv[]) {
  printf("*********** DRIVER PROGRAM FOR LAGRANGIAN PARTICLE SOLVER ***********\n\n");

  // Parse input file options
  Options options;
  if (!parseOptions(argc,argv,options)){
   return 0;
  }
  cout << options << endl;

  // Load state
  MatrixXd input = load_csv<MatrixXd>(options.inputfile);

  // Pass parsed program options to simulation
  LagrangianState state(input);
  ParticlePhysics physics(options, state);
  TimeIntegration integrator(options, physics, state);

  // Solve
  integrator.euler();

  // Output
  state.writeXY(options.outputfile+"_final.csv");

  return 0;
}


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

#ifndef MCL_CENTERIZE_HPP
#define MCL_CENTERIZE_HPP 1

#include <Eigen/Core>
#include <limits>

namespace mcl
{

// Moves all of the vertices so that the center of the mesh
// is at the origin. Returns translation used.
template <typename DerivedV>
static inline void centerize(Eigen::MatrixBase<DerivedV> &V)
{
	int cols = V.cols();
	for (int i=0; i<cols; ++i)
	{
		typename DerivedV::Scalar ci = V.col(i).mean();
		V.col(i).array() -= ci;
	}
} // end centerize

// Returns the index of the center-most vertex
template <typename DerivedV>
static inline int get_center_index(const Eigen::MatrixBase<DerivedV> &V)
{
	typedef typename DerivedV::Scalar T;
	typedef Eigen::Matrix<T,3,1> Vec3t;
	int cols = std::min(3, int(V.cols()));

	Vec3t center = Vec3t::Zero();
	for (int i=0; i<cols; ++i)
		center[i] = V.col(i).mean();

	int min_idx = -1;
	T min_dist = std::numeric_limits<T>::max();

	int nv = V.rows();
	for (int i=0; i<nv; ++i)
	{
		Vec3t vi = Vec3t::Zero();
		for (int j=0; j<cols; ++j)
			vi[j] = V(i,j);

		T dist = (center-vi).norm();
		if (dist < min_dist)
		{
			min_dist = dist;
			min_idx = i;
		}
	}

	return min_idx;
}

// Scales all of the vertices in V to a target radius.
// Returns the (uniform) scaling used.
template <typename DerivedV>
inline double scale_to_sphere(Eigen::MatrixBase<DerivedV> &V, double radius)
{
	using namespace Eigen;
	centerize(V);
	int dim = V.cols();
	int nv = V.rows();
	if (nv == 0 || dim < 2 || dim > 3)
		return 1.0;

	auto get_v3 = [&](int idx)
	{
		Vector3d v = Vector3d::Zero();
		for(int i=0; i<dim; ++i)
			v[i]=V(idx,i);
		return v;
	};

	double rad = 1e-20;
	for (int i=0; i<nv; ++i)
	{
		Vector3d v = get_v3(i);
		double d = v.norm();
		if (d > rad)
			rad = d;
	}

	double scale = radius / rad;
	V *= scale;
	return scale;

} // end scale to sphere

} // end ns mcl

#endif


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

int main() {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(3, 2);
    std::cout << "A =" << std::endl << A << std::endl;
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(2, 3);
    std::cout << "B =" << std::endl << B << std::endl;
    Eigen::MatrixXd C = A*B;
    std::cout << "C =" << std::endl << C << std::endl;
    return 0;
}