#include <Spirit_Defines.h>
#include <engine/Method_MMF.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <io/IO.hpp>
#include <utility/Logging.hpp>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
//#include <unsupported/Eigen/CXX11/Tensor>
#include <GenEigsSolver.h>  // Also includes <MatOp/DenseGenMatProd.h>
#include <GenEigsRealShiftSolver.h>

#include <fmt/format.h>

using Utility::Log_Level;
using Utility::Log_Sender;

namespace Engine
{
	template <Solver solver>
    Method_MMF<solver>::Method_MMF(std::shared_ptr<Data::Spin_System_Chain_Collection> collection, int idx_chain) :
        Method_Solver<solver>(collection->parameters, -1, idx_chain), collection(collection)
    {
		int noc = collection->noc;
		int nos = collection->chains[0]->images[0]->nos;
		switched1 = false;
		switched2 = false;
		this->SenderName = Utility::Log_Sender::MMF;

        // The systems we use are the last image of each respective chain
        for (int ichain = 0; ichain < noc; ++ichain)
		{
			this->systems.push_back(this->collection->chains[ichain]->images.back());
		}

		// History
        this->history = std::map<std::string, std::vector<scalar>>{
			{"max_torque_component", {this->force_max_abs_component}} };

		// We assume that the systems are not converged before the first iteration
		this->force_max_abs_component = this->collection->parameters->force_convergence + 1.0;

		this->hessian = std::vector<MatrixX>(noc, MatrixX(3*nos, 3*nos));	// [noc][3nos]
		// Forces
		this->gradient   = std::vector<vectorfield>(noc, vectorfield(nos));	// [noc][3nos]
		this->minimum_mode = std::vector<vectorfield>(noc, vectorfield(nos));	// [noc][3nos]
		this->xi = vectorfield(this->nos, {0,0,0});

		// Last iteration
		this->spins_last = std::vector<vectorfield>(noc, vectorfield(nos));	// [noc][3nos]
		this->spins_last[0] = *this->systems[0]->spins;
		this->Rx_last = 0.0;

		// Force function
		// ToDo: move into parameters
		this->mm_function = "Spectra Matrix"; // "Spectra Matrix" "Spectra Prefactor" "Lanczos"

        //---- Initialise Solver-specific variables
        this->Initialize();
    }

	template <Solver solver>
    void Method_MMF<solver>::Calculate_Force(const std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & forces)
    {
		if (this->mm_function == "Spectra Matrix")
		{
			this->Calculate_Force_Spectra_Matrix(configurations, forces);
		}
		/*else if (this->mm_function == "Spectra Prefactor")
		{
			this->Calculate_Force_Spectra_Prefactor(configurations, forces);
		}
		else if (this->mm_function == "Lanczos")
		{
			this->Calculate_Force_Lanczos(configurations, forces);
		}*/
		#ifdef SPIRIT_ENABLE_PINNING
			Vectormath::set_c_a(1, forces[0], forces[0], this->parameters->pinning->mask_unpinned);
		#endif // SPIRIT_ENABLE_PINNING
    }

	MatrixX projector(vectorfield & image)
	{
		int nos = image.size();
		int size = 3*nos;

		// Get projection matrix M=1-S, blockwise S=x*x^T
		MatrixX proj = MatrixX::Identity(size, size);
		for (int i = 0; i < nos; ++i)
		{
			proj.block<3, 3>(3*i, 3*i) -= image[i] * image[i].transpose();
		}

		return proj;
	}

	template <Solver solver>
	void Method_MMF<solver>::Calculate_Force_Spectra_Matrix(const std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & forces)
	{
		const int nos = configurations[0]->size();
		// std::cerr << "mmf iteration" << std::endl;

		// Loop over chains and calculate the forces
		for (int ichain = 0; ichain < this->collection->noc; ++ichain)
		{
			auto& image = *configurations[ichain];
			auto& hess = hessian[ichain];

			// Copy std::vector<Eigen::Vector3> into one single Eigen::VectorX
			Eigen::Ref<VectorX> x = Eigen::Map<VectorX>(image[0].data(), 3 * nos);

			// The gradient (unprojected)
			this->systems[ichain]->hamiltonian->Gradient(image, gradient[ichain]);

			// The Hessian (unprojected)
			this->systems[ichain]->hamiltonian->Hessian(image, hess);

			// The projection matrix
			auto P = projector(image);

			// std::cerr << "------------------------" << std::endl;
			// std::cerr << "x:             " << x.transpose() << std::endl;
			// std::cerr << "gradient:      " << gradient.transpose() << std::endl;
			// std::cerr << "hessian:       " << std::endl << hess << std::endl;
			// std::cerr << "projector:     " << std::endl << P << std::endl;
			// std::cerr << "gradient proj: " << std::endl << (P*grad).transpose() << std::endl;
			// std::cerr << "hessian proj:  " << std::endl << P*hess*P << std::endl;

			// Eigen::EigenSolver<Eigen::MatrixXd> estest1(hess);
			// td::cerr << "hessian:    " << std::endl << hess << std::endl;
			// std::cerr << "eigen vals: " << std::endl << estest1.eigenvalues() << std::endl;
			// std::cerr << "eigen vecs: " << std::endl << estest1.eigenvectors().real() << std::endl;

			/*
			/  Remove Hessian's components in the basis of the image (project it into tangent space)
			/      and add the gradient contributions (inner and outer product)
			*/

			// Correction components
			// TODO: write Kernel for this
			//     the following is equivalent to hess = P*hess*P but that could not be parallelized on the GPU
			// for (int i = 0; i < nos; ++i)
			// {
			// 	for (int j = 0; j < nos; ++j)
			// 	{
			// 		hess.block<3, 3>(3*i, 3*j) = P.block<3, 3>(3*i, 3*i) * hess.block<3, 3>(3*i, 3*j) * P.block<3, 3>(3*j, 3*j);
			// 	}
			// }

			// // Hessian projection components (diagonal contribution blocks)
			// // TODO: write Kernel for this
			// for (int i = 0; i < nos; ++i)
			// {
			// 	hess.block<3, 3>(3*i, 3*i) -= P.block<3, 3>(3*i, 3*i)*(image[i].dot(gradient[ichain][i])) + (P.block<3, 3>(3*i, 3*i)*gradient[ichain][i])*image[i].transpose();
			// }

			// // std::cerr << "hessian final: " << std::endl << hess << std::endl;

    		// // Eigen::EigenSolver<Eigen::MatrixXd> estest1(hess);
			// // Eigen::VectorXd estestvals = estest1.eigenvalues().real();
			// // std::sort(estestvals.data(),estestvals.data()+estestvals.size());
			// // std::cerr << "eigen vals (" << estestvals.size() << "): " << std::endl << estestvals.transpose() << std::endl;
			// // int n_zero=0;
			// // for (int _i=0; _i<estestvals.size(); ++_i)
			// // {
			// // 	if (std::abs(estestvals[_i]) < 1e-7) ++n_zero;
			// // }
			// // std::cerr << std::endl << "number of zero eigenvalues: " << n_zero << std::endl;

			// // Get the lowest Eigenvector
			// //		Create a Spectra solver
			// Spectra::DenseGenMatProd<scalar> op(hess);
			// Spectra::GenEigsSolver< scalar, Spectra::SMALLEST_REAL, Spectra::DenseGenMatProd<scalar> > hessian_spectrum(&op, 1, 3*nos);
			// hessian_spectrum.init();
			// //		Compute the specified spectrum
			// int nconv = hessian_spectrum.compute();
			// if (hessian_spectrum.info() == Spectra::SUCCESSFUL)
			// {
			// 	// Calculate the Force
			// 	// 		Retrieve the Eigenvalues
			// 	VectorX evalues = hessian_spectrum.eigenvalues().real();

			// 	//std::cerr << "spectra val: " << std::endl << hessian_spectrum.eigenvalues() << std::endl;
			// 	//std::cerr << "spectra vec: " << std::endl << -reorder_vector(hessian_spectrum.eigenvectors().col(0).real()) << std::endl;

			// 	// 		Check if the lowest eigenvalue is negative
			// 	if (evalues[0] < -1e-5)// || switched2)
			// 	{
			// 		// std::cerr << "negative region " << evalues.transpose() << std::endl;
			// 		if (switched1)
			// 			switched2 = true;
			// 		// Retrieve the Eigenvectors
			// 		MatrixX evectors = hessian_spectrum.eigenvectors().real();
			// 		Eigen::Ref<VectorX> evec = evectors.col(0);

			// 		// The following line does not seem to work with Eigen3.3
			// 		// this->minimum_mode[ichain] = vectorfield(evec.data(), evec.data() + evec.rows()*evec.cols());
			// 		// TODO: use Eigen's Map or Ref instead
			// 		for (int n=0; n<nos; ++n)
			// 		{
			// 			this->minimum_mode[ichain][n] = {evec[3*n], evec[3*n+1], evec[3*n+2]};
			// 		}

			// 		scalar check = Vectormath::dot(minimum_mode[ichain], image);
			// 		scalar checknorm = Manifoldmath::norm(minimum_mode[ichain]);
			// 		if (std::abs(check) > 1e-8 || std::abs(checknorm) < 1e-8)
			// 		{
			// 			std::cerr << "-------------------------" << std::endl;
			// 			std::cerr << "BAD MODE! evalue = " << evalues[0] << std::endl;
			// 			std::cerr << "-------------------------" << std::endl;
			// 		}

			// 		//for (int _i = 0; _i < forces[ichain].size(); ++_i) minimum_mode[ichain][_i] = -minimum_mode[ichain][_i];
			// 		// 		Normalize the mode vector in 3N dimensions
			// 		Engine::Manifoldmath::normalize(this->minimum_mode[ichain]);

			// 		// Invert the gradient force along the minimum mode
			// 		Engine::Manifoldmath::invert_parallel(gradient[ichain], minimum_mode[ichain]);
			// 		// Copy out the forces
			// 		for (unsigned int _i = 0; _i < forces[ichain].size(); ++_i) forces[ichain][_i] = -gradient[ichain][_i];
			// 		//forces[ichain] = gradient[ichain];

			// 		// std::cerr << "grad      " << gradient[ichain][0] << " " << gradient[ichain][1] << " " << gradient[ichain][2] << std::endl;
			// 		// std::cerr << "min mode: " << std::endl << evec << std::endl;
			// 		// std::cerr << "mode      " << minimum_mode[ichain][0] << " " << minimum_mode[ichain][1] << " " << minimum_mode[ichain][2] << std::endl;
			// 		// std::cerr << "force     " << forces[ichain][0] << " " << forces[ichain][1] << " " << forces[ichain][2] << std::endl;
			// 	}
			// 	//		Otherwise we seek for the lowest nonzero eigenvalue
			// 	else
			// 	{
			// 		switched1 = true;
			// 		// std::cerr << "positive region " << evalues.transpose() << std::endl;
			// 		///////////////////////////////////
			// 		//// The Eigen way... calculate them all... Inefficient! Do it the spectra way instead!
			// 		//Eigen::EigenSolver<Eigen::MatrixXd> estest1(hess);
			// 		//std::cerr << "hessian:    " << std::endl << hess << std::endl;
			// 		//std::cerr << "eigen vals: " << std::endl << estest1.eigenvalues() << std::endl;
			// 		//std::cerr << "eigen vecs: " << std::endl << reorder_matrix(estest1.eigenvectors().real()) << std::endl;
			// 		///////////////////////////////////

			// 		// // Create the Minimum Mode
			// 		// Spectra::DenseGenRealShiftSolve<scalar> op_pos(hess);
			// 		// Spectra::GenEigsRealShiftSolver< scalar, Spectra::SMALLEST_REAL, Spectra::DenseGenRealShiftSolve<scalar> > hessian_spectrum_pos(&op_pos, 1, 3 * nos, 1.0e-5);
			// 		// hessian_spectrum_pos.init();
			// 		// //		Compute the specified spectrum
			// 		// int nconv = hessian_spectrum_pos.compute();
			// 		// // 		Retrieve the Eigenvectors
			// 		// //std::cerr << "-----" << std::endl;
			// 		// //std::cerr << "spectra:" << std::endl;
			// 		// //std::cerr << hessian_spectrum_pos.eigenvalues() << std::endl;
			// 		// //std::cerr << hessian_spectrum_pos.eigenvectors().real() << std::endl;
			// 		// //std::cerr << "-----" << std::endl;
			// 		// Eigen::MatrixXd evectors = hessian_spectrum_pos.eigenvectors().real();
			// 		// //auto evec = evectors.col(0);
			// 		// Eigen::Ref<Eigen::VectorXd> evec = evectors.col(0);

			// 		// for (int n=0; n<nos; ++n)
			// 		// {
			// 		// 	this->minimum_mode[ichain][n] = {evec[3*n], evec[3*n+1], evec[3*n+2]};
			// 		// }

			// 		// Engine::Manifoldmath::normalize(this->minimum_mode[ichain]);

			// 		// //this->minimum_mode[ichain] = std::vector<scalar>(evec.data(), evec.data() + evec.rows()*evec.cols());
			// 		// ////for (int _i = 0; _i < forces[ichain].size(); ++_i) minimum_mode[ichain][_i] = -minimum_mode[ichain][_i];
			// 		// //// 		Normalize the mode vector in 3N dimensions
			// 		// //Utility::Vectormath::Normalize(this->minimum_mode[ichain]);

			// 		// ////// 		Copy via assignment
			// 		// ////this->minimum_mode[ichain] = std::vector<scalar>(evec_min.data(), evec_min.data() + evec_min.rows()*evec_min.cols());
			// 		// ////// 		Normalize the mode vector in 3N dimensions
			// 		// ////Utility::Vectormath::Normalize(this->minimum_mode[ichain]);
			// 		// ////// We are too close to the local minimum so we have to use a strong force
			// 		// //////		We apply the force against the minimum mode of the positive eigenvalue
			// 		// scalar v1v2 = 0.0;
			// 		// for (int i = 0; i < nos; ++i)
			// 		// {
			// 		// 	v1v2 += gradient[ichain][i].dot(minimum_mode[ichain][i]);
			// 		// }
			// 		// // Take out component in direction of v2
			// 		// for (int i = 0; i < nos; ++i)
			// 		// {
			// 		// 	gradient[ichain][i] =  -v1v2 * minimum_mode[ichain][i]; // -gradient[ichain][i]
			// 		// }

			// 		// for (int i = 0; i < nos; ++i)
			// 		// {
			// 		// 	forces[ichain][i] =  -gradient[ichain][i];
			// 		// }

			// 		// Copy out the forces
			// 		forces[ichain] = gradient[ichain];
			// 	}
			// 	//std::cerr << "------------------------" << std::endl;
			// }
			// else
			// {
			// 	Log(Log_Level::Error, Log_Sender::MMF, "Failed to calculate eigenvectors of the Hessian!");
			// 	Log(Log_Level::Info, Log_Sender::MMF, "Zeroing the MMF force...");
			// 	for (Vector3 x : forces[ichain]) x.setZero();
			// }
		}
		// std::cerr << "mmf iteration done" << std::endl;
	}

	void printmatrix(MatrixX & m)
	{
		std::cerr << m << std::endl;
	}

    // Check if the Forces are converged
	template <Solver solver>
    bool Method_MMF<solver>::Converged()
    {
		if (this->force_max_abs_component < this->collection->parameters->force_convergence) return true;
		return false;
    }

	template <Solver solver>
    void Method_MMF<solver>::Hook_Pre_Iteration()
    {

	}

	template <Solver solver>
    void Method_MMF<solver>::Hook_Post_Iteration()
    {
        // --- Convergence Parameter Update
		this->force_max_abs_component = 0;
		for (int ichain = 0; ichain < collection->noc; ++ichain)
		{
			scalar fmax = this->Force_on_Image_MaxAbsComponent(*(this->systems[ichain]->spins), gradient[ichain]);
			if (fmax > this->force_max_abs_component) this->force_max_abs_component = fmax;
		}

        // --- Update the chains' last images
		for (auto system : this->systems) system->UpdateEnergy();
		for (auto chain : collection->chains)
		{
			int i = chain->noi - 1;
			if (i>0) chain->Rx[i] = chain->Rx[i - 1] + Engine::Manifoldmath::dist_geodesic(*chain->images[i]->spins, *chain->images[i-1]->spins);
		}
    }

	template <Solver solver>
    void Method_MMF<solver>::Save_Current(std::string starttime, int iteration, bool initial, bool final)
	{
		// History save
        this->history["max_torque_component"].push_back(this->force_max_abs_component);

		// File save
		if (this->parameters->output_any)
		{
			// Convert indices to formatted strings
            std::string s_img = fmt::format("{:0>2}", this->idx_image);

			std::string preSpinsFile;
			std::string preEnergyFile;
            std::string fileTag;

			if (this->collection->parameters->output_file_tag == "<time>")
                fileTag = starttime + "_";
            else if (this->collection->parameters->output_file_tag != "")
                fileTag = this->collection->parameters->output_file_tag + "_";
            else
                fileTag = "";

			preSpinsFile = this->parameters->output_folder + "/" + fileTag + "Spins_" + s_img;
			preEnergyFile = this->parameters->output_folder + "/" + fileTag + "Energy_" + s_img;

			// Function to write or append image and energy files
			auto writeOutputConfiguration = [this, preSpinsFile, preEnergyFile, iteration](std::string suffix, bool append)
			{
				// File name and comment
				std::string spinsFile = preSpinsFile + suffix + ".txt";
                std::string comment = std::to_string( iteration );
				// Spin Configuration
                IO::Write_Spin_Configuration( *( this->systems[0] )->spins,
                                              *( this->systems[0] )->geometry, spinsFile,
                                              IO::VF_FileFormat::SPIRIT_WHITESPACE_SPIN,
                                              comment, append );
			};

			auto writeOutputEnergy = [this, preSpinsFile, preEnergyFile, iteration](std::string suffix, bool append)
			{
				int base = (int)log10(this->parameters->n_iterations);
				std::string s_iter = fmt::format("{:0>"+fmt::format("{}",base)+"}", iteration);
				bool normalize = this->systems[0]->llg_parameters->output_energy_divide_by_nspins;

				// File name
				std::string energyFile = preEnergyFile + suffix + ".txt";
				std::string energyFilePerSpin = preEnergyFile + suffix + "_perSpin.txt";

				// Energy
				// Check if Energy File exists and write Header if it doesn't
				std::ifstream f(energyFile);
				if (!f.good()) IO::Write_Energy_Header(*this->systems[0], energyFile);
				// Append Energy to File
				//IO::Append_Image_Energy(*this->systems[0], iteration, energyFile, normalize);

				//
				scalar Rx = Rx_last + Engine::Manifoldmath::dist_geodesic(spins_last[0], *this->systems[0]->spins);
				spins_last[0] = *this->systems[0]->spins;
				Rx_last = Rx;
				//
				scalar nd = 1.0;
				if (this->collection->parameters->output_energy_divide_by_nspins) nd /= this->systems[0]->nos; // nos divide
				std::string output_to_file = s_iter + fmt::format("    {18.10f}    {18.10f}\n", Rx, this->systems[0]->E * nd);
				IO::Append_String_to_File(output_to_file, energyFile);
			};

			// Initial image before simulation
			if (initial && this->parameters->output_initial)
			{
				writeOutputConfiguration("_initial", false);
				writeOutputEnergy("_initial", false);
			}
			// Final image after simulation
			else if (final && this->parameters->output_final)
			{
				writeOutputConfiguration("_final", false);
				writeOutputEnergy("_final", false);
			}

			// Single file output
            int base = (int)log10(this->parameters->n_iterations);
            std::string s_iter = fmt::format("{:0>"+fmt::format("{}",base)+"}", iteration);
			if (this->systems[0]->llg_parameters->output_configuration_step)
			{
				writeOutputConfiguration("_" + s_iter, false);
			}
			if (this->systems[0]->llg_parameters->output_energy_step)
			{
				writeOutputEnergy("_" + s_iter, false);
			}

			// Archive file output (appending)
			if (this->systems[0]->llg_parameters->output_configuration_archive)
			{
				writeOutputConfiguration("_archive", true);
			}
			if (this->systems[0]->llg_parameters->output_energy_archive)
			{
				writeOutputEnergy("_archive", true);
			}

			//if (initial && this->parameters->output_initial) return;

			// Insert copies of the current systems into their corresponding chains
			// - this way we will be able to look at the history of the optimizations
			// for (int ichain=0; ichain<collection->noc; ++ichain)
			// {
			//     // Copy the image
			//	   this->systems[ichain]->Lock()
			//     auto copy = std::shared_ptr<Data::Spin_System>(new Data::Spin_System(*this->systems[ichain]));
			//	   this->systems[ichain]->Unlock()

			//     // Insert into chain
			//     auto chain = collection->chains[ichain];
			//     chain->noi++;
			//     chain->images.insert(chain->images.end(), copy);
			//     chain->climbing_image.insert(chain->climbing_image.end(), false);
			//     chain->falling_image.insert(chain->falling_image.end(), false);
			// }

			// Reallocate and recalculate the chains' Rx, E and interpolated values for their last two images

			// Append Each chain's new image to it's corresponding archive

			// In the final save, we save all chains to file?
			//if (final && this->parameters->output_final)
			//{

			//}

			//auto writeoutput = [this, starttime, iteration](std::string suffix)
			//{
			//	// Convert indices to formatted strings
			//	auto s_img = IO::int_to_formatted_string(this->idx_image, 2);
			//	auto s_iter = IO::int_to_formatted_string(iteration, 6);

			//	// if (this->parameters->output_archive)
			//	// {
			//		// Append Spin configuration to Spin_Archieve_File
			//		std::string spinsFile, energyFile;
			//		if (this->collection->parameters->output_tag_time)
			//			spinsFile = this->parameters->output_folder + "/" + starttime + "_Spins_" + s_img + suffix + ".txt";
			//		else
			//			spinsFile = this->parameters->output_folder + "/Spins_" + s_img + suffix + ".txt";
			//		IO::Write_Spin_Configuration( *(this->systems[0])->spins, iteration, spinsFile,
            //                                     true );
			//
			//		if (this->collection->parameters->output_energy)
			//		{
			//			// Append iteration, Rx and E to Energy file
			//			scalar nd = 1.0 / this->systems[0]->nos; // nos divide
			//			const int buffer_length = 200;
			//			std::string output_to_file = "";
			//			output_to_file.reserve(int(1E+08));
			//			char buffer_string_conversion[buffer_length + 2];
			//			if (this->collection->parameters->output_tag_time)
			//				energyFile = this->parameters->output_folder + "/" + starttime + "_Energy_" + s_img + suffix + ".txt";
			//			else
			//				energyFile = this->parameters->output_folder + "/Energy_" + s_img + suffix + ".txt";
			//			//
			//			scalar Rx = Rx_last + Engine::Manifoldmath::dist_geodesic(spins_last[0], *this->systems[0]->spins);
			//			//
			//			snprintf(buffer_string_conversion, buffer_length, "    %18.10f    %18.10f\n",
			//				Rx, this->systems[0]->E * nd);
			//			//
			//			spins_last[0] = *this->systems[0]->spins;
			//			Rx_last = Rx;
			//			//
			//			output_to_file += s_iter;
			//			output_to_file.append(buffer_string_conversion);
			//			IO::Append_String_to_File(output_to_file, energyFile);
			//		}
			//	// }

			//	//// Do it manually to avoid the adding of header
			//	//auto s = this->systems[0];
			//	//const int buffer_length = 80;
			//	//std::string output_to_file = "";
			//	//output_to_file.reserve(int(1E+08));
			//	//char buffer_string_conversion[buffer_length + 2];
			//	////------------------------ End Init ----------------------------------------

			//	//for (int iatom = 0; iatom < s->nos; ++iatom) {
			//	//	snprintf(buffer_string_conversion, buffer_length, "\n %18.10f %18.10f %18.10f",
			//	//		(*s->spins)[0 * s->nos + iatom], (*s->spins)[1 * s->nos + iatom], (*s->spins)[2 * s->nos + iatom]);
			//	//	output_to_file.append(buffer_string_conversion);
			//	//}
			//	//output_to_file.append("\n");
			//	//IO::Append_String_to_File(output_to_file, spinsFile);

			//};

			//std::string suffix = "_archive";
			//writeoutput(suffix);
		}
    }

	template <Solver solver>
    void Method_MMF<solver>::Finalize()
    {
        this->collection->iteration_allowed=false;
    }

	template <Solver solver>
    bool Method_MMF<solver>::Iterations_Allowed()
	{
		return this->collection->iteration_allowed;
	}

    // Method name as string
	template <Solver solver>
    std::string Method_MMF<solver>::Name() { return "MMF"; }

	// Template instantiations
	template class Method_MMF<Solver::SIB>;
	template class Method_MMF<Solver::Heun>;
	template class Method_MMF<Solver::Depondt>;
	template class Method_MMF<Solver::NCG>;
	template class Method_MMF<Solver::VP>;
}


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;

void rotate_quat(Quaternionf & q, Vector3f & gibbs_vector, Quaternionf* ret) {
  Quaternionf t(2, gibbs_vector.x(), gibbs_vector.y(), gibbs_vector.z());
  // Eigen's multiply doesn't work the same as simulink...
  // let's keep this to be safe
  ret->x() = q.x() * t.w() + q.w() * t.x() - q.z() * t.y() + q.y() * t.z();
  ret->y() = q.y() * t.w() + q.z() * t.x() + q.w() * t.y() - q.x() * t.z();
  ret->z() = q.z() * t.w() - q.y() * t.x() + q.x() * t.y() + q.w() * t.z();
  ret->w() = q.w() * t.w() - q.x() * t.x() - q.y() * t.y() - q.z() * t.z();
  if (ret->w() < 0) {
    ret->w() = -ret->w(); ret->x() = -ret->x(); ret->y() = -ret->y(); ret->z() = -ret->z();
  }
  // Eigen normalize function doesn't work here, I don't understand why
  if (ret->w() != 0) {
    float mag = sqrt(ret->x() * ret->x() + ret->y() * ret->y() + ret->z() * ret->z() + ret->w() * ret->w());
    ret->x() /= mag; ret->y() /= mag; ret->z() /= mag; ret->w() /= mag;
  }
}

void apply_delta_state(
    float* delta_state, int delta_state_length, unsigned short update_flag,
    float* quat_ISS2B_in, float* gyro_bias_in,
    float* V_B_ISS_ISS_in, float* accel_bias_in, float* P_B_ISS_ISS_in,
    float* ml_quat_ISS2cam_in, float* ml_P_cam_ISS_ISS_in, unsigned short kfl_status_in,
    float* of_quat_ISS2cam_in, float* of_P_cam_ISS_ISS_in,
    float* quat_ISS2B_out, float* gyro_bias_out,
    float* V_B_ISS_ISS_out, float* accel_bias_out, float* P_B_ISS_ISS_out,
    float* ml_quat_ISS2cam_out, float* ml_P_cam_ISS_ISS_out, unsigned short* kfl_status_out,
    float* of_quat_ISS2cam_out, float* of_P_cam_ISS_ISS_out) {
  int num_augs = (delta_state_length - 21) / 6;
  Map<VectorXf> ds(delta_state, delta_state_length);
  Map<Quaternionf> quat(quat_ISS2B_in);
  Map<Quaternionf> out_quat(quat_ISS2B_out);
  Map<Vector3f> gyro_bias(gyro_bias_in);
  Map<Vector3f> out_gyro_bias(gyro_bias_out);
  Map<Vector3f> vel(V_B_ISS_ISS_in);
  Map<Vector3f> out_vel(V_B_ISS_ISS_out);
  Map<Vector3f> accel_bias(accel_bias_in);
  Map<Vector3f> out_accel_bias(accel_bias_out);
  Map<Vector3f> pos(P_B_ISS_ISS_in);
  Map<Vector3f> out_pos(P_B_ISS_ISS_out);
  Map<Quaternionf> ml_quat(ml_quat_ISS2cam_in);
  Map<Quaternionf> out_ml_quat(ml_quat_ISS2cam_out);
  Map<Vector3f> ml_pos(ml_P_cam_ISS_ISS_in);
  Map<Vector3f> out_ml_pos(ml_P_cam_ISS_ISS_out);

  Quaternionf temp_quat1, temp_quat2;
  Vector3f temp_vector = ds.segment<3>(0);
  temp_quat1 = quat;
  rotate_quat(temp_quat1, temp_vector, &temp_quat2);
  out_quat = temp_quat2;
  out_gyro_bias = gyro_bias + ds.segment<3>(3);
  out_vel = vel + ds.segment<3>(6);
  out_accel_bias = accel_bias + ds.segment<3>(9);
  out_pos = pos + ds.segment<3>(12);

  temp_quat1 = ml_quat;
  temp_vector = ds.segment<3>(15);
  rotate_quat(temp_quat1, temp_vector, &temp_quat2);
  out_ml_quat = temp_quat2;
  out_ml_pos = ml_pos + ds.segment<3>(18);

  *kfl_status_out = (kfl_status_in & ~3) | update_flag;

  Map<MatrixXf> of_quat(of_quat_ISS2cam_in, num_augs, 4);
  Map<MatrixXf> out_of_quat(of_quat_ISS2cam_out, num_augs, 4);
  Map<MatrixXf> of_pos(of_P_cam_ISS_ISS_in, num_augs, 3);
  Map<MatrixXf> out_of_pos(of_P_cam_ISS_ISS_out, num_augs, 3);

  for (int i = 0; i < num_augs; i++) {
    Quaternionf t_quat(of_quat(i, 3), of_quat(i, 0), of_quat(i, 1), of_quat(i, 2));
    Quaternionf r;
    temp_vector = ds.segment<3>(21 + 6 * i);
    rotate_quat(t_quat, temp_vector, &r);
    out_of_quat(i, 0) = r.x();
    out_of_quat(i, 1) = r.y();
    out_of_quat(i, 2) = r.z();
    out_of_quat(i, 3) = r.w();
    out_of_pos.row(i) = of_pos.row(i) + ds.segment<3>(24 + 6 * i).transpose();
  }
}


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

#include "paddle/phi/kernels/matrix_rank_tol_kernel.h"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include "paddle/phi/core/kernel_registry.h"
#include "paddle/phi/kernels/elementwise_multiply_kernel.h"
#include "paddle/phi/kernels/full_kernel.h"
#include "paddle/phi/kernels/funcs/compare_functors.h"
#include "paddle/phi/kernels/funcs/eigen/common.h"
#include "paddle/phi/kernels/funcs/elementwise_base.h"
#include "paddle/phi/kernels/impl/matrix_rank_kernel_impl.h"
#include "paddle/phi/kernels/reduce_max_kernel.h"
#include "paddle/phi/kernels/reduce_sum_kernel.h"

namespace phi {

template <typename T>
void BatchEigenvalues(const T* x_data,
                      T* eigenvalues_data,
                      int batches,
                      int rows,
                      int cols,
                      int k) {
  // Eigen::Matrix API need non-const pointer.
  T* input = const_cast<T*>(x_data);
  int stride = rows * cols;
  for (int i = 0; i < batches; i++) {
    auto m = Eigen::Map<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        input + i * stride, rows, rows);
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
        eigen_solver(m);
    auto eigenvalues = eigen_solver.eigenvalues().cwiseAbs();
    for (int j = 0; j < k; j++) {
      *(eigenvalues_data + i * k + j) = eigenvalues[j];
    }
  }
}

template <typename T>
void BatchSVD(const T* x_data,
              T* eigenvalues_data,
              int batches,
              int rows,
              int cols,
              int k) {
  // Eigen::Matrix API need non-const pointer.
  T* input = const_cast<T*>(x_data);
  int stride = rows * cols;
  Eigen::BDCSVD<
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      svd;
  for (int i = 0; i < batches; i++) {
    auto m = Eigen::Map<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        input + i * stride, rows, cols);
    svd.compute(m);
    auto res_s = svd.singularValues();
    for (int j = 0; j < k; j++) {
      eigenvalues_data[i * k + j] = res_s[j];
    }
  }
}

template <typename T, typename Context>
void MatrixRankTolKernel(const Context& dev_ctx,
                         const DenseTensor& x,
                         const DenseTensor& atol_tensor,
                         bool use_default_tol,
                         bool hermitian,
                         DenseTensor* out) {
  auto* x_data = x.data<T>();
  dev_ctx.template Alloc<int64_t>(out);
  auto dim_x = x.dims();
  auto dim_out = out->dims();
  int rows = dim_x[dim_x.size() - 2];
  int cols = dim_x[dim_x.size() - 1];
  int k = std::min(rows, cols);
  auto numel = x.numel();
  int batches = numel / (rows * cols);

  T rtol_T = 0;

  if (use_default_tol) {
    rtol_T = std::numeric_limits<T>::epsilon() * std::max(rows, cols);
  }

  DenseTensor eigenvalue_tensor;
  eigenvalue_tensor.Resize(detail::GetEigenvalueDim(dim_x, k));
  auto* eigenvalue_data = dev_ctx.template Alloc<T>(&eigenvalue_tensor);

  if (hermitian) {
    BatchEigenvalues<T>(x_data, eigenvalue_data, batches, rows, cols, k);
  } else {
    BatchSVD<T>(x_data, eigenvalue_data, batches, rows, cols, k);
  }

  DenseTensor max_eigenvalue_tensor;
  max_eigenvalue_tensor.Resize(detail::RemoveLastDim(eigenvalue_tensor.dims()));
  dev_ctx.template Alloc<T>(&max_eigenvalue_tensor);
  phi::MaxKernel<T, Context>(dev_ctx,
                             eigenvalue_tensor,
                             std::vector<int64_t>{-1},
                             false,
                             &max_eigenvalue_tensor);

  DenseTensor temp_rtol_tensor;
  temp_rtol_tensor =
      phi::Full<T, Context>(dev_ctx, {1}, static_cast<T>(rtol_T));

  DenseTensor rtol_tensor =
      phi::Multiply<T>(dev_ctx, temp_rtol_tensor, max_eigenvalue_tensor);

  DenseTensor tol_tensor;
  tol_tensor.Resize(dim_out);
  dev_ctx.template Alloc<T>(&tol_tensor);
  funcs::ElementwiseCompute<GreaterElementFunctor<T>, T, T>(
      dev_ctx,
      atol_tensor,
      rtol_tensor,
      -1,
      GreaterElementFunctor<T>(),
      &tol_tensor);

  tol_tensor.Resize(detail::NewAxisDim(tol_tensor.dims(), 1));

  DenseTensor compare_result;
  compare_result.Resize(detail::NewAxisDim(dim_out, k));
  dev_ctx.template Alloc<int64_t>(&compare_result);
  int axis = -1;
  if (eigenvalue_tensor.dims().size() >= tol_tensor.dims().size()) {
    funcs::ElementwiseCompute<funcs::GreaterThanFunctor<T, int64_t>, T, int>(
        dev_ctx,
        eigenvalue_tensor,
        tol_tensor,
        axis,
        funcs::GreaterThanFunctor<T, int64_t>(),
        &compare_result);
  } else {
    funcs::ElementwiseCompute<funcs::LessThanFunctor<T, int64_t>, T, int>(
        dev_ctx,
        eigenvalue_tensor,
        tol_tensor,
        axis,
        funcs::LessThanFunctor<T, int64_t>(),
        &compare_result);
  }

  phi::SumKernel<int64_t>(dev_ctx,
                          compare_result,
                          std::vector<int64_t>{-1},
                          compare_result.dtype(),
                          false,
                          out);
}
}  // namespace phi

PD_REGISTER_KERNEL(
    matrix_rank_tol, CPU, ALL_LAYOUT, phi::MatrixRankTolKernel, float, double) {
}


/**
 * TODO split in two different test cases
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "EntropyRegularizedEisner"

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include <vector>

#include "dynet/expr.h"
#include "dynet/param-init.h"
#include "dynet/grad-check.h"

#include "diffdp/dynet/eisner.h"

BOOST_AUTO_TEST_CASE(test_dynet_eisner_algdiff)
{
    const unsigned size = 10u;

    int argc = 1;
    char **argv;
    dynet::initialize(argc, argv);

    dynet::ParameterCollection pc;

    std::vector<float> weights(size * size);
    for (unsigned i = 0 ; i < weights.size() ; ++i)
        weights.at(i) = (float) i;
    auto p_weights = pc.add_parameters(dynet::Dim({size, size}), dynet::ParameterInitFromVector(weights));

    dynet::ComputationGraph cg;
    cg.set_immediate_compute(true);
    cg.set_check_validity(true);

    auto e_weights = dynet::parameter(cg, p_weights);

    {
        auto e_arcs = dynet::algorithmic_differentiable_eisner(
                e_weights,
                diffdp::DiscreteMode::ForwardRegularized,
                diffdp::DependencyGraphMode::Adjacency,
                diffdp::DependencyGraphMode::Adjacency
        );

        for (unsigned head = 0u; head < size; ++head)
        {
            for (unsigned mod = 0u; mod < size; ++mod)
            {
                auto e_output = dynet::strided_select(
                        e_arcs,
                        {(int) 1u, (int) 1u},
                        {(int) head, (int) mod},
                        {(int) head + 1, (int) mod + 1} // not included
                );

                BOOST_CHECK(check_grad(pc, e_output, 0));
            }
        }
    }
    {
        auto e_arcs = dynet::entropy_regularized_eisner(
                e_weights,
                diffdp::DiscreteMode::ForwardRegularized,
                diffdp::DependencyGraphMode::Adjacency,
                diffdp::DependencyGraphMode::Adjacency
        );

        for (unsigned head = 0u; head < size; ++head)
        {
            for (unsigned mod = 0u; mod < size; ++mod)
            {
                auto e_output = dynet::strided_select(
                        e_arcs,
                        {(int) 1u, (int) 1u},
                        {(int) head, (int) mod},
                        {(int) head + 1, (int) mod + 1} // not included
                );
                BOOST_CHECK(check_grad(pc, e_output, 0));
            }
        }
    }
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* Test_matmul.cpp - Testing the functionality of multiplying an encrypted
 * vector by a plaintext matrix, either over the extension- or the
 * base-field/ring.
 */
#include <cassert>
#include <NTL/lzz_pXFactoring.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include "matmul.h"

// Forward declerations
static MatMulBase* buildRandomMatrix(const EncryptedArray& ea,
                                     long dim, long giantStep);
static MatMulBase* buildRandomMultiMatrix(const EncryptedArray& ea,
                                          long dim, long giantStep);
static MatMulBase* buildRandomBlockMatrix(const EncryptedArray& ea,
                                          long dim);
static MatMulBase* buildRandomMultiBlockMatrix(const EncryptedArray& ea,
                                               long dim);

// The callback interface for the matrix-multiplication routines.

//! \cond FALSE (make doxygen ignore these classes)
template<class type> class RandomMultiMatrix : public  MatMul<type> {
public:
  PA_INJECT(type)

private:
  vector< vector< vector< RX > > > data;
  long dim;

public:
  virtual ~RandomMultiMatrix() {}
  RandomMultiMatrix(const EncryptedArray& _ea, long _dim, long g)
    : MatMul<type>(_ea, g), dim(_dim)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long n = _ea.size();
    long d = _ea.getDegree();
    long D = _ea.sizeOfDimension(dim);

    data.resize(n/D);
    for (long k = 0; k < n/D; k++) {
      data[k].resize(D);
      for (long i = 0; i < D; i++) {
	data[k][i].resize(D);
	for (long j = 0; j < D; j++) {
	  random(data[k][i][j], d);
	}
      }
    }
  }

  virtual bool multiGet(RX& out, long i, long j, long k) const
  {
    long D = this->getEA().sizeOfDimension(dim);
    assert(i >= 0 && i < D);
    assert(j >= 0 && j < D);
    assert(k >= 0 && k < this->getEA().size()/D);
    if (IsZero(data[k][i][j])) return true;
    out = data[k][i][j];
    return false;
  }
};

static MatMulBase*
buildRandomMultiMatrix(const EncryptedArray& ea, long dim, long giantStep)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new RandomMultiMatrix<PA_GF2>(ea, dim, giantStep);
    }
    case PA_zz_p_tag: {
      return new RandomMultiMatrix<PA_zz_p>(ea, dim, giantStep);
    }
    default: return 0;
  }
}
//********************************

template<class type>
class RandomMultiBlockMatrix : public BlockMatMul<type> {
  PA_INJECT(type)

  vector< vector< vector< mat_R > > > data;
  long dim;

public:
  virtual ~RandomMultiBlockMatrix() {}
  RandomMultiBlockMatrix(const EncryptedArray& _ea, long _dim):
    BlockMatMul<type>(_ea), dim(_dim)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long n = _ea.size();
    long d = _ea.getDegree();
    long D = _ea.sizeOfDimension(dim);

    data.resize(n/D);
    for (long k = 0; k < n/D; k++) {
      data[k].resize(D);
      for (long i = 0; i < D; i++) {
	data[k][i].resize(D);
	for (long j = 0; j < D; j++) {
          data[k][i][j].SetDims(d, d);
	  for (long u = 0; u < d; u++)
	    for (long v = 0; v < d; v++)
	      random(data[k][i][j][u][v]);
	}
      }
    }
  }

  virtual long size() const // how many transformations
  {
    const EncryptedArray& ea = this->getEA();
    long n = ea.size();
    long D = ea.sizeOfDimension(dim);
    return n/D;
  }

  virtual bool multiGet(mat_R& out, long i, long j, long k) const
  {
    const EncryptedArray& ea = this->getEA();
    long n = ea.size();
    long D = ea.sizeOfDimension(dim);

    assert(i >= 0 && i < D);
    assert(j >= 0 && j < D);
    assert(k >= 0 && k < n/D);
    if (IsZero(data[k][i][j])) return true;
    out = data[k][i][j];
    return false;
  }
};

static MatMulBase*
buildRandomMultiBlockMatrix(const EncryptedArray& ea, long dim)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new RandomMultiBlockMatrix<PA_GF2>(ea, dim);
    }
    case PA_zz_p_tag: {
      return new RandomMultiBlockMatrix<PA_zz_p>(ea, dim);
    }
    default: return 0;
  }
}
//********************************

template<class type> class RandomMatrix : public  MatMul<type> {
public:
  PA_INJECT(type)

private:
  vector< vector< RX > > data;
  long dim;

public:
  virtual ~RandomMatrix() {}
  RandomMatrix(const EncryptedArray& _ea, long _dim, long g):
    MatMul<type>(_ea,g), dim(_dim)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long n = _ea.size();
    long d = _ea.getDegree();
    long D = _ea.sizeOfDimension(dim);

    data.resize(D);
    for (long i = 0; i < D; i++) {
      data[i].resize(D);
      for (long j = 0; j < D; j++) {
        random(data[i][j], d);
      }
    }
  }

  virtual bool get(RX& out, long i, long j) const {
    long D = this->getEA().sizeOfDimension(dim);

    assert(i >= 0 && i < D);
    assert(j >= 0 && j < D);
    if (IsZero(data[i][j])) return true;
    out = data[i][j];
    return false;
  }
};

static MatMulBase*
buildRandomMatrix(const EncryptedArray& ea, long dim, long giantStep)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new RandomMatrix<PA_GF2>(ea, dim, giantStep);
    }
    case PA_zz_p_tag: {
      return new RandomMatrix<PA_zz_p>(ea, dim, giantStep);
    }
    default: return 0;
  }
}
//********************************

template<class type>
class RandomBlockMatrix : public BlockMatMul<type> {
  PA_INJECT(type)

  vector< vector< mat_R > > data;
  long dim;

public:
  ~RandomBlockMatrix() { /*cout << "destructor: random matrix\n";*/ }

  RandomBlockMatrix(const EncryptedArray& _ea, long _dim):
    BlockMatMul<type>(_ea), dim(_dim)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long n = _ea.size();
    long d = _ea.getDegree();
    long D = _ea.sizeOfDimension(dim);

    data.resize(D);
    for (long i = 0; i < D; i++) {
      data[i].resize(D);
      for (long j = 0; j < D; j++) {
        data[i][j].SetDims(d, d);
        for (long u = 0; u < d; u++)
          for (long v = 0; v < d; v++)
            random(data[i][j][u][v]);
      }
    }
  }

  virtual bool get(mat_R& out, long i, long j) const
  {
    const EncryptedArray& ea = this->getEA();
    long D = ea.sizeOfDimension(dim);
    assert(i >= 0 && i < D);
    assert(j >= 0 && j < D);
    if (IsZero(data[i][j])) return true;
    out = data[i][j];
    return false;
  }
};

static MatMulBase*
buildRandomBlockMatrix(const EncryptedArray& ea, long dim)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new RandomBlockMatrix<PA_GF2>(ea,dim);
    }
    case PA_zz_p_tag: {
      return new RandomBlockMatrix<PA_zz_p>(ea, dim);
    }
    default: return 0;
  }
}
//! \endcond

void  TestIt(FHEcontext& context, long g, long dim, bool verbose)
{
  if (verbose) {
    context.zMStar.printout();
    cout << endl;
  }

  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(/*w=*/64); // A Hamming-weight-w secret key

  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need
  EncryptedArray ea(context, context.alMod);

  // Test a "normal" matrix over the extension field
  {
    // choose a random plaintext square matrix
    std::unique_ptr< MatMulBase > ptr(buildRandomMatrix(ea, dim, g));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << " Multiplying 1D with MatMulBase... " << std::flush;
    matMul1D(v, *ptr, dim);
    matMul1D(ctxt2, *ptr, dim, cachezzX);
       // multiply ciphertext and build cache
    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";

    cout << " Multiplying 1D with MatMulBase+dcrt cache... " << std::flush;
    ctxt2 = ctxt;
    matMul1D(ctxt2, *ptr, dim, cacheDCRT); // upgrade cache and use in multiplication

    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }
  {
    // choose a random plaintext square matrix
    std::unique_ptr< MatMulBase > ptr(buildRandomMatrix(ea,dim,g));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << " Multiplying 1D with MatMulBase+zzx cache... " << std::flush;
    buildCache4MatMul1D(*ptr, dim, cachezzX);// build the cache
    matMul1D(ctxt, *ptr, dim);               // then use it
    matMul1D(v, *ptr, dim);     // multiply the plaintext vector

    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }

  // Test a "multi" matrix over the extension field
  {
    // choose a random plaintext square matrix
    std::unique_ptr< MatMulBase > ptr(buildRandomMultiMatrix(ea,dim,g));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << "\n Multiplying multi 1D with MatMulBase... " << std::flush;
    matMulti1D(v, *ptr, dim);
    matMulti1D(ctxt2, *ptr, dim, cachezzX); // multiply ciphertext and build cache
    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";

    cout <<" Multiplying multi 1D with MatMulBase+dcrt cache... "<< std::flush;
    ctxt2 = ctxt;
    matMulti1D(ctxt2, *ptr, dim, cacheDCRT); // upgrade cache and use in multiplication

    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))    // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }
  {
    // choose a random plaintext square matrix
    std::unique_ptr< MatMulBase > ptr(buildRandomMultiMatrix(ea,dim,g));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << " Multiplying multi 1D with MatMulBase+zzx cache... " << std::flush;
    buildCache4MatMulti1D(*ptr, dim, cachezzX);// build the cache
    matMulti1D(ctxt, *ptr, dim);               // then use it
    matMulti1D(v, *ptr, dim);     // multiply the plaintext vector

    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }

  // Test a "block matrix" over the base field
  {
    // choose a random plaintext square matrix
    shared_ptr<MatMulBase> ptr(buildRandomBlockMatrix(ea,dim));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << endl << " Multiplying 1D with BlockMatMul... "
	 << std::flush;
    blockMatMul1D(ctxt2, *ptr, dim, cachezzX);
                                  // multiply ciphertext and build cache
    blockMatMul1D(v, *ptr, dim); // multiply the plaintext vector
    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";

    cout << " Multiplying 1D with BlockMatMul+dcrt cache... " << std::flush;
    ctxt2 = ctxt;
    blockMatMul1D(ctxt2, *ptr, dim, cacheDCRT);
                                  // upgrade cache and use in multiplication
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }
  {
    // choose a random plaintext square matrix
    shared_ptr<MatMulBase> ptr(buildRandomBlockMatrix(ea,dim));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << " Multiplying 1D with BlockMatMul+zzx cache... " << std::flush;
    buildCache4BlockMatMul1D(*ptr, dim, cachezzX);// build the cache
    blockMatMul1D(ctxt2, *ptr, dim);              // then use it
    blockMatMul1D(v, *ptr, dim); // multiply the plaintext vector
    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";
  }
  // Test multiple "block" matrices over the base field
  {
    // choose a random plaintext square matrix
    shared_ptr<MatMulBase> ptr(buildRandomMultiBlockMatrix(ea,dim));

    // choose a random plaintext vector
    NewPlaintextArray v(ea);
    random(ea, v);

    // encrypt the random vector
    Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, v);
    Ctxt ctxt2 = ctxt;

    cout << " Multiplying multi 1D with BlockMatMul... " << std::flush;
    blockMatMulti1D(v, *ptr, dim);     // multiply the plaintext vector
    blockMatMulti1D(ctxt2, *ptr, dim); // multiply the ciphertext vector

    NewPlaintextArray v1(ea);
    ea.decrypt(ctxt2, secretKey, v1); // decrypt the ciphertext vector

    if (equals(ea, v, v1))        // check that we've got the right answer
      cout << "Nice!!\n";
    else
      cout << "Grrr@*\n";

  }
}

/* Testing the functionality of multiplying an encrypted vector by a
 * plaintext matrix, either over the extension- or the base-field/ring.
 *
 * Usage: Test_matmul1D [optional params]
 *
 *  m defines the cyclotomic polynomial Phi_m(X)
 *  p is the plaintext base [default=2]
 *  r is the lifting [default=1]
 *  g is the giant-step parameter [defauls=2]
 *  L is the # of primes in the modulus chain [default=4]
 *  dim is the dimension alng which we multiply [default=0]
 *  verbose print timing info [default=0]
 */
int main(int argc, char *argv[])
{
  ArgMapping amap;

  long m=2047;
  amap.arg("m", m, "defines the cyclotomic polynomial Phi_m(X)");
  long p=2;
  amap.arg("p", p, "plaintext base");
  long r=1;
  amap.arg("r", r,  "lifting");
  long g=2;
  amap.arg("g", g,  "giant-step parameter");
  long L=4;
  amap.arg("L", L, "# of levels in the modulus chain");
  long dim=0;
  amap.arg("dim", dim, "dimension along which to multiply");
  long verbose=0;
  amap.arg("verbose", verbose, "print timing and other info");

  NTL::Vec<long> gens;
  amap.arg("gens", gens, "use specified vector of generators", NULL);
  amap.note("e.g., gens='[562 1871 751]'");
  NTL::Vec<long> ords;
  amap.arg("ords", ords, "use specified vector of orders", NULL);
  amap.note("e.g., ords='[4 2 -4]', negative means 'bad'");

  amap.parse(argc, argv);

  cout << "*** matmul1D: m=" << m
       << ", p=" << p
       << ", r=" << r
       << ", L=" << L
       << ", g=" << g
       << ", dim=" << dim
       // << ", gens=" << gens
       // << ", ords=" << ords
       << endl;

  vector<long> gens1, ords1;
  convert(gens1, gens);
  convert(ords1, ords);

  setTimersOn();

  FHEcontext context(m, p, r, gens1, ords1);
  buildModChain(context, L, /*c=*/3);

  TestIt(context, g, dim, verbose);
  cout << endl;
  if (verbose) {
    printAllTimers();
    cout << endl;
  }
}


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

int main(int, char**)
{
    using namespace mtl; using namespace mtl::mat;

    const unsigned n= 40;
    dense2D<double>                            A(n, n), B(n, n);
    morton_dense<double, doppled_64_row_mask>  C(n, n), D(n, n);

    hessian_setup(A, 3.0); hessian_setup(B, 1.0);
    hessian_setup(C, 2.0); hessian_setup(D, 11.0);

    A+= B * B + C * B - B * B * C * D;

    return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_MATH_IS_ZERO_BOOST_UNITS_HPP_INCLUDED
#define FCPPT_MATH_IS_ZERO_BOOST_UNITS_HPP_INCLUDED

#include <fcppt/math/is_zero.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/unit.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt::math
{
/**
\brief Integrates boost.units with #fcppt::math::is_zero.

\ingroup fcpptboost
*/
template <typename Unit, typename Type>
struct is_zero_impl<boost::units::quantity<Unit, Type>>
{
  static bool get(boost::units::quantity<Unit, Type> const &_value)
  {
    return fcppt::math::is_zero(_value.value());
  }
};

}

#endif


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

#ifndef NETKET_SGD_HPP
#define NETKET_SGD_HPP

#include "abstract_optimizer.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>

namespace netket {

class Sgd : public AbstractOptimizer {
  // decay constant
  double eta_;

  int npar_;

  double l2reg_;

  double decay_factor_;

public:
  // Json constructor
  explicit Sgd(const json &pars) {
    npar_ = -1;

    from_json(pars);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Sgd optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Learning Rate = " << eta_ << std::endl;
    InfoMessage() << "L2 Regularization = " << l2reg_ << std::endl;
    InfoMessage() << "Decay Factor = " << decay_factor_ << std::endl;
  }

  void Init(const Eigen::VectorXd &pars) override { npar_ = pars.size(); }

  void Init(const Eigen::VectorXcd &pars) override { npar_ = 2 * pars.size(); }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    eta_ *= decay_factor_;

    for (int i = 0; i < npar_; i++) {
      pars(i) = pars(i) - (grad(i) + l2reg_ * pars(i)) * eta_;
    }
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    eta_ *= decay_factor_;

    for (int i = 0; i < pars.size(); i++) {
      pars(i) = pars(i) - (grad(i) + l2reg_ * pars(i)) * eta_;
    }
  }

  void SetDecayFactor(double decay_factor) {
    assert(decay_factor <= 1.00001);
    decay_factor_ = decay_factor;
  }

  void Reset() override {}

  void from_json(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    std::string section = "Optimizer";
    if (!FieldExists(pars, section)) {
      section = "Learning";
    }

    eta_ = FieldVal(pars[section], "LearningRate");
    l2reg_ = FieldOrDefaultVal(pars[section], "L2Reg", 0.0);
    SetDecayFactor(FieldOrDefaultVal(pars[section], "DecayFactor", 1.0));
  }
};

} // namespace netket

#endif


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file facility_location_long_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-15
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_orlib_fl.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/data_structures/facility_location/fl_algo.hpp"
#include "paal/local_search/facility_location/facility_location.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/test/unit_test.hpp>

#include <iterator>
#include <iostream>
#include <iomanip>

using namespace paal::local_search;
using namespace paal;

BOOST_AUTO_TEST_CASE(FacilityLocationLong) {
    std::string test_dir = paal::system::get_test_data_dir("FL_ORLIB");
    using paal::system::build_path;
    parse(build_path(test_dir, "uncapopt.txt"),
          [&](const std::string & fname, std::istream & is_test_cases) {
        double optTemp;
        is_test_cases >> optTemp;
        long long opt = cast(optTemp);

        LOGLN("TEST " << fname);
        LOGLN(std::setprecision(20) << "OPT " << opt);

        std::ifstream ifs(build_path(test_dir, "cases/" + fname));
        assert(ifs.good());
        std::vector<long long> facCost;
        std::vector<int> facCap;
        std::vector<int> demands;
        boost::integer_range<int> fac(0, 0);
        boost::integer_range<int> clients(0, 0);
        auto metric = paal::read_orlib_FL<cap::uncapacitated>(
            ifs, facCost, facCap, demands, fac, clients);

        auto cost = paal::utils::make_array_to_functor(facCost);

        typedef paal::data_structures::voronoi<decltype(metric)> VorType;

        typedef paal::data_structures::facility_location_solution<
            decltype(cost), VorType> Sol;
        typedef typename VorType::GeneratorsSet FSet;
        VorType voronoi(FSet{}, FSet(clients.begin(), clients.end()), metric);
        Sol sol(std::move(voronoi), FSet(fac.begin(), fac.end()), cost);

        default_remove_fl_components rem;
        default_add_fl_components add;
        default_swap_fl_components swap;
        utils::always_false nop;

        facility_location_local_search(
            sol, paal::local_search::first_improving_strategy{}, nop, nop, rem,
            add, swap);

        long long c = simple_algo::get_fl_cost(metric, cost, sol);
        check_result(c, opt, 3., paal::utils::less_equal(), MULTIPL);
    });
}


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // scaling a distributed std::vector
    std::vector<float> in = {1,2,3,4};
    std::vector<float> out = {2,4,6,8};
    auto in_bm = vec_to_bcm<float> (in); // distributed vector in
    scal<float> (in_bm,2); // in_bm = in_bm*2
    BOOST_CHECK (in_bm.moveto_vector() == out);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x4");

    // checking scal() operation
    auto row = make_row_vector<float> (bm,1);
    scal<float>(row,5); // row = 5 * row
    bm.save("./out_4x4");

    // checking whether the above operation successfully taken place
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


#include <cmath>
#include <Eigen/Geometry>
#include "rose499/diffdrive.hpp"

using namespace Eigen;

void DriveSystem::step( StateType x,
                        StateType& dxdt,
                        double t,
                        ValueType turn,
                        ValueType speed )
{
    Map<Matrix<ValueType, 3, 1>>(dxdt.data()) << speed * std::cos(x[2]),
                                                 speed * std::sin(x[2]),
                                                 turn;
}

DriveController::DriveController(   DriveSystem & driver,
                                    Matrix<ValueType, 2, 1> goal,
                                    ValueType goalTol )
  : mSystem(driver),
    mGoal(goal),
    mGoalRadius(goalTol),
    mPlanIndex(0),
    mOperatingLambda(0)
{
}

Matrix<DriveSystem::ValueType, 3, 1> const & DriveSystem::state() const { return mState; }
Matrix<DriveSystem::ValueType, 3, 1> const & DriveSystem::flow() const { return mFlow; }
double DriveSystem::time() const { return mTime; }
void DriveSystem::state(Eigen::Matrix<ValueType, 3, 1> x) { mState = x; }
void DriveSystem::flow(Eigen::Matrix<ValueType, 3, 1> f) { mFlow = f; }
void DriveSystem::time(double t) { mTime = t; }

World::Polygon DriveSystem::viewCone() const
{
    World::Polygon viewFrustum;
    geom::append(viewFrustum.outer(), World::Point(mState[0], mState[1]));
    {
        Vector2d dir(std::cos(mState[2]), std::sin(mState[2]));
        Rotation2D<double> right(std::atan2(-1, 1));
        Rotation2D<double> left(std::atan2(1, 1));
        Vector2d viewRight = right * dir;
        Vector2d viewLeft = left * dir;

        geom::append(viewFrustum.outer(), World::Point(mState[0] + 50 * viewLeft[0], mState[1] + 50 * viewLeft[1]));
        geom::append(viewFrustum.outer(), World::Point(mState[0] + 50 * viewRight[0], mState[1] + 50 * viewRight[1]));
    }
    geom::append(viewFrustum.outer(), World::Point(mState[0], mState[1]));
    return viewFrustum;
}

void DriveController::operator() (StateType x, StateType& dxdt, double t)
{
    mSystem.step(x, dxdt, t, genTurnControl(x, t), genSpeedControl(x, t));
}

std::ostream& DriveController::printHeaders(std::ostream& stream) const
{
    stream << "time, x_1, x_2, x_3, dx_1, dx_2, dx_3, plan_ind";
    return printSpecificHeaders(stream);
}

std::ostream& DriveController::printData(std::ostream& stream) const
{
    auto state = mSystem.state();
    auto flow = mSystem.flow();
    stream << mSystem.time() << "," << state[0] << "," << state[1] << "," << state[2]
                             << "," << flow[0] << "," << flow[1] << "," << flow[2]
                             << "," << mPlanIndex;
    return printSpecificData(stream);
}

std::ostream& DriveController::printSpecificHeaders(std::ostream& s) const { return s; }
std::ostream& DriveController::printSpecificData(std::ostream& s) const { return s; }

DriveController::ValueType DriveController::genTurnControl(StateType, double)
{
    return 0;
}

DriveController::ValueType DriveController::genSpeedControl(StateType, double)
{
    return 1;
}

bool DriveController::updateKnownWorld(std::list< std::pair<World::Box, int> > const & obstacles, bool forceReplan)
{
    auto newList = obstacles;
    auto newEnd = std::remove_if(   newList.begin(),
                                    newList.end(),
                                    [this](std::pair<World::Box, int> a) -> bool {
                                        return mKnownBoxes.find(a.second) != mKnownBoxes.end();
                                    } );

    newList.erase(newEnd, newList.end());

    mKnownWorld.insert(newList.begin(), newList.end());

    for(auto&& pair : newList)
        mKnownBoxes.insert(pair.second);

    if( !newList.empty() || forceReplan ) {
        replan();
        return true;
    }
    return false;
}

void DriveController::replan()
{
    auto state = mSystem.state();

    Spline spline;
    std::vector< std::pair< World::Box, int > > setCollisions;
    do
    {
        setCollisions.clear();

        auto path = planRRT( mKnownWorld,
                             World::XMax,
                             World::YMax,
                             World::Point(state[0], state[1]),
                             state[2],
                             World::Point(mGoal[0], mGoal[1]),
                             mGoalRadius );

        Matrix<ValueType, 2, Dynamic> waypoints(2, path.size());
        for(int i = 0; i < waypoints.cols(); ++i)
        {
            waypoints.col(i)[0] = path.front().get<0>();
            waypoints.col(i)[1] = path.front().get<1>();
            path.pop_front();
        }

        spline = Spline(waypoints, state[2]);

        auto const & linApprox = spline.approximation();

        for(auto&& segment : linApprox)
        {
            mKnownWorld.query( geom::index::intersects(segment), std::back_inserter(setCollisions) );

            // Let's not waste time checking ...if there is a collision!
            if(!setCollisions.empty())
                break;
        }

    } while( !setCollisions.empty() );

    path(spline);
    operatingPoint(0.0);
    lastPlanTime = system().time();
    ++mPlanIndex;
}

void DriveController::path( Spline const & s ) { mPath = s; }
Spline const & DriveController::path() const { return mPath; }

void DriveController::operatingPoint( Spline::ValueType v ) { mOperatingLambda = v; }
Spline::ValueType DriveController::operatingPoint() const { return mOperatingLambda; }

DriveSystem const & DriveController::system() const { return mSystem; }

bool DriveController::hasDiverged() const { return false; }

Matrix<DriveController::ValueType, 2, 1> const & DriveController::goal() const { return mGoal; }

std::ostream& std::operator << (std::ostream& s, DriveController const & c)
{
    return c.printData(s);
}


/**
 * \file TimeVaryingSecondOrderFilter.cpp
 * @see http://abvolt.com/research/publications2.htm
 * @see http://www.music.mcgill.ca/~ich/classes/FiltersChap2.pdf for the allpass filter
 */

#include <ATK/EQ/TimeVaryingSecondOrderFilter.h>
#include <ATK/EQ/TimeVaryingIIRFilter.h>

#include <cassert>
#include <cmath>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  /// Time varying coefficient base class. Two input ports, the first is the data, the second one is the central frequency
  template <typename DataType>
  TimeVaryingBaseSecondOrderCoefficients<DataType>::TimeVaryingBaseSecondOrderCoefficients()
    :Parent(2, 1)
  {
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.clear();
    coefficients_in.reserve((in_order+1) * number_of_steps);
    coefficients_out.clear();
    coefficients_out.reserve(out_order * number_of_steps);
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_min_frequency(double min_frequency)
  {
    if(min_frequency <= 0)
    {
      throw std::out_of_range("Min frequency must be positive");
    }
    this->min_frequency = min_frequency;
    setup();
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_min_frequency() const
  {
    return min_frequency;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_max_frequency(double max_frequency)
  {
    if(max_frequency <= min_frequency)
    {
      throw std::out_of_range("Max frequency must be greater than min frequency");
    }
    this->max_frequency = max_frequency;
    setup();
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_max_frequency() const
  {
    return max_frequency;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_number_of_steps(int number_of_steps)
  {
    if(number_of_steps <= 0)
    {
      throw std::out_of_range("Number of steps must be strictly positive");
    }
    this->number_of_steps = number_of_steps;
    setup();
  }

  template <typename DataType>
  int TimeVaryingBaseSecondOrderCoefficients<DataType>::get_number_of_steps() const
  {
    return number_of_steps;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_memory(double memory)
  {
    if(memory < 0 || memory >= 1)
    {
      throw std::out_of_range("Memory for time varying EQ had to be in the range [0, 1[");
    }
    this->memory = memory;
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_memory() const
  {
    return memory;
  }

  template<typename DataType>
  TimeVaryingBandPassCoefficients<DataType>::TimeVaryingBandPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);
      DataType Q_inv = 1 / Q;

      coefficients_in.push_back(-Q_inv * c / d);
      coefficients_in.push_back(0);
      coefficients_in.push_back(Q_inv * c / d);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template <typename DataType_>
  void TimeVaryingBandPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  TimeVaryingLowPassCoefficients<DataType>::TimeVaryingLowPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in.push_back(c * c / d);
      coefficients_in.push_back(2 * c * c / d);
      coefficients_in.push_back(c * c / d);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template<typename DataType>
  TimeVaryingHighPassCoefficients<DataType>::TimeVaryingHighPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in.push_back(1);
      coefficients_in.push_back(-2);
      coefficients_in.push_back(1);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template<typename DataType>
  TimeVaryingBandPassPeakCoefficients<DataType>::TimeVaryingBandPassPeakCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType Q_inv = 1 / Q;
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = 1 + V0 * Q_inv * c + c * c;

        coefficients_in.push_back((1 - Q_inv * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + Q_inv * c + c * c) / d);
        coefficients_out.push_back(-(1 - V0 * Q_inv * c + c * c) / d);
        coefficients_out.push_back(-2 * (c * c - 1) / d);
      }
      else
      {
        DataType V0 = gain;
        DataType d = 1 + Q_inv * c + c * c;

        coefficients_in.push_back((1 - V0 * Q_inv * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + V0 * Q_inv * c + c * c) / d);
        coefficients_out.push_back(-(1 - Q_inv * c + c * c) / d);
        coefficients_out.push_back(-2 * (c * c - 1) / d);
      }
    }
  }

  template <typename DataType_>
  void TimeVaryingBandPassPeakCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void TimeVaryingBandPassPeakCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  TimeVaryingAllPassCoefficients<DataType>::TimeVaryingAllPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * Q);
      DataType d = -std::cos(2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);

      coefficients_in.push_back(1);
      coefficients_in.push_back(d * (1 - c));
      coefficients_in.push_back(-c);
      coefficients_out.push_back(c);
      coefficients_out.push_back(-d * (1 - c));
    }
  }

  template <typename DataType_>
  void TimeVaryingAllPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  TimeVaryingLowShelvingCoefficients<DataType>::TimeVaryingLowShelvingCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c);

        coefficients_in.push_back((1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c) / d);
        coefficients_out.push_back(- 2 * (V0 * c * c - 1) / d);
      }
      else
      {
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

        coefficients_in.push_back((1 - std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d);
        coefficients_in.push_back(2 * (gain * c * c - 1) / d);
        coefficients_in.push_back((1 + std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - 1) / d);
      }
    }
  }

  template <typename DataType_>
  void TimeVaryingLowShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  TimeVaryingHighShelvingCoefficients<DataType>::TimeVaryingHighShelvingCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = (V0 + std::sqrt(static_cast<DataType>(2.) * V0) * c + c * c);

        coefficients_in.push_back(-(1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_in.push_back(-2 * (c * c - 1) / d);
        coefficients_in.push_back(-(1 + std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_out.push_back(- (V0 - std::sqrt(static_cast<DataType>(2.0) * V0) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - V0) / d);
      }
      else
      {
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

        coefficients_in.push_back(-(gain - std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d);
        coefficients_in.push_back(-2 * (c * c - gain) / d);
        coefficients_in.push_back(-(gain + std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - 1) / d);
      }
    }
  }

  template<typename DataType_>
  void TimeVaryingHighShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

#if ATK_ENABLE_INSTANTIATION
  template class TimeVaryingBaseSecondOrderCoefficients<float>;

  template class ATK_EQ_EXPORT TimeVaryingBandPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingLowPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingHighPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingBandPassPeakCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingAllPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingLowShelvingCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingHighShelvingCoefficients<float>;
#endif
  template class TimeVaryingBaseSecondOrderCoefficients<double>;

  template class ATK_EQ_EXPORT TimeVaryingBandPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingLowPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingHighPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingBandPassPeakCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingAllPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingLowShelvingCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingHighShelvingCoefficients<double>;
}


#include "cnn/nodes.h"
#include "cnn/cnn.h"
#include "cnn/training.h"
#include "cnn/gpu-ops.h"
#include "cnn/expr.h"
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <iostream>
#include <fstream>

using namespace std;
using namespace cnn;
using namespace cnn::expr;

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  // parameters
  const unsigned HIDDEN_SIZE = 8;
  const unsigned ITERATIONS = 200;
  Model m;
  SimpleSGDTrainer sgd(&m);

  ComputationGraph cg;

  Expression W = parameter(cg, m.add_parameters({HIDDEN_SIZE, 2}));
  Expression b = parameter(cg, m.add_parameters({HIDDEN_SIZE}));
  Expression V = parameter(cg, m.add_parameters({1, HIDDEN_SIZE}));
  Expression a = parameter(cg, m.add_parameters({1}));

  // set x_values to change the inputs to the network
  Dim x_dim({2}, 4), y_dim({1}, 4);
  cerr << "x_dim=" << x_dim << ", y_dim=" << y_dim << endl;
  vector<cnn::real> x_values = {1.0, 1.0, 1.0, -1.0, -1.0, 1.0, -1.0, -1.0};
  Expression x = input(cg, x_dim, &x_values);
  // set y_values expressing the output
  vector<cnn::real> y_values = {-1.0, 1.0, 1.0, -1.0};
  Expression y = input(cg, y_dim, &y_values);

  Expression h = tanh(W*x + b);
  //Expression h = tanh(affine_transform({b, W, x}));
  //Expression h = softsign(W*x + b);
  Expression y_pred = V*h + a;
  Expression loss = squared_distance(y_pred, y);
  Expression sum_loss = sum_batches(loss);

  cg.PrintGraphviz();
  if (argc == 2) {
    ifstream in(argv[1]);
    boost::archive::text_iarchive ia(in);
    ia >> m;
  }

  // train the parameters
  for (unsigned iter = 0; iter < ITERATIONS; ++iter) {
    float my_loss = as_scalar(cg.forward()) / 4;
    cg.backward();
    sgd.update(0.25);
    sgd.update_epoch();
    cerr << "E = " << my_loss << endl;
  }
  //boost::archive::text_oarchive oa(cout);
  //oa << m;
}


#include <boost/test/unit_test.hpp>

#include <sway/math.h>

using namespace sway;

BOOST_AUTO_TEST_SUITE(TSizeTestSuite)

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_DefaultConstructor) {
	const math::size2i_t size;

	BOOST_CHECK_EQUAL(size.getW(), 0);
	BOOST_CHECK_EQUAL(size.getH(), 0);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_ComponentConstructor) {
	const s32_t w = 1, h = 2;
	const math::size2i_t size(w, h);

	BOOST_CHECK_EQUAL(size.getW(), w);
	BOOST_CHECK_EQUAL(size.getH(), h);
}

BOOST_AUTO_TEST_CASE(TSizeTestCase_ScalarConstructor) {
	const s32_t w = 1, h = 2;
	const math::size2i_t size(math::TSize<s32_t>(w, h));

	BOOST_CHECK_EQUAL(size.getW(), w);
	BOOST_CHECK_EQUAL(size.getH(), h);
}

/*!
 * \brief
 *    Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_EqualityOperator) {
	const math::size2i_t size(1, 2);

	BOOST_CHECK_EQUAL(size == math::size2i_t(1, 2), true);
	BOOST_CHECK_EQUAL(size == math::size2i_one, false);
}

/*!
 * \brief
 *    Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TSizeTestCase_NonEqualityOperator) {
	const math::size2i_t size(2, 1);

	BOOST_CHECK_EQUAL(size != math::size2i_one, true);
	BOOST_CHECK_EQUAL(size != math::size2i_t(2, 1), false);
}

BOOST_AUTO_TEST_SUITE_END()


///\file IsingModel-Visual.cpp
///\author Ethan Knox
///\date 7/19/2020.

#include <boost/filesystem/fstream.hpp>
#include <boost/program_options.hpp>
#include "../include/Ising.h"
#include "../include/run_simulation.h"
#include <iostream>

namespace opt = boost::program_options;

int main(int argc, const char* argv[])
{
    long nrows, ncols, stopiter, framestep;
    double J, h, T;
    char method, geometry;

    opt::options_description params("Simulation Parameters");

    params.add_options()
            ("help", "Show usage")
            ("nrows,r", opt::value<long>(&nrows)->default_value(500), "Number of rows")
            ("ncols,c", opt::value<long>(&ncols)->default_value(500), "Number of columns")
            ("stopiter,s", opt::value<long>(&stopiter)->default_value(250000), "Number of iterations")
            ("framestep,f", opt::value<long>(&framestep)->default_value(2500), "Number of iterations between frames")
            ("J,j", opt::value<double>(&J)->default_value(1.0), "Ferromagnetic Coupling Constant")
            ("h,h", opt::value<double>(&h)->default_value(0.0), "Magnetic Field Strength")
            ("T,t", opt::value<double>(&T)->default_value(1.8), "Temperature")
            ("method,m", opt::value<char>(&method)->default_value('M'), "Solution Method")
            ("geometry,g", opt::value<char>(&geometry)->default_value('S'), "Grid Geometry")
            ;

    opt::variables_map vm;
    opt::store(opt::parse_command_line(argc, argv, params), vm);

    if (vm.count("help")) {
        std::cout << params << std::endl;
        return 1;
    }
    else {
        opt::notify(vm);

        std::cout << "\n2D ISING MODEL - VISUAL SIMULATION" << std::endl;
        std::cout << "----------------------------------" << std::endl;

        Ising p = Ising(nrows, ncols, stopiter, framestep, J, h, T, method, geometry);

        std::cout << "Using the " << (p.method == 'M' ? "Metropolis-Hastings" : "Wolff") << " Algorithm ";
        std::cout << "with " << (p.geometry == 'S' ? "Standard Square" : "Hexagonal") << " Geometry" << std::endl;
        std::cout << p.nrows << "x" << p.ncols << " Grid, ";
        std::cout << p.stopiter << " Iterations, " << p.framestep << " Steps Between Frames" << std::endl;
        std::cout << "J = " << p.J << ", h = " << p.h << ", T = " << p.T << std::endl;
        std::cout << "\nRunning..." << std::endl;

        p.randomize();
        run_simulation(p); // Go baby go

        std::cout << "Done!" << std::endl;
        std::cout << "----------------------------------" << std::endl;

        return 0;
    }
}


#ifdef USE_BOOST_SAFE_NUMERICS
#include <boost/numeric/conversion/cast.hpp>
#include <boost/safe_numerics/safe_integer.hpp>
#else
#error "boost safe numerics lib has not been installed and enabled"
#endif

#include "demo.h"
#include "numeric_cast.h"

// need C++14

void test()
{
    std::cout << "================ boost safe numerics =============" << std::endl;
    using namespace boost::safe_numerics;

    try{
        auto i = std::to_integer<int16_t>(safe<std::int32_t>(1));
        auto j = std::to_integer<int16_t>(safe<std::int32_t>(INT_MAX));

        safe<std::int32_t> x = INT_MAX;
        safe<std::int32_t> y = 100;
        safe<std::int32_t> z;
        // rather than producing an invalid result an exception is thrown
        z = x + y;
    }
    catch(std::exception & e){
        // which we can catch here
        std::cout << "error detected:" << e.what() << std::endl;
    }
}

int main()
{
    using namespace boost::safe_numerics;
    std::cout << std::boolalpha;

    test();

    try{
        auto i = std::to_integer<int16_t>(safe<std::int32_t>(1));
        auto j = std::to_integer<int16_t>(safe<std::int32_t>(INT_MAX));
    }
    catch(std::exception & e){
        // which we can catch here
        std::cout << "error detected:" << e.what() << std::endl;
    }
    //test_conversion<double, safe<std::int32_t>>("to_integer"); // does not compile
/*
    test_conversion<double, unsigned int>("to_unsigned");
    test_conversion<double, uint64_t>("to_integer");
    test_conversion<double, int64_t>("numeric_cast");
    test_conversion<int64_t, bool>("to_integer");
    //test_conversion<int64_t, std::byte>("to_unsigned");
    test_conversion<int32_t, char16_t>("to_integer");

    test_conversion<half_float::half, char16_t>("to_unsigned");
    test_conversion<char32_t, half_float::half>("numeric_cast");
*/
    std::cout << "============= demo completed ============\n";
    return 0;

}


#ifndef SHASTA_CHOKE_POINTS_HPP
#define SHASTA_BOTTLENECKS_HPP

#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/reverse_graph.hpp>

// #include "iostream.hpp"
#include <list>
#include <map>
#include "vector.hpp"

namespace shasta {

    // Given a directed graph and a start vertex,
    // find vertices (the "choke points") that are on all paths
    // starting at the start vertex.
    template<class Graph> void findForwardChokePoints(
        const Graph&,
        typename Graph::vertex_descriptor startVertex,
        vector<typename Graph::vertex_descriptor>& chokePoints);

    // Same, but in the opposite direction.
    template<class Graph> void findBackwardChokePoints(
        const Graph&,
        typename Graph::vertex_descriptor startVertex,
        vector<typename Graph::vertex_descriptor>& chokePoints);
}

// Given a directed graph and a start vertex,
// find vertices (the "choke points") that are on all paths
// starting at the start vertex.
// The Graph must have a vertex_index property
// (which in practice means its second template parameter must be vecS).
// They are computed as follows:
// - Compute the dominator tree for the given start vertex.
// - For all vertices with out-degree 0 (in the graph,
//   not the dominator tree), walk up the dominator tree
//   up to the start vertex.
// - The common portion of the walks up the dominator tree
//   gives the choke points.
template<class Graph> void shasta::findForwardChokePoints(
    const Graph& graph,
    typename Graph::vertex_descriptor startVertex,
    vector<typename Graph::vertex_descriptor>& chokePoints)
{
    using vertex_descriptor = typename Graph::vertex_descriptor;
    using boost::lengauer_tarjan_dominator_tree;
    using boost::make_assoc_property_map;

    // Compute the dominator tree.
    std::map<vertex_descriptor, vertex_descriptor> predecessorMap;
    lengauer_tarjan_dominator_tree(
        graph,
        startVertex,
        make_assoc_property_map(predecessorMap));

    /*
    cout << "Dominator tree: " << endl;
    for(const auto& p: predecessorMap) {
        cout << graph[p.first].segmentId << " " << graph[p.second].segmentId << endl;
    }
    */

    // Follow the tree up from all terminal vertices - that is,
    // vertices with out-degree 0.
    // Find the common portion of the path up.
    vector<vertex_descriptor> commonPath;
    vector<vertex_descriptor> path;
    bool firstTime = true;
    BGL_FORALL_VERTICES_T(v, graph, Graph) {

        // If this is not a terminal vertex, skip.
        if(out_degree(v, graph) != 0) {
            continue;
        }

        // If there is no predecessor, it means that this
        // vertex is not accessible from the start vertex. Skip.
        if(predecessorMap.find(v) == predecessorMap.end()) {
            continue;
        }

        // cout << "Walk up the tree from " << graph[v].segmentId << endl;

        // Walk the tree up.
        path.clear();
        while(v != startVertex) {
            path.push_back(v);
            v = predecessorMap[v];
        }
        reverse(path.begin(), path.end());

        /*
        cout << "Path:";
        for(const vertex_descriptor u: path) {
            cout << " " << graph[u].segmentId;
        }
        cout << endl;
        */

        // Find the common portion.
        if(firstTime) {
            commonPath = path;
            firstTime = false;
        } else {
            uint64_t index = 0;
            for(; index<path.size() and index<commonPath.size(); index++) {
                if(path[index] != commonPath[index]) {
                    // cout << "Found a difference at index " << index << endl;
                    break;
                }
            }
            commonPath.resize(index);
        }

    }
    chokePoints = commonPath;
}

template<class Graph> void shasta::findBackwardChokePoints(
    const Graph& graph,
    typename Graph::vertex_descriptor startVertex,
    vector<typename Graph::vertex_descriptor>& chokePoints)
{
    findForwardChokePoints(
        boost::make_reverse_graph(graph),
        startVertex,
        chokePoints);
}

#endif