/*
 * SWE_Plane_Normal_Modes.hpp
 *
 *  Created on: 17 Nov 2019
 *      Author: Pedro Peixoto <pedrosp@ime.usp.br>
 *
 *      based on previous implementation by Martin Schreiber in swe_plane.cpp
 *
 */

#ifndef SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_
#define SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_

#include <rexi/EXPFunctions.hpp>
#include <sweet/plane/PlaneData_Spectral.hpp>
#include <sweet/plane/PlaneData_SpectralComplex.hpp>
#include <sweet/SimulationVariables.hpp>
#include <sweet/plane/PlaneOperators.hpp>
#include <functional>
#if SWEET_EIGEN
#include <Eigen/Eigenvalues>
#endif
/**
 * SWE Plane normal mode
 */
class SWE_Plane_Normal_Modes
{
public:

#if SWEET_QUADMATH && 0
	typedef __float128 T;
#else
	typedef double T;
#endif
	typedef std::complex<T> complex;

	static
	void add_normal_mode(
			std::size_t ik0,				//wavenumber in x
			std::size_t ik1,				// wavenumeber in y
			double geo_mode,    //Coeficient multiplying geostrophic mode
			double igwest_mode, //Coeficient multiplying west gravity mode
			double igeast_mode, //Coeficient multiplying east gravity mode
			PlaneData_Spectral &io_h, // h: surface height (perturbation)
			PlaneData_Spectral &io_u, // u: velocity in x-direction
			PlaneData_Spectral &io_v, // v: velocity in y-direction
			SimulationVariables &i_simVars // Simulation variables
	)
	{

		EXPFunctions<T> rexiFunctions;

		const PlaneDataConfig *planeDataConfig = io_h.planeDataConfig;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		//std::cout<<"Adding mode to fields"<<std::endl;

		//Check if k0 is in correct sprectral area
		//std::cout<< io_h.planeDataConfig->spectral_data_size[1] << std::endl;
		if( ik1<0 || ik1 >= planeDataConfig->spectral_data_size[1])
			SWEETError("Normal_mode: mode not within reach");

		if( ik0<0 || ik0 >= planeDataConfig->spectral_data_size[0])
			SWEETError("Normal_mode: mode not within reach");

		//Check for mirror effects
		T k0 = (T)ik0;
		T k1;
		if (ik1 < planeDataConfig->spectral_data_size[1]/2)
			k1 = (T)ik1;
		else
			k1 = (T)((int)ik1-(int)planeDataConfig->spectral_data_size[1]);

		complex v[3][3];
		complex lambda[3];

		SWE_Plane_Normal_Modes::sw_eigen_decomp(
				k0,				//wavenumber in x
				k1,				// wavenumeber in y
				i_simVars,  // Input Simulation variables
				false, // Direct EV matrix (not inverse)
				v , // EV matrix
				lambda // output eigen values */
		);

		complex U[3];
		// Set normal mode acording to desired wave type
		// These are weights for the modes
		U[0] = geo_mode;
		U[1] = igwest_mode;
		U[2] = igeast_mode;

		//Define normal mode as combination of eigen vectors
		complex UEV[3] = {0.0, 0.0, 0.0};
		for (int k = 0; k < 3; k++)
			for (int j = 0; j < 3; j++)
				UEV[k] += v[k][j] * U[j];

		//std::cout<<"spectral before"<< std::endl;
		//io_v.print_spectralIndex();

		complex h_add, u_add, v_add;
		h_add = io_h.spectral_get(ik1, ik0)+UEV[0];
		u_add = io_u.spectral_get(ik1, ik0)+UEV[1];
		v_add = io_v.spectral_get(ik1, ik0)+UEV[2];

		/* Add normal mode to data */
		io_h.spectral_set(ik1, ik0, h_add);
		io_u.spectral_set(ik1, ik0, u_add);
		io_v.spectral_set(ik1, ik0, v_add);

		io_h.spectral_zeroAliasingModes();
		io_u.spectral_zeroAliasingModes();
		io_v.spectral_zeroAliasingModes();

/*Debug output*/
#if 0

		std::cout<<"EV matrix"<<std::endl;
		for (int j = 0; j < 3; j++)	{
			for (int i = 0; i < 3; i++)
				std::cout<< v[j][i]<<" ";
			std::cout<<std::endl;
		}

		std::cout<<"Eigen values"<<std::endl;
		for (int j = 0; j < 3; j++)
			std::cout<< lambda[j]<<" ";
		std::cout<<std::endl;

		std::cout<<"Adding normal mode"<<std::endl;
		for (int j = 0; j < 3; j++)
			std::cout<< UEV[j]<<" ";
		std::cout<<std::endl;

		std::cout<<ik0<<" "<<ik1<< " "<< io_v.p_spectral_get(ik1, ik0) << UEV[2] << std::endl;

#endif
		return;
	}

	static
	void convert_allspectralmodes_to_normalmodes(
			PlaneData_Spectral &i_h, // h: surface height (perturbation)
			PlaneData_Spectral &i_u, // u: velocity in x-direction
			PlaneData_Spectral &i_v, // v: velocity in y-direction
			SimulationVariables &i_simVars, // Simulation variables
			PlaneData_Spectral &o_geo_mode,    //Output: Coeficients multiplying geostrophic mode
			PlaneData_Spectral &o_igwest_mode, //Output: Coeficients multiplying west gravity mode
			PlaneData_Spectral &o_igeast_mode //Output: Coeficients multiplying east gravity mode
	)
	{
		const PlaneDataConfig *planeDataConfig = i_h.planeDataConfig;

		o_geo_mode.spectral_set_zero();
		o_igwest_mode.spectral_set_zero();
		o_igeast_mode.spectral_set_zero();

		complex geo_mode_c;
		complex igwest_mode_c;
		complex igeast_mode_c;
		for (std::size_t ik1 = 0; ik1 < planeDataConfig->spectral_data_size[1]; ik1++)
		{
			for (std::size_t ik0 = 0; ik0 < planeDataConfig->spectral_data_size[0]; ik0++)
			{

				SWE_Plane_Normal_Modes::convert_spectralmode_to_normalmode(
									ik0, ik1,
									i_h,
									i_u,
									i_v,
									i_simVars,
									geo_mode_c,
									igwest_mode_c,
									igeast_mode_c
							);
				o_geo_mode.spectral_set(ik1, ik0, geo_mode_c);
				o_igwest_mode.spectral_set(ik1, ik0, igwest_mode_c);
				o_igeast_mode.spectral_set(ik1, ik0, igeast_mode_c);

			}
		}
		return;
	};

	static
	void convert_spectralmode_to_normalmode(
			std::size_t ik0,				//wavenumber in x
			std::size_t ik1,				// wavenumeber in y
			PlaneData_Spectral &i_h, // h: surface height (perturbation)
			PlaneData_Spectral &i_u, // u: velocity in x-direction
			PlaneData_Spectral &i_v, // v: velocity in y-direction
			SimulationVariables &i_simVars, // Simulation variables
			complex &o_geo_mode,    //Output: Coeficient multiplying geostrophic mode
			complex &o_igwest_mode, //Output: Coeficient multiplying west gravity mode
			complex &o_igeast_mode //Output: Coeficient multiplying east gravity mode
	)
	{

		EXPFunctions<T> rexiFunctions;

		const PlaneDataConfig *planeDataConfig = i_h.planeDataConfig;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		//std::cout<<"Adding mode to fields"<<std::endl;

		//Check if k0 is in correct sprectral area
		//std::cout<< io_h.planeDataConfig->spectral_data_size[1] << std::endl;
		if( ik1<0 || ik1 >= planeDataConfig->spectral_data_size[1])
			SWEETError("Normal_mode: mode not within reach");

		if( ik0<0 || ik0 >= planeDataConfig->spectral_data_size[0])
			SWEETError("Normal_mode: mode not within reach");

		//Check for mirror effects
		T k0 = (T)ik0;
		T k1;
		if (ik1 < planeDataConfig->spectral_data_size[1]/2)
			k1 = (T)ik1;
		else
			k1 = (T)((int)ik1-(int)planeDataConfig->spectral_data_size[1]);

		complex v[3][3];
		complex lambda[3];

		SWE_Plane_Normal_Modes::sw_eigen_decomp(
				k0,				//wavenumber in x
				k1,				// wavenumeber in y
				i_simVars,  // Input Simulation variables
				true, // Inverse ev matrix
				v , // inverse EV matrix
				lambda // output eigen values */
		);

		complex U[3];
		// Set (h,u,v) spectral coeficients
		// These are weights for the modes
		U[0] = i_h.spectral_get(ik1, ik0);
		U[1] = i_u.spectral_get(ik1, ik0);
		U[2] = i_v.spectral_get(ik1, ik0);

		//Apply inverse EV matrix to obtain data in EV space
		complex UEV[3] = {0.0, 0.0, 0.0};
		for (int k = 0; k < 3; k++)
			for (int j = 0; j < 3; j++)
				UEV[k] += v[k][j] * U[j];

		//Return the modes
		o_geo_mode=UEV[0];
		o_igwest_mode=UEV[1];
		o_igeast_mode=UEV[2];
		//std::cout<< " Geost: "<< o_geo_mode<<std::endl;
		//std::cout<< " IGWest: "<< o_igwest_mode<<std::endl;
		//std::cout<< " IGEast: "<< o_igeast_mode<<std::endl;

		return;
	}

	/* Get linear shallow water operator eigen decomposition */
	static
	void sw_eigen_decomp(
			T k0,				//wavenumber in x
			T k1,				// wavenumeber in y
			SimulationVariables &i_simVars, // Input Simulation variables
			bool i_inverse = false, // Input true, returns inverse matriz, false: returns direct
			complex o_v[3][3] = {0}, // output eigen vector (direct or inverse)
			complex o_evalues[3] =  0 // output eigen values (optional)
	)
	{
		EXPFunctions<T> rexiFunctions;
		bool i_evalues = false;
		if (o_evalues){
			i_evalues = true;
			//std::cout<<i_evalues<<" "<< o_evalues[0]   << std::endl;
		}
		else{
			i_evalues = false;
		}
		//std::cout<<o_evalues[1]<<std::endl;
		//std::cout<<i_inverse<<std::endl;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		complex I(0.0, 1.0);
		//std::cout<<"Calculating EV for mode (" << k0 << ", " << k1 << ")" << std::endl;
		//std::cout<<"hi"<< std::endl;
		T s0 = i_simVars.sim.plane_domain_size[0];
		T s1 = i_simVars.sim.plane_domain_size[1];

		T f = i_simVars.sim.plane_rotating_f0;
		T h = i_simVars.sim.h0;
		T g = i_simVars.sim.gravitation;

		T sqrt_h = rexiFunctions.l_sqrt(h);
		T sqrt_g = rexiFunctions.l_sqrt(g);

		complex b = -k0*I;	// d/dx exp(I*k0*x) = I*k0 exp(I*k0*x)
		complex c = -k1*I;

		b = b*rexiFunctions.pi2/s0;
		c = c*rexiFunctions.pi2/s1;

		/*
		 * Matrix with Eigenvectors (column-wise)
		 */
		complex v[3][3];
		complex v_inv[3][3];

		/*
		 * Eigenvalues
		 */
		complex lambda[3];

		if (i_simVars.sim.plane_rotating_f0 == 0)
		{
			/*
			 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c%7D,%7Bg*b,0,0%7D,%7Bg*c,0,0%7D%7D
			 */
			if (k0 == 0 && k1 == 0)
			{
				v[0][0] = 1;
				v[1][0] = 0;
				v[2][0] = 0;

				v[0][1] = 0;
				v[1][1] = 1;
				v[2][1] = 0;

				v[0][2] = 0;
				v[1][2] = 0;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = 0;
					lambda[2] = 0;
				}
			}
			else if (k0 == 0)
			{
				v[0][0] = 0;
				v[1][0] = 1;
				v[2][0] = 0;

				v[0][1] = -sqrt_h/sqrt_g;
				v[1][1] = 0;
				v[2][1] = 1;

				v[0][2] = sqrt_h/sqrt_g;
				v[1][2] = 0;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -c*sqrt_g*sqrt_h;
					lambda[2] = c*sqrt_g*sqrt_h;;
				}
			}
			else if (k1 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c*0%7D,%7Bg*b,0,0%7D,%7Bg*c*0,0,0%7D%7D
				 */

				v[0][0] = 0;
				v[1][0] = 0;
				v[2][0] = 1;

				v[0][1] = -sqrt_h/sqrt_g;
				v[1][1] = 1;
				v[2][1] = 0;

				v[0][2] = sqrt_h/sqrt_g;
				v[1][2] = 1;
				v[2][2] = 0;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -b*sqrt_g*sqrt_h;
					lambda[2] = b*sqrt_g*sqrt_h;
				}
			}
			else
			{
				v[0][0] = 0;
				v[1][0] = -c/b;
				v[2][0] = 1.0;

				v[0][1] = -(sqrt_h*rexiFunctions.l_sqrtcplx(b*b + c*c))/(c*sqrt_g);
				v[1][1] = b/c;
				v[2][1] = 1.0;

				v[0][2] = (sqrt_h*rexiFunctions.l_sqrtcplx(b*b + c*c))/(c*sqrt_g);
				v[1][2] = b/c;
				v[2][2] = 1.0;

				if (i_evalues){
					lambda[0] = 0.0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(b*b + c*c)*sqrt_h*sqrt_g;
					lambda[2] = rexiFunctions.l_sqrtcplx(b*b + c*c)*sqrt_h*sqrt_g;
				}
			}
		}
		else
		{
			if (k0 == 0 && k1 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,0,0%7D,%7B0,0,f%7D,%7B0,-f,0%7D%7D
				 */
				v[0][0] = 0;
				v[1][0] = -I;
				v[2][0] = 1;

				v[0][1] = 0;
				v[1][1] = I;
				v[2][1] = 1;

				v[0][2] = 1;
				v[1][2] = 0;
				v[2][2] = 0;

				if (i_evalues){
					lambda[0] = I*f;
					lambda[1] = -I*f;
					lambda[2] = 0;
				}
			}
			else if (k0 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b*0,h*c%7D,%7Bg*b*0,0,f%7D,%7Bg*c,-f,0%7D%7D
				 */
				v[0][0] = f/(c*g);
				v[1][0] = 1;
				v[2][0] = 0;

				v[0][1] = -(c*h)/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[1][1] =  -f/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[2][1] = 1;

				v[0][2] = (c*h)/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[1][2] = f/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(c*c*g*h-f*f);
					lambda[2] = rexiFunctions.l_sqrtcplx(c*c*g*h-f*f);
				}
			}
			else if (k1 == 0)
			{
					/*
					 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c*0%7D,%7Bg*b,0,f%7D,%7Bg*c*0,-f,0%7D%7D
					 */
				v[0][0] = -f/(b*g);
				v[1][0] = 0;
				v[2][0] = 1;

				v[0][1] = -(b*h)/f;
				v[1][1] = rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h)/f;
				v[2][1] = 1;

				v[0][2] = -(b*h)/f;
				v[1][2] = -rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h)/f;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(b*b*g*h-f*f);
					lambda[2] = rexiFunctions.l_sqrtcplx(b*b*g*h-f*f);
				}
			}
			else
				{
					/*
					 * Compute EV's of
					 * Linear operator
					 *
					 * [ 0  hb  hc ]
					 * [ gb  0   f ]
					 * [ gc -f   0 ]
					 *
					 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c%7D,%7Bg*b,0,f%7D,%7Bg*c,-f,0%7D%7D
					 */

					v[0][0] = -f/(b*g);
					v[1][0] = -c/b;
					v[2][0] = 1.0;

					v[0][1] = -(c*f*h + b*h*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h))/(b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[1][1] = -(f*f - b*b*g*h)/(b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[2][1] = 1.0;

					v[0][2] = -(-c*f*h + b*h*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h))/(-b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[1][2] =  -(-f*f + b*b*g*h)/(-b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[2][2] = 1.0;

					if (i_evalues){
						lambda[0] = 0.0;
						lambda[1] = -rexiFunctions.l_sqrtcplx(b*b*g*h + c*c*g*h - f*f);
						lambda[2] =  rexiFunctions.l_sqrtcplx(b*b*g*h + c*c*g*h - f*f);
					}
				}
		}

			/*
			 * Invert Eigenvalue matrix
			 */

		if (i_inverse){
			v_inv[0][0] =  (v[1][1]*v[2][2] - v[1][2]*v[2][1]);
			v_inv[0][1] = -(v[0][1]*v[2][2] - v[0][2]*v[2][1]);
			v_inv[0][2] =  (v[0][1]*v[1][2] - v[0][2]*v[1][1]);

			v_inv[1][0] = -(v[1][0]*v[2][2] - v[1][2]*v[2][0]);
			v_inv[1][1] =  (v[0][0]*v[2][2] - v[0][2]*v[2][0]);
			v_inv[1][2] = -(v[0][0]*v[1][2] - v[0][2]*v[1][0]);

			v_inv[2][0] =  (v[1][0]*v[2][1] - v[1][1]*v[2][0]);
			v_inv[2][1] = -(v[0][0]*v[2][1] - v[0][1]*v[2][0]);
			v_inv[2][2] =  (v[0][0]*v[1][1] - v[0][1]*v[1][0]);

			complex s = v[0][0]*v_inv[0][0] + v[0][1]*v_inv[1][0] + v[0][2]*v_inv[2][0];

			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					v_inv[j][i] /= s;
			}
			//Return inverse matrix
			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					o_v[j][i] = v_inv[j][i] ;
			}
		}
		else{
			//Return direct matrix
			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					o_v[j][i] = v[j][i] ;
			}
		}
		if (i_evalues){
			for (int j = 0; j < 3; j++)	{
				o_evalues[j] = lambda[j] ;
			}
		}
		return;
}

	template <typename TCallbackClass>
	static
	void normal_mode_analysis(
			PlaneData_Spectral &io_prog_h_pert, // h: surface height (perturbation)
			PlaneData_Spectral &io_prog_u, // u: velocity in x-direction
			PlaneData_Spectral &io_prog_v, // v: velocity in y-direction
			int number_of_prognostic_variables,
			SimulationVariables &i_simVars, // Simulation variables
			TCallbackClass *i_class,
			void(TCallbackClass::* const i_run_timestep_method)(void)
	)
	{

		const PlaneDataConfig *planeDataConfig = io_prog_h_pert.planeDataConfig;

		// dummy time step to get time step size
		if (i_simVars.timecontrol.current_timestep_size <= 0)
			SWEETError("Normal mode analysis requires setting fixed time step size");

		/*
		 *
		 * Mode-wise normal mode analysis
		 *
		 *
		 */

		if (i_simVars.misc.normal_mode_analysis_generation == 4)
		{
#if SWEET_EIGEN
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			SWEETError("SWE_Plane_Normal_Modes: This test was build for linear or linearized models, so please compile without dealising --plane-spectral-dealiasing=disable.");
#endif

			/*
			 * Setup all output files
			 */
			const char* filename; //general filename
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_t%020.8f.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_plane", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to files of the form '" << buffer_real << "'" << std::endl;

			//Positive inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igpos", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igpos(buffer_real, std::ios_base::trunc);

			//Negative inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igneg", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igneg(buffer_real, std::ios_base::trunc);

			//Geostrophic modes
			sprintf(buffer_real, filename, "normal_modes_plane_geo", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_geo(buffer_real, std::ios_base::trunc);

			//std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);
			file_igpos << std::setprecision(20);
			file_igneg << std::setprecision(20);
			file_geo << std::setprecision(20);

			file << "# dt " << i_simVars.timecontrol.current_timestep_size << std::endl;
			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif
			file << std::endl;

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			int number_of_prognostic_variables = 3;
			//The basic state is with zero in all variables
			// The only non zero variable in the basic state is the total height
			//    for which the constant is added within run_timestep()
			io_prog_h_pert.spectral_set_zero();
			io_prog_u.spectral_set_zero();
			io_prog_v.spectral_set_zero();

			//int num_timesteps = 1;

			// Timestep and perturbation
			double dt = i_simVars.timecontrol.current_timestep_size;
			double eps = dt;

			//Matrix representing discrete linear operator in spectral space
			Eigen::MatrixXcf A(3,3) ;
			//Eigen solver
			Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces;
			//Final eigenvalues
			std::complex<double> eval[3];

			//For each spectral mode
			//for (int r = 0; r < 2; r++) //only required to get the symmetric half of the spectrum
			//{
			int r = 0;

			for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
			{
				std::cout << "." << std::flush;
				for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
				{
					//This is the mode to be analysed
					//std::cout << "Mode (i,j)= (" << i << " , " << j <<")"<< std::endl;

					for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
					{

						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog[outer_prog_id]->spectral_set(j, i, 1.0);
						//Activate the symetric couterpart of the mode (only needed if j>0 )
						if (j > 0)
							prog[outer_prog_id]->spectral_set(planeDataConfig->spectral_data_size[1]-j, i, 1.0);

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						/*
						 * compute
						 * 1/dt * (U(t+1) - U(t))
						 */

						std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
						val = val - 1.0; //subtract U(0) from mode
						prog[outer_prog_id]->spectral_set(j, i, val);

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							(*prog[inner_prog_id]) /= eps;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							A(inner_prog_id,outer_prog_id)=prog[inner_prog_id]->spectral_get(j, i);;
						}

					}

					//std::cout << "Lik matrix" << std::endl;
					//std::cout << A << std::endl;

					//std::cout<<"Normal modes" << std::endl;
					ces.compute(A);
					for(int i=0; i<3; i++)
					{
						eval[i]=ces.eigenvalues()[i];
						//std::cout << "Eigenvalue "<< i << " : " << eval[i].real() <<" "<<eval[i].imag() << std::endl;

					}
					/* We will try to separate the modes in 3 types:
					 * -positive inertia-gravity (imag>f) - we will adopt coriolis f to test as if > zero, since the exact freq is sqrt(f^2+cK*K)
					 * -negative inertia-gravity (imag<-f)
					 * -negative inertia-gravity (imag aprox 0) - we will fit all other modes here
					 */
					int count_igpos=0;
					int count_igneg=0;
					int count_geo=0;
					for(int i=0; i<3; i++)
					{
						if(eval[i].imag() > 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG pos mode: " << eval[i].imag() << std::endl;
							//file_igpos << eval[i].imag();
							file_igpos << eval[i].real()<< "\t" << eval[i].imag();
							file_igpos << "\t";
							count_igpos++;
						}
						if(eval[i].imag() < - 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG neg mode: " << eval[i].imag() << std::endl;
							//file_igneg << eval[i].imag();
							file_igneg << eval[i].real()<< "\t" << eval[i].imag();
							file_igneg << "\t";
							count_igneg++;
						}
						if(eval[i].imag() >= - 0.5 * i_simVars.sim.plane_rotating_f0 && eval[i].imag() <=  0.5 * i_simVars.sim.plane_rotating_f0 )
						{
							//std::cout<< "IG geo mode: " << eval[i].imag() << std::endl;
							//file_geo << eval[i].imag();
							file_geo << eval[i].real()<< "\t" << eval[i].imag();
							file_geo << "\t";
							count_geo++;
						}
					}
					//Check if we got the correct modes
					if ( count_igpos * count_igneg * count_geo > 0 )
					{
						count_igpos=0;
						count_igneg=0;
						count_geo=0;
					}
					else
					{
						SWEETError("SWE_Plane_Normal_Modes: Could not separate modes!!");
					}

					//std::cout<<"-------------------------" << std::endl;
				}
				file_igpos << std::endl;
				file_igneg << std::endl;
				file_geo << std::endl;
			}

			//}
			//std::cout<<"-------------------------" << std::endl;
			//SWEETError("still needs work...");
#else
			SWEETError("SWE_Plane_Normal_Modes: Cannot test this without Eigen library. Please compile with --eigen=enable");
#endif
		}
		/*
		 * Do a normal mode analysis using perturbation, see
		 * Hillary Weller, John Thuburn, Collin J. Cotter,
		 * "Computational Modes and Grid Imprinting on Five Quasi-Uniform Spherical C Grids"
		 */
		else
		{

			//run_timestep();
			const char* filename;
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_normalmodes.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_physical", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to file '" << buffer_real << "'" << std::endl;

			std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			/*
			 * Maximum number of prognostic variables
			 *
			 * Advection e.g. has only one
			 */
			if (number_of_prognostic_variables <= 0)
				SWEETError("simVars.pde.number_of_prognostic_variables must be set!");

			if (number_of_prognostic_variables == 3)
			{
				io_prog_h_pert.spectral_set_zero();
				io_prog_u.spectral_set_zero();
				io_prog_v.spectral_set_zero();
			}
			else if (number_of_prognostic_variables == 1)
			{
				io_prog_h_pert.spectral_set_zero();
			}
			else
			{
				SWEETError("Not yet supported");
			}

#if 0
			if (i_simVars.disc.timestepping_method == SimulationVariables::Discretization::LEAPFROG_EXPLICIT)
			{
				SWEETError("Not yet tested and supported");
				std::cout << "WARNING: Leapfrog time stepping doesn't make real sense since 1st step is based on RK-like method" << std::endl;
				std::cout << "We'll do two Leapfrog time steps here to take the LF errors into account!" << std::endl;
				std::cout << "Therefore, we also halve the time step size here" << std::endl;

				i_simVars.timecontrol.current_timestep_size = 0.5*i_simVars.sim.CFL;
				i_simVars.sim.CFL = -i_simVars.timecontrol.current_timestep_size;
			}
#endif

			int num_timesteps = 1;
			if (i_simVars.misc.normal_mode_analysis_generation >= 10)
			{
				if (i_simVars.timecontrol.max_timesteps_nr > 0)
					num_timesteps = i_simVars.timecontrol.max_timesteps_nr;
			}

			if (i_simVars.timecontrol.max_simulation_time > 0)
				file << "# t " << i_simVars.timecontrol.max_simulation_time << std::endl;
			else
				file << "# t " << (num_timesteps*(-i_simVars.timecontrol.current_timestep_size)) << std::endl;

			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
//			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";

#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif

			file << std::endl;

			// iterate over all prognostic variables
			for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
			{
				if (i_simVars.misc.normal_mode_analysis_generation == 1 || i_simVars.misc.normal_mode_analysis_generation == 11)
				{
					// iterate over physical space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->physical_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode
						PlaneData_Physical tmp = prog[outer_prog_id]->toPhys();
						tmp.physical_space_data[outer_i] = 1;
						prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

						/*
						 * RUN timestep
						 */

						(i_class->*i_run_timestep_method)();

						if (i_simVars.misc.normal_mode_analysis_generation == 1)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							tmp = prog[outer_prog_id]->toPhys();
							tmp.physical_space_data[outer_i] -= 1.0;
							prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
						}

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							tmp = prog[outer_prog_id]->toPhys();
							for (std::size_t k = 0; k < planeDataConfig->physical_array_data_number_of_elements; k++)
							{
								file << tmp.physical_space_data[k];
								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->physical_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#if 1
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
#if !SWEET_USE_PLANE_SPECTRAL_SPACE
					SWEETError("Only available with if plane spectral space is activated during compile time!");
#else

					// iterate over spectral space
					for (int r = 0; r < 2; r++)
					{

						for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
						{
							for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
							{
								// reset time control
								i_simVars.timecontrol.current_timestep_nr = 0;
								i_simVars.timecontrol.current_simulation_time = 0;

								std::cout << "." << std::flush;

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
									prog[inner_prog_id]->spectral_set_zero();

								// activate mode via real coefficient
								prog[outer_prog_id]->spectral_set(j, i, 1.0);

								/*
								 * RUN timestep
								 */
								(i_class->*i_run_timestep_method)();

								if (i_simVars.misc.normal_mode_analysis_generation == 3)
								{
									/*
									 * compute
									 * 1/dt * (U(t+1) - U(t))
									 */

									std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
									val = val - 1.0;
									prog[outer_prog_id]->spectral_set(j, i, val);

									for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
										(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{

									/*
									 * REAL
									 */

									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).real();
												file << "\t";
											}
										}
									}
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									/*
									 * IMAG
									 */
									int c = 0;
									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).imag();

												if (inner_prog_id != number_of_prognostic_variables-1 || c != specmodes-1)
													file << "\t";
												else
													file << std::endl;

												c++;
											}
										}
									}
								}
							}
						}
					}
#endif
				}
#else
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
					PlaneDataComplex t1(planeDataConfig);
					PlaneDataComplex t2(planeDataConfig);
					PlaneDataComplex t3(planeDataConfig);
					PlaneDataComplex* prog_cplx[3] = {&t1, &t2, &t3};

					// iterate over spectral space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->spectral_complex_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog_cplx[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog_cplx[outer_prog_id]->request_data_spectral();
						prog_cplx[outer_prog_id]->spectral_space_data[outer_i].real(1);

						// convert PlaneDataComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							*prog[inner_prog_id] = Convert_PlaneDataComplex_To_PlaneData::physical_convert(*prog_cplx[inner_prog_id]);
							prog[inner_prog_id]->spectral_zeroAliasingModes();
						}

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog[inner_prog_id]->spectral_zeroAliasingModes();
#warning "update this physical_convert maybe to spectral_convert"

							*prog_cplx[inner_prog_id] = Convert_PlaneData_To_PlaneDataComplex::physical_convert(*prog[inner_prog_id]);

							prog_cplx[inner_prog_id]->request_data_spectral();
						}

						if (i_simVars.misc.normal_mode_analysis_generation == 3)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							prog_cplx[outer_prog_id]->request_data_spectral();
							prog_cplx[outer_prog_id]->spectral_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								prog_cplx[inner_prog_id]->operator*=(1.0/i_simVars.timecontrol.current_timestep_size);
						}

						// convert PlaneDataComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog_cplx[inner_prog_id]->request_data_spectral();

							/*
							 * REAL
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].real();
								file << "\t";
							}

							/*
							 * IMAG
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].imag();

								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->spectral_complex_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#endif
			}
		}
	}

	~SWE_Plane_Normal_Modes()
	{

	}
};

#endif /* SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_ */


#include <RcppEigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::DiagonalMatrix;

// [[Rcpp::export]]
float r_score(Eigen::MatrixXd x, Eigen::MatrixXd x0) {
  int nr = x.rows();
  int nc = x.cols();
  int n0r = x0.rows();
  Eigen::MatrixXd A  = x.transpose()*((x*x.transpose()+MatrixXd::Identity(nr, nr)*(1.0/nc)).inverse());
  Eigen::MatrixXd IJ = MatrixXd::Identity(n0r, n0r)-(MatrixXd::Identity(n0r, n0r)*(1.0/n0r));
  float q1 = n0r-1+(IJ*x0).array().square().sum();
  Eigen::MatrixXd IJX0A = IJ*x0*A;
  Eigen::MatrixXd IJX0AX = IJX0A*x;
  float q2 = IJX0A.array().square().sum() + IJX0AX.array().square().sum();
  float q12 = (x0.transpose()*IJX0AX).trace();
  return q12/(sqrt(q1*q2));
}

// [[Rcpp::export]]
float pev_score(Eigen::MatrixXd X, Eigen::MatrixXd X0){
  int p = X.cols();
  int n = X.rows();
  int n0 = X0.rows();
  Eigen::MatrixXd x(n,p+1);
  x << (Eigen::ArrayXd::Zero(n)+1), X;
  Eigen::MatrixXd x0(n0,p+1);
  x0 << (Eigen::ArrayXd::Zero(n0)+1), X0;
  return (x0*((x.transpose()*x+(Eigen::MatrixXd::Identity(p+1,p+1)*(1.0/p))).inverse())*x0.transpose()).trace();
}

// [[Rcpp::export]]
float cd_score(Eigen::MatrixXd x, Eigen::MatrixXd x0){
  int p = x.cols();
  Eigen::MatrixXd cd = x0*(x.transpose()*x+(Eigen::MatrixXd::Identity(p,p)*1.0)).inverse()*x.transpose();
  return (((cd*cd.transpose()).diagonal().array())/((x0*x0.transpose()).diagonal().array())).mean();
}


#include <vector>
#include <boost/math/distributions/students_t.hpp>
#include "students_t_dist.h"

stochastic::StudentstDistribution::StudentstDistribution(double mean,
                                                         double std_dev,
                                                         double dof)
    : Distribution(),
      mean_{mean},
      std_dev_{std_dev},
      dof_{dof},
      distribution_{dof_}
{}

std::vector<double> stochastic::StudentstDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, (locations[i] - mean_) / std_dev_);
  }

  return evaluations;
}

std::vector<double> stochastic::StudentstDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] =
        std_dev_ * quantile(distribution_, probabilities[i]) + mean_;
  }

  return evaluations;
}


#define ARMA_ALLOW_FAKE_GCC
#define ARMA_NO_DEBUG

#include "TimeSeries.hpp"

#include <armadillo>
#include <cmath>
#include <iostream>
#include <string>

using namespace arma;

namespace TimeSeries {

/*
 * Discretize Normal for given grid.
 */
vec discretizeNormal(vec grid,
    const double mean,
    const double sigma,
    bool cutTails)
{
  uword sz = grid.n_elem;
  vec prs(sz);

  if (sz == 1) {
    [[unlikely]] prs(0) = 1.0;
    return prs;
  } else if (sz == 2) {
    [[unlikely]] prs(0) = normcdf((grid(0) + grid(1)) / 2.0, mean, sigma);
    prs(1) = 1.0 - prs(0);
    return prs;
  }

#pragma omp parallel for
  for (uword ix = 1; ix < sz - 1; ++ix)
    prs(ix) = normcdf((grid(ix) + grid(ix + 1)) / 2.0, mean, sigma) - normcdf((grid(ix - 1) + grid(ix)) / 2.0, mean, sigma);

  if (cutTails) {
    const double stepOut = (grid(1) - grid(0)) / 2.0;
    prs(0) = normcdf(grid(0) + stepOut, mean, sigma) - normcdf(grid(0) - stepOut, mean, sigma);
    prs(sz - 1) = normcdf(grid(sz - 1) + stepOut, mean, sigma) - normcdf(grid(sz - 1) - stepOut, mean, sigma);

    prs /= sum(prs);
  } else {
    prs(0) = normcdf((grid(0) + grid(1)) / 2.0, mean, sigma);
    prs(sz - 1) = 1.0 - normcdf((grid(sz - 2) + grid(sz - 1)) / 2.0, mean, sigma);
  }

  return prs;
}

/*
 * Discretize Normal over uniformly spaced grid
 * spanning +/- pmSigma standard deviations.
 */
DiscreteRV
discretizeNormal(const double mean,
    const double sigma,
    const uword n_elem,
    const double pmSigma,
    bool cutTails)
{
  DiscreteRV drv;
  if (n_elem == 1) {
    drv.support = { mean };
    drv.probabilities = { 1.0 };
  } else {
    [[likely]] drv.support = linspace<vec>(mean - pmSigma * sigma, mean + pmSigma * sigma, n_elem);
    drv.probabilities = discretizeNormal(drv.support, mean, sigma, cutTails);
  }
  return drv;
}

/*
 *
 * VAR(1)
 *
 */
const MvNormal
VAR::conditional(vec& current) const
{
  MvNormal mvn;
  mvn.mean = m_intercept + m_rho * current;
  mvn.varCovar = m_sigma;
  return mvn;
}

void VAR::findStationary()
{
  const cx_vec theEigs = eig_gen(m_rho);
  if (any(abs(theEigs) >= 1.0)) {
    [[unlikely]] cout << "Eigenvalues exceeding 1 in abs value. Nonstationary."
                      << endl;
    is_stationary = false;
    m_statMean.reset();
    m_statSigma.reset();
    return;
  }

  is_stationary = true;

  const mat eyeRho = eye(m_size, m_size) - m_rho;
  m_statMean = solve(eyeRho, m_intercept);

  const mat kprod = kron(m_rho, m_rho);
  const mat theEye(kprod.n_rows, kprod.n_cols, fill::eye);
  const vec vecSigma = m_sigma.as_col();
  const vec tmp = solve(theEye - kprod, vecSigma);
  m_statSigma = reshape(tmp, m_rho.n_rows, m_rho.n_cols);
}

vec& VAR::stationaryMean()
{
  if (m_statMean.is_empty())
    findStationary();
  return m_statMean;
}

mat& VAR::stationarySigma()
{
  if (m_statSigma.is_empty())
    findStationary();
  return m_statSigma;
}

bool VAR::stationaryQ()
{
  if (m_statMean.is_empty())
    findStationary();
  return is_stationary;
}

void VAR::print()
{
  cout << "Intercept: " << endl;
  intercept().print();
  cout << "Rho: " << endl;
  rho().print();
  cout << "Sigma: " << endl;
  sigma().print();

  cout << "Stationary: " << endl;
  stationaryMean().print();
  cout << "Stationary Sigma: " << endl;
  stationarySigma().print();
}

/*
 *
 * Markov Chain
 *
 */
MarkovChain::MarkovChain(const AR process,
    const uword sz,
    const double pmMCsd,
    bool expSupport)
{
  m_size = sz;
  double pMean = process.stationaryMean();
  double pSd = process.stationarySigma();
  if (sz == 1)
    m_support = { pMean };
  else
    [[likely]] m_support = linspace<rowvec>(pMean - pmMCsd * pSd, pMean + pmMCsd * pSd, sz);

  m_tran.set_size(sz, sz);
#pragma omp parallel for
  for (uword rIx = 0; rIx < sz; ++rIx) {
    double condMean = process.intercept() + process.rho() * m_support(rIx);
    vec condPrs = discretizeNormal(m_support.t(), condMean, process.sigma());
    m_tran.row(rIx) = condPrs.t();
  }

  if (expSupport)
    m_support = exp(m_support);
}

rowvec
stationaryDistribution(const mat& transitionMatrix)
{
  cx_vec eigval;
  cx_mat eigvec;

  const mat tranT = transitionMatrix.t();
  const sp_mat sp_ver(tranT);

  eigs_opts opts;
  // opts.tol = 1.0E-6;
  eigs_gen(eigval, eigvec, sp_ver, 1, 1.0001, opts);

  const uword unitEigIx = (abs(abs(eigval) - 1.0)).index_min();

  cout << "Stationary eigenvalue " << eigval(unitEigIx) << endl;

  const rowvec stat = abs(eigvec.col(unitEigIx)).t();
  return stat / sum(stat);
}

uvec simulateChain(const mat& transitionMatrix,
    const uword initState,
    const uword simSz)
{
  const mat cumMat = cumsum(transitionMatrix, 1);
  const uword sz = transitionMatrix.n_rows;

  uvec state(simSz);

  state(0) = initState;
  const vec draws(simSz, fill::randu);
  for (uword tIx = 1; tIx < simSz; ++tIx) {
    uword newState = 0;
    for (uword stateIx = 1; stateIx < sz; ++stateIx)
      if (cumMat(state(tIx - 1), stateIx) > draws(tIx)) {
        newState = stateIx;
        break;
      }
    state(tIx) = newState;
  }

  return state;
}

const rowvec&
MarkovChain::stationary()
{
  if (m_stationary.is_empty()) {
    m_stationary = stationaryDistribution(m_tran);
  }

  return m_stationary;
}

void MarkovChain::save(const std::string fname) const
{
  m_support.save(hdf5_name(fname, "MC/support", hdf5_opts::replace));
  m_stationary.save(hdf5_name(fname, "MC/stationary", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "MC/transitions", hdf5_opts::replace));
}

void MarkovChain::print() const
{ /* TODO */
}

void trimMarkovChain(mat& grids, mat& transition)
{
  /*
   * Based on Gordon 2020 wip.
   */
  rowvec probs = stationaryDistribution(transition);
  uword sz = probs.n_elem;

  uvec removePoints;
  uword removeCount = 0;
  for (uword ix = 0; ix < sz; ++ix)
    if (probs(ix) <= minPr) {
      [[likely]] removePoints.resize(removeCount + 1);
      removePoints(removeCount) = ix;
      removeCount++;
    }

  uword newSz = sz - removeCount;
  cout << "Removing " << removeCount << " points out of " << sz << "." << endl;

  grids.shed_rows(removePoints);
  transition.shed_rows(removePoints);
  transition.shed_cols(removePoints);
#pragma omp parallel for
  for (uword fIx = 0; fIx < newSz; ++fIx)
    transition.row(fIx) = transition.row(fIx) / sum(transition.row(fIx));
}

/*
 *
 * Discretized VAR(1)
 *
 */
void DiscreteVAR::impl(bool trimGrids, OrthoMethod method)
{
  // Sizing
  m_size = m_var.size();
  m_flatSize = prod(m_supportSizes);
  m_grids.set_size(m_flatSize, m_size);
  umat m_map(m_flatSize, m_size);

  OrthogonalizedVAR ortho(m_var, method);
  mat LL = ortho.getSupportRotationMatrix();
  VAR orthog = ortho.getVAR();

  const vec Atilde = orthog.intercept();
  const mat Btilde = orthog.rho();
  const vec DD = orthog.sigma().diag();

  const vec uncondE = orthog.stationaryMean();
  const vec uncondV = orthog.stationarySigma().diag();

  // Prepare logical grids
  field<vec> m_orthogGrids(m_size);
#pragma omp parallel for
  for (uword vIx = 0; vIx < m_size; ++vIx)
    if (m_supportSizes(vIx) > 1) {
      [[likely]] m_orthogGrids(vIx) = linspace<vec>(
          uncondE(vIx) - pmSd * sqrt(uncondV(vIx)),
          uncondE(vIx) + pmSd * sqrt(uncondV(vIx)),
          m_supportSizes(vIx));
    } else {
      m_orthogGrids(vIx) = { uncondE(vIx) };
    }

    // Prepare flat grids
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    uword tmp = flatIx;
    for (uword vIx = 0; vIx < m_size; ++vIx) {
      const vec logical = m_orthogGrids(vIx);
      m_map(flatIx, vIx) = tmp % m_supportSizes(vIx);
      m_grids(flatIx, vIx) = logical(m_map(flatIx, vIx));
      tmp = tmp / m_supportSizes(vIx);
    }
  }

  // Transition matrix
  m_tran.set_size(m_flatSize, m_flatSize);
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const rowvec tmpVal = m_grids.row(flatIx);
    const vec condMeans = Atilde + Btilde * tmpVal.t();
    const vec condSd = sqrt(DD);
    field<vec> condPrs(m_size);
#pragma omp parallel for
    for (uword vIx = 0; vIx < m_size; ++vIx)
      condPrs(vIx) = discretizeNormal(m_orthogGrids(vIx), condMeans(vIx), condSd(vIx));

#pragma omp parallel for
    for (uword flatPrIx = 0; flatPrIx < m_flatSize; ++flatPrIx) {
      double goPr = 1.0;
      for (uword vPrIx = 0; vPrIx < m_size; ++vPrIx) {
        vec tmpDist = condPrs(vPrIx);
        goPr *= tmpDist(m_map(flatPrIx, vPrIx));
      }
      m_tran(flatIx, flatPrIx) = goPr;
    }
  }

  // Adjust grids
  m_grids = (LL * m_grids.t()).t();

  if (trimGrids) {
    trimMarkovChain(m_grids, m_tran);
    m_flatSize = m_tran.n_rows;
  }

  // Finding midIx
  vec distances(m_flatSize);
  const vec target = m_var.stationaryMean();
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const vec tmp = target - m_grids.row(flatIx).t();
    distances(flatIx) = sum(tmp % tmp);
  }
  m_midIx = distances.index_min();
}

void DiscreteVAR::print() const
{
  cout << "Grid:" << endl;
  m_grids.print();
  cout << endl
       << "Transition matrix: " << endl;
  m_tran.print();
}

void DiscreteVAR::save(const std::string fname) const
{
  uvec tmp = { m_midIx };
  tmp.save(hdf5_name(fname, "DVAR/midIx", hdf5_opts::replace));
  m_grids.save(hdf5_name(fname, "DVAR/grids", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "DVAR/transitions", hdf5_opts::replace));
  m_var.intercept().save(
      hdf5_name(fname, "DVAR/varIntercept", hdf5_opts::replace));
  m_var.rho().save(hdf5_name(fname, "DVAR/varRho", hdf5_opts::replace));
  m_var.sigma().save(hdf5_name(fname, "DVAR/varSigma", hdf5_opts::replace));
}

/*
 *
 * Stochastic volatility VAR
 *
 */
void DiscreteStochVolVAR::impl(bool trimGrids)
{
  m_size = m_var.size();
  uword justDimsSz = prod(m_supportSizes);
  m_flatSize = justDimsSz * m_volGridSize;
  m_grids.set_size(m_flatSize, m_size + 1);
  umat m_map(m_flatSize, m_size + 1);

  const MarkovChain volMC(m_vol, m_volGridSize, pmSd, true);
  const rowvec& vols = volMC.support();
  const mat& volTran = volMC.transition();

  OrthogonalizedVAR ortho(m_var); // Default to Cholesky
  mat rotationLL = ortho.getSupportRotationMatrix();
  VAR orthog = ortho.getVAR();
  const vec uncondE = orthog.stationaryMean();
  const vec uncondV = orthog.stationarySigma().diag();

  const vec Atilde = orthog.intercept();
  const mat Btilde = orthog.rho();
  const vec DD = orthog.sigma().diag();

  // Prepare logical grids
  field<vec> m_orthogGrids(m_size, m_volGridSize);
#pragma omp parallel for collapse(2)
  for (uword volIx = 0; volIx < m_volGridSize; ++volIx)
    for (uword iIx = 0; iIx < m_size; ++iIx)
      if (m_supportSizes(iIx) > 1) {
        [[likely]] m_orthogGrids(iIx, volIx) = linspace<vec>(
            uncondE(iIx) - pmSd * vols(volIx) * sqrt(uncondV(iIx)),
            uncondE(iIx) + pmSd * vols(volIx) * sqrt(uncondV(iIx)),
            m_supportSizes(iIx));
      } else {
        m_orthogGrids(iIx, volIx) = { uncondE(iIx) };
      }

      // Prepare flat grids
#pragma omp parallel for collapse(2)
  for (uword volIx = 0; volIx < m_volGridSize; ++volIx) {
    for (uword flatIx = 0; flatIx < justDimsSz; ++flatIx) {
      uword withV = justDimsSz * volIx + flatIx;

      m_map(withV, m_size) = volIx;
      m_grids(withV, m_size) = vols(volIx);

      uword tmp = flatIx;
      for (uword iIx = 0; iIx < m_size; ++iIx) {
        const vec logical = m_orthogGrids(iIx, volIx);
        m_map(withV, iIx) = tmp % m_supportSizes(iIx);
        m_grids(withV, iIx) = logical(m_map(withV, iIx));
        tmp /= m_supportSizes(iIx);
      }
    }
  }

  // Transition matrix
  m_tran.set_size(m_flatSize, m_flatSize);
  m_tran.fill(0.0);
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const uword thisVolIx = m_map(flatIx, m_size);
    const rowvec thisValues = m_grids.row(flatIx).head(m_size);
    const vec condMeans = Atilde + Btilde * thisValues.t();
    const vec condSdReference = sqrt(DD);

    field<vec> condPrs(m_size);
    for (uword iIx = 0; iIx < m_size; ++iIx)
      condPrs(iIx) = discretizeNormal(m_orthogGrids(iIx, thisVolIx), condMeans(iIx), vols(thisVolIx) * condSdReference(iIx));

    for (uword flatPrIx = 0; flatPrIx < m_flatSize; ++flatPrIx) {
      double goPr = 1.0;
      for (uword iiIx = 0; iiIx < m_size; ++iiIx) {
        const vec condPrVec = condPrs(iiIx);
        goPr *= condPrVec(m_map(flatPrIx, iiIx));
      }
      const uword vPrIx = m_map(flatPrIx, m_size);
      m_tran(flatIx, flatPrIx) = goPr * volTran(thisVolIx, vPrIx);
    }
  }

  // Adjust grids
  m_grids.cols(0, m_size - 1) = (rotationLL * m_grids.cols(0, m_size - 1).t()).t();

  if (trimGrids) {
    [[likely]] trimMarkovChain(m_grids, m_tran);
    m_flatSize = m_tran.n_rows;
  }

  // Finding midIx
  vec distances(m_flatSize);
  vec uncondEextended(m_size + 1);
  uncondEextended.head(m_size) = m_var.stationaryMean();
  uncondEextended(m_size) = vols(m_volGridSize / 2);
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const vec tmp = uncondEextended - m_grids.row(flatIx).t();
    distances(flatIx) = sum(tmp % tmp);
  }
  m_midIx = distances.index_min();
}

void DiscreteStochVolVAR::save(const std::string fname) const
{
  uvec tmp = { m_midIx };
  tmp.save(hdf5_name(fname, "DsvVAR/midIx", hdf5_opts::replace));
  m_grids.save(hdf5_name(fname, "DsvVAR/grids", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "DsvVAR/transitions", hdf5_opts::replace));
  m_var.intercept().save(
      hdf5_name(fname, "DsvVAR/varIntercept", hdf5_opts::replace));
  m_var.rho().save(hdf5_name(fname, "DsvVAR/varRho", hdf5_opts::replace));
  m_var.sigma().save(hdf5_name(fname, "DsvVAR/varSigma", hdf5_opts::replace));

  vec volAR = { m_vol.intercept(), m_vol.rho(), m_vol.sigma() };
  volAR.save(
      hdf5_name(fname, "DsvVAR/volatilityAR1params", hdf5_opts::replace));
}

void DiscreteStochVolVAR::print() const
{ /* TODO */
}

} // namespace TimeSeries


#include "cor_algorithm/sources/utilities.h"
#include "cor_algorithm/sources/bit_operation.h"
#include "cor_system/sources/logger.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(bit_operation)

BOOST_AUTO_TEST_CASE(bit_operation_count_one)
{
    {
        auto f = [=](cor::RInt32 a, cor::RSize b){
            auto v = a;
            auto sz = cor::algorithm::BitOperation::count_one(v);
            BOOST_CHECK_EQUAL(sz, b);
        };

        f(7, 3);
        f(406959, 12);
        f(0xf0000000, 4);
    }

    {
        auto f = [=](cor::RInt64 a, cor::RSize b){
            auto v = a;
            auto sz = cor::algorithm::BitOperation::count_one(v);
            BOOST_CHECK_EQUAL(sz, b);
        };

        f(0xf000000000000000, 4);
    }
}

BOOST_AUTO_TEST_CASE(bit_operation_ciel_pow_two)
{
    {
        auto f = [=](cor::RInt32 a, cor::RSize b){
            auto v = a;
            auto sz = cor::algorithm::BitOperation::ciel_pow_two(v);
            BOOST_CHECK_EQUAL(sz, b);
        };

        f(7, 3);
        f(406959, 19);
        f(0xf0000000, 32);
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-exponential
    Function object implementing exp2 capabilities

    base two exponential function: \f$2^{x}\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = exp2(x);
    @endcode

    is similar to:

    @code
    T r = pow(T(2), x);
    @endcode

    @par Note:

    - provisions are made to obtain a flint result from a flint input

    @par Decorators

    std_ for floating entries

    @see exp, exp10, pow, pow2

  **/
  const boost::dispatch::functor<tag::exp2_> exp2 = {};
} }
#endif

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

#endif


//
//  main_picking.cpp
//  HCI 557 Picking example
//
//  Created by Rafael Radkowski on 5/28/15.
//  Copyright (c) 2015 -. All rights reserved.
//

// stl include
#include <iostream>
#include <string>
#include <math.h>
#include <map>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <glm/gtx/transform.hpp> // after <glm/glm.hpp>

#include "Bezier.h"

using namespace std;

// stores all binomnal coefficents that have been calculated in this program.
// [  n , [k, coeff] ]
map<unsigned int, map< unsigned int,  unsigned int> > binominal_coefficients;

bool eigenvaluessort(pair<float, Eigen::Vector3f > i, pair<float, Eigen::Vector3f > j)
{
    return (i.first > j.first); // largest first
}

/*!
@brief - calculate the eigenvectors for a given point using the points in its surrounding.
@param centroid - the centroid of the eigenvectors
@param points - all the other points
@param eigenvectors - a 4x4 matrix which column vectors are the eigen vectors
*/
//static
void calcEigenvectors(const glm::vec3& centroid, const vector<glm::vec3>& points, vector<glm::vec3>& eigenvectors)
{

    Eigen::Matrix3f cov;
    cov << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

    size_t size = points.size();
    for (int i=0; i<size; i++) {

        glm::vec3 p = points[i];
        cov(0,0) += (p[0] - centroid[0] ) * (p[0] - centroid[0]);
        cov(0,1) += (p[0] - centroid[0] ) * (p[1] - centroid[1]);
        cov(0,2) += (p[0] - centroid[0] ) * (p[2] - centroid[2]);

        cov(1,0) += (p[1] - centroid[1] ) * (p[0] - centroid[0]);
        cov(1,1) += (p[1] - centroid[1] ) * (p[1] - centroid[1]);
        cov(1,2) += (p[1] - centroid[1] ) * (p[2] - centroid[2]);

        cov(2,0) += (p[2] - centroid[2] ) * (p[0] - centroid[0]);
        cov(2,1) += (p[2] - centroid[2] ) * (p[1] - centroid[1]);
        cov(2,2) += (p[2] - centroid[2] ) * (p[2] - centroid[2]);
    }

    cov(0,0) /= size;
    cov(0,1) /= size;
    cov(0,2) /= size;

    cov(1,0) /= size;
    cov(1,1) /= size;
    cov(1,2) /= size;

    cov(2,0) /= size;
    cov(2,1) /= size;
    cov(2,2) /= size;

    Eigen::EigenSolver<Eigen::Matrix3f> es(cov);

    // Columns are the eigenvectors
    Eigen::Matrix3f D = es.pseudoEigenvalueMatrix();
    Eigen::Matrix3f V = es.pseudoEigenvectors();

    Eigen::Vector3f e0( V(0), V(1), V(2));
    Eigen::Vector3f e1( V(3), V(4), V(5));
    Eigen::Vector3f e2( V(6), V(7), V(8));

    //Eigen::Vector3f e0( V(0), V(3), V(6));
    //Eigen::Vector3f e1( V(1), V(4), V(7));
    //Eigen::Vector3f e2( V(2), V(5), V(8));

    float ev0 = D(0);
    float ev1 = D(4);
    float ev2 = D(8);

    vector<pair<float, Eigen::Vector3f > > results(3);
    results[0] = make_pair(ev0, e0);
    results[1] = make_pair(ev1, e1);
    results[2] = make_pair(ev2, e2);

    // sort the vectors, largest first.
    std::sort (results.begin(), results.end(), eigenvaluessort);

    eigenvectors = vector<glm::vec3>(3);
    eigenvectors[0] = glm::vec3(results[0].second(0), results[0].second(1), results[0].second(2));
    eigenvectors[1] = glm::vec3(results[1].second(0), results[1].second(1), results[1].second(2));
    eigenvectors[2] = glm::vec3(results[2].second(0), results[2].second(1), results[2].second(2));

   // cout << "Covariance matrix:" << endl << cov << endl;
  //  cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
  //  cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;

    return;

}

/*!
@brief Function to calculate all eigenvectors for all points
@param all_matches -  the matches for all points in "points". The index of each matchs complies with the point index.
@param points - all points
@param eigenvectors_per_point - the result eigenvectors per point. The index of each matchs complies with the point index.
*/
void computeReferenceFrames( const int n, const int m, glm::vec3 up, const vector< vector<float> >& points, vector< vector<glm::vec3> >& eigenvector_per_point )
{

    for(int i=0; i<m; i++)
    {
        for(int j=0; j<n; j++)
        {
            int current = i*n + j;
            vector<float> p = points[current];

            // find neighbors.
            vector< glm::vec3 > neighbors;

            const int box[8] = { -n-1, -1 ,n-1,
                                -n ,       n,
                                -n+1, +1, n+1};

            for(int k=0; k<8; k++)
            {
                int idx = i + box[k];
                if(idx > 0 && idx < points.size() ) neighbors.push_back( glm::vec3(points[idx][0],points[idx][1], points[idx][2] ));
            }

            glm::vec3 centroid(p[0], p[1], p[2]);
            vector<glm::vec3> eigenvectors;

            calcEigenvectors(centroid, neighbors, eigenvectors);

             ///////////////////////////////////////////////////////////////////
            //
            // Check for right-hand coordinate system.
            //
            // Calc the cross product between x, and y
            glm::vec3 cp = cross(eigenvectors[0], eigenvectors[1]);
            float angle = acos( dot(cp, eigenvectors[2]) );

            if( angle != angle ) // Check for NaN - tiny value, good.
            {
                // do not do anything. vector is fine.
            }
            else if(angle > 3.00)
            {
            //   cout << "replace " << i << " a: " << angle << endl;
                eigenvectors[2] = cp; //replace with cp
            }

            ///////////////////////////////////////////////////////////////////
            //
            // Verify alignment and flip the coordinate system if it is not aligned with
            // the alignment axis. (Should be the camera axis later. )
            //
            static glm::vec3 flip(-1,-1,-1);

            float angle_alignment = acos( dot(eigenvectors[2], up) );

            if(angle_alignment > 1.57)
            {
                eigenvectors[1] = flip * eigenvectors[1];
                eigenvectors[2] = flip * eigenvectors[2];

            }

            // store the eigenvectors
            eigenvector_per_point.push_back(eigenvectors);
        }
    }

}

/*!
@brief - transfers an eigenvector into a matrix in homogenous coordinates.
@param eigenvectors  - vector with eigenvectors sorted corresponding to the eigenvalues, largest first ev1 > ev2 > ev3
@param matrix - the glm matrix.
Notice that all matrix types are column-major rather than row-major.
*/
//static
void Eigenvec2matrix(const vector< glm::vec3 >& eigenvectors, glm::mat4& matrix)
{
    matrix = glm::mat4();

    matrix[0][0] = eigenvectors[0][0];
    matrix[0][1] = eigenvectors[0][1];
    matrix[0][2] = eigenvectors[0][2];

    matrix[1][0] = eigenvectors[1][0];
    matrix[1][1] = eigenvectors[1][1];
    matrix[1][2] = eigenvectors[1][2];

    matrix[2][0] = eigenvectors[2][0];
    matrix[2][1] = eigenvectors[2][1];
    matrix[2][2] = eigenvectors[2][2];

    matrix[3][0] = 0.0;
    matrix[3][1] = 0.0;
    matrix[3][2] = 0.0;

}

/*!
Calculates the factorial of a number
@param x - the value that should be factorized.
@return - the factor.
*/
int factorial(int x, int result) {
  if (x == 1  || x == 0) return result;
  else return factorial(x - 1, x * result);
}

/*!
Calculate the binominal coefficient for n and k
@param n - the number of line segments where the number of supporting points is n+1
@param k - the current point
*/
int BinomialCoeff(int n, int k)
{
    int c = binominal_coefficients[n][k];

    if(c==0) // binominal coefficient is min = 1, 0! = 1
    {
        c = factorial(n)/ ( factorial(k) * factorial(n-k) );
        binominal_coefficients[n][k] = c;

        return c;

    }
    return c;
    //return factorial(n)/ ( factorial(k) * factorial(n-k) );
}

/*!
Calculate the bernstein polynom for P(x) = BC(n,k)  x^k (1-x)^{n-k}
@param x - the interpolation variable x = [0,1]
@param n - the number of line segments where the number of supporting points is n+1
@param k - the current point
*/
float ComputeBernsteinP(float x, int n, int k)
{
    int c = BinomialCoeff(n,k);

    float p_x = float(c) * pow(x, k) *  pow((1.0-x), n-k);

    return p_x;
}

/*!
Computes the points for a cubic spline
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline
*/
bool ComputeCubicSpline(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();

    float increment = 1.0/float(num-1);

    vector<float> cp0 = control_points[0];
    vector<float> cp1 = control_points[1];
    vector<float> cp2 = control_points[2];
    vector<float> cp3 = control_points[3];

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

        double t = increment * i;

        vector<float>p(3);
        p[0] = cp0[0] * ComputeBernsteinP(t,3,0) + cp1[0] * ComputeBernsteinP(t,3,1) + cp2[0] * ComputeBernsteinP(t,3,2) + cp3[0] * ComputeBernsteinP(t,3,3);
        p[1] = cp0[1] * ComputeBernsteinP(t,3,0) + cp1[1] * ComputeBernsteinP(t,3,1) + cp2[1] * ComputeBernsteinP(t,3,2) + cp3[1] * ComputeBernsteinP(t,3,3);
        p[2] = cp0[2] * ComputeBernsteinP(t,3,0) + cp1[2] * ComputeBernsteinP(t,3,1) + cp2[2] * ComputeBernsteinP(t,3,2) + cp3[2] * ComputeBernsteinP(t,3,3);

        cout << t << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;

        result.push_back(p);
    }

    return true;
}

/*!
Computes the points for a cubic spline
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline
*/
bool ComputeCubicSplineC(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();

    float increment = 1.0/float(num-1);

    vector<float> cp0 = control_points[0];
    vector<float> cp1 = control_points[1];
    vector<float> cp2 = control_points[2];
    vector<float> cp3 = control_points[3];

    // the bezier matrix
    static vector< vector<float> > bm( 4, vector<float>(4, 0.0));
    bm[0][0] = -1; bm[0][1] = 3;  bm[0][2] = -3; bm[0][3] = 1;
    bm[1][0] = 3;  bm[1][1] = -6; bm[1][2] = 3;  bm[1][3] = 0;
    bm[2][0] = -3; bm[2][1] = 3;  bm[2][2] = 0;  bm[2][3] = 0;
    bm[3][0] = 1; bm[3][1] = 0;  bm[3][2] = 0;  bm[3][3] = 0;

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

        float t = increment * i;

        float C0 = bm[0][0] * pow(t, 3) + bm[1][0] * pow(t, 2) + bm[2][0] * t + bm[3][0];
        float C1 = bm[0][1] * pow(t, 3) + bm[1][1] * pow(t, 2) + bm[2][1] * t + bm[3][1];
        float C2 = bm[0][2] * pow(t, 3) + bm[1][2] * pow(t, 2) + bm[2][2] * t + bm[3][2];
        float C3 = bm[0][3] * pow(t, 3) + bm[1][3] * pow(t, 2) + bm[2][3] * t + bm[3][3];

        vector<float>p(3);
        p[0] = C0 * cp0[0] + C1 * cp1[0] + C2 * cp2[0] +  C3 * cp3[0];
        p[1] = C0 * cp0[1] + C1 * cp1[1] + C2 * cp2[1] +  C3 * cp3[1];
        p[2] = C0 * cp0[2] + C1 * cp1[2] + C2 * cp2[2] +  C3 * cp3[2];

        cout << t << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;
        result.push_back(p);
    }

    return true;
}

/*!
Computes a cubic surface patch
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline.

NOTE: This code is not fast, it is written that way for clarity.
Usually, one would precomupute all values for BEZ * BEZ and put them into a table.

*/
bool ComputeCubicPatch(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();
    float increment = 1.0/float(num-1);

    vector<float> cp00 = control_points[0];
    vector<float> cp01 = control_points[1];
    vector<float> cp02 = control_points[2];
    vector<float> cp03 = control_points[3];

    vector<float> cp10 = control_points[4];
    vector<float> cp11 = control_points[5];
    vector<float> cp12 = control_points[6];
    vector<float> cp13 = control_points[7];

    vector<float> cp20 = control_points[8];
    vector<float> cp21 = control_points[9];
    vector<float> cp22 = control_points[10];
    vector<float> cp23 = control_points[11];

    vector<float> cp30 = control_points[12];
    vector<float> cp31 = control_points[13];
    vector<float> cp32 = control_points[14];
    vector<float> cp33 = control_points[15];

    // runs along the first coordinate
    for (int i=0; i<num; i++) {
        float t = increment * i;

        // runs along the second corrdinate
        for (int j=0; j<num; j++) {
            float v = increment * j;

            vector<float>p(3);

            // x-coordinate
            float px0 =  cp00[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float px1 =  cp10[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float px2 =  cp20[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float px3 =  cp30[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[0] = px0 + px1 + px2 + px3;

            // y-coordinate
            float py0 =  cp00[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float py1 =  cp10[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float py2 =  cp20[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float py3 =  cp30[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[1] = py0 + py1 + py2 + py3;

            // z-coordinate
            float pz0 =  cp00[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float pz1 =  cp10[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float pz2 =  cp20[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float pz3 =  cp30[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[2] = pz0 + pz1 + pz2 + pz3;

            cout << t << ", " << v << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;
            result.push_back(p);

        }
    }

    return true;
}

/*!
Computes / reorganizes all points so they can be used as triangles to render a mesh
@param n, m, the size of the mesh in number of points as columns n and rows m
@param control_points, control points as vec3 <float> [x, y, x].

     0------1-------2-------3
     |      |       |       |
     4------5-------6-------7
     |      |       |       |
     8------9-------10------11
     |      |       |       |
     12-----13------14------15

@param vertices, the vertices in an vector. Three vertices in a row are one triangle
@param normals, a normal vector for each point
*/
bool TriangulateCubicPatch(const int n, const int m, const vector< vector<float> >& points, const glm::vec3 up, vector< glm::vec3 >& vertices, vector< glm::vec3 >& normals )
{
    int num = (n * m) - n;

    vertices.clear();
    normals.clear();

    // Here, we compute the normal vectors.
    // Note, this can also be optimized in order to speedup the process.
    // However, I compute everything in single steps so you guys can easily find and distinguish them.
    vector< vector<glm::vec3> > eigenvector_per_point;
    computeReferenceFrames( n, m, up, points, eigenvector_per_point);

    // lets find the vertices and let's associate one normal vector per vertex.
    for(int i=0; i<num; i++)
    {
        // suppose we have four control points per quad. We need two triangles per quad
        /*
            0--n--2n--3n
            | /|
            |/ |
            1--n+1

        */

        // triangle 0
        if( i%(n) < n-1 )
        {
            int idx0 = i;
            int idx1 = i+1;
            int idx2 = i + n;

            //cout << 0 <<": " << idx0 << "\t:" << idx1 << "\t" << idx2 << endl;

            vertices.push_back( glm::vec3( points[idx0][0], points[idx0][1], points[idx0][2]  ));
            vertices.push_back( glm::vec3( points[idx1][0], points[idx1][1], points[idx1][2]  ));
            vertices.push_back( glm::vec3( points[idx2][0], points[idx2][1], points[idx2][2]  ));

            normals.push_back(eigenvector_per_point[idx0][2]);
            normals.push_back(eigenvector_per_point[idx1][2]);
            normals.push_back(eigenvector_per_point[idx2][2]);
        }

        // triangle 1
        if( i%(n) < n-1 )
        {
            int idx0 = i+1;
            int idx1 = idx0+n;
            int idx2 = idx0+n-1;

            //cout << 1 <<": " << idx0 << "\t:" << idx1 << "\t" << idx2 << endl;

            vertices.push_back( glm::vec3( points[idx0][0], points[idx0][1], points[idx0][2]  ));
            vertices.push_back( glm::vec3( points[idx1][0], points[idx1][1], points[idx1][2]  ));
            vertices.push_back( glm::vec3( points[idx2][0], points[idx2][1], points[idx2][2]  ));

            normals.push_back(eigenvector_per_point[idx0][2]);
            normals.push_back(eigenvector_per_point[idx1][2]);
            normals.push_back(eigenvector_per_point[idx2][2]);
        }
    }

    return true;
}


#include <fstream>
#include <map>
#include <set>
#include <vector>

#include <iostream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

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

#include "LinearAlgebra.hxx"

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;
typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> VectorXq;

template<class Archive>
void eval_mat::serialize (Archive & ar, unsigned int const) {
  ar & values;
}

void eval_mat::save (std::string const & filename) const {
  std::ofstream file;
  file.open(filename);
  boost::archive::text_oarchive oa {file};
  oa << *this;
}

void eval_mat::load (std::string const & filename) {
  std::ifstream file;
  file.open(filename);
  boost::archive::text_iarchive ia {file};
  ia >> *this;
}

std::set<size_t> eval_mat::row_set () const {
  std::set<size_t> ret;
  std::transform (values.cbegin(), values.cend(), std::inserter (ret, ret.begin()),
    [] (auto const & v) {
      return v.first.second;
    });
  return ret;
}

std::set<size_t> findDependentVariables (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols) {
  MatrixXq mq(cols, rows);

  std::for_each(matrix.cbegin(), matrix.cend(),
    [&mq] (auto const & v) {
      mq (v.first.second, v.first.first) = v.second;
    });

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "rank of the system : " << lu_decompq.rank() << std::endl;

  std::set<size_t> ret;
  VectorXq vector = VectorXq::Zero (cols);
  for (size_t counter = 0; counter < cols; ++counter) {
    if (counter != 0) {
      vector (cols - counter) = 0;
    }
    vector (cols - counter - 1) = 1;
    if (lu_decompq.solve(vector).isZero()) {
      ret.insert (cols - counter - 1);
    }
  }

  return ret;
}

std::map<size_t, std::map<size_t, mpq_class>> solveLinearSystem (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols) {
  MatrixXq mq (rows, cols);

  std::for_each (matrix.cbegin(), matrix.cend(),
      [&mq] (auto const & v) {
        mq (v.first.first, v.first.second) = v.second;
      });

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "rank of the system : " << lu_decompq.rank() << std::endl;

  std::cout << lu_decompq.kernel() << std::endl;

  return {};

}


/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#ifndef EIGEN_UTILITIES_COMPARE_UTILITIES_H
    #define EIGEN_UTILITIES_COMPARE_UTILITIES_H

#include <Eigen/Dense>

namespace eigen_utilities
{

/**
 * @brief Ensure that two matrices have nans in the same spot.
 * To be used in conjunction with norm_nanless()
 */
    template<typename Derived>
inline bool nan_compare(const Eigen::MatrixBase<Derived> &A, const Eigen::MatrixBase<Derived> &B)
{
    return ((A.array() == A.array()) == (B.array() == B.array())).all();
}

inline double nan_compare(double a, double b)
{
    return std::isnan(a) == std::isnan(b);
}

/**
 * @brief Check if there are any NAN elements
 */
    template<typename Derived>
inline bool hasnan(const Eigen::MatrixBase<Derived> &X)
{
    return (X.array() != X.array()).any();
}

/**
 * @brief Check if all are NAN elements
 */
    template<typename Derived>
inline bool isnan(const Eigen::MatrixBase<Derived> &X)
{
    return (X.array() != X.array()).all();
}

/**
 * @brief Zero out all NAN matrix entries
 * @ref http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/01/msg00020.html
 */
    template<typename Derived>
inline double norm_nanless(const Eigen::MatrixBase<Derived> &X)
{
    Eigen::MatrixXd temp = (X.array() == X.array()).select(X, 0);
    return temp.norm();
}

inline double norm_nanless(double x)
{
    if (std::isnan(x))
        return 0;
    else
        return std::fabs(x);
}

/**
 * @brief Zero NAN elements, take norm, compare (a) and (b). Return difference
 */
template<typename A>
double norm_nanless_compare(const A &a, const A &b, bool relative = false)
{
    if (!nan_compare(a, b))
        return INFINITY;
    else
    {
        if (relative)
            return (norm_nanless_compare(a, b));
        else
            return norm_nanless(a - b);
    }
}

/**
 * @brief Return relative difference
 * @param A
 * @param B
 * @return
 *  (B - A) / A,    if (A != 0 & B != 0)
 *  (B - A),        if (A == 0)
 * @todo Also using B == 0 because for it will cause an issue for small values that may not matter
 */
inline double diff_relative_nonzero(double a, double b)
{
    if (a == 0 || b == 0)
        return b - a;
    else
        return (b - a) / a;
}

/**
 * @brief See documentation for scalar version
 */
    template<typename DerivedA, typename DerivedB>
inline Eigen::MatrixXd diff_relative_nonzero(const Eigen::MatrixBase<DerivedA> &A, const Eigen::MatrixBase<DerivedB> &B)
{
    return ((A.array() != 0) * (B.array() != 0)).select((B - A).array() / A.array(), B - A);
}

inline double set_nans(double &X, double value = 0.)
{
    if (std::isnan(X))
        X = value;
    return X;
}

/**
 * @brief Set all nan entries to a non-NAN value
 */
template<typename Derived>
inline void set_nans(Eigen::MatrixBase<Derived> &X, double value = 0.)
{
    for (int i = 0; i < X.size(); ++i)
        set_nans(X(i), value);
}

}

#endif // EIGEN_UTILITIES_COMPARE_UTILITIES_H


#ifndef TRIUMF_SUPERCONDUCTIVITY_BCS_HPP
#define TRIUMF_SUPERCONDUCTIVITY_BCS_HPP

#include <cmath>
#include <limits>
#include <tuple>

#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/ooura_fourier_integrals.hpp>
#include <boost/math/tools/roots.hpp>

#include <triumf/constants/codata_2018.hpp>
#include <triumf/superconductivity/phenomenology.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace superconductivity {

// Bardeen-Cooper-Schrieffer (BCS) theory of superconductivity
namespace bcs {

/// Find the reduced gap Delta(t) for a given reduced temperature x = T / T_c.
template <typename T = double> T reduced_gap_solver(T x) {
  // Use 1 as the initial guess for the Delta(t).
  // This ensures that the first "root" is found as Delta(t) -> 0.0,
  // otherwise the wrong solution is found when t >= 0.9.
  T guess = 1.0;
  // T guess = std::cos(boost::math::constants::half_pi<T>() * std::pow(x, 2));

  // Bound the possible values for the gap.
  // The adjustments by machine epsilon needed for "exact" results!?
  T min = 0.0 - std::numeric_limits<T>::epsilon();
  T max = 1.0 + std::numeric_limits<T>::epsilon();

  // Maximum possible binary digits accuracy for type T.
  const int digits = std::numeric_limits<T>::digits;

  // Digits used to control how accurate to try to make the result.
  // Accuracy triples with each step, so stop when just over one third of the
  // digits are correct.
  int get_digits = static_cast<int>(0.4 * digits);

  // Limit the number of iterations taken to find the "root" of the expression.
  // boost::uintmax_t max_iterations = 20;
  boost::uintmax_t max_iterations =
      std::numeric_limits<boost::uintmax_t>::max();

  // Return the "root" of Thouless' Eqn. using both its 1st and 2nd derivatives.
  T result = boost::math::tools::halley_iterate(
      [x](const T &delta) {
        // Return a tuple containing: f(delta), f'(delta), and  f''(delta).
        T f = std::tanh(delta / x) - delta;
        T df_dx = 1.0 / (x * std::pow(std::cosh(delta / x), 2)) - 1.0;
        T df2_dx2 =
            -2.0 * std::tanh(delta / x) / std::pow(x * std::cosh(delta / x), 2);
        return std::make_tuple(f, df_dx, df2_dx2);
      },
      guess, min, max, get_digits, max_iterations);
  //
  return result;
}

/// temperature dependence of the (reduced) energy gap
template <typename T = double> T reduced_gap(T reduced_temperature) {
  if (reduced_temperature >= 1.0) {
    return 0.0;
  } else if (reduced_temperature <= 0.0) {
    return 1.0;
  } else {
    return reduced_gap_solver<T>(reduced_temperature);
  }
}

/// temperature dependence of the (reduced) energy gap
template <typename T = double>
T reduced_gap(T temperature, T critical_temperature) {
  T reduced_temperature = temperature / critical_temperature;
  return reduced_gap<T>(reduced_temperature);
}

/// temperature dependence of the energy gap
template <typename T = double>
T gap(T temperature, T critical_temperature, T gap_meV) {
  return gap_meV * reduced_gap<T>(temperature, critical_temperature);
}

/// energy gap at absolute zero
template <typename T = double> T gap_meV(T critical_temperature) {
  // Boltzmann constant (meV / K)
  constexpr T k_B_meV_per_K =
      1e3 *
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  //
  return boost::math::constants::pi<T>() *
         std::exp(-boost::math::constants::euler<T>()) * k_B_meV_per_K *
         critical_temperature;
}

// energy gap ratio (at absolute zero)
template <typename T = double> T gap_ratio(T critical_temperature, T gap_meV) {
  // Boltzmann constant (meV / K)
  constexpr T k_B_meV_per_K =
      1e3 *
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  //
  return (2.0 * gap_meV) / (k_B_meV_per_K * critical_temperature);
}

/// helper function for BCS Kernel
template <typename T = double>
T a(T temperature, T critical_temperature, T gap_meV) {
  return boost::math::constants::pi<T>() * 1e3 *
         triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<
             T>::value() *
         temperature / gap<T>(temperature, critical_temperature, gap_meV);
}

/// helper function for BCS Kernel
template <typename T = double>
T f(T temperature, T critical_temperature, T gap_meV, T n) {
  return std::sqrt(1.0 +
                   std::pow(a<T>(temperature, critical_temperature, gap_meV) *
                                (2.0 * n + 1.0),
                            2));
}

/// temperature dependence of the BCS coherence length (helper function)
template <typename T = double>
T coherence_length(T temperature, T critical_temperature, T gap_meV, T xi_0,
                   T mean_free_path, T n) {
  T fraction_1 = boost::math::constants::two_div_pi<T>() *
                 f<T>(temperature, critical_temperature, gap_meV, n) *
                 reduced_gap<T>(temperature, critical_temperature) / xi_0;
  T fraction_2 = 1.0 / mean_free_path;
  T fraction = fraction_1 + fraction_2;
  // handle a some edge cases for very big/small args
  if (std::isinf(fraction)) {
    return 0.0;
  } else if (fraction == 0.0) {
    return std::numeric_limits<T>::infinity();
  } else {
    return 1.0 / fraction;
  }
}

/// helper function for BCS Kernel
template <typename T = double>
T Lambda(T temperature, T critical_temperature, T gap_meV, T xi_0,
         T mean_free_path, T lambda_0, T exponent, T n) {
  return std::pow(
             lambda_0,
             // triumf::superconductivity::phenomenology::penetration_depth<T>(
             //    temperature, critical_temperature, exponent, lambda_0),
             2) *
         std::pow(f<T>(temperature, critical_temperature, gap_meV, n), 3) *
         (1.0 + coherence_length<T>(temperature, critical_temperature, gap_meV,
                                    xi_0, mean_free_path, n) /
                    mean_free_path) /
         (2.0 * a<T>(temperature, critical_temperature, gap_meV));
}

/// helper function for the BCS Kernel
template <typename T = double> T g(T x) {
  // return correct result when x ~ 0.
  if (x < 1.0e-4) {
    return 1.0;
  } else {
    return (3.0 / 2.0) * ((1.0 + x * x) * std::atan(x) - x) / (x * x * x);
  }
}

/// BCS Kernel
template <typename T = double>
T kernel(T q, T temperature, T critical_temperature, T gap_meV, T xi_0,
         T mean_free_path, T lambda_0, T exponent) {
  //
  T sum = 0.0;
  T change = 0.0;
  // const T precision = std::sqrt(std::numeric_limits<T>::epsilon());
  const T precision = std::numeric_limits<T>::epsilon();
  const T max_iterations = 100;
  T n = 0.0;
  do {
    //
    T x = q * coherence_length<T>(temperature, critical_temperature, gap_meV,
                                  xi_0, mean_free_path, n);
    //
    change = g<T>(x) / Lambda<T>(temperature, critical_temperature, gap_meV,
                                 xi_0, mean_free_path, lambda_0, exponent, n);
    //
    sum += change;
    n += 1.0;
  } while ((std::abs(change) > precision) and (n < max_iterations));
  //
  return sum;
}

/// (reduced) BCS Kernel
template <typename T = double>
T reduced_kernel(T q, T temperature, T critical_temperature, T gap_meV, T xi_0,
                 T mean_free_path, T lambda_0, T exponent) {
  return kernel<T>(q, temperature, critical_temperature, gap_meV, xi_0,
                   mean_free_path, lambda_0, exponent) /
         kernel<T>(0.0, temperature, critical_temperature, gap_meV, xi_0,
                   mean_free_path, lambda_0, exponent);
}

/// BCS magnetic penetration depth
template <typename T = double>
T penetration_depth(T temperature, T critical_temperature, T gap_meV, T xi_0,
                    T mean_free_path, T lambda_0, T exponent) {
  T K_0 = kernel<T>(0.0, temperature, critical_temperature, gap_meV, xi_0,
                    mean_free_path, lambda_0, exponent);
  return std::sqrt(1.0 / K_0);
}

/// (reduced) BCS magnetic field penetration profile
template <typename T = double>
T reduced_field_penetration(T z, T temperature, T critical_temperature,
                            T gap_meV, T xi_0, T mean_free_path, T lambda_0,
                            T exponent) {
  //
  if (z <= 0.0) {
    return 1.0;
  } else {
    //
    auto bcs_integrand = [&](T q) -> T {
      T K = kernel<T>(q, temperature, critical_temperature, gap_meV, xi_0,
                      mean_free_path, lambda_0, exponent);
      return q / (q * q + K);
    };
    // create the integrator w/ default tolerance and evaluation levels
    // (root_epsilon and eight levels for type double).
    static boost::math::quadrature::ooura_fourier_sin<T> bcs_integrator =
        boost::math::quadrature::ooura_fourier_sin<T>();
    // evaluate the integral, which returns a pair
    // (first = integral, second = relative error)
    std::pair<T, T> result = bcs_integrator.integrate(bcs_integrand, z);
    // return the integral multiplied by the prefactors to get B vs. z
    return boost::math::constants::two_div_pi<T>() * result.first;
  }
}

/// BCS magnetic field penetration proifile
template <typename T = double>
T field_penetration(T z, T temperature, T critical_temperature, T gap_meV,
                    T xi_0, T mean_free_path, T lambda_0, T exponent,
                    T applied_field) {
  return applied_field * reduced_field_penetration<T>(
                             z, temperature, critical_temperature, gap_meV,
                             xi_0, mean_free_path, lambda_0, exponent);
}

} // namespace bcs

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_BCS_HPP


#include <stdio.h>
#include <algorithm>
#include <math.h>
#include <float.h>

#include "timing.h"

#include <Eigen/Dense>

using namespace Eigen;

int main() {

	int n = 10000000;
	double xo = 0;
	double yo = 0;
	double zo = 0;
	double xd = 1;
	double yd = 0;
	double zd = 0;

  ArrayXd xc(n);
  ArrayXd yc(n);
  ArrayXd zc(n);

	for(int i = 0; i < n; i++) {
		xc[i] = i;
		yc[i] = i;
		zc[i] = i;
	}

  ArrayXd b(n);
  ArrayXd c(n);
  ArrayXd disc(n);
    ArrayXd res(n);
	b.fill(0);
	c.fill(0);
	disc.fill(0);
	res.fill(0);
	double begin = current_time();

	double a = 1;

	double r = DBL_MAX;

  b = 2*(xd*(xo-xc)+yd*(yo-yc)+zd*(zo-zc));
  c = (xo-xc)*(xo-xc)+(yo-yc)*(yo-yc)+(zo-zc)*(zo-zc)-1;
  disc = b*b-4*c;
  res = ((disc<0).select(((-b - disc.sqrt())/2),DBL_MAX)); //.min( (-b + disc)/2),DBL_MAX));
  r = res.minCoeff();

	printf("%f\n",r);
	printf("Elapsed: %f\n", current_time()-begin);
	return 0;
}


#include <boost/test/unit_test.hpp>

#include <ayla/geometry/vector.hpp>
#include <ayla/geometry/matrix.hpp>
#include <ayla/geometry/rigid_transform.hpp>

#include <glm/gtc/matrix_transform.hpp>

BOOST_AUTO_TEST_SUITE(ayla)
BOOST_AUTO_TEST_SUITE(matrix4)

BOOST_AUTO_TEST_CASE( mat4_constructor16 ) {
	glm::mat4 testcase(1.0f,2.0f,3.0f,4.0f,
			5.0f,6.0f,7.0f,8.0f,
			9.0f,10.0f,11.0f,12.0f,
			13.0f,14.0f,15.0f,16.0f);

	Float count=1.0f;
	for (SizeType row = 0; row < 4; ++row) {
		for (SizeType col = 0; col < 4; ++col) {
			BOOST_CHECK_EQUAL(testcase[row][col], count);
			count += 1.0;
		}
	}
}

BOOST_AUTO_TEST_CASE( mat4_scale_transformation_and_inverse ) {
	glm::mat4 scale = glm::scale(glm::mat4(1.0f), glm::vec3(2.0, 3.0, 5.0));
	glm::vec4 point(1.0,1.0,1.0,1.0);

	BOOST_CHECK_EQUAL( (scale*point).x, 2.0 );
	BOOST_CHECK_EQUAL( (scale*point).y, 3.0 );
	BOOST_CHECK_EQUAL( (scale*point).z, 5.0 );
	BOOST_CHECK_EQUAL( (scale*point).w, 1.0 );

	point = scale*point;

	BOOST_CHECK_EQUAL( (scale*point).x, 4.0 );
	BOOST_CHECK_EQUAL( (scale*point).y, 9.0 );
	BOOST_CHECK_EQUAL( (scale*point).z, 25.0 );
	BOOST_CHECK_EQUAL( (scale*point).w, 1.0 );

	point = scale*point;

	glm::mat4 scaleInv = glm::inverse(scale);
	scale = scaleInv;

	point = scaleInv*point;
	point = scale*point;

	BOOST_CHECK( glm::epsilonEqual(point.x, 1.0f, 0.00001f) );
	BOOST_CHECK( glm::epsilonEqual(point.y, 1.0f, 0.00001f) );
	BOOST_CHECK( glm::epsilonEqual(point.z, 1.0f, 0.00001f) );
	BOOST_CHECK( glm::epsilonEqual(point.w, 1.0f, 0.00001f) );

	const glm::mat4 identity = scale*glm::inverse(scaleInv);
	BOOST_CHECK( epsilonEqual(identity, glm::mat4(1.0f), 0.00001f) );
}

BOOST_AUTO_TEST_CASE( mat4_constructor_from_rigid_transform_translate ) {
	RigidTransform t = RigidTransform::getIdentity();
	t.translate(glm::vec3(3.0,5.0,7.0));

	glm::mat4 mat4= t.toMatrix4();

	BOOST_CHECK_EQUAL( mat4,
		glm::mat4(
			1,0,0,0,
			0,1,0,0,
			0,0,1,0,
			3,5,7,1
		)
	);
}

BOOST_AUTO_TEST_CASE( mat4_constructor_from_rigid_transform_ytranslate ) {
	for (SizeType i = 0; i < 360; ++i) {
		RigidTransform t = RigidTransform::getIdentity();
		const Float rotationAmount = glm::radians(Float(i));
		t.rotate(rotationAmount,glm::vec3(0,1,0));
		const glm::mat4 fromRigidTransform = t.toMatrix4();
		const glm::mat4 rotateY = glm::rotate(glm::mat4(1.0f), rotationAmount, glm::vec3(0,1,0));

		BOOST_CHECK_MESSAGE( epsilonEqual(fromRigidTransform, rotateY,0.000001f), fromRigidTransform << " != " << rotateY);
	}
}

BOOST_AUTO_TEST_CASE( mat4_constructor_from_rigid_transform_with_rotation_and_translation ) {
	for (SizeType i = 0; i < 1; ++i) {
		RigidTransform t = RigidTransform::getIdentity();
		const Float rotationAmount = glm::radians(Float(i));
		const glm::vec3 translateAmount(glm::vec3(Float(3*i),Float(5*i),Float(7*i)));
		t.rotate(rotationAmount,glm::vec3(0,1,0));
		t.translate(translateAmount);

		const glm::mat4 fromRigidTransform = t.toMatrix4();

		const glm::mat4 fromMatrix = glm::rotate(
			glm::translate(glm::mat4(1.0f), translateAmount),
			rotationAmount,
			glm::vec3(0,1,0)
		);

		BOOST_CHECK_MESSAGE( epsilonEqual(fromRigidTransform, fromMatrix, 0.000001f), fromRigidTransform << " != " << fromMatrix);
	}
}

BOOST_AUTO_TEST_CASE( mat4_constructor_from_rigid_transform_lot_of_rotations_then_translate ) {
	RigidTransform t = RigidTransform::getIdentity();
	const Float rotationAmountA = glm::radians(Float(13));
	const Float rotationAmountB = glm::radians(Float(17));

	for (SizeType i = 0; i < 5; ++i) {
		t.rotate(rotationAmountA,glm::vec3(0,1,0));
		t.translate(glm::vec3(2,3,5));
		t.rotate(rotationAmountB,glm::vec3(1,0,0));
	}
	glm::mat4 fromRigidTransform = t.toMatrix4();

	glm::mat4 fromMatrix = glm::mat4(1.0f);
	for (SizeType i = 0; i < 5; ++i) {
		fromMatrix = glm::rotate(glm::mat4(1.0f), rotationAmountA, glm::vec3(0,1,0)) * fromMatrix; // glm::mat4::rotateY(rotationAmountA)*fromMatrix;
		fromMatrix = glm::rotate(glm::mat4(1.0f), rotationAmountB, glm::vec3(1,0,0)) * fromMatrix; //glm::mat4::rotateX(rotationAmountB)*fromMatrix;
	}
	fromMatrix = glm::translate(glm::mat4(1.0f), glm::vec3(2*5,3*5,5*5))*fromMatrix;

	//High tolerance to errors because matrix accumulate a lot of numerical instability
	BOOST_CHECK_MESSAGE( epsilonEqual(fromRigidTransform, fromMatrix, 0.1f), fromRigidTransform << " != " << fromMatrix);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()