#include <Eigen/Dense>
/*
  Helper functions that support operations like
  data wrangling and transformations.
*/

Eigen::MatrixXd insert_bias(Eigen::MatrixXd X) {
  /*
    Inserts the bias into a dataset X by
    preprending a column of all ones.
  */
  Eigen::MatrixXd biased;
  int n_rows, n_cols;

  n_rows = X.rows();
  n_cols = X.cols() + 1;  // With extra bias column

  biased = Eigen::MatrixXd::Constant(n_rows, n_cols, 1.0);
  for (int col = 1; col < n_cols; col++) {
    biased.col(col) = X.col(col - 1);
  }

  return biased;
}


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

using namespace std;
using namespace Eigen;

int main(int argc, char ** argv){
    Matrix<double, 2, 3> matrix_23;
    matrix_23 << 1,2,3,4,5,6;
    cout << matrix_23 << endl;

    Matrix<int, 2, 3> matrix_232;
    matrix_232 << 1,2,3,4,5,6;

    Matrix<double, Dynamic, Dynamic> matrix_dd;
    matrix_dd = matrix_23 * matrix_232.cast<double>().transpose();
    // cout << matrix_23*matrix_232.cast<double>().transpose() << endl;
    cout << matrix_dd << endl;
    cout << matrix_dd.sum() << endl;
    cout << matrix_dd.trace() << endl;
    cout << matrix_dd.inverse() << endl;
    cout << matrix_dd.determinant() << endl;

    Matrix<double, 5, 5> matrix_nn = MatrixXd::Random(5,5);
    matrix_nn = matrix_nn * matrix_nn.transpose();
    Matrix<double, 5, 1> vector_n = MatrixXd::Random(5,1);
    Matrix<double, Dynamic, Dynamic> x = matrix_nn.inverse()*vector_n;
    cout << x << endl;
    x = matrix_nn.colPivHouseholderQr().solve(vector_n);
    cout << x << endl;
    x = matrix_nn.ldlt().solve(vector_n);
    cout << x << endl;
}


#ifndef VECTOR_3D_HPP
#define VECTOR_3D_HPP

#include <manu343726/portable_cpp/specifiers.hpp>
#include <boost/operators.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <string>
#include <tuple>
#include <cmath>

namespace math
{
	template<typename T>
	struct vector3
	     : boost::addable< vector3<T>             // vector + vector
    	 , boost::subtractable< vector3<T>        // vector - vector
    	 , boost::dividable2< vector3<T>, T       // vector / T
    	 , boost::multipliable2< vector3<T>, T    // vector * T, T * vector
    	 , boost::equality_comparable< vector3<T> // vector != vector
      > > > > >
	{
		T x, y, z;

		vector3() : vector3{0, 0, 0}
		{}

		vector3(T xx, T yy, T zz) :
			x{xx},
			y{yy},
			z{zz}
		{}

		template<typename U>
		explicit vector3(const vector3<U>& v) : vector3{v.x, v.y, v.z}
		{}

		vector3(const vector3& begin, const vector3& end) :
			x{end.x - begin.x},
			y{end.y - begin.y},
			z{end.z - begin.z}
		{}

		T squared_length() const NOEXCEPT
		{
			return x*x + y*y + z*z;
		}

		T length() const NOEXCEPT
		{
			return std::sqrt(squared_length());
		}

		vector3& operator+=(const vector3& v)
		{
			x += v.x;
			y += v.y;
			z += v.z;

			return *this;
		}

		vector3& operator-=(const vector3& v)
		{
			x -= v.x;
			y -= v.y;
			z -= v.z;

			return *this;
		}

		vector3& operator*=(T v)
		{
			x *= v;
			y *= v;
			z *= v;

			return *this;
		}

		vector3& operator/=(T v)
		{
			x /= v;
			y /= v;
			z /= v;

			return *this;
		}

		friend T operator*(const vector3& lhs, const vector3& rhs)
		{
			return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z;
		}

		friend std::ostream& operator<<(std::ostream& os, const vector3& v)
		{
			return os << "(" << v.x << "," << v.y << "," << v.z << ")";
		}

		friend std::istream& operator>>(std::istream& is, vector3& v)
		{
			char placeholder;

			return is >> placeholder >> v.x >> placeholder >> v.y >> placeholder >> v.z >> placeholder;
		}

		std::string to_string() const
		{
			return boost::lexical_cast<std::string>(*this);
		}

		friend bool operator==(const vector3& lhs, const vector3& rhs)
		{
			return std::tie(lhs.x, lhs.y, lhs.z) == std::tie(rhs.x, rhs.y, rhs.z);
		}
	};
}

#endif /* VECTOR_2D_HPP */


#include <iostream>
#include <Eigen/SVD>
using namespace Eigen;
using namespace std;

float inv_cond(const Ref<const MatrixXf>& a)
{
  const VectorXf sing_vals = a.jacobiSvd().singularValues();
  return sing_vals(sing_vals.size()-1) / sing_vals(0);
}

int main()
{
  Matrix4f m = Matrix4f::Random();
  cout << "matrix m:" << endl << m << endl << endl;
  cout << "inv_cond(m):          " << inv_cond(m)                      << endl;
  cout << "inv_cond(m(1:3,1:3)): " << inv_cond(m.topLeftCorner(3,3))   << endl;
  cout << "inv_cond(m+I):        " << inv_cond(m+Matrix4f::Identity()) << endl;
}


// FEM_2D_Plane_Stress.cpp (Main)

#include "pch.h"
#include "FEM_Input.h"
#include "FEM_GNUPlot.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>

//Element data type
struct Element
{
	void CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets);

	Eigen::Matrix<float, 3, 6> B;
	int nodesIds[3];
};

//Boundary constraint data type
struct Constraint
{
	enum Type
	{
		UX = 1 << 0,
		UY = 1 << 1,
		UXY = UX | UY
	};
	int node;
	Type type;
};

//Globals
int				nodesCount;
Eigen::VectorXf			nodesX;
Eigen::VectorXf			nodesY;
Eigen::VectorXf			loads;
std::vector< Element >		elements;
std::vector< Constraint >	constraints;

//Function for calculating the element stiffness matrix.
void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];

	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;

	//Calculating coefficients for shape functions (a1, a2, a3).
	//These are relevant for interpolation.
	Eigen::Matrix3f IC = C.inverse();

	//Assemble B matrix
	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}

	//Calculate element stiffness (det(C)/2 = area of triangle).
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f;

	//Store values of element stiffness matrix with corresponding indices in global stiffness matrix in triplets.
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}

//Function for setting constraints.
void SetConstraints(Eigen::SparseMatrix<float>::InnerIterator& it, int index)
{
	if (it.row() == index || it.col() == index)
	{
		it.valueRef() = it.row() == it.col() ? 1.0f : 0.0f;
	}
}

//Function for applying constraints in stiffness matrix.
void ApplyConstraints(Eigen::SparseMatrix<float>& K, const std::vector<Constraint>& constraints)
{
	std::vector<int> indicesToConstraint;

	for (std::vector<Constraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it)
	{
		if (it->type & Constraint::UX)
		{
			indicesToConstraint.push_back(2 * it->node + 0);
		}
		if (it->type & Constraint::UY)
		{
			indicesToConstraint.push_back(2 * it->node + 1);
		}
	}

	for (int k = 0; k < K.outerSize(); ++k)
	{
		for (Eigen::SparseMatrix<float>::InnerIterator it(K, k); it; ++it)
		{
			for (std::vector<int>::iterator idit = indicesToConstraint.begin(); idit != indicesToConstraint.end(); ++idit)
			{
				SetConstraints(it, *idit);
			}
		}
	}
}

int main(void)
{
	std::cout << "---------------------- 2D FEM SOLVER ---------------------" << std::endl;
	std::cout << "FEM software for solving elastic 2D plain stress problems." << std::endl;
	std::cout << "Created by Joshua Simon. Date: 25.02.2019." << std::endl;
	std::cout << std::endl;

	//1. Paths and filenames
	string mesh_data_file;								//This filename is read from user input.
	string solver_input_file = "Solver_Input.txt";
	string displacement_plot_data = "GNUPlot_Input_displacement.txt";
	string stress_plot_data = "GNUPlot_Input_contour.txt";

	//User input of mesh data file
	std::cout << "Enter the filename of the GiD mesh data file. If the data file is " << std::endl;
	std::cout << "not in same folder as this application, than enter the whole path " << std::endl;
	std::cout << "of the mesh data file with filename. Use \\\\ for \\ in address. " << std::endl;
	std::cout << std::endl << "Filename >> ";
	std::cin >> mesh_data_file;
	std::cout << std::endl << std::endl;

	//2. Pre Processing:
	//Read GiD mesh Data and write solver input.
	std::cout << "Pre Processor: Define boundary conditions and loads." << std::endl << std::endl;
	generateSolverInput(mesh_data_file, solver_input_file);
	std::cout << "Pre Processor: Solver input generated!" << std::endl << std::endl;

	std::ifstream infile(solver_input_file);
	std::ofstream outfile("Solution_Data.txt");					//This file contains solution data.
	std::ofstream outfile_gnuplot(displacement_plot_data);				//This file contains plot data.
	std::ofstream outfile_gnuplot_contur(stress_plot_data);				//This file contains plot data.

	//3. Solution:
	std::cout << "Solver: Creating mathematical model..." << std::endl;

	//Read material specifications
	float poissonRatio, youngModulus;
	infile >> poissonRatio >> youngModulus;

	//Assemble elasticity matrix D
	Eigen::Matrix3f D;
	D <<
		1.0f, poissonRatio, 0.0f,
		poissonRatio, 1.0, 0.0f,
		0.0f, 0.0f, (1.0f - poissonRatio) / 2.0f;

	D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));

	//Read number of nodes and their coordinates
	infile >> nodesCount;
	nodesX.resize(nodesCount);
	nodesY.resize(nodesCount);

	for (int i = 0; i < nodesCount; ++i)
	{
		infile >> nodesX[i] >> nodesY[i];
	}

	//Read number of elements and their nodes
	int elementCount;
	infile >> elementCount;

	for (int i = 0; i < elementCount; ++i)
	{
		Element element;
		infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
		elements.push_back(element);
	}

	//Read number of constraints and their node settings
	int constraintCount;
	infile >> constraintCount;

	for (int i = 0; i < constraintCount; ++i)
	{
		Constraint constraint;
		int type;
		infile >> constraint.node >> type;
		constraint.type = static_cast<Constraint::Type>(type);
		constraints.push_back(constraint);
	}

	loads.resize(2 * nodesCount);
	loads.setZero();

	//Read number of nodal loads and nodal forces
	int loadsCount;
	infile >> loadsCount;

	for (int i = 0; i < loadsCount; ++i)
	{
		int node;
		float x, y;
		infile >> node >> x >> y;
		loads[2 * node + 0] = x;
		loads[2 * node + 1] = y;
	}

	//Calculate stiffness matrix for each element
	std::vector<Eigen::Triplet<float> > triplets;
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		it->CalculateStiffnessMatrix(D, triplets);
	}

	//Assemble global stiffness matirx
	Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
	globalK.setFromTriplets(triplets.begin(), triplets.end());

	//Apply Constraints
	ApplyConstraints(globalK, constraints);

	std::cout << "Solver: Mathematical model created!" << std::endl;

	//Solving
	std::cout << "Solver: Solving in progress..." << std::endl;
	Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);
	Eigen::VectorXf displacements = solver.solve(loads);
	std::cout << "Solver: Solving done!" << std::endl << std::endl;

	//Writing output and display on console
	//std::cout << "Loads vector:" << std::endl << loads << std::endl << std::endl;			//Loads
	//std::cout << "Displacements vector:" << std::endl << displacements << std::endl;		//Displaysments

	outfile << displacements << std::endl;

	//std::cout << "Stresses:" << std::endl;							//Von Mises Stress

	int m = 0;
	float sigma_max = 0.0;
	float *sigma_mises = new float[elementCount];

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		Eigen::Matrix<float, 6, 1> delta;
		delta << displacements.segment<2>(2 * it->nodesIds[0]),
			displacements.segment<2>(2 * it->nodesIds[1]),
			displacements.segment<2>(2 * it->nodesIds[2]);

		Eigen::Vector3f sigma = D * it->B * delta;
		sigma_mises[m] = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);

		//Search for maximum stress
		if (sigma_mises[m] > sigma_max) {
			sigma_max = sigma_mises[m];
		}

		//std::cout << sigma_mises[m] << std::endl;						//Von Mises Stress
		outfile << sigma_mises[m] << std::endl;

		m++;
	}

	//4. Post Processing:
	//4.1 Writing GNUPlot output file for ploting mesh and mesh + displacements
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		//Prints x,y,dis-x,dis-y for every node of element in one line
		for (int i = 0; i < 3; i++) {
			outfile_gnuplot << nodesX(it->nodesIds[i]) << " " << nodesY(it->nodesIds[i]) \
				<< " " << displacements(it->nodesIds[i] * 2) << " " << displacements(it->nodesIds[i] * 2 + 1) << std::endl;
		}
		//First node of element has to appear twice for plotting purpose
		outfile_gnuplot << nodesX(it->nodesIds[0]) << " " << nodesY(it->nodesIds[0]) \
			<< " " << displacements(it->nodesIds[0] * 2) << " " << displacements(it->nodesIds[0] * 2 + 1) << std::endl;

		//Empty line to sperate between elements
		outfile_gnuplot << std::endl;
	}

	//4.2 Writing GNUPlot output file for stress contour plot
	outfile_gnuplot_contur << "unset xtics" << std::endl;
	outfile_gnuplot_contur << "unset ytics" << std::endl;
	outfile_gnuplot_contur << "set cbrange [0:1]" << std::endl << std::endl;
	outfile_gnuplot_contur << "plot[-15:15][-15:15] \\" << std::endl;

	//Write color information for every element
	int mm = 0;
	for (std::vector<Element>::iterator it = elements.begin(); it != (elements.end()-1); ++it) {
		outfile_gnuplot_contur << "\"-\" title \"\" with filledcurve lt palette cb " \
			                   << sigma_mises[mm] / sigma_max << " \\" << std::endl;
		outfile_gnuplot_contur << "fillstyle transparent solid 1.000000 ,\\" << std::endl;
		mm++;
	}

	//Write color information for last element
	outfile_gnuplot_contur << "\"-\" title \"\" with filledcurve lt palette cb " \
						   << sigma_mises[elementCount-1] / sigma_max << " \\" << std::endl;
	outfile_gnuplot_contur << "fillstyle transparent solid 1.000000 ;" << std::endl;

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		//Elements and their nodes:
		//Prints x,y for every node of element in one line
		for (int i = 0; i < 3; i++) {
			outfile_gnuplot_contur << nodesX(it->nodesIds[i]) << " " << nodesY(it->nodesIds[i]) << std::endl;
		}
		//First node of element has to appear twice for plotting purpose
		outfile_gnuplot_contur << nodesX(it->nodesIds[0]) << " " << nodesY(it->nodesIds[0]) << std::endl;

		//'e' to sperate between elements
		outfile_gnuplot_contur << "e" << std::endl;
	}

	std::cout << "Post Processor: Plotting solution..." << std::endl << std::endl;

	//4.3 Plot results
	plot(displacement_plot_data);

	delete[] sigma_mises;

	return 0;
}


#include "problemes.h"
#include "arithmetique.h"
#include "premiers.h"

#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(214, "Totient Chains") {
    // Let φ be Euler's totient function, i.e. for a natural number n, φ(n) is the number of k, 1 ≤ k ≤ n, for which
    // gcd(k,n) = 1.
    //
    // By iterating φ, each positive integer generates a decreasing chain of numbers ending in 1.
    // E.g. if we start with 5 the sequence 5,4,2,1 is generated.
    // Here is a listing of all chains with length 4:
    //
    //                                      5,4,2,1
    //                                      7,6,2,1
    //                                      8,4,2,1
    //                                      9,6,2,1
    //                                      10,4,2,1
    //                                      12,4,2,1
    //                                      14,6,2,1
    //                                      18,6,2,1
    //
    // Only two of these chains start with a prime, their sum is 12.
    //
    // What is the sum of all primes less than 40000000 which generate a chain of length 25?
    nombre limite = 40000000;
    nombre chaine = 25;
    vecteur premiers;
    premiers::crible235<nombre>(limite, std::back_inserter(premiers));

    nombre resultat = 0;
    for (nombre p: premiers) {
        if (p >= limite)
            break;

        nombre longueur = 1;
        nombre m = p;
        while (m != 1 && longueur < chaine) {
            m = arithmetique::phi(m, premiers);
            ++longueur;
        }

        if (m == 1 && longueur == chaine)
            resultat += p;
    }

    return std::to_string(resultat);
}


#include "rkintegrator.hpp"
#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

template <class Function>
void errors(const Function &f, const double &T, const Eigen::VectorXd &y0,
	const Eigen::MatrixXd &A, const Eigen::VectorXd &b){
		std::vector<unsigned int> N = {1,2,4,8,16,32,64,128, 256, 512, 1024, 2048, 4096, 8192,16384,32768};
		unsigned int N_f=32768;
		double sum=0;
		double count =0;
		std::vector<double> err_vect;
		err_vect.push_back(1.);
		RKIntegrator<Eigen::VectorXd> RK(A,b);
		auto y_f=RK.solve(f,T,y0,N_f);
		for(unsigned int i = 0; i < N.size(); ++i){
			auto temp = RK.solve(f,T,y0,N[i]);
			double err= (temp.back()-y_f.back()).norm();
			double order = log2(err_vect.back()/err);
			order= std::abs(order);
			std::cout << "n = " << N[i] << std::setw(15)  << "error = " << err << std::setw(15)<< "order is : " << order<< std::endl;
			err_vect.push_back(err);
			if(i>1 && err>1e-16){
				sum+=order; count+=1;
			}
		}

std::cout << " average order : " <<sum/count  << std::endl;

};

Nummerical Methods for CSE/PS12/order.hpp


#include <iostream>
#include <functional>
#include <armadillo>
#include <string>

// include code from b.cc (except main function)
#define NO_MAIN
#include "b.cc"
#undef NO_MAIN

using namespace std;
using namespace arma;

const std::string CHECK_MARK = "✓";
const std::string BALLOT_X = "✗";

struct test { std::function<bool()> func; std::string name; };

const double TOLERANCE = 1e-13;

/**
 * Test maxoff() on a symmetric matrix
**/
bool test_maxoff_sym() {
  // create known symmetric matrix
  mat M(5, 5, arma::fill::zeros);

  M(0,0) = 3;
  M(1,1) = 9;
  M(2,2) = 17;
  M(3,3) = 0;
  M(4,4) = -9;
  M(2,0) = M(0,2) = 9;
  M(2,1) = M(1,2) = -5;
  M(3,0) = M(0,3) = -11;
  M(3,1) = M(1,3) = 10;
  M(3,2) = M(2,3) = -7;
  M(4,0) = M(0,4) = 5;

  // expected values
  size_t exp_k = 3, exp_l = 0; // maximal element position
  double exp_a = 121;          // square of maximal element

  // find maximal element
  size_t k, l;
  double a = maxoff(M, k, l);

  // compare (up to order of k and l, since M is symmetric)
  return (abs(a - exp_a) < TOLERANCE && ((k == exp_k && l == exp_l) || (k == exp_l && l == exp_k)));
}

/**
 * Check that jacobi_step() conserves orthonormality of S.
**/
bool test_ortho() {
  // if a matrix preserves scalar product (or orthonormality) then it must be an orthogonal matrix,
  // so S * S^T = I

  // set of random, pre-calculated tau, k and l values (where k != l)
  const double tau_values[] = { 0.5 };
  const double kl_values[][2] = { { 1, 2 } };
  const size_t count = 1;

  // initial P matrix (identity) and constant identity matrix I
  mat P(5, 5, arma::fill::eye);
  const mat I(5, 5, arma::fill::eye);

  // for each step, P' = P * S where S is calculated from a random tau value (see above list)
  for(size_t i = 0; i < count; i++) {
    apply_rot_col(kl_values[i][0], kl_values[i][1], tau_values[i], P);

    // check that P * P^T = I, within tolerance
    if(arma::abs(P * P.t() - I).max() >= TOLERANCE)
      return false;
  }

  // for each step, P' = S^T * P where S is calculated from a random tau value (see above list)
  for(size_t i = 0; i < count; i++) {
    apply_rot_row(kl_values[i][0], kl_values[i][1], tau_values[i], P);

    // check that P * P^T = I, within tolerance
    if(arma::abs(P * P.t() - I).max() >= TOLERANCE)
      return false;
  }

  return true;
}

int main() {
  // define tests
  const struct test tests[] = {
    { test_maxoff_sym, "Maximal diagonal test on symmetric matrix" },
    { test_ortho, "Orthonormality test" },
  };
  const size_t test_count = 2;

  // run tests
  std::cout << "Running tests:" << std::endl;
  for(size_t i = 0; i < test_count; i++) {
    auto test = tests[i];

    std::cout << "  " << test.name << std::endl;

    if(test.func()) {
      std::cout << "    " << CHECK_MARK << " test passed" << std::endl;
    } else {
      std::cout << "    " << BALLOT_X << " test failed" << std::endl;
    }
  }
}


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

Eigen::Matrix3d OthMatrix(const Eigen::Matrix3d& input) {
    Eigen::Quaterniond q(input);
    q.normalize();
    return q.toRotationMatrix();
}

// 解决如下问题
// 1. 万向锁时，分解欧拉角是否仍然有效
// 2. 万向锁反应了一个什么样的问题？
int main(int argv, char** argc) {
    double theta_z = M_PI * 60 / 180;
    double theta_y = M_PI * 30 / 180;
    double theta_x = M_PI * 40 / 180;
    Eigen::Matrix3d rz =
        Eigen::AngleAxisd(theta_z, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    Eigen::Matrix3d ry =
        Eigen::AngleAxisd(theta_y, Eigen::Vector3d::UnitY()).toRotationMatrix();
    Eigen::Matrix3d rx =
        Eigen::AngleAxisd(theta_x, Eigen::Vector3d::UnitX()).toRotationMatrix();

    // 1 没有万向锁
    Eigen::Matrix3d composition = rz * ry * rx;
    composition = OthMatrix(composition);
    Eigen::Vector3d euler_angles = composition.eulerAngles(2, 1, 0);
    std::cout << "正常情况:\n";
    std::cout << euler_angles.transpose() * 180 / M_PI << std::endl;

    // 2 将y改为90度，观察万向锁时的分解结果
    theta_y = M_PI * 90 / 180;
    ry =
        Eigen::AngleAxisd(theta_y, Eigen::Vector3d::UnitY()).toRotationMatrix();
    composition = rz * ry * rx;
    composition = OthMatrix(composition);
    std::cout << "万向锁对应的分解:\n";
    std::cout << composition << std::endl;
    euler_angles = composition.eulerAngles(2, 1, 0);
    std::cout << "eular angles :\n";
    std::cout << euler_angles.transpose() * 180 / M_PI << std::endl;

    // 3 x用合成角度
    theta_x = (40 - 60) * M_PI / 180;
    rx =
        Eigen::AngleAxisd(theta_x, Eigen::Vector3d::UnitX()).toRotationMatrix();
    Eigen::Matrix3d composition_yx = ry * rx;
    composition_yx = OthMatrix(composition_yx);

    std::cout << "\n考虑万向锁丢失了轴，仅用两个轴得到旋转矩阵，并进行分解\n";
    std::cout << composition_yx << std::endl;
    euler_angles = composition_yx.eulerAngles(2, 1, 0);
    std::cout << "eular angles :\n";
    std::cout << euler_angles.transpose() * 180 / M_PI << std::endl;

    // 4 使用
    double diff = (composition_yx - composition).norm();
    std::cout << "difference between zyx and yx: " << diff << std::endl;
}


#include "writer.hpp"
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <cmath>
#include <iostream>
#include <stdexcept>

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//! Vector type
typedef Eigen::VectorXd Vector;

typedef double (*FunctionPointer)(double);

//----------------poissonBegin----------------
//! Create the 1D Poisson matrix
//! @param[out] A will contain the Poisson matrix
//! @param[in] N the number of points
void createPoissonMatrix(SparseMatrix &A, int N) {
	A.resize(N, N);
	std::vector<Triplet> triplets;
	triplets.reserve(N + 2 * N - 2);
	for (int i = 0; i < N; ++i) {
		// This is the diagonal
		triplets.push_back(Triplet(i, i, 2));

		// (write your solution here)
		if (i > 0) {
			triplets.push_back(Triplet(i - 1, i, -1));
		}
		if (i < N - 1) {
			triplets.push_back(Triplet(i + 1, i, -1));
		}
	}

	A.setFromTriplets(triplets.begin(), triplets.end());
}
//----------------poissonEnd----------------

//----------------RHSBegin----------------
//! Create the right hand side for the poisson problem.
//! @note This scales the right hand side (ie. $dx^2 * f(x)$)
//!
//! @param[out] rhs will contain the right hand side
//! @param[in] f function pointer to f
//! @param[in] N the number of points to use
//! @param[in] dx the cell length
void createRHS(Vector &rhs, FunctionPointer f, int N, double dx) {
	rhs.resize(N);
	// Set RHS
	// (write your solution here)
	for (int i = 0; i < N; i++) {
		rhs[i] = dx * dx * f(i * dx);
	}
}
//----------------RHSEnd----------------

//! Solves the Poisson equation
//!
//!   $-u''(x) = f(x) $
//!
//! on [0,1] with boundary values $u(0)=u(1) = 0$.
//!
//! @param[out] u should contain the solution u at the end
//! @param[in] f should be a function pointer to f
//! @param[in] N as in the exercise
void poissonSolve(Vector &u, FunctionPointer f, int N) {
	double dx = 1.0 / (N + 1);

	SparseMatrix A;
	// create the matrix
	// (write your solution here)
	createPoissonMatrix(A, N);
	Vector rhs;

	// create RHS
	// (write your solution here)
	createRHS(rhs, f, N, dx);

	Eigen::SparseLU<SparseMatrix> solver;

	solver.compute(A);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix");
	}

	// Find u: ....
	// (write your solution here)
	u.resize(N);
	u.setZero();
	u = solver.solve(rhs);
}

double F(double x) {
	return sin(2 * M_PI * x);
}

double exact(double x) {
	return 1.0 / (4 * M_PI * M_PI) * F(x);
}

//! Test if the Poisson matrix is correctly set up for one case
//! This does NOT guarantee that the code is correct, it is only a small
//! indiciation
void testPoissonMatrix() {
	SparseMatrix A;
	const int    N = 13;
	createPoissonMatrix(A, N);
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < N; j++) {
			if (i == j) {
				if (A.coeff(i, j) != 2) {
					throw std::runtime_error("Poisson matrix: Wrong Poisson matrix!");
				}
			} else if (i == j - 1 || i == j + 1) {
				if (A.coeff(i, j) != -1) {
					throw std::runtime_error("Poisson matrix: Wrong upper or lower diagonal");
				}
			} else {
				if (A.coeff(i, j) != 0) {
					throw std::runtime_error("Poisson matrix: Matrix is not band diagonal!");
				}
			}
		}
	}
}

//----------------convergenceBegin----------------
//! Computes error for a range of cell lengths and stores them to errors
//! @param[out] errors the errors computed
//! @param[out] resolutions the resolutions used
void poissonConvergence(std::vector<double> &errors,
                        std::vector<int> &   resolutions) {
	const int startK = 2;
	const int endK   = 13;
	errors.resize(endK - startK);
	resolutions.resize(errors.size());
	for (int k = startK; k < endK; ++k) {
		const int N = 1 << (k - 1);
		// compute the solution and the error
		// (write your solution here)
		resolutions[k - startK] = N;
		Vector u;
		poissonSolve(u, F, N);
		double error = 0;
		for (int i = 0; i < N; i++) {
			double e = abs(u[i] - exact(i / double(N)));
			if (e > error) {
				error = e;
			}
		}
		errors[k - startK] = error;
	}
}
//----------------convergenceEnd----------------

int main(int, char **) {
	testPoissonMatrix();
	Vector u;
	poissonSolve(u, F, 50);
	writeToFile("u_fd.txt", u);

	std::vector<double> errors;
	std::vector<int>    resolutions;
	poissonConvergence(errors, resolutions);
	writeToFile("errors_fd.txt", errors);
	writeToFile("resolutions_fd.txt", resolutions);
}


#include <utils/math_utils.hh>

#include <utils/debug_utils.hh>

#include <Eigen/Eigenvalues>

namespace math
{

bool is_equal(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2, const double tol)
{
    IS_EQUAL(mat1.rows(), mat2.rows());
    IS_EQUAL(mat1.cols(), mat2.cols());
    return ((mat1 - mat2).array().abs() < tol).all();
}

bool is_symmetric(const Eigen::MatrixXd &mat, const double tol)
{
    IS_EQUAL(mat.rows(), mat.cols());
    return math::is_equal(mat, mat.transpose(), tol);
}

Eigen::VectorXd gradient(
        const std::function<double(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt, const double delta)
{
    IS_GREATER(delta, 0);

    const int dim = pt.size();
    IS_GREATER(dim, 0);

    Eigen::VectorXd gradient(dim);

    for (int i = 0; i < dim; ++i)
    {
        Eigen::VectorXd pt_positive = pt;
        pt_positive[i] += delta;

        Eigen::VectorXd pt_negative = pt;
        pt_negative[i] -= delta;

        gradient(i) = (func(pt_positive) - func(pt_negative)) / (2.0 * delta);
    }

    return gradient;
}

Eigen::MatrixXd jacobian(
        const std::function<Eigen::VectorXd(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt, const double delta)
{
    IS_TRUE(func);
    IS_GREATER(delta, 0);

    const int in_dim = pt.size();
    IS_GREATER(in_dim, 0);

    const int out_dim = func(pt).size();
    IS_GREATER(out_dim, 0);

    Eigen::MatrixXd jacobian(out_dim, in_dim);

    for (int i = 0; i < in_dim; ++i)
    {
        Eigen::VectorXd pt_positive = pt;
        pt_positive[i] += delta;

        Eigen::VectorXd pt_negative = pt;
        pt_negative[i] -= delta;

        jacobian.col(i) = (func(pt_positive) - func(pt_negative)) / (2.0 * delta);
    }

    return jacobian;
}

Eigen::MatrixXd hessian(
        const std::function<double(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt, const double delta)
{
    IS_GREATER(delta, 0);
    const int dim = pt.size();
    IS_GREATER(dim, 0);

    // Precompute constants.
    const double two_delta = 2.0*delta;
    const double delta_sq = delta*delta;
    const double ij_eq_div = 12.*delta_sq;
    const double ij_neq_div = 4.*delta_sq;

    Eigen::MatrixXd hessian(dim, dim);
    // Central difference approximation based on
    // https://v8doc.sas.com/sashtml/ormp/chap5/sect28.htm
    for (int i = 0; i < dim; ++i)
    {
        for (int j = i; j < dim; ++j)
        {
            Eigen::VectorXd p1 = pt;
            Eigen::VectorXd p2 = pt;
            Eigen::VectorXd p3 = pt;
            Eigen::VectorXd p4 = pt;
            double value = 0;
            if (i == j)
            {
                p1[i] += two_delta;
                p2[i] += delta;
                p3[i] -= delta;
                p4[i] -= two_delta;
                value = (-func(p1) + 16.*(func(p2) + func(p3)) - 30.*func(pt) - func(p4))
                    / (ij_eq_div);
            }
            else
            {
                p1[i] += delta;
                p1[j] += delta;

                p2[i] += delta;
                p2[j] -= delta;

                p3[i] -= delta;
                p3[j] += delta;

                p4[i] -= delta;
                p4[j] -= delta;

                value = (func(p1) - func(p2) - func(p3) + func(p4)) / (ij_neq_div);
            }

            hessian(i,j) = value;
            hessian(j,i) = value;
        }
    }

    return hessian;
}

Eigen::MatrixXd project_to_psd(const Eigen::MatrixXd &symmetric_mat, const double min_eigval)
{
    // Cheap check to confirm matrix is square.
    IS_EQUAL(symmetric_mat.rows(), symmetric_mat.cols());

    // O(n^2) check to make sure that the matrix is symmetric.
    IS_TRUE(math::is_symmetric(symmetric_mat));

    const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(symmetric_mat);
    Eigen::MatrixXd evals = es.eigenvalues().asDiagonal();
    const Eigen::MatrixXd evecs = es.eigenvectors();

    // Indices of eigen values that are less than the minimum eigenvalue threshold.
    int num_failed_evals = 0;
    const int num_evals = evals.rows();
    for (int i = 0; i < num_evals; ++i)
    {
        if (evals(i,i) < min_eigval)
        {
            ++num_failed_evals;
            evals(i,i) = min_eigval;
        }
    }
    // If we didn't have eigenvalues less than the minimum threshold, then return the original
    // matrix.
    if (num_failed_evals == 0)
    {
        return symmetric_mat;
    }

    // We can reconstruct with P D P\inv = P D P^T if P is orthonormal.
    const Eigen::MatrixXd projection = evecs * evals * evecs.transpose() ;
    return projection;
}

void check_psd(const Eigen::MatrixXd &mat, const double min_eigval)
{
    // Cheap check to confirm matrix is square.
    IS_EQUAL(mat.rows(), mat.cols());

    // O(n^2) check to make sure that the matrix is symmetric.
    IS_TRUE(math::is_symmetric(mat));

    const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(mat, Eigen::EigenvaluesOnly);
    const auto evals = es.eigenvalues();

    for (int i = 0; i < evals.size(); ++i)
    {
        // Throw an exception if this is not true.
        IS_GREATER_EQUAL(evals(i), min_eigval);
    }
}

} // namespace math


/**
 * Write a function 'filter()' that implements a multi-
 *   dimensional Kalman Filter for the example given
 */

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

using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kalman Filter variables
VectorXd x;	// object state
MatrixXd P;	// object covariance matrix
VectorXd u;	// external motion
MatrixXd F; // state transition matrix
MatrixXd H;	// measurement matrix
MatrixXd R;	// measurement covariance matrix
MatrixXd S;    //used in creation of Kalman Gain??  Why named S?
MatrixXd K;   //Kalman Gain Matrix
MatrixXd Y;  //measurment matrix
MatrixXd C;  //C matrix
MatrixXd I; // Identity matrix
MatrixXd Q;	// process covariance matrix
MatrixXd B; //B matrix

void update_prediction(VectorXd &x);
void initial_process_cov_matrix();
void update_predicted_process_cov_matrix();
void calculate_Kalman_gain();
void import_new_observation(int speed, int velocity);
void update_current_state();
void update_process_cov_matrix();

int main() {
  /**
   * Code used as example to work with Eigen matrices
  */

  float dt = 1.;

  // design the KF with 1D motion
  x = VectorXd(2);
  x << 4000, 280;
  P = MatrixXd(2, 2);

  u = VectorXd(1);
  u << 2;

  F = MatrixXd(2, 2);
  F << 1, dt, 0, 1;

//  H = MatrixXd(1, 2);
//  H << 1, 0;
  H = MatrixXd::Identity(2,2);

  R = MatrixXd(2, 2);
  R << 625, 0, 0, 36;

  S = MatrixXd(2,2);
  S << 0, 0, 0, 0;

  K = MatrixXd(2,2);

  C = MatrixXd::Identity(2, 2);

  B = MatrixXd(2,1);
  B << .5*pow(dt,2), dt;

  Y = MatrixXd(2,1);

  I = MatrixXd::Identity(2, 2);

  Q = MatrixXd(2, 2);
  Q << 0, 0, 0, 0;

  // create a list of measurements
  VectorXd pos_measurements(4);
  pos_measurements << 4260, 4550, 4860, 5110;

  VectorXd vel_measurements(4);
  vel_measurements << 282, 285, 286, 290;

  initial_process_cov_matrix();
  // call Kalman filter algorithm for each measurement (1Hz)

  for (unsigned int n = 0; n < 4; ++n) {
    cout << "********** N: " << n << endl;
    update_prediction(x);
    update_predicted_process_cov_matrix();
    calculate_Kalman_gain();
    import_new_observation(pos_measurements[n], vel_measurements[n]);
    update_current_state();
    update_process_cov_matrix();
  }

  return 0;
}

void initial_process_cov_matrix() {
  cout << "Initial_process_cov_matrix" << endl;
  P << 400, 0, 0, 25;
  cout << P << endl;
}

void update_prediction(VectorXd &x) {
  cout << "update_prediction" << endl;
  x = (F * x) + (B * u);
  cout << x << endl;
}

void update_predicted_process_cov_matrix() {
  cout << "update_predicted_process_cov_matrix" << endl;
  P = (F * P) * F.transpose() + Q;
  P(0,1) = 0;
  P(1,0) = 0;

  cout << "P after update: " << P << endl;

}

void calculate_Kalman_gain() {
    cout << "calculate_Kalman_gain" << endl;
    S = R + (H * (P * H.transpose()));
    K = (P * H.transpose()) * S.inverse();
    cout << "Kalman gain matrix: " << K << endl;
}

void import_new_observation(int speed, int velocity) {
    MatrixXd measurement = MatrixXd(2, 1);
    measurement << speed, velocity;
    cout << "measurement: "<<  measurement  << endl;
    Y  = C * measurement;
    cout << "Y: " << Y << endl;
}

void update_current_state() {
  cout << "x: " << x << endl;
  cout << "H: " << H << endl;
  cout << "Y: " << Y << endl;

  MatrixXd delta = Y - (H * x);
  cout << "delta: " << delta << endl;

  x = x + (K * delta);
  cout << "adjusted x: " << x << endl;
}

void update_process_cov_matrix() {

    MatrixXd I = MatrixXd::Identity(2, 2);

    P = P * (I - (K * H)) * (I - (K * H)).transpose() + (K * R) * K.transpose();

   cout << "Updated Process Covariance Matrix: " << P << endl;

}


/**
 * @file Utils.cpp
 *
 * Fichier d'implémentation pour la calculation mathématique
 */

#include <iostream>
#include <armadillo>
#include "Solution1DHO.h"
#include "Utils.h"

/** Calcul de la factorielle de n
*
* Cette fonction a pour but de calculer la factorielle de n par récurrence.
*
* @param n entier
*
* @return le résultat de la factorielle de n
*/
int Utils::fac(int n)
{
    int f;

    if (n==0 || n==1)
        f=1;
    else
        f=fac(n-1)*n;

    return f;
}

/**
 * Calcul de dérivée
 *
 * Cette fonction a pour but d'approximer la dérivée du premier ordre
 *
 * @param F1 vecteur contenant les n valeurs d'une fonction
 * @param F2 vecteur contenant les n valeurs d'une fonction décalée d'une valeur
 * @param Z1 vecteur contenant les n points où sont évaluée la fonction
 * @param Z2 vecteur contenant les n points où sont évaluée la fonction décalée d'une valeur
 *
 * @return res vecteur contenant les valeurs de la fonction dérivée
 */
arma::mat Utils::derivative(arma::mat F1, arma::mat F2, arma::mat Z1, arma::mat Z2)
{
    arma::mat res = (F2-F1)/(Z2-Z1);
    return res;
}


#include <iostream>
#include <cmath>
using namespace std;

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

#include "sophus/so3.h"
#include "sophus/se3.h"

int main(int argc, char** argv) {
    //rotate 90 along the Z axis
    Eigen::Matrix3d R=Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Sophus::SO3 SO3_R(R);  //from the matrix
    Sophus::SO3 SO3_v(0,0,M_PI/2);  //from the vector
    Sophus::Quaterniond q(R);  //from the quaternion

    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_R<<endl;
    cout<<"SO(3) from quaternion: "<<SO3_R<<endl;

    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    cout<<"so3 hat"<<Sophus::SO3::hat(so3)<<endl; //hat:vector->matrix
    cout<<"so3 hat vee"<<Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose()<<endl; //vee:matrix->vector

    //BCH
    Eigen::Vector3d update_so3(1e-4,0,0);
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"Sophus updated = "<<SO3_updated<<endl;

    Eigen::Vector3d t(1,0,0);
    Sophus::SE3 SE3_Rt(R,t);
    Sophus::SE3 SE3_qt(q,t);
    cout<<"SE3 from R,t = "<<SE3_Rt<<endl;
    cout<<"SE3 from q,t = "<<SE3_qt<<endl;

    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose();//transfer is the former, rotation is the latter
    cout<<"se3 hat = "<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 vee = "<<Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose()<<endl;

    //update
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<SE3_updated.matrix()<<endl;

    return 0;
}


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

#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

    int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }
    int values() const { return m_values; }

};

struct my_functor : Functor<double>
{
    my_functor(void): Functor<double>(2,2) {}
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        // Implement y = 10*(x0+3)^2 + (x1-5)^2
        fvec(0) = 10.0*pow(x(0)+3.0,2) +  pow(x(1)-5.0,2);
        fvec(1) = 0;

        return 0;
    }
};

int main(int argc, char *argv[]) {
    Eigen::VectorXd x(2);
    x(0) = 2.0;
    x(1) = 3.0;
    std::cout << "x: " << x << std::endl;

    my_functor functor;
    Eigen::NumericalDiff<my_functor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
    lm.parameters.maxfev = 2000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << lm.parameters.maxfev << std::endl;

    int ret = lm.minimize(x);
    std::cout << lm.iter << std::endl;
    std::cout << ret << std::endl;

    std::cout << "x that minimizes the function: " << x << std::endl;

    std::cout << "press [ENTER] to continue " << std::endl;
    std::cin.get();
    return 0;
}


#include <eigen3/Eigen/Core>
#include <gtest/gtest.h>

#include <math.h>
#include <Eigen/Dense>

#include "mpi_cpp_tools/basic_tools.hpp"
#include "mpi_cpp_tools/math.hpp"
#include "mpi_cpp_tools/dynamical_systems.hpp"

using namespace mct;

double epsilon = 1e-10;

TEST(linear_dynamics, evolution_with_zero_initial_position)
{
    double jerk = 1.0;
    LinearDynamics linear_dynamics(jerk, 0.0, 0.0, 0.0);

    for(size_t t = 0; t < 100; t++)
    {
        EXPECT_TRUE(approx_equal(linear_dynamics.get_acceleration(t),
                                 t * jerk));
        EXPECT_TRUE(approx_equal(linear_dynamics.get_velocity(t),
                                 0.5 * t * t * jerk));
        EXPECT_TRUE(approx_equal(linear_dynamics.get_position(t),
                                 0.5 / 3.0 * t * t * t * jerk));
    }
}

// should be linear in initial state and control
TEST(linear_dynamics, check_linearity)
{
    Eigen::Vector4d parameters_a = Eigen::Vector4d(3.4, -5.6, 2.3, 7.2);
    Eigen::Vector4d parameters_b = Eigen::Vector4d(-2.1, -3.0, 5.5, -10.2);

    LinearDynamics linear_dynamics_a(parameters_a);
    LinearDynamics linear_dynamics_b(parameters_b);
    LinearDynamics linear_dynamics_sum(parameters_a + parameters_b);

    for(size_t t = 0; t < 100; t++)
    {
        EXPECT_TRUE(approx_equal(linear_dynamics_a.get_acceleration(t) +
                                 linear_dynamics_b.get_acceleration(t),
                                 linear_dynamics_sum.get_acceleration(t),
                                 1e-6));
        EXPECT_TRUE(approx_equal(linear_dynamics_a.get_velocity(t) +
                                 linear_dynamics_b.get_velocity(t),
                                 linear_dynamics_sum.get_velocity(t),
                                 1e-6));
        EXPECT_TRUE(approx_equal(linear_dynamics_a.get_position(t) +
                                 linear_dynamics_b.get_position(t),
                                 linear_dynamics_sum.get_position(t),
                                 1e-6));
    }
}

TEST(linear_dynamics, test_find_t_given_velocity_basic)
{
    Eigen::Vector4d parameters_basic = Eigen::Vector4d(1.0, -2.0, 0.0, 0.0);
    LinearDynamics dynamics_basic(parameters_basic);

    LinearDynamics::Vector solutions =
            dynamics_basic.find_t_given_velocity(- 3.0 / 2.0);

    EXPECT_TRUE(solutions.size() == 2);
    EXPECT_TRUE(contains(solutions, 1));
    EXPECT_TRUE(contains(solutions, 3));
    EXPECT_FALSE(contains(solutions, 4));
}

TEST(linear_dynamics, test_find_t_given_velocity_consistency)
{
    Eigen::Vector4d parameters_a = Eigen::Vector4d(3.4, -5.6, 2.3, 7.2);
    Eigen::Vector4d parameters_b = Eigen::Vector4d(-2.1, -3.0, 5.5, -10.2);
    LinearDynamics linear_dynamics_a(parameters_a);
    LinearDynamics linear_dynamics_b(parameters_b);

    for(size_t t = 0; t < 100; t++)
    {
        auto solutions_a = linear_dynamics_a.find_t_given_velocity(
                    linear_dynamics_a.get_velocity(t));
        EXPECT_TRUE(contains(solutions_a, t));
        auto solutions_b = linear_dynamics_b.find_t_given_velocity(
                    linear_dynamics_b.get_velocity(t));
        EXPECT_TRUE(contains(solutions_b, t));
    }
}

TEST(linear_dynamics_with_acceleration_constraint, test_time_evolution)
{
    Eigen::Matrix<double, 5, 1> parameters;
    parameters << -2.0, 15.0, 5.5, -10.2, 17.0;
    double jerk_duration = (17.0 + 15.0) / 2.0;

    LinearDynamicsWithAccelerationConstraint constrained_dynamics(parameters);
    LinearDynamics dynamics(parameters.topRows(4));
    LinearDynamicsWithAccelerationConstraint
            constrained_dynamics_b(-2.0,
                                   dynamics.get_acceleration(jerk_duration),
                                   dynamics.get_velocity(jerk_duration),
                                   dynamics.get_position(jerk_duration), 17.0);

    EXPECT_TRUE(approx_equal(dynamics.get_acceleration(jerk_duration), -17));
    EXPECT_TRUE(approx_equal(-17, constrained_dynamics.get_acceleration(
                                 jerk_duration + 100)));
    EXPECT_TRUE(approx_equal(dynamics.get_position(jerk_duration),
                             constrained_dynamics_b.get_position(0)));

    // make sure that ther is no discontinuity at jerk_duration ----------------
    double large_epsilon = std::sqrt(epsilon) / 100.0;
    double numeric, exact;

    numeric = constrained_dynamics.get_position(jerk_duration)
            + large_epsilon * constrained_dynamics.get_velocity(jerk_duration);
    exact = constrained_dynamics.get_position(jerk_duration + large_epsilon);
    ASSERT_TRUE(approx_equal(numeric, exact));

    numeric = constrained_dynamics.get_position(jerk_duration)
            - large_epsilon * constrained_dynamics.get_velocity(jerk_duration);
    exact = constrained_dynamics.get_position(jerk_duration - large_epsilon);
    ASSERT_TRUE(approx_equal(numeric, exact));

    // make sure dynamics coincide
    for(size_t t = 0; t < 10000; t++)
    {
        double acceleration_b = dynamics.get_acceleration(jerk_duration);
        double velocity_b =
                dynamics.get_velocity(jerk_duration) +
                dynamics.get_acceleration(jerk_duration) * t;
        double position_b =
                dynamics.get_position(jerk_duration) +
                dynamics.get_velocity(jerk_duration) * t +
                dynamics.get_acceleration(jerk_duration) * 0.5 * pow(t, 2);
        EXPECT_TRUE(approx_equal(acceleration_b,
                                 constrained_dynamics_b.get_acceleration(t)));
        EXPECT_TRUE(approx_equal(velocity_b,
                                 constrained_dynamics_b.get_velocity(t)));
        EXPECT_TRUE(approx_equal(position_b,
                                 constrained_dynamics_b.get_position(t)));

        EXPECT_TRUE(contains(constrained_dynamics.find_t_given_velocity(
                                 constrained_dynamics.get_velocity(t)), t));

        if(t <= jerk_duration)
        {
            EXPECT_TRUE(approx_equal(dynamics.get_acceleration(t),
                                     constrained_dynamics.get_acceleration(t)));
            EXPECT_TRUE(approx_equal(dynamics.get_velocity(t),
                                     constrained_dynamics.get_velocity(t)));
            EXPECT_TRUE(approx_equal(dynamics.get_position(t),
                                     constrained_dynamics.get_position(t)));
        }
        else
        {
            EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_acceleration(t - jerk_duration),
                                     constrained_dynamics.get_acceleration(t)));
            EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_velocity(t - jerk_duration),
                                     constrained_dynamics.get_velocity(t)));
            EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_position(t - jerk_duration),
                                     constrained_dynamics.get_position(t)));
        }
    }
}

TEST(linear_dynamics_with_acceleration_constraint, consistency_with_linear_dynamics)
{

    LinearDynamicsWithAccelerationConstraint
            constrained_dynamics(-1.5, -13.0, 5.5, 10.2, 13.0);
    LinearDynamics  dynamics(0.0, -13.0, 5.5, 10.2);

    // make sure dynamics coincide
    for(size_t t = 0; t < 100; t++)
    {

        EXPECT_TRUE(approx_equal(dynamics.get_acceleration(t),
                                 constrained_dynamics.get_acceleration(t)));
        EXPECT_TRUE(approx_equal(dynamics.get_velocity(t),
                                 constrained_dynamics.get_velocity(t)));
        EXPECT_TRUE(approx_equal(dynamics.get_position(t),
                                 constrained_dynamics.get_position(t)));

        LinearDynamics::Vector solutions =
                dynamics.find_t_given_velocity(t - 50.0);
        LinearDynamics::Vector constrained_solutions =
                constrained_dynamics.find_t_given_velocity(t - 50.0);

        EXPECT_TRUE(solutions.size() == constrained_solutions.size());

        for(size_t i = 0; i < solutions.size(); i++)
        {
            EXPECT_TRUE(contains(solutions, constrained_solutions[i]));
            EXPECT_TRUE(contains(constrained_solutions, solutions[i]));
            EXPECT_TRUE(approx_equal(solutions[i], - (t - 50.0 -5.5) / 13.0));
        }
    }
}

TEST(linear_dynamics_with_acceleration_constraint,
     test_find_t_given_velocity_basic_1)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics_basic(1.0, -2.0, 0.0, 0.0, 100.0);

    LinearDynamics::Vector solutions =
            dynamics_basic.find_t_given_velocity(- 3.0 / 2.0);

    EXPECT_TRUE(solutions.size() == 2);
    EXPECT_TRUE(contains(solutions, 1));
    EXPECT_TRUE(contains(solutions, 3));
    EXPECT_FALSE(contains(solutions, 4));
}

TEST(linear_dynamics_with_acceleration_constraint,
     test_find_t_given_velocity_basic_2)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics_basic(1.0, -5.0, 3.0, -10.0, 10.0);

    LinearDynamics::Vector solutions =
            dynamics_basic.find_t_given_velocity(-9);

    EXPECT_TRUE(solutions.size() == 2);
    EXPECT_TRUE(contains(solutions, 4));
    EXPECT_TRUE(contains(solutions, 6));
    EXPECT_FALSE(contains(solutions, 7));
}

TEST(linear_dynamics_with_acceleration_constraint,
     test_find_t_given_velocity_basic_3)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics(1.0, -1.0, 3.0, -2.0, 2.0);

    EXPECT_TRUE(approx_equal(dynamics.get_velocity(10.0), 18.5));

    LinearDynamics::Vector solutions = dynamics.find_t_given_velocity(18.5);

    EXPECT_TRUE(solutions.size() == 1);
    EXPECT_TRUE(approx_equal(solutions[0], 10.0));
}

TEST(linear_dynamics_with_acceleration_constraint,
     test_find_t_given_velocity_consistency)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics_a(3.4, -5.6, 2.3, 7.2, 20);
    LinearDynamicsWithAccelerationConstraint
            dynamics_b(-2.1, -3.0, 5.5, -10.2, 44.3);

    for(size_t t = 0; t < 100; t++)
    {
        auto solutions_a = dynamics_a.find_t_given_velocity(
                    dynamics_a.get_velocity(t));
        EXPECT_TRUE(contains(solutions_a, t));
        auto solutions_b = dynamics_b.find_t_given_velocity(
                    dynamics_b.get_velocity(t));
        EXPECT_TRUE(contains(solutions_b, t));
    }
}

TEST(linear_dynamics_with_acceleration_constraint,
     will_exceed_jointly)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics(-0.4, 2.6, 200.3, 7.2, 3);
    double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2;

    std::vector<Eigen::Vector2d> trajectory;
    size_t n_iterations = 1000;
    for(size_t i = 0; i < n_iterations; i++)
    {
        double t = double(i) / n_iterations * max_t;

        Eigen::Vector2d point;
        point[0] = dynamics.get_velocity(t);
        point[1] = dynamics.get_position(t);
        trajectory.push_back(point);
    }

    for(auto& point : trajectory)
    {
        std::vector<Eigen::Vector2d> constraints;
        constraints.push_back(point + Eigen::Vector2d(0.001, 0.001));
        constraints.push_back(point - Eigen::Vector2d(epsilon, epsilon));

        constraints.push_back(point + Eigen::Vector2d(-epsilon, std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(-epsilon, -std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(0.001, -std::numeric_limits<double>::infinity()));

        constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -epsilon));
        constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), -epsilon));
        constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), 0.001));

        for(auto& constraint : constraints)
        {

            bool label = false;
            for(auto& point : trajectory)
            {
                if(point[0] > constraint[0] && point[1] > constraint[1])
                {
                    //                    std::cout << "point: " << point.transpose() << std::endl;
                    label = true;
                    break;
                }
            }

            double certificate_time;
            bool assigned_label =
                    dynamics.will_exceed_jointly(constraint[0], constraint[1],
                    certificate_time);

            if(label != assigned_label)
            {
                std::cout << "---------------------" << std::endl;
                std::cout << "constraint: " << constraint.transpose() << std::endl;

                std::cout << "label: " << label << "  assigned_label: " << assigned_label << std::endl;
                std::cout << "certificate time: " << certificate_time
                          << " certificate point: "
                          << dynamics.get_velocity(certificate_time) << ", "
                          << dynamics.get_position(certificate_time) << std::endl;
            }
            EXPECT_TRUE(label == assigned_label);
        }
    }
}

TEST(linear_dynamics_with_acceleration_constraint,
     will_exceed_jointly_2)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics(-0.4, 20.6, -2.3, 7.2, 30);
    double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2;

    std::vector<Eigen::Vector2d> trajectory;
    size_t n_iterations = 1000;
    for(size_t i = 0; i < n_iterations; i++)
    {
        double t = double(i) / n_iterations * max_t;

        Eigen::Vector2d point;
        point[0] = dynamics.get_velocity(t);
        point[1] = dynamics.get_position(t);
        trajectory.push_back(point);
    }

    for(auto& point : trajectory)
    {
        std::vector<Eigen::Vector2d> constraints;
        constraints.push_back(point + Eigen::Vector2d(0.001, 0.001));
        constraints.push_back(point - Eigen::Vector2d(epsilon, epsilon));

        constraints.push_back(point + Eigen::Vector2d(-epsilon, std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(-epsilon, -std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(0.001, -std::numeric_limits<double>::infinity()));

        constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -epsilon));
        constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), -epsilon));
        constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), 0.001));

        for(auto& constraint : constraints)
        {

            bool label = false;
            for(auto& point : trajectory)
            {
                if(point[0] > constraint[0] && point[1] > constraint[1])
                {
                    //                    std::cout << "point: " << point.transpose() << std::endl;
                    label = true;
                    break;
                }
            }

            double certificate_time;
            bool assigned_label =
                    dynamics.will_exceed_jointly(constraint[0], constraint[1],
                    certificate_time);

            if(label != assigned_label)
            {
                std::cout << "---------------------" << std::endl;
                std::cout << "constraint: " << constraint.transpose() << std::endl;

                std::cout << "label: " << label << "  assigned_label: " << assigned_label << std::endl;
                std::cout << "certificate time: " << certificate_time
                          << " certificate point: "
                          << dynamics.get_velocity(certificate_time) << ", "
                          << dynamics.get_position(certificate_time) << std::endl;
            }
            EXPECT_TRUE(label == assigned_label);
        }
    }
}

TEST(linear_dynamics_with_acceleration_constraint,
     will_deceed_jointly)
{
    LinearDynamicsWithAccelerationConstraint
            dynamics(0.4, 2.6, -200.3, 7.2, 3);
    double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2;

    std::vector<Eigen::Vector2d> trajectory;
    size_t n_iterations = 1000;
    for(size_t i = 0; i < n_iterations; i++)
    {
        double t = double(i) / n_iterations * max_t;

        Eigen::Vector2d point;
        point[0] = dynamics.get_velocity(t);
        point[1] = dynamics.get_position(t);
        trajectory.push_back(point);
    }

    for(auto& point : trajectory)
    {
        std::vector<Eigen::Vector2d> constraints;
        constraints.push_back(point - Eigen::Vector2d(0.001, 0.001));
        constraints.push_back(point + Eigen::Vector2d(epsilon, epsilon));

        constraints.push_back(point + Eigen::Vector2d(epsilon, std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(epsilon, -std::numeric_limits<double>::infinity()));
        constraints.push_back(point + Eigen::Vector2d(-0.001, std::numeric_limits<double>::infinity()));

        constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), epsilon));
        constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), epsilon));
        constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -0.001));

        for(auto& constraint : constraints)
        {

            bool label = false;
            for(auto& point : trajectory)
            {
                if(point[0] < constraint[0] && point[1] < constraint[1])
                {
                    //                    std::cout << "point: " << point.transpose() << std::endl;
                    label = true;
                    break;
                }
            }

            double certificate_time;
            bool assigned_label =
                    dynamics.will_deceed_jointly(constraint[0], constraint[1],
                    certificate_time);

            if(label != assigned_label)
            {
                std::cout << "---------------------" << std::endl;
                std::cout << "constraint: " << constraint.transpose() << std::endl;

                std::cout << "label: " << label << "  assigned_label: " << assigned_label << std::endl;
                std::cout << "certificate time: " << certificate_time
                          << " certificate point: "
                          << dynamics.get_velocity(certificate_time) << ", "
                          << dynamics.get_position(certificate_time) << std::endl;
            }
            EXPECT_TRUE(label == assigned_label);
        }
    }
}

double sample_uniformely(const double& min, const double& max)
{
    return min + double(rand()) / RAND_MAX * (max - min);
}

TEST(find_max_admissible_acceleration, generated_trajectories)
{
    srand(0);

    for(size_t unused = 0; unused < 100; unused++)
    {
        // initialize parameters randomly --------------------------------------

        double initial_velocity = sample_uniformely(-20.0, 20.0);
        double initial_position = sample_uniformely(-20.0, 20.0);
        NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0);
        NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0);

        double initial_acceleration = sample_uniformely(-abs_acceleration_limit,
                                                        abs_acceleration_limit);

        // find some constraints which is just barely satisfied --------------------
        LinearDynamicsWithAccelerationConstraint dynamics(-abs_jerk_limit,
                                                          initial_acceleration,
                                                          initial_velocity,
                                                          initial_position,
                                                          abs_acceleration_limit);

        double max_t = 20.0;
        if(dynamics.find_t_given_velocity(0).size() > 0)
        {
            max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2;
        }

        size_t n_iterations = 10000;
        int T = rand() % n_iterations;
        Eigen::Vector2d constraint;
        constraint[0] = dynamics.get_velocity(T);
        constraint[1] = dynamics.get_position(T);

        Eigen::Vector2d position_constraint(-std::numeric_limits<double>::infinity(),
                                            -std::numeric_limits<double>::infinity());
        Eigen::Vector2d velocity_constraint(-std::numeric_limits<double>::infinity(),
                                            -std::numeric_limits<double>::infinity());

        for(size_t i = 0; i < n_iterations; i++)
        {
            double t = double(i) / n_iterations * max_t;
            Eigen::Vector2d point;
            point[0] = dynamics.get_velocity(t);
            point[1] = dynamics.get_position(t);
            if(point[0] > constraint[0] && point[1] > constraint[1])
                constraint = point;
            velocity_constraint[0] = std::max(velocity_constraint[0], point[0]);
            position_constraint[1] = std::max(position_constraint[1], point[1]);
        }

        // test that we get the same initial acceleration --------------------------
        for(auto& c : {constraint, position_constraint, velocity_constraint})
        {

            double max_admissible_initial_acceleration =
                    find_max_admissible_acceleration(
                        initial_velocity,
                        initial_position,
                        c[0],
                    c[1],
                    abs_jerk_limit,
                    abs_acceleration_limit);

            if(std::fabs(initial_acceleration -
                         max_admissible_initial_acceleration) > 0.02)
            {
                if(max_admissible_initial_acceleration - 0.0001 >
                        -abs_acceleration_limit)
                {
                    LinearDynamicsWithAccelerationConstraint
                            below_limit_dynamics(-abs_jerk_limit,
                                                 max_admissible_initial_acceleration
                                                 - 0.0001,
                                                 initial_velocity,
                                                 initial_position,
                                                 abs_acceleration_limit);
                    ASSERT_TRUE(below_limit_dynamics.will_exceed_jointly(c[0], c[1])
                            == false);
                }

                if(max_admissible_initial_acceleration + 0.0001 <
                        abs_acceleration_limit)
                {
                    LinearDynamicsWithAccelerationConstraint
                            above_limit_dynamics(-abs_jerk_limit,
                                                 max_admissible_initial_acceleration
                                                 + 0.0001,
                                                 initial_velocity,
                                                 initial_position,
                                                 abs_acceleration_limit);
                    ASSERT_TRUE(above_limit_dynamics.will_exceed_jointly(c[0], c[1])
                            == true);
                }

                //                std::cout << "------------------------------------  " << std::endl;

                //                dynamics.print_parameters();

                //                std::cout << "constraint " << c << std::endl;

                //                std::cout << " initial_acceleration " << initial_acceleration
                //                          << " max_admissible_initial_acceleration "
                //                          << max_admissible_initial_acceleration << std::endl;

                //                bool will_exceed_epsilon = dynamics.will_exceed_jointly(c[0], c[1]);

                //                bool will_exceed_minus_epsilon = dynamics.will_exceed_jointly(c[0] - epsilon, c[1] - epsilon);

                //                bool limit_dynamics_will_exceed = limit_dynamics.will_exceed_jointly(c[0] - epsilon, c[1] - epsilon);

                //                std::cout << " will_exceed_epsilon " << will_exceed_epsilon
                //                          << " will_exceed_minus_epsilon " << will_exceed_minus_epsilon
                //                          << " limit_dynamics_will_exceed " << limit_dynamics_will_exceed

                //                          << std::endl;

                ASSERT_TRUE(approx_equal(c[0], initial_velocity) ||
                        approx_equal(c[1], initial_position));
            }

            //            ASSERT_TRUE(std::fabs(initial_acceleration -
            //                                  max_admissible_initial_acceleration) <= 0.001);
        }
    }
}

TEST(find_max_admissible_acceleration, random_points)
{
    srand(0);

    for(size_t unused = 0; unused < 10000; unused++)
    {
        // initialize parameters randomly --------------------------------------
        double initial_velocity = sample_uniformely(-20.0, 20.0);
        double initial_position = sample_uniformely(-20.0, 20.0);

        double max_velocity = sample_uniformely(-20.0, 20.0);
        double max_position = sample_uniformely(-20.0, 20.0);

        NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0);
        NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0);

        double max_admissible_acceleration =
                find_max_admissible_acceleration(
                    initial_velocity,
                    initial_position,
                    max_velocity,
                    max_position,
                    abs_jerk_limit,
                    abs_acceleration_limit);

        if(std::fabs(max_admissible_acceleration - 0.0001) <=
                abs_acceleration_limit)
        {
            LinearDynamicsWithAccelerationConstraint
                    below_limit_dynamics(-abs_jerk_limit,
                                         max_admissible_acceleration
                                         - 0.0001,
                                         initial_velocity,
                                         initial_position,
                                         abs_acceleration_limit);
            ASSERT_TRUE(below_limit_dynamics.will_exceed_jointly(max_velocity, max_position)
                        == false);
        }
        if(std::fabs(max_admissible_acceleration + 0.0001) <=
                abs_acceleration_limit)
        {
            LinearDynamicsWithAccelerationConstraint
                    above_limit_dynamics(-abs_jerk_limit,
                                         max_admissible_acceleration
                                         + 0.0001,
                                         initial_velocity,
                                         initial_position,
                                         abs_acceleration_limit);
            ASSERT_TRUE(above_limit_dynamics.will_exceed_jointly(max_velocity, max_position)
                        == true);
        }
    }
}

TEST(find_min_admissible_acceleration, random_points)
{
    srand(0);

    for(size_t unused = 0; unused < 10; unused++)
    {
        // initialize parameters randomly --------------------------------------
        double initial_velocity = sample_uniformely(-20.0, 20.0);
        double initial_position = sample_uniformely(-20.0, 20.0);

        double min_velocity = sample_uniformely(-20.0, 20.0);
        double min_position = sample_uniformely(-20.0, 20.0);

        NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0);
        NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0);

        double min_admissible_acceleration =
                find_min_admissible_acceleration(
                    initial_velocity,
                    initial_position,
                    min_velocity,
                    min_position,
                    abs_jerk_limit,
                    abs_acceleration_limit);

        if(std::fabs(min_admissible_acceleration - 0.0001) <=
                abs_acceleration_limit)
        {
            LinearDynamicsWithAccelerationConstraint
                    below_limit_dynamics(abs_jerk_limit,
                                         min_admissible_acceleration
                                         - 0.0001,
                                         initial_velocity,
                                         initial_position,
                                         abs_acceleration_limit);
            ASSERT_TRUE(below_limit_dynamics.will_deceed_jointly(min_velocity,
                                                                 min_position)
                        == true);
        }
        if(std::fabs(min_admissible_acceleration + 0.0001) <=
                abs_acceleration_limit)
        {
            LinearDynamicsWithAccelerationConstraint
                    above_limit_dynamics(abs_jerk_limit,
                                         min_admissible_acceleration
                                         + 0.0001,
                                         initial_velocity,
                                         initial_position,
                                         abs_acceleration_limit);
            ASSERT_TRUE(above_limit_dynamics.will_deceed_jointly(min_velocity,
                                                                 min_position)
                        == false);
        }
    }
}


#include "convhull_volume.h"
#define CONVHULL_3D_ENABLE
#include "convhull_3d.h"
#include <Eigen/Dense>

double convexHullVolume(const Eigen::MatrixXd& verts)
{
  // convert data
  int nVerts = verts.cols();
  ch_vertex* vertices;
  vertices = (ch_vertex*)malloc(nVerts*sizeof(ch_vertex));
  for (int i = 0; i < nVerts; i++) {
    vertices[i].x = verts(0, i);
    vertices[i].y = verts(1, i);
    vertices[i].z = verts(2, i);
  }
  int* faceIndices = NULL;
  int nFaces;
  convhull_3d_build(vertices, nVerts, &faceIndices, &nFaces);
  Eigen::Map<Eigen::MatrixXi> faces(faceIndices, 3, nFaces);
  double vol = polygonVolume(verts, faces);
  free(vertices);
  free(faceIndices);
  return vol;
}

double polygonVolume(const Eigen::MatrixXd& vertices, const Eigen::MatrixXi& faces)
{
  double vol = 0;
  Eigen::Vector3d com = vertices.rowwise().mean();
  for (int i=0; i < faces.cols(); i++)
  {
    Eigen::Vector3i f = faces.col(i);
    Eigen::Vector3d a = vertices.col(f(0)) - com;
    Eigen::Vector3d b = vertices.col(f(1)) - com;
    Eigen::Vector3d c = vertices.col(f(2)) - com;

    double e_vol = a.cross(b).dot(c);
    vol += e_vol;
  }
  vol /= 6.0;
  return vol;
}


/** @file main.cpp Demonstrates purely functional, tail-recursive,
  * arbitrary-precision factorial and Fibonacci functions using
  * Boost.Multiprecision.
  *
  * @note That this implementation passes parameters by constant reference, but
  * returns by value.  This approach strikes a balance between high- and
  * low-level concerns.
  *
  * @see pretty.cpp for less noisy (but potentially less efficient) code.
  *
  * @see control.cpp for tighter control over memory.
  */

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using big_int = boost::multiprecision::cpp_int;

big_int factorial(int n, big_int const& r =1)
{
    return n ? factorial(n - 1, r * n) : r;
}

big_int fibonacci(int n, big_int const& a =1, big_int const& b =1)
{
    return n ? fibonacci(n - 1, b, a + b) : a;
}

int main()
{
    for (int i = 0; i < 100; ++i) {
        std::cout << i << ' ' << factorial(i) << ' ' << fibonacci(i) << '\n';
    }
}


//author: snassr
#pragma once

//.includes
//..stl library
#include <string>
	//for std::string
#include <algorithm>
	//for std::max
#include <vector>
	//for std::vector
//..armadillo library
#include <armadillo>
	//for arma::mat
//..sLab library
#include "EditDistance.hpp"

//.globals and declarations
/*
*
*
*/

/* Levenshtein Minimum Edit Distance
*
* Definition:
* -----------
* A computation of the distance between two strings D(n,m)
* 		[where n= length of stringOne, m = length of stringTwo]
* This is done via a tabular (matrix) computation by combining solutions to subproblems.
*
* Specific-to-General Solution:
* -----------------------------
* 	1. compute D(i,j) for small i,j (origin 0)
* 	2. compute D(i,j) for consequently larger substrings via character appension until both strings are fully compared at D(n,m)
* 			- compute D(i,j) for all i(0 < i < n) and j(0 < j < m)
*
* Formula:
* --------
* D(i,j) = Min( { D(i-1,j) + 1, //deletion operation from i to compare j
* 				{ D(i,j-1) + 1, //insertion operation to i to compare j
* 				{ D(i-1, j-1) +  2; x(i) != y(j) //substitution character operation in i (deletion & insertion)
* 				{				 0; x(i) == y(j))// to compare j, cost is 0 if no substitution is required, otherwise x (x:the set value of subCost)
*			  )
*
*/

class LevenshteinDistance : public EditDistance
{
	//...member functions
	//...public interface
public:
	//.....overload operators
	//....constructors
	//.....default constructor
	//.....parameterized constructor
		LevenshteinDistance(std::string stringOne, std::string stringTwo, int inSubstitutionCost);
	//.....destructor
	//....operations
	//.....observers
		double calculateDistance();
		double averageStringLengthSimilarity();
		double maximizedStringLengthSimilarity();
		double maximizedStringElementSimilarity();
	//.....mutators
	//...private members
private:
	//....data members
	//....functions
};

//non-member functions

//-------------------------------------------------------------------------------------------implementation---
LevenshteinDistance::LevenshteinDistance(std::string inStringOne, std::string inStringTwo, int inCost) {
	setStringOne(inStringOne);
	setStringTwo(inStringTwo);
	setSubstitutionCost(inCost);
}

double LevenshteinDistance::calculateDistance() {
	std::string strOne = getStringOne();
	std::string strTwo = getStringTwo();
	char charInStrOne = 0;
	char charInStrTwo = 0;
	int matrixRows = getStringOneLength() + 1;
	int matrixCols = getStringTwoLength() + 1;
	int cost;

	arma::mat distanceMatrix(matrixRows, matrixCols, arma::fill::zeros);

	for (int a = 0; a < matrixRows; a++)
		distanceMatrix(a, 0) = a;
	for (int b = 0; b < matrixCols; b++)
		distanceMatrix(0, b) = b;

	for (int rowNum = 1; rowNum < matrixRows; rowNum++) {
		charInStrOne = strOne.at(rowNum - 1);

		for (int colNum = 1; colNum < matrixCols; colNum++) {
			charInStrTwo = strTwo.at(colNum - 1);
			if (charInStrOne == charInStrTwo)
				cost = 0;
			else
				cost = getSubstitutionCost();

		distanceMatrix(rowNum, colNum) = findMinimum(distanceMatrix(rowNum - 1, colNum) + 1, distanceMatrix(rowNum, colNum - 1) + 1, distanceMatrix(rowNum - 1, colNum - 1) + cost);
		}
	}
	return distanceMatrix(getStringOneLength(), getStringTwoLength());
}

double LevenshteinDistance::averageStringLengthSimilarity() { return 0.0; }

double LevenshteinDistance::maximizedStringLengthSimilarity() { return 0.0; }

double LevenshteinDistance::maximizedStringElementSimilarity() { return 0.0; }


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// The scalar function for which the gradient is needed
dual f(const VectorXdual& x, dual p)
{
    return x.cwiseProduct(x).sum() * exp(p); // sum([x(i) * x(i) for i = 1:5]) * exp(p)
}

int main()
{
    VectorXdual x(5);    // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    dual p = 3;    // the input parameter vector p with 3 variables

    dual u;  // the output scalar u = f(x, p) evaluated together with gradient below

    VectorXd gpx = gradient(f, wrtpack(p, x), at(x, p), u);  // evaluate the function value u and its gradient vector gp = [du/dp, du/dx]

    cout << "u = " << u << endl;    // print the evaluated output u
    cout << "gpx = \n" << gpx << endl;  // print the evaluated gradient vector gp = [du/dp, du/dx]
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;

std::ofstream debug("debug_g2o.txt");

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, Sophus::SE3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void setToOriginImpl() override {
        _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl(const double* update) override {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> update_v(update);
        _estimate = Sophus::SE3d::exp(update_v) * _estimate;
    }

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        _estimate = Sophus::SE3d(
            Quaterniond(data[6], data[3], data[4], data[5]),
            Vector3d(data[0], data[1], data[2])
        );
        return true;
    }

    virtual bool write(ostream &os) const override {
        os << id() << " ";
        Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
        return true;
    }

};

class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, Sophus::SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void computeError() override {
        auto* v0 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[0]);
        auto* v1 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[1]);
        _error = (_measurement.inverse() * v0->estimate().inverse() * v1->estimate()).log();

        Sophus::SE3d rt0 = v0->estimate();
        Sophus::SE3d rt1 = v1->estimate();
        debug << v0->id() << " " << v1->id() << " ";
        // debug << rt0.translation().transpose();
        auto q = _measurement.unit_quaternion();
        debug << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
        // for (int i = 0; i < 6; ++i)
        // debug << _error[i] << " ";
        // debug << _error.transpose();
        debug << "\n";
    }

    virtual void linearizeOplus() override {
        // auto* v0 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[0]);
        auto* v1 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[1]);
        // auto T0 = v0->estimate();
        auto T1 = v1->estimate();

        Sophus::SE3d e = Sophus::SE3d::exp(_error);
        Eigen::Matrix<double, 6, 6> J_inv = Eigen::Matrix<double, 6, 6>::Zero();
        J_inv.block<3, 3>(0, 0) = Sophus::SO3d::hat(e.so3().log());
        J_inv.block<3, 3>(3, 3) = Sophus::SO3d::hat(e.so3().log());
        J_inv.block<3, 3>(0, 3) = Sophus::SO3d::hat(e.translation());
        J_inv *= 0.5;
        J_inv += Eigen::Matrix<double, 6, 6>::Identity();

        // also possible to approximate J_inv with Identity

        _jacobianOplusXi = -J_inv * T1.inverse().Adj();
        _jacobianOplusXj = J_inv * T1.inverse().Adj();
    }

    virtual bool read(std::istream& is) override {
        double data[7];
        for (int i = 0; i < 7; ++i)
            is >> data[i];
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        Eigen::Vector3d t(data);
        _measurement = Sophus::SE3d(q, t);
        for (int i = 0; i < _information.rows() && is.good(); ++i)
        {
            for (int j = i; j < _information.cols() && is.good(); ++j)
            {
                is >> _information(i, j);
                _information(j, i) = _information(i, j);
            }
        }
        return true;
    }

    virtual bool write(std::ostream& os) const override {
        auto *v0 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[0]);
        auto *v1 = dynamic_cast<VertexSE3LieAlgebra*>(_vertices[1]);
        os << v0->id() << " " << v1->id() << " ";
        auto q = _measurement.unit_quaternion();
        auto t = _measurement.translation();
        os << t.x() << " " << t.y() << " " << t.z() << " ";
        os << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";

        for (int i = 0; i < _information.rows(); ++i)
        {
            for (int j = i; j < _information.cols(); ++j)
            {
                os << _information(i, j) << " ";
            }
        }
        os << std::endl;
        return true;
    }

};

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    int vertexCnt = 0, edgeCnt = 0;

    vector<VertexSE3LieAlgebra *> vectices;
    vector<EdgeSE3LieAlgebra *> edges;
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // 顶点
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
            int idx1, idx2;
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(1);

    cout << "saving optimization results ..." << endl;

    ofstream fout("result_lie.g2o");
    for (VertexSE3LieAlgebra *v:vectices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for (EdgeSE3LieAlgebra *e:edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    debug.close();
    return 0;
}


// Copyright John Maddock 2006
// Copyright Paul A. Bristow 2007, 2008, 2010

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

#ifdef _MSC_VER
#  pragma warning(disable: 4512) // assignment operator could not be generated.
#  pragma warning(disable: 4510) // default constructor could not be generated.
#  pragma warning(disable: 4610) // can never be instantiated - user defined constructor required.
#  pragma warning(disable: 4180) // qualifier has no effect (in Fusion).
#endif

#include <iostream>
using std::cout; using std::endl;
using std::left; using std::fixed; using std::right; using std::scientific;
#include <iomanip>
using std::setw;
using std::setprecision;

#include <boost/math/distributions/fisher_f.hpp>

void f_test(
       double sd1,     // Sample 1 std deviation
       double sd2,     // Sample 2 std deviation
       double N1,      // Sample 1 size
       double N2,      // Sample 2 size
       double alpha)  // Significance level
{
   //
   // An F test applied to two sets of data.
   // We are testing the null hypothesis that the
   // standard deviation of the samples is equal, and
   // that any variation is down to chance.  We can
   // also test the alternative hypothesis that any
   // difference is not down to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda359.htm
   //
   // Avoid "using namespace boost::math;" because of potential name ambiguity.
   using boost::math::fisher_f;

   // Print header:
   cout <<
      "____________________________________\n"
      "F test for equal standard deviations\n"
      "____________________________________\n\n";
   cout << setprecision(5);
   cout << "Sample 1:\n";
   cout << setw(55) << left << "Number of Observations" << "=  " << N1 << "\n";
   cout << setw(55) << left << "Sample Standard Deviation" << "=  " << sd1 << "\n\n";
   cout << "Sample 2:\n";
   cout << setw(55) << left << "Number of Observations" << "=  " << N2 << "\n";
   cout << setw(55) << left << "Sample Standard Deviation" << "=  " << sd2 << "\n\n";
   //
   // Now we can calculate and output some stats:
   //
   // F-statistic:
   double F = (sd1 / sd2);
   F *= F;
   cout << setw(55) << left << "Test Statistic" << "=  " << F << "\n\n";
   //
   // Finally define our distribution, and get the probability:
   //
   fisher_f dist(N1 - 1, N2 - 1);
   double p = cdf(dist, F);
   cout << setw(55) << left << "CDF of test statistic: " << "=  "
      << setprecision(3) << scientific << p << "\n";
   double ucv = quantile(complement(dist, alpha));
   double ucv2 = quantile(complement(dist, alpha / 2));
   double lcv = quantile(dist, alpha);
   double lcv2 = quantile(dist, alpha / 2);
   cout << setw(55) << left << "Upper Critical Value at alpha: " << "=  "
      << setprecision(3) << scientific << ucv << "\n";
   cout << setw(55) << left << "Upper Critical Value at alpha/2: " << "=  "
      << setprecision(3) << scientific << ucv2 << "\n";
   cout << setw(55) << left << "Lower Critical Value at alpha: " << "=  "
      << setprecision(3) << scientific << lcv << "\n";
   cout << setw(55) << left << "Lower Critical Value at alpha/2: " << "=  "
      << setprecision(3) << scientific << lcv2 << "\n\n";
   //
   // Finally print out results of null and alternative hypothesis:
   //
   cout << setw(55) << left <<
      "Results for Alternative Hypothesis and alpha" << "=  "
      << setprecision(4) << fixed << alpha << "\n\n";
   cout << "Alternative Hypothesis                                    Conclusion\n";
   cout << "Standard deviations are unequal (two sided test)          ";
   if((ucv2 < F) || (lcv2 > F))
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Standard deviation 1 is less than standard deviation 2    ";
   if(lcv > F)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << "Standard deviation 1 is greater than standard deviation 2 ";
   if(ucv < F)
      cout << "NOT REJECTED\n";
   else
      cout << "REJECTED\n";
   cout << endl << endl;
}

int main()
{
   //
   // Run tests for ceramic strength data:
   // see http://www.itl.nist.gov/div898/handbook/eda/section4/eda42a1.htm
   // The data for this case study were collected by Said Jahanmir of the
   // NIST Ceramics Division in 1996 in connection with a NIST/industry
   // ceramics consortium for strength optimization of ceramic strength.
   //
   f_test(65.54909, 61.85425, 240, 240, 0.05);
   //
   // And again for the process change comparison:
   // see http://www.itl.nist.gov/div898/handbook/prc/section3/prc32.htm
   // A new procedure to assemble a device is introduced and tested for
   // possible improvement in time of assembly. The question being addressed
   // is whether the standard deviation of the new assembly process (sample 2) is
   // better (i.e., smaller) than the standard deviation for the old assembly
   // process (sample 1).
   //
   f_test(4.9082, 2.5874, 11, 9, 0.05);
   return 0;
}

/*

Output:

  f_test.cpp
  F-test_example1.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Debug\F_test_example1.exe
  ____________________________________
  F test for equal standard deviations
  ____________________________________

  Sample 1:
  Number of Observations                                 =  240
  Sample Standard Deviation                              =  65.549

  Sample 2:
  Number of Observations                                 =  240
  Sample Standard Deviation                              =  61.854

  Test Statistic                                         =  1.123

  CDF of test statistic:                                 =  8.148e-001
  Upper Critical Value at alpha:                         =  1.238e+000
  Upper Critical Value at alpha/2:                       =  1.289e+000
  Lower Critical Value at alpha:                         =  8.080e-001
  Lower Critical Value at alpha/2:                       =  7.756e-001

  Results for Alternative Hypothesis and alpha           =  0.0500

  Alternative Hypothesis                                    Conclusion
  Standard deviations are unequal (two sided test)          REJECTED
  Standard deviation 1 is less than standard deviation 2    REJECTED
  Standard deviation 1 is greater than standard deviation 2 REJECTED

  ____________________________________
  F test for equal standard deviations
  ____________________________________

  Sample 1:
  Number of Observations                                 =  11.00000
  Sample Standard Deviation                              =  4.90820

  Sample 2:
  Number of Observations                                 =  9.00000
  Sample Standard Deviation                              =  2.58740

  Test Statistic                                         =  3.59847

  CDF of test statistic:                                 =  9.589e-001
  Upper Critical Value at alpha:                         =  3.347e+000
  Upper Critical Value at alpha/2:                       =  4.295e+000
  Lower Critical Value at alpha:                         =  3.256e-001
  Lower Critical Value at alpha/2:                       =  2.594e-001

  Results for Alternative Hypothesis and alpha           =  0.0500

  Alternative Hypothesis                                    Conclusion
  Standard deviations are unequal (two sided test)          REJECTED
  Standard deviation 1 is less than standard deviation 2    REJECTED
  Standard deviation 1 is greater than standard deviation 2 NOT REJECTED

  ____________________________________
  F test for equal standard deviations
  ____________________________________

  Sample 1:
  Number of Observations                                 =  240
  Sample Standard Deviation                              =  65.549

  Sample 2:
  Number of Observations                                 =  240
  Sample Standard Deviation                              =  61.854

  Test Statistic                                         =  1.123

  CDF of test statistic:                                 =  8.148e-001
  Upper Critical Value at alpha:                         =  1.238e+000
  Upper Critical Value at alpha/2:                       =  1.289e+000
  Lower Critical Value at alpha:                         =  8.080e-001
  Lower Critical Value at alpha/2:                       =  7.756e-001

  Results for Alternative Hypothesis and alpha           =  0.0500

  Alternative Hypothesis                                    Conclusion
  Standard deviations are unequal (two sided test)          REJECTED
  Standard deviation 1 is less than standard deviation 2    REJECTED
  Standard deviation 1 is greater than standard deviation 2 REJECTED

  ____________________________________
  F test for equal standard deviations
  ____________________________________

  Sample 1:
  Number of Observations                                 =  11.00000
  Sample Standard Deviation                              =  4.90820

  Sample 2:
  Number of Observations                                 =  9.00000
  Sample Standard Deviation                              =  2.58740

  Test Statistic                                         =  3.59847

  CDF of test statistic:                                 =  9.589e-001
  Upper Critical Value at alpha:                         =  3.347e+000
  Upper Critical Value at alpha/2:                       =  4.295e+000
  Lower Critical Value at alpha:                         =  3.256e-001
  Lower Critical Value at alpha/2:                       =  2.594e-001

  Results for Alternative Hypothesis and alpha           =  0.0500

  Alternative Hypothesis                                    Conclusion
  Standard deviations are unequal (two sided test)          REJECTED
  Standard deviation 1 is less than standard deviation 2    REJECTED
  Standard deviation 1 is greater than standard deviation 2 NOT REJECTED

*/


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXf A = MatrixXf::Random(4,4);
MatrixXf B = MatrixXf::Random(4,4);
RealQZ<MatrixXf> qz(4); // preallocate space for 4x4 matrices
qz.compute(A,B);  // A = Q S Z,  B = Q T Z

// print original matrices and result of decomposition
cout << "A:\n" << A << "\n" << "B:\n" << B << "\n";
cout << "S:\n" << qz.matrixS() << "\n" << "T:\n" << qz.matrixT() << "\n";
cout << "Q:\n" << qz.matrixQ() << "\n" << "Z:\n" << qz.matrixZ() << "\n";

// verify precision
cout << "\nErrors:"
  << "\n|A-QSZ|: " << (A-qz.matrixQ()*qz.matrixS()*qz.matrixZ()).norm()
  << ", |B-QTZ|: " << (B-qz.matrixQ()*qz.matrixT()*qz.matrixZ()).norm()
  << "\n|QQ* - I|: " << (qz.matrixQ()*qz.matrixQ().adjoint() - MatrixXf::Identity(4,4)).norm()
  << ", |ZZ* - I|: " << (qz.matrixZ()*qz.matrixZ().adjoint() - MatrixXf::Identity(4,4)).norm()
  << "\n";

  return 0;
}


/**
 * @file matode.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>

namespace MatODE {

/* SAM_LISTING_BEGIN_3 */
Eigen::MatrixXd eeulstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                         double h) {
  //====================
  // Your code goes here

  // explicit Euler step
  return Y0 + h * A * Y0;

  //====================
  //return Y0;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::MatrixXd ieulstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                         double h) {
  //====================
  // Your code goes here

  // implicit Euler step

  return (Eigen::MatrixXd::Identity(Y0.rows(), Y0.cols()) - h*A).lu().solve(Y0);

  //====================
  //return Y0;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::MatrixXd impstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                        double h) {
  //====================
  // Your code goes here

  return (Eigen::MatrixXd::Identity(Y0.rows(), Y0.cols()) - h/2. * A).lu().solve(Y0 + h/2. * A * Y0);

  //====================
  //return Y0;
}
/* SAM_LISTING_END_5 */

}  // namespace MatODE


#include <iostream>
struct init {
  init() { std::cout << "[" << "init" << "]" << std::endl; }
};
init init_obj;
// [init]
#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
  MatrixXd A(2,2);
  A << 2, -1, 1, 3;
  cout << "Here is the input matrix A before decomposition:\n" << A << endl;
cout << "[init]" << endl;

cout << "[declaration]" << endl;
  PartialPivLU<Ref<MatrixXd> > lu(A);
  cout << "Here is the input matrix A after decomposition:\n" << A << endl;
cout << "[declaration]" << endl;

cout << "[matrixLU]" << endl;
  cout << "Here is the matrix storing the L and U factors:\n" << lu.matrixLU() << endl;
cout << "[matrixLU]" << endl;

cout << "[solve]" << endl;
  MatrixXd A0(2,2); A0 << 2, -1, 1, 3;
  VectorXd b(2);    b << 1, 2;
  VectorXd x = lu.solve(b);
  cout << "Residual: " << (A0 * x - b).norm() << endl;
cout << "[solve]" << endl;

cout << "[modifyA]" << endl;
  A << 3, 4, -2, 1;
  x = lu.solve(b);
  cout << "Residual: " << (A0 * x - b).norm() << endl;
cout << "[modifyA]" << endl;

cout << "[recompute]" << endl;
  A0 = A; // save A
  lu.compute(A);
  x = lu.solve(b);
  cout << "Residual: " << (A0 * x - b).norm() << endl;
cout << "[recompute]" << endl;

cout << "[recompute_bis0]" << endl;
  MatrixXd A1(2,2);
  A1 << 5,-2,3,4;
  lu.compute(A1);
  cout << "Here is the input matrix A1 after decomposition:\n" << A1 << endl;
cout << "[recompute_bis0]" << endl;

cout << "[recompute_bis1]" << endl;
  x = lu.solve(b);
  cout << "Residual: " << (A1 * x - b).norm() << endl;
cout << "[recompute_bis1]" << endl;

}