//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#include <fcppt/make_unique_ptr.hpp>
#include <fcppt/strong_typedef.hpp>
#include <fcppt/unique_ptr.hpp>
#include <fcppt/cast/size.hpp>
#include <fcppt/container/grid/map.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/math/at_c.hpp>
#include <fcppt/math/dim/comparison.hpp>
#include <fcppt/math/dim/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <string>
#include <utility>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_map
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::object<
		std::string,
		2
	>
	string_grid;

	typedef
	fcppt::container::grid::object<
		unsigned,
		2
	>
	uint_grid;

	uint_grid const grid(
		uint_grid::dim{
			2u,
			3u
		},
		[](
			uint_grid::pos const _pos
		)
		{
			return
				fcppt::cast::size<
					unsigned
				>(
					fcppt::math::at_c<
						0
					>(
						_pos
					)
					+
					fcppt::math::at_c<
						1
					>(
						_pos
					)
				);
		}
	);

	string_grid const result(
		fcppt::container::grid::map(
			grid,
			[](
				unsigned const _value
			)
			{
				return
					std::to_string(
						_value
					);
			}
		)
	);

	BOOST_REQUIRE_EQUAL(
		grid.size(),
		result.size()
	);

	BOOST_CHECK_EQUAL(
		result[
			string_grid::pos(
				0u,
				0u
			)
		],
		std::string(
			"0"
		)
	);

	BOOST_CHECK_EQUAL(
		result[
			string_grid::pos(
				1u,
				2u
			)
		],
		std::string(
			"3"
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_map_move
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::unique_ptr<
		unsigned
	>
	uint_unique_ptr;

	FCPPT_MAKE_STRONG_TYPEDEF(
		uint_unique_ptr,
		strong_result
	);

	typedef
	fcppt::container::grid::object<
		uint_unique_ptr,
		2
	>
	unique_ptr_grid;

	typedef
	fcppt::container::grid::object<
		strong_result,
		2
	>
	strong_grid;

	strong_grid const result(
		fcppt::container::grid::map(
			unique_ptr_grid(
				unique_ptr_grid::dim{
					3u,
					2u
				},
				[](
					unique_ptr_grid::pos const _pos
				)
				{
					return
						fcppt::make_unique_ptr<
							unsigned
						>(
							fcppt::cast::size<
								unsigned
							>(
								fcppt::math::at_c<
									0
								>(
									_pos
								)
								+
								fcppt::math::at_c<
									1
								>(
									_pos
								)
							)
						);
				}
			),
			[](
				uint_unique_ptr &&_value
			)
			{
				return
					strong_result(
						std::move(
							_value
						)
					);
			}
		)
	);

	BOOST_REQUIRE_EQUAL(
		strong_grid::dim(
			3u,
			2u
		),
		result.size()
	);

	BOOST_CHECK_EQUAL(
		*result[
			strong_grid::pos(
				0u,
				0u
			)
		].get(),
		0u
	);

	BOOST_CHECK_EQUAL(
		*result[
			strong_grid::pos(
				2u,
				1u
			)
		].get(),
		3u
	);
}


/**
 * @file EqRoutine.hpp
 * @author A. Domahidi [domahidi@embotech.com]
 * @license (new license) License BSD-3-Clause
 * @copyright Copyright (c) [2012-2015] Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
 * @date 2012-2015
 * @brief ECOS - Embedded Conic Solver
 *
 * Modified to c++ code by New York University and Max Planck Gesellschaft, 2017
 */

#pragma once

#include <memory>
#include <Eigen/Sparse>
#include <solver/interface/Cone.hpp>
#include <solver/interface/SolverSetting.hpp>

namespace solver {

  /*! Equilibration routine to improve condition number of
   *  matrices involved in the optimization problem. The method
   *  provided by default is Ruiz equilibration.
   */
  class EqRoutine
  {
    public:
	  EqRoutine(){}
	  ~EqRoutine(){}

	  void setEquilibration(const Cone& cone, const SolverSetting& stgs, SolverStorage& stg);
	  void unsetEquilibration(SolverStorage& stg);
	  void scaleVariables(OptimizationVector& opt);

      Vector& equilVec() { return equil_vec_; }
      const Vector& equilVec() const { return equil_vec_; }

    private:
	  void ruizEquilibration(SolverStorage& stg);
	  void maxRowsCols(double *row_vec, double *col_vec, const Eigen::SparseMatrix<double>& mat);
	  void equilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix<double>& mat);
	  void unequilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix<double>& mat);

    private:
	  Vector equil_vec_;
	  std::shared_ptr<Cone> cone_;
	  std::shared_ptr<SolverSetting> stgs_;
  };

}


#include <game_of_life.h>

#include <cstdio>
#include <fstream>
#include <iostream>

#include <boost/unordered_map.hpp>

GameOfLife::GameOfLife() {
    //
}

void GameOfLife::step() {
    population_t prev_population;
    boost::unordered_map<cell_t, int> density;

    prev_population.swap(population_);

    // Compute density around each of the cells of the population
    for (const auto& cell : prev_population) {
        int x = cell.first;
        int y = cell.second;

        // Iterate over the neighbors
        for (int i : {-1, 0, 1}) {
            for (int j : {-1, 0, 1}) {
                // Try to insert the new point
                auto ret = density.emplace(cell_t(x + i, y + j), 1);
                // If it already existed, add to its count
                if (!ret.second) {
                    ret.first->second++;
                }
            }
        }
    }

    // Create the new populatin
    for (const auto& [cell, cnt] : density) {
        if (cnt == 3 || (cnt == 4 && prev_population.count(cell))) {
            population_.emplace(cell);
        }
    }
}

void GameOfLife::set_population(const population_t &population) {
    population_ = population;
}

population_t GameOfLife::get_population() {
    return population_;
}

void GameOfLife::print_population() {
    for (const auto& cell : population_) {
        printf("(%d, %d),", cell.first, cell.second);
    }
    printf("\n");
}

void GameOfLife::write_vtk(std::string file_name, int width, int height, cell_t bottom_left) {
    std::ofstream vtk_file;

    vtk_file.open(file_name);

    if (vtk_file.is_open()) {
        std::cout << "Success opening " << file_name << std::endl;

        vtk_file << "# vtk DataFile Version 2.0" << std::endl;
        vtk_file << "Game of Life - Population" << std::endl;
        vtk_file << "BINARY" << std::endl;
        vtk_file << "DATASET STRUCTURED_POINTS" << std::endl;
        vtk_file << "DIMENSIONS " << width << " " << height << " 1" << std::endl;
        vtk_file << "ORIGIN " << bottom_left.first << " " << bottom_left.second << " 0" << std::endl;
        vtk_file << "SPACING 1 1 1" << std::endl;
        vtk_file << "POINT_DATA " << width * height << std::endl;
        vtk_file << "SCALARS cell unsigned_char" << std::endl;
        vtk_file << "LOOKUP_TABLE default" << std::endl;

        for (int j = bottom_left.second; j < bottom_left.second + height; ++j) {
            for (int i = bottom_left.first; i < bottom_left.first + width; ++i) {
                char live = (char) population_.count(cell_t(i, j));
                vtk_file.write((char*) &live, 1);
            }
        }

        vtk_file.close();
    }
    else {
        std::cout << "Error opening " << file_name << std::endl;
    }
}


#include <cstdlib>
#include <iostream>
#include <climits>
#include <algorithm>
#include <vector>
#include <utility>
#include <ctime>

#include "qset.hh"
#include "graphTypes.hh"

#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/subgraph.hpp>

//Basically just a DFS
//We start at v, and search
//We expand vertices in S
//And we add to our count (without expanding) for those not in S
int sizeQ(int n, const VSet &S, Vertex v, const Graph& G)
{
	//std::cout << "Q: starting with S = " << showSet(S) << "\n";

	//int n = boost::num_vertices(G);

	std::vector<Vertex> open; //TODO replace
	open.reserve(n);
	open.push_back(v);

	//Uses more memory than VSet, but is faster
	VSet closed; //TODO replace

	int numInQ = 0;

	while (!open.empty())
	{
		Vertex w = open.back();
		open.pop_back();

		auto outEdges = boost::out_edges(w, G);

		auto loopEnd = outEdges.second;
		for (auto iter = outEdges.first; iter != loopEnd; ++iter)
		{
			Edge e = *iter;
			Vertex u = boost::target(e, G);

			if (!closed.contains(u))
			{
				closed.insert(u);

				if (S.contains(u) && u != v )
				{
					open.push_back(u);
				}
				else if (u != v)
				{
					numInQ++;

				}
			}
		}
	}

	return numInQ;

}

void findQvalues(int n, const VSet &S, const Graph &G, std::vector<int>& outValues)
{

	//Store which vertices we've seen over all DFSes, makes sure we get all connected components
	VSet globalUnseen = S;

	//Store the set of vertices each connected component can reach
	std::vector<VSet> canReach(n);

	//For each vertex not in S, store which connected components it connects to
	std::vector<std::vector<Vertex>> reachableFrom(n);
	for (auto& subVec : reachableFrom)
	{
		subVec.reserve(n);
	}

	//Also store it as a set, to keep the vector as small as possible
	std::vector<VSet> reachableFromSet(n);

	if (!S.empty())
	{
		while (!globalUnseen.empty())
		{
			std::vector<Vertex> open;

			Vertex first = globalUnseen.first();
			Vertex currentCC = first;

			//std::cout << "CC " << currentCC << "\n";

			open.push_back(first);

			//Vertices we've already seen
			VSet closed;

			while (!open.empty())
			{
				Vertex w = open.back();
				open.pop_back();

				//Mark that we've seen this vertex in our connected component search
				globalUnseen.erase(w);

				auto outEdges = boost::out_edges(w, G);
				//std::cout << "Q: expanding " << w << "\n";

				for (auto iter = outEdges.first; iter != outEdges.second; ++iter)
				{
					Vertex u = boost::target(*iter, G);

					//std::cout << "Q: found neighbour " << u << "\n";
					if (!closed.contains(u))
					{
						//std::cout << "Q: adding " << u << " to closed\n";

						closed.insert(u);

						if (S.contains(u))
						{
							//std::cout << "Q: adding " << u << " to queue\n";
							open.push_back(u);
							reachableFrom[u].push_back(currentCC);

						}
						else if (!reachableFromSet[u].contains(currentCC))
						{
							//Mark that this CC and our vertex u, not in s, can reach each other
							reachableFrom[u].push_back(currentCC);
							reachableFromSet[u].insert(currentCC);
							canReach[currentCC].insert(u);
							//std::cout << "Reached " << u << " from " << w << " in CC " << currentCC << "\n";
						}
					}
				}
			}
		}
	}

	//For each vertex, we union the reachable vertices from each connected component
	//and take the number of elements
	//We also add in any vertices immediately adjacent to v which aren't in S
	auto vertInfo = vertices(G);
	for (auto iter = vertInfo.first; iter != vertInfo.second; ++iter)
	{
		Vertex v = *iter;
		VSet allReachableVerts;
		auto loopEnd = reachableFrom[v].end();

		for (auto connComp = reachableFrom[v].begin(); connComp != loopEnd; ++connComp)
		{
			allReachableVerts.addAll(canReach[*connComp]);
		}

		auto outEdges = boost::out_edges(v, G);
		//std::cout << "Q: expanding " << w << "\n";

		//Every edge of our current vertex is also in its Q set, unless it's in S
		for (auto iter = outEdges.first; iter != outEdges.second; ++iter)
		{
			//Edge e = *iter;
			Vertex u = boost::target(*iter, G);
			if (!S.contains(u))
			{
				allReachableVerts.insert(u);
				;
			}
		}

		allReachableVerts.erase(v);
		outValues[v] = allReachableVerts.size();
	}

}

int qCheck(int n, VSet S, Vertex v, Graph G)
{
	int numInQ = 0;
	auto vertInfo = vertices(G);
	auto edgeInfo = edges(G);
	for (auto iter = vertInfo.first; iter != vertInfo.second; iter++)
	{
		Vertex u = *iter;
		if (u != v && !S.contains(u))
		{
			VSet inducingSet = S;
			inducingSet.insert(v);
			inducingSet.insert(u);
			std::vector<Vertex> members;
			inducingSet.members(members);

			Graph GG;
			for (auto addVert = vertInfo.first; addVert != vertInfo.second; addVert++)
			{
				boost::add_vertex(GG);
			}

			for (auto edgeIter = edgeInfo.first; edgeIter != edgeInfo.second; edgeIter++)
			{
				Vertex from = boost::source(*edgeIter, G);
				Vertex to = boost::target(*edgeIter, G);
				if (inducingSet.contains(to) && inducingSet.contains(from))
				{
					boost::add_edge(to, from, GG);
				}
			}

			std::vector<int> distances(boost::num_vertices(G) + 1);
			boost::breadth_first_search(GG,
				v,
				boost::visitor(boost::make_bfs_visitor(boost::record_distances(&distances[0], boost::on_tree_edge()))));
			if (distances[u] != 0)
			{
				numInQ++;
			}

		}

	}
	return numInQ;
}

std::string showSet(VSet S) {
	std::ostringstream result;

	result << "{";

	std::vector<Vertex> members;
	S.members(members);

	for (auto iter = members.begin(); iter != members.end(); ++iter)
	{
		result << *iter << " ; ";
	}

	result << "}";

	return result.str();

}


#include "../graph/light_graph.h"
#include "../graph/cplex_graph.h"
#include "../problem_generator.h"

#include <chrono>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace std;
using namespace boost;

inline std::ostream& operator<<(std::ostream& os, const std::vector<std::pair<int, int>>& input)
{
	for (auto const& i : input) {
		os << i.first << "-" << i.second << " ";
	}
	return os;
}

void light_graph_dijkstra_acctest() {
	using path = vector<int>;

	cout << "Light graph Dijkstra accuracy test..." << endl;

	const int SAMPLES = 1000;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<problem::graph_type> graphs;
	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		graphs.emplace_back(prob.graph);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];
		auto& graph = graphs[i];

		// Light graph version
		path path1 = lgraph.shortest_path(from, to);

		// Boost version
		vector<int> parents(num_vertices(graph));
		auto index_map = boost::get(vertex_index, graph);
		auto parent_map = make_iterator_property_map(parents.begin(), index_map);

		dijkstra_shortest_paths(graph, from, predecessor_map(parent_map));

		// Trace back the path
		path path2;
		int curr = to;
		while (curr != from) {
			path2.push_back(curr);
			curr = parents[curr];
		}
		path2.push_back(from);
		std::reverse(path2.begin(), path2.end());

		cost_type cost1, cost2;
		cost1 = lgraph.get_path_cost(path1);
		cost2 = lgraph.get_path_cost(path2);

		if (abs(cost1 - cost2) > 0.01) {
			cerr << "Test failed (" << cost1 << " != " << cost2 << ")" << endl;
			return;
		}
	}

	cout << "Light graph Dijkstra produced accurate results" << endl;
}

void light_graph_dijkstra_perftest() {
	using path = vector<int>;

	cout << "Light graph Dijkstra (5 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 100;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<problem::graph_type> graphs;
	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		graphs.emplace_back(prob.graph);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start1 = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			path p = lgraph.shortest_path(from, to);
		}
	}

	auto end1 = chrono::high_resolution_clock::now();
	double time1 = chrono::duration<double>(end1 - start1).count();

	auto start2 = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& graph = graphs[i];

		LOOP(j, REPEAT) {
			vector<int> parents(num_vertices(graph));
			auto index_map = boost::get(vertex_index, graph);
			auto parent_map = make_iterator_property_map(parents.begin(), index_map);

			dijkstra_shortest_paths(graph, from, predecessor_map(parent_map));

			// Trace back the path
			path path;
			int curr = to;
			while (curr != from) {
				path.push_back(curr);
				curr = parents[curr];
			}
			path.push_back(from);
			std::reverse(path.begin(), path.end());
		}
	}

	auto end2 = chrono::high_resolution_clock::now();
	double time2 = chrono::duration<double>(end2 - start2).count();

	cout << "Light graph: " << time1 * 1000 / TOTAL << " ms" << endl;
	cout << "Boost graph: " << time2 * 1000 / TOTAL << " ms" << endl;
}

void light_graph_yen_acctest() {
	using path = vector<int>;

	cout << "Light graph Yen accuracy test..." << endl;

	const int SAMPLES = 1000;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths = lgraph.k_shortest_paths(from, to, 100);

		// Verify that the costs are in ascending order
		cost_type last_cost = lgraph.get_path_cost(paths[0]);
		for (int j = 1; j < paths.size(); j++) {
			cost_type cost = lgraph.get_path_cost(paths[j]);
			if (cost - last_cost < -1e-3) {
				cerr << "Costs not in ascending order (" << cost << " < " << last_cost << ")" << endl;
				return;
			}
			last_cost = cost;
		}

		// Test for duplicated nodes
		for (path p : paths) {
			std::sort(p.begin(), p.end());
			if (std::unique(p.begin(), p.end()) != p.end()) {
				cerr << "Duplicated node" << endl;
				return;
			}
		}

		// Test for duplicated paths (destroy order of paths)
		std::sort(paths.begin(), paths.end());
		if (std::unique(paths.begin(), paths.end()) != paths.end()) {
			cerr << "Duplicated path" << endl;
			return;
		}
	}

	cout << "Light graph Yen produced accurate results" << endl;
}

void light_graph_yen_perftest() {
	using path = vector<int>;

	cout << "Light graph Yen (1000 paths, 5 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.k_shortest_paths(from, to, 1000);
		}
	}

	auto end = chrono::high_resolution_clock::now();
	double time = chrono::duration<double>(end - start).count();

	cout << "Average time: " << time * 1000 / TOTAL << " ms" << endl;
}

void light_graph_toll_unique_acctest() {
	using path = vector<int>;

	cout << "Light graph Toll unique accuracy test..." << endl;

	const int SAMPLES = 100;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths = lgraph.toll_unique_paths(from, to, 1000);

		// Verify that the toll sets are unique
		using odpair = pair<int, int>;
		set<set<odpair>> all_sets;

		for (const path& path : paths) {
			// Get the set of toll arcs
			set<odpair> toll_set;
			for (int i = 0; i < path.size() - 1; i++)
				if (lgraph.edge(path[i], path[i + 1]).is_tolled)
					toll_set.emplace(path[i], path[i + 1]);

			// Add to all sets, throw if existed
			if (!all_sets.insert(toll_set).second) {
				cerr << "Toll set is not unique" << endl;
				return;
			}
		}
	}

	cout << "Light graph Toll unique produced accurate results" << endl;
}

void light_graph_toll_unique_perftest() {
	using path = vector<int>;

	cout << "Light graph Toll unique (1000 paths, 5 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.k_shortest_paths(from, to, 1000);
		}
	}

	auto end = chrono::high_resolution_clock::now();
	double time = chrono::duration<double>(end - start).count();

	cout << "K paths: " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.toll_unique_paths(from, to, 1000);
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Toll unique: " << time * 1000 / TOTAL << " ms" << endl;
}

void light_graph_price_from_src_acctest() {
	using path = vector<int>;

	cout << "Light graph Price from src accuracy test..." << endl;

	const int SAMPLES = 1000;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<problem::graph_type> graphs;
	vector<light_graph> lgraphs;
	vector<int> froms;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		graphs.emplace_back(prob.graph);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i];
		auto& lgraph = lgraphs[i];
		auto& graph = graphs[i];

		vector<cost_type> prices = lgraph.price_from_src(from);

		LOOP(v, lgraph.V) {
			path p = lgraph.shortest_path(from, v);
			cost_type cost = lgraph.get_path_cost(p);

			if (abs(cost - prices[v]) > 0.01) {
				cerr << "Test failed (" << cost << " != " << prices[v] << ")" << endl;
				return;
			}
		}
	}

	cout << "Light graph Price from src produced accurate results" << endl;
}

void light_graph_price_to_dst_acctest() {
	using path = vector<int>;

	cout << "Light graph Price to dst accuracy test..." << endl;

	const int SAMPLES = 1000;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<problem::graph_type> graphs;
	vector<light_graph> lgraphs;
	vector<int> tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		graphs.emplace_back(prob.graph);
		lgraphs.emplace_back(prob.graph);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int to = tos[i];
		auto& lgraph = lgraphs[i];
		auto& graph = graphs[i];

		vector<cost_type> prices = lgraph.price_to_dst(to);

		LOOP(v, lgraph.V) {
			path p = lgraph.shortest_path(v, to);
			cost_type cost = lgraph.get_path_cost(p);

			if (abs(cost - prices[v]) > 0.01) {
				cerr << "Test failed (" << cost << " != " << prices[v] << ")" << endl;
				return;
			}
		}
	}

	cout << "Light graph Price to dst produced accurate results" << endl;
}

void light_graph_bilevel_feasible_yen_acctest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible (Yen's version) accuracy test..." << endl;

	const int SAMPLES = 100;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths = lgraph.bilevel_feasible_paths_yen(from, to, 1000);

		// Verify that the toll sets are bilevel feasible
		using iipair = pair<int, int>;
		vector<set<iipair>> all_sets;

		for (const path& path : paths) {
			// Get the set of toll arcs
			set<iipair> tset = lgraph.get_toll_set(path);

			// Check if it is not a superset of any previous set
			if (std::any_of(all_sets.begin(), all_sets.end(),
							[&](const auto& s) {
								return std::includes(tset.begin(), tset.end(), s.begin(), s.end());
							})) {
				cerr << "Toll set is not bilevel feasible" << endl;
				return;
			}
			else {
				all_sets.emplace_back(std::move(tset));
			}
		}
	}

	cout << "Light graph Bilevel feasible (Yen's version) produced accurate results" << endl;
}

void light_graph_bilevel_feasible_yen_perftest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible (Yen's version) (1000 paths, 5 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.k_shortest_paths(from, to, 1000);
		}
	}

	auto end = chrono::high_resolution_clock::now();
	double time = chrono::duration<double>(end - start).count();

	cout << "K paths: " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.toll_unique_paths(from, to, 1000);
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Toll unique: " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_yen(from, to, 1000);
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Bilevel feasible: " << time * 1000 / TOTAL << " ms" << endl;
}

void light_graph_bilevel_feasible_acctest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible accuracy test..." << endl;

	const int SAMPLES = 100;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths_yen = lgraph.bilevel_feasible_paths_yen(from, to, 1000);
		vector<path> paths_new = lgraph.bilevel_feasible_paths(from, to, paths_yen.size());

		// Verify that the costs are in ascending order
		cost_type last_cost = lgraph.get_path_cost(paths_new[0]);
		for (int j = 1; j < paths_new.size(); j++) {
			cost_type cost = lgraph.get_path_cost(paths_new[j]);
			if (cost - last_cost < -1e-3) {
				cerr << "Costs not in ascending order (" << cost << " < " << last_cost << ")" << endl;
				break;
			}
			last_cost = cost;
		}

		// Verify that the toll sets are bilevel feasible
		using iipair = pair<int, int>;
		vector<set<iipair>> all_sets;
		for (const path& path : paths_new) {
			// Get the set of toll arcs
			auto tset = lgraph.get_toll_set(path);

			// Check if it is not a superset of any previous set
			if (std::any_of(all_sets.begin(), all_sets.end(),
							[&](const auto& s) {
								return std::includes(tset.begin(), tset.end(), s.begin(), s.end());
							})) {
				cerr << "Toll set is not bilevel feasible" << endl;
				break;
			}
			else {
				all_sets.emplace_back(std::move(tset));
			}
		}

		// Verify the two lists are the same
		if (paths_yen != paths_new) {
			cerr << "Yen version:" << endl;
			for (const path& p : paths_yen) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "New version:" << endl;
			for (const path& p : paths_new) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "List of bilevel feasible paths are not the same" << endl;
			return;
		}
	}

	cout << "Light graph Bilevel feasible produced accurate results" << endl;
}

void light_graph_bilevel_feasible_perftest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible (10000 paths, 5 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;
	vector<int> sizes(SAMPLES);

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_yen(from, to, 10000);
			if (j == 0)
				sizes[i] = ps.size();
		}
	}

	auto end = chrono::high_resolution_clock::now();
	auto time = chrono::duration<double>(end - start).count();

	cout << "Yen version: " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths(from, to, sizes[i]);
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "New version: " << time * 1000 / TOTAL << " ms" << endl;

	cout << "Average num paths: " << std::accumulate(sizes.begin(), sizes.end(), 0) / SAMPLES << endl;
}

void light_graph_bilevel_feasible_stat() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible (5 x 12 grid, K = 40) statistics..." << endl;

	const int SAMPLES = 100;
	const int MAX_PATHS = 500;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<problem> probs;

	LOOP(i, SAMPLES) {
		probs.emplace_back(random_grid_problem(5, 12, 40, 0.2, random_engine));
	}

	LOOP(i, SAMPLES) {
		problem& prob = probs[i];
		light_graph lgraph(prob.graph);

		LOOP(k, prob.commodities.size()) {
			commodity& comm = prob.commodities[k];
			vector<path> ps = lgraph.bilevel_feasible_paths(comm.origin, comm.destination, MAX_PATHS);
			if (ps.size() < MAX_PATHS || lgraph.is_toll_free(ps.back()))
				cout << ps.size() << endl;
			else
				cout << MAX_PATHS + 1 << endl;
		}
	}
}

void light_graph_bilevel_feasible_2_acctest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible v2 accuracy test..." << endl;

	const int SAMPLES = 100;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths_v1 = lgraph.bilevel_feasible_paths(from, to, 500);
		vector<path> paths_v2 = lgraph.bilevel_feasible_paths_2(from, to, 500);

		// Verify that the costs are in ascending order
		cost_type last_cost = lgraph.get_path_cost(paths_v2[0]);
		for (int j = 1; j < paths_v2.size(); j++) {
			cost_type cost = lgraph.get_path_cost(paths_v2[j]);
			if (cost - last_cost < -1e-3) {
				cerr << "Costs not in ascending order (" << cost << " < " << last_cost << ")" << endl;
				break;
			}
			last_cost = cost;
		}

		// Verify that the toll sets are bilevel feasible
		using iipair = pair<int, int>;
		vector<set<iipair>> all_sets;
		for (const path& path : paths_v2) {
			// Get the set of toll arcs
			auto tset = lgraph.get_toll_set(path);

			// Check if it is not a superset of any previous set
			if (std::any_of(all_sets.begin(), all_sets.end(),
							[&](const auto& s) {
								return std::includes(tset.begin(), tset.end(), s.begin(), s.end());
							})) {
				cerr << "Toll set is not bilevel feasible" << endl;
				break;
			}
			else {
				all_sets.emplace_back(std::move(tset));
			}
		}

		// Verify the two lists are the same
		if (paths_v1 != paths_v2) {
			cerr << "Version 1:" << endl;
			for (const path& p : paths_v1) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "Version 2:" << endl;
			for (const path& p : paths_v2) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "List of bilevel feasible paths are not the same" << endl;
			return;
		}
	}

	cout << "Light graph Bilevel feasible v2 produced accurate results" << endl;
}

void light_graph_bilevel_feasible_2_perftest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible v2 (12 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<light_graph> lgraphs;
	vector<int> froms, tos;
	vector<int> sizes1(SAMPLES);
	vector<int> sizes2(SAMPLES);
	vector<int> sizes2u(SAMPLES);

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(12, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths(from, to, 200);
			if (j == 0)
				sizes1[i] = ps.size();
		}
	}

	auto end = chrono::high_resolution_clock::now();
	auto time = chrono::duration<double>(end - start).count();

	cout << "Version 1 (max 200): " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_2(from, to, 1000);
			if (j == 0)
				sizes2[i] = ps.size();
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Version 2 (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_2(from, to, 1000, false);
			if (j == 0)
				sizes2u[i] = ps.size();
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Version 2u (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	cout << "Average num paths 1: " << std::accumulate(sizes1.begin(), sizes1.end(), 0) / SAMPLES << endl;
	cout << "Average num paths 2: " << std::accumulate(sizes2.begin(), sizes2.end(), 0) / SAMPLES << endl;
	cout << "Average num paths 2u: " << std::accumulate(sizes2u.begin(), sizes2u.end(), 0) / SAMPLES << endl;
}

void light_graph_bilevel_feasible_3_acctest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible v3 accuracy test..." << endl;

	const int SAMPLES = 100;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<cplex_graph> lgraphs;
	vector<int> froms, tos;

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(5, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		vector<path> paths_v2 = lgraph.bilevel_feasible_paths_2(from, to, 500);
		vector<path> paths_v3 = lgraph.bilevel_feasible_paths_3(from, to, 500);

		// Verify that the costs are in ascending order
		cost_type last_cost = lgraph.get_path_cost(paths_v3[0]);
		for (int j = 1; j < paths_v3.size(); j++) {
			cost_type cost = lgraph.get_path_cost(paths_v3[j]);
			if (cost - last_cost < -1e-3) {
				cerr << "Costs not in ascending order (" << cost << " < " << last_cost << ")" << endl;
				break;
			}
			last_cost = cost;
		}

		// Verify that the toll sets are bilevel feasible
		using iipair = pair<int, int>;
		vector<set<iipair>> all_sets;
		for (const path& path : paths_v3) {
			// Get the set of toll arcs
			auto tset = lgraph.get_toll_set(path);

			// Check if it is not a superset of any previous set
			if (std::any_of(all_sets.begin(), all_sets.end(),
							[&](const auto& s) {
								return std::includes(tset.begin(), tset.end(), s.begin(), s.end());
							})) {
				cerr << "Toll set is not bilevel feasible" << endl;
				break;
			}
			else {
				all_sets.emplace_back(std::move(tset));
			}
		}

		// Verify the two lists are the same
		if (paths_v2 != paths_v3) {
			cerr << "Version 2:" << endl;
			for (const path& p : paths_v2) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "Version 3:" << endl;
			for (const path& p : paths_v3) {
				auto tlist = lgraph.get_toll_list(p);
				cost_type cost = lgraph.get_path_cost(p);
				cerr << "  " << tlist << "    " << cost << endl;
			}
			cerr << "List of bilevel feasible paths are not the same" << endl;
			return;
		}
	}

	cout << "Light graph Bilevel feasible v3 produced accurate results" << endl;
}

void light_graph_bilevel_feasible_3_perftest() {
	using path = vector<int>;

	cout << "Light graph Bilevel feasible v3 (12 x 12 grid) performance test..." << endl;

	const int SAMPLES = 100;
	const int REPEAT = 1;
	const int TOTAL = SAMPLES * REPEAT;

	auto seed = chrono::high_resolution_clock::now().time_since_epoch().count();
	auto random_engine = default_random_engine(seed);

	vector<cplex_graph> lgraphs;
	vector<int> froms, tos;
	vector<int> sizes2(SAMPLES);
	vector<int> sizes2u(SAMPLES);
	vector<int> sizes3(SAMPLES);
	vector<int> sizes3u(SAMPLES);

	LOOP(i, SAMPLES) {
		problem prob = random_grid_problem(12, 12, 1, 0.2, random_engine);
		lgraphs.emplace_back(prob.graph);
		froms.push_back(prob.commodities[0].origin);
		tos.push_back(prob.commodities[0].destination);
	}

	auto start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_2(from, to, 1000);
			if (j == 0)
				sizes2[i] = ps.size();
		}
	}

	auto end = chrono::high_resolution_clock::now();
	auto time = chrono::duration<double>(end - start).count();

	cout << "Version 2 (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_2(from, to, 1000, false);
			if (j == 0)
				sizes2u[i] = ps.size();
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Version 2u (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_3(from, to, 1000);
			if (j == 0)
				sizes3[i] = ps.size();
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Version 3 (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	start = chrono::high_resolution_clock::now();

	LOOP(i, SAMPLES) {
		int from = froms[i], to = tos[i];
		auto& lgraph = lgraphs[i];

		LOOP(j, REPEAT) {
			vector<path> ps = lgraph.bilevel_feasible_paths_3(from, to, 1000, false);
			if (j == 0)
				sizes3u[i] = ps.size();
		}
	}

	end = chrono::high_resolution_clock::now();
	time = chrono::duration<double>(end - start).count();

	cout << "Version 3u (max 1000): " << time * 1000 / TOTAL << " ms" << endl;

	cout << "Average num paths 2: " << std::accumulate(sizes2.begin(), sizes2.end(), 0) / SAMPLES << endl;
	cout << "Average num paths 2u: " << std::accumulate(sizes2u.begin(), sizes2u.end(), 0) / SAMPLES << endl;
	cout << "Average num paths 3: " << std::accumulate(sizes3.begin(), sizes3.end(), 0) / SAMPLES << endl;
	cout << "Average num paths 3u: " << std::accumulate(sizes3u.begin(), sizes3u.end(), 0) / SAMPLES << endl;
}


#include <armadillo>
#include <iostream>
#include <ostream>
#include <target/nb.hpp>
#include <target/utils.hpp>

using namespace arma;
using std::string;
using std::cout;
using std::endl;

int main(int argc, char **argv) {
    mat A(2, 2);
    A.fill(1);
    A(1, 1) = 2;
    cout << target::BLUE << "Naive Bayes test\n\n";
    std::cout << inv(A) << std::endl;
    std::cout << target::COL_RESET;
    return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

#include "core/matrix.h"
#include "hssh/local_topological/area.h"
#include "hssh/local_topological/area_visitor.h"
#include "hssh/local_topological/areas/serialization.h"
#include "hssh/local_topological/local_topo_map.h"
#include "math/clustering.h"
#include "mpepc/simulator/dynamic_object_trajectory.h"
#include "mpepc/training/agent_state.h"
#include "utils/histogram.h"
#include "utils/serialized_file_io.h"
#include <boost/tuple/tuple.hpp>
#include <fstream>
#include <gnuplot-iostream.h>
#include <iostream>

using namespace vulcan;
using namespace vulcan::hssh;
using namespace vulcan::mpepc;

class MotionVisitor : public LocalAreaVisitor
{
public:
    MotionVisitor(const MotionObs& motions);

    AreaType type(void) const { return type_; }
    double eigRatio(void) const { return eigvals_[0] / eigvals_[1]; }

    MotionObs samples(void) const { return motions_; }

    bool isValidExample(void) const { return (eigvals_[1] > 0) && (motions_.size() > 2); }

    // LocalAreaVisitor interface
    void visitDestination(const LocalDestination& destination) override;
    void visitDecisionPoint(const LocalDecisionPoint& decision) override;
    void visitPathSegment(const LocalPathSegment& path) override;

private:
    AreaType type_ = AreaType::area;

    Matrix eigvecs_;
    Vector eigvals_;

    double maxWidth_;   // maximum width of a gateway in the area

    MotionObs motions_;

    Vector computeEigs(void);
    void mapMotionsViaPCA(const Vector& mean);
    void normalizeLateralDists(const LocalArea& area);
};

void cluster_eig_ratios(const std::vector<MotionVisitor>& motions);
void cluster_lateral_dist(const std::vector<MotionVisitor>& motions);
void cluster_longitudinal_dist(const std::vector<MotionVisitor>& motions);
void cluster_longitudinal_vels(const std::vector<MotionVisitor>& motions);
void plot_1d_clusters(const math::clustering_result_t& clusters,
                      const std::vector<double>& values,
                      const std::string& name);
void plot_2d_clusters(const math::clustering_result_t& clusters,
                      const std::vector<Point<float>>& values,
                      const std::string& name);

int main(int argc, char** argv)
{
    if ((argc < 3) || (argc % 2 == 0)) {
        std::cout << "Usage: social_norms_clustering 'ltm' 'ltm1' ...\n";
        return 1;
    }

    std::vector<MotionVisitor> motions;

    for (int n = 1; n < argc; ++n) {
        LocalTopoMap ltm;
        if (!utils::load_serializable_from_file(argv[n], ltm)) {
            std::cerr << "ERROR: Failed to load LTM from " << argv[n] << '\n';
            return 1;
        }

        MotionObsMap observations = load_motion_observations(argv[n]);
        if (observations.empty()) {
            std::cerr << "ERROR: Failed to load any data from " << argv[n] << '\n';
            return 1;
        }

        // Process each batch of observations
        for (auto& areaToObs : observations) {
            auto area = ltm.areaWithId(areaToObs.first);

            if (area) {
                MotionVisitor visitor(areaToObs.second);
                area->accept(visitor);

                // Not every area ends up with enough data to analyze
                if (visitor.isValidExample()) {
                    motions.push_back(visitor);
                }
            }
        }
    }

    cluster_eig_ratios(motions);
    cluster_lateral_dist(motions);
    cluster_longitudinal_dist(motions);
    cluster_longitudinal_vels(motions);

    return 0;
}

void cluster_eig_ratios(const std::vector<MotionVisitor>& motions)
{
    std::vector<double> ratios(motions.size());
    std::transform(motions.begin(), motions.end(), ratios.begin(), [](auto& m) {
        return m.eigRatio();
    });

    auto clusters = math::em_1d_linear(ratios.begin(), ratios.end(), 5);

    std::cout << "Eig Ratio Clusters:\n";
    for (int n = 0; n < clusters.numClusters; ++n) {
        std::cout << n << ": size: " << clusters.clusterSizes[n];

        double mean = 0.0;
        int numPaths = 0;
        int numPlaces = 0;

        for (std::size_t i = 0; i < clusters.assignedCluster.size(); ++i) {
            if (clusters.assignedCluster[i] == n) {
                mean += ratios[i];

                if (motions[i].type() == AreaType::path_segment) {
                    ++numPaths;
                } else {
                    ++numPlaces;
                }
            }
        }

        mean /= clusters.clusterSizes[n];

        std::cout << " mean: " << mean << " paths: " << numPaths << " places:" << numPlaces << '\n';
    }
}

void cluster_lateral_dist(const std::vector<MotionVisitor>& motions)
{
    std::vector<double> latDists;

    //     std::vector<MotionVisitor> paths;
    //     std::copy_if(motions.begin(), motions.end(), std::back_inserter(paths), [](const auto& m) {
    //         return m.type() == AreaType::path_segment;
    //     });
    //
    //     auto maxSampleIt = std::max_element(paths.begin(), paths.end(), [](const auto& lhs, const auto& rhs) {
    //         return lhs.samples().size() < rhs.samples().size();
    //     });

    for (auto& m : motions) {
        if (m.type() == AreaType::path_segment) {
            for (auto& sample : m.samples()) {
                latDists.push_back(sample.y);
            }
        }
    }

    auto clusters = math::em_1d_linear(latDists.begin(), latDists.end(), 10, 100);

    std::cout << "Lateral Distance Clusters:\n";
    for (int n = 0; n < clusters.numClusters; ++n) {
        std::cout << n << ": size: " << clusters.clusterSizes[n];

        double mean = 0.0;

        for (std::size_t i = 0; i < clusters.assignedCluster.size(); ++i) {
            if (clusters.assignedCluster[i] == n) {
                mean += latDists[i];
            }
        }

        mean /= clusters.clusterSizes[n];

        std::cout << " mean: " << mean << '\n';
    }

    plot_1d_clusters(clusters, latDists, "Lateral Distance");
}

void cluster_longitudinal_dist(const std::vector<MotionVisitor>& motions)
{
    std::vector<double> longDists;

    for (auto& m : motions) {
        if (m.type() == AreaType::path_segment) {
            for (auto& sample : m.samples()) {
                longDists.push_back(sample.xVel);
            }
        }
    }

    auto clusters = math::em_1d_linear(longDists.begin(), longDists.end(), 5, 100);

    std::cout << "Longitudinal Distance Clusters:\n";
    for (int n = 0; n < clusters.numClusters; ++n) {
        std::cout << n << ": size: " << clusters.clusterSizes[n];

        double mean = 0.0;

        for (std::size_t i = 0; i < clusters.assignedCluster.size(); ++i) {
            if (clusters.assignedCluster[i] == n) {
                mean += longDists[i];
            }
        }

        mean /= clusters.clusterSizes[n];

        std::cout << " mean: " << mean << '\n';
    }

    plot_1d_clusters(clusters, longDists, "Longitudinal Distance");
}

void cluster_longitudinal_vels(const std::vector<MotionVisitor>& motions)
{
    std::vector<Point<float>> longVels;
    //
    //     std::vector<MotionVisitor> paths;
    //     std::copy_if(motions.begin(), motions.end(), std::back_inserter(paths), [](const auto& m) {
    //         return m.type() == AreaType::path_segment;
    //     });
    //
    //     auto maxSampleIt = std::max_element(paths.begin(), paths.end(), [](const auto& lhs, const auto& rhs) {
    //         return lhs.samples().size() < rhs.samples().size();
    //     });
    //
    //     for(auto& sample : maxSampleIt->samples())
    //     {
    //         longVels.emplace_back(sample.xVel, sample.normDist);
    //     }

    for (auto& m : motions) {
        if (m.type() == AreaType::path_segment) {
            for (auto& sample : m.samples()) {
                longVels.emplace_back(sample.xVel, sample.normDist);
            }
        }
    }

    auto clusters = math::em_2d_linear(longVels.begin(), longVels.end(), 8, 1000);

    std::cout << "Longitudinal Velocity Clusters:\n";
    for (int n = 0; n < clusters.numClusters; ++n) {
        std::cout << n << ": size: " << clusters.clusterSizes[n];

        Point<float> mean;

        for (std::size_t i = 0; i < clusters.assignedCluster.size(); ++i) {
            if (clusters.assignedCluster[i] == n) {
                mean += longVels[i];
            }
        }

        mean.x /= clusters.clusterSizes[n];
        mean.y /= clusters.clusterSizes[n];

        std::cout << " mean: " << mean << '\n';
    }

    plot_2d_clusters(clusters, longVels, "Velocity vs. Lateral Distance");
}

void plot_1d_clusters(const math::clustering_result_t& clusters,
                      const std::vector<double>& values,
                      const std::string& name)
{
    // Assign values to their respective clusters
    std::vector<std::vector<double>> clusterValues(clusters.numClusters);
    for (std::size_t n = 0; n < values.size(); ++n) {
        clusterValues[clusters.assignedCluster[n]].push_back(values[n]);
    }

    Gnuplot plot;

    plot << "set title '" << name << "'\n";
    plot << "plot ";

    for (int n = 0; n < clusters.numClusters; ++n) {
        plot << "'-' using 1:0 title '" << n << "'";

        if (n != clusters.numClusters - 1) {
            plot << ',';
        }
    }

    plot << '\n';

    for (int n = 0; n < clusters.numClusters; ++n) {
        std::sort(clusterValues[n].begin(), clusterValues[n].end());
        plot.send1d(clusterValues[n]);
    }
}

void plot_2d_clusters(const math::clustering_result_t& clusters,
                      const std::vector<Point<float>>& values,
                      const std::string& name)
{
    // Assign values to their respective clusters
    std::vector<std::vector<boost::tuple<float, float>>> clusterValues(clusters.numClusters);
    for (std::size_t n = 0; n < values.size(); ++n) {
        clusterValues[clusters.assignedCluster[n]].emplace_back(values[n].x, values[n].y);
    }

    Gnuplot plot;

    plot << "set title '" << name << "'\n";
    plot << "plot ";

    for (int n = 0; n < clusters.numClusters; ++n) {
        plot << "'-' using 1:2 title '" << n << "'";

        if (n != clusters.numClusters - 1) {
            plot << ',';
        }
    }

    plot << '\n';

    for (int n = 0; n < clusters.numClusters; ++n) {
        plot.send1d(clusterValues[n]);
    }
}

/////////////////   MotionVisitor implementation   ////////////////////

MotionVisitor::MotionVisitor(const MotionObs& motions) : motions_(motions)
{
    assert(!motions.empty());

    auto mean = computeEigs();
    mapMotionsViaPCA(mean);
}

void MotionVisitor::visitDestination(const LocalDestination& destination)
{
    type_ = AreaType::destination;
}

void MotionVisitor::visitDecisionPoint(const LocalDecisionPoint& decision)
{
    type_ = AreaType::decision_point;

    std::cout << "\nDecision eig ratio: " << (eigvals_[0] / eigvals_[1]) << '\n';
}

void MotionVisitor::visitPathSegment(const LocalPathSegment& path)
{
    type_ = AreaType::path_segment;

    Vector pathDir(2);
    pathDir[0] = path.plusTransition().target().x - path.minusTransition().target().x;
    pathDir[1] = path.plusTransition().target().y - path.minusTransition().target().y;

    pathDir /= arma::norm(pathDir);

    normalizeLateralDists(path);

    std::cout << "\nPath: " << path.description() << '\n';
    std::cout << "Path vec:\n" << pathDir << " Vel vec:\n" << eigvecs_.col(1);
    std::cout << "Dot product path axis: " << arma::dot(pathDir, eigvecs_.col(1)) << '\n';
    std::cout << "Path eig ratio: " << (eigvals_[0] / eigvals_[1]) << '\n';
}

Vector MotionVisitor::computeEigs(void)
{
    Vector mean(4);
    mean.zeros();
    for (auto& obs : motions_) {
        mean[0] += obs.x;
        mean[1] += obs.y;
        mean[2] += obs.xVel;
        mean[3] += obs.yVel;
    }

    mean /= motions_.size();

    Matrix cov(2, 2);
    cov.zeros();

    for (auto& obs : motions_) {
        Vector obsVec(2);
        obsVec[0] = obs.xVel;
        obsVec[1] = obs.yVel;

        Vector diff = obsVec - mean.rows(2, 3);
        cov += diff * arma::trans(diff);
    }

    arma::eig_sym(eigvals_, eigvecs_, cov);

    return mean;
}

void MotionVisitor::mapMotionsViaPCA(const Vector& mean)
{
    double orientation = std::atan2(eigvecs_(1, 1), eigvecs_(0, 1));
    orientation = wrap_to_pi_2(orientation);

    Line<double> xAxis(mean[0], mean[1], mean[0] + std::cos(orientation), mean[1] + std::sin(orientation));
    Line<double> yAxis(mean[0],
                       mean[1],
                       mean[0] + std::cos(orientation + M_PI_2),
                       mean[1] + std::sin(orientation + M_PI_2));

    for (auto& m : motions_) {
        Point<double> pos(m.x, m.y);

        m.x = distance_to_line(pos, yAxis);
        m.y = distance_to_line(pos, xAxis);

        if (left_of_line(yAxis, pos)) {
            m.x *= -1;
        }

        if (!left_of_line(xAxis, pos)) {
            m.y *= -1;
        }

        Point<double> vel(m.xVel, m.yVel);
        vel = rotate(vel, orientation);
        m.xVel = vel.x;
        m.yVel = vel.y;
    }
}

void MotionVisitor::normalizeLateralDists(const LocalArea& area)
{
    auto gwys = area.gateways();
    auto maxGwyIt = std::max_element(gwys.begin(), gwys.end(), [](const auto& lhs, const auto& rhs) {
        return lhs.length() < rhs.length();
    });

    if (maxGwyIt == gwys.end()) {
        return;
    }

    maxWidth_ = maxGwyIt->length();

    for (auto& m : motions_) {
        m.y /= maxWidth_;
    }
}


#include <z3++.h>
#include "symbolic-expr.hpp"
#include <variant>
#include <boost/functional/hash.hpp>

namespace Symbolic {
    struct Z3Expr {
        std::variant<z3::expr, z3::func_decl, int> content;

        bool is_expr() const;
        bool is_func() const;
        bool is_bool() const;
        z3::expr get_expr();
        z3::expr get_bool();
        z3::func_decl get_func();

        Z3Expr(): content(0) {}
    };
    struct Z3Context {
        z3::context ctx;
        std::unordered_map<boost::uuids::uuid, Z3Expr, boost::hash<boost::uuids::uuid>> cache_;
        std::unordered_map<std::string, Z3Expr> var_cache_;
    };

    Z3Expr gen_z3_expr(Z3Context &ctx, ExprPtr expr);

    bool verify_with_z3(Z3Context &ctx, ExprPtr pre_cond, ExprPtr target, bool do_fork=false);

    void print_z3_expr(ExprPtr expr, std::ostream &os);
}


/*
 neighbours.hxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#pragma once

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

namespace ublas = boost::numeric::ublas;

class Neighbour {

public:

  Neighbour(unsigned long nbas,
            ublas::matrix<double> plat,
            ublas::matrix<double> basis);

  ~Neighbour();

  void SetInverseLattice(ublas::matrix<double> iplat,
                         ublas::matrix<double> xbasis);
  void GetSiteList(double rmax);
  void GetNhbrList(double rmax);
  void LinkedListBlocks(double rmax);
  unsigned long FindCell(ublas::vector<double> sitex);

  ublas::vector<long> Total;
  ublas::vector<long> Pointers;
  ublas::vector<long> BasisPointers;
  ublas::matrix<double> Sites;

private:

  long NumberOfAtoms;
  ublas::matrix<double> LatticeVectors;
  ublas::matrix<double> BasisVectors;
  ublas::vector<long> SupercellExtension;
  ublas::matrix<double> InverseLatticeVectors;
  ublas::matrix<double> InverseBasisVectors;
  ublas::vector<long> Blocks;
  ublas::vector<double> Centre;
  ublas::vector<long> Head;
  ublas::vector<long> LinkedList;

};


/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-compare"
#include <boost/test/unit_test.hpp>
#pragma GCC diagnostic pop

#include <eosio/testing/tester.hpp>
#include <eosio/chain/abi_serializer.hpp>

#include <Runtime/Runtime.h>

#include <fc/variant_object.hpp>
#include <fc/io/json.hpp>
#include <fc/crypto/sha256.hpp>

#include <openssl/rsa.h>
#include <openssl/pem.h>
#include <openssl/bio.h>
#include <openssl/x509.h>

#include <boost/algorithm/string/predicate.hpp>

#include <contracts.hpp>

#ifdef NON_VALIDATING_TEST
#define TESTER tester
#else
#define TESTER validating_tester
#endif

using namespace eosio;
using namespace eosio::chain;
using namespace eosio::testing;
using namespace fc;

using mvo = fc::mutable_variant_object;
using bytes = std::vector<char>;

using RSA_ptr = std::unique_ptr<RSA, decltype(&::RSA_free)>;
using BIO_ptr = std::unique_ptr<BIO, decltype(&::BIO_free)>;
using EVP_PKEY_ptr = std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)>;

class rsa_tester : public TESTER {
public:
    void deploy_contract() {
        create_accounts( {N(rsa)} );
        set_code( N(rsa), contracts::rsa_wasm() );
        set_abi( N(rsa), contracts::rsa_abi().data() );
    }

    RSA_ptr new_keys() {
        static bool init = true;
        if( init ) {
            ERR_load_crypto_strings();
            init = false;
        }

        auto rsa = RSA_ptr(RSA_generate_key(2048, 65537, NULL, NULL), ::RSA_free);
        return rsa;
    }

    std::string sign(const RSA_ptr& rsa, const sha256& digest) {
        bytes signature;
        signature.resize(RSA_size(rsa.get()));
        uint32_t len;
        RSA_sign(NID_sha256, (uint8_t*)digest.data(), 32,
                 (unsigned char*)signature.data(), &len, rsa.get());
        return fc::base64_encode(signature.data(), signature.size());
    }

    std::string pem_pubkey(const RSA_ptr& rsa) {
        auto pkey = EVP_PKEY_ptr(EVP_PKEY_new(), ::EVP_PKEY_free);
        EVP_PKEY_set1_RSA(pkey.get(), rsa.get());

        auto mem = BIO_ptr(BIO_new(BIO_s_mem()), ::BIO_free);
        PEM_write_bio_PUBKEY(mem.get(), pkey.get());

        BUF_MEM *bio_buf;
        BIO_get_mem_ptr(mem.get(), &bio_buf);

        std::stringstream ss({bio_buf->data, bio_buf->length});
        std::string pem;

        while (ss) {
            std::string tmp;
            std::getline(ss, tmp);
            if (tmp[0] != '-') {
                pem += tmp;
            }
        }

        return pem;
    }
};

BOOST_AUTO_TEST_SUITE(rsa_tests)

BOOST_FIXTURE_TEST_CASE( success, rsa_tester ) {
    deploy_contract();

    std::string message = "I'm message";
    fc::sha256 digest = fc::sha256::hash(message);
    auto keys = new_keys();
    auto signature = sign(keys, digest);

    push_action(N(rsa), N(verify), N(rsa), mvo()
        ("digest", digest)
        ("sig", signature)
        ("pubkey", pem_pubkey(keys))
    );
}

BOOST_FIXTURE_TEST_CASE( invalid_signature, rsa_tester ) {
    deploy_contract();

    std::string message = "I'm new message";
    fc::sha256 digest = fc::sha256::hash(message);
    auto keys = new_keys();
    auto keys2 = new_keys();
    auto signature = sign(keys2, digest); // <- signature by another key

    BOOST_CHECK_THROW(push_action(N(rsa), N(verify), N(rsa), mvo()
        ("digest", digest)
        ("sig", signature)
        ("pubkey", pem_pubkey(keys))
    ), fc::exception);
}

BOOST_FIXTURE_TEST_CASE( invalid_key, rsa_tester ) {
    deploy_contract();

    std::string message = "I'm new message";
    fc::sha256 digest = fc::sha256::hash(message);
    auto keys = new_keys();
    auto keys2 = new_keys();
    auto signature = sign(keys, digest);
    auto pem = pem_pubkey(keys2); // <- another key

    BOOST_CHECK_THROW(push_action(N(rsa), N(verify), N(rsa), mvo()
        ("digest", digest)
        ("sig", signature)
        ("pubkey", pem)
    ), fc::exception);
}

BOOST_FIXTURE_TEST_CASE( invalid_digest, rsa_tester ) {
    deploy_contract();

    std::string message = "I'm new message";
    std::string message2 = "I'm another new message";

    fc::sha256 digest = fc::sha256::hash(message);
    fc::sha256 digest2 = fc::sha256::hash(message2); // <- digest from another message

    auto keys = new_keys();
    auto signature = sign(keys, digest);

    BOOST_CHECK_THROW(push_action(N(rsa), N(verify), N(rsa), mvo()
        ("digest", digest2)
        ("sig", signature)
        ("pubkey", pem_pubkey(keys))
    ), fc::exception);
}

BOOST_FIXTURE_TEST_CASE( long_message, rsa_tester ) {
    deploy_contract();

    std::string message;
    for (auto i = 0; i < 1024; ++i) { // <- generate long message
        message += "A";
    }

    auto digest = fc::sha256::hash(message);
    auto keys = new_keys();
    auto signature = sign(keys, digest);

    push_action(N(rsa), N(verify), N(rsa), mvo()
        ("digest", digest)
        ("sig", signature)
        ("pubkey", pem_pubkey(keys))
    );
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include <boost/test/unit_test.hpp>
#include <explore_internal.h>
#include <rand_state.h>
#include "test_common.h"

BOOST_AUTO_TEST_CASE(reproduce_max_boundary_issue)
{
  uint64_t seed = 58587211;
  const uint64_t new_random_seed = uniform_hash(&seed, sizeof(seed), seed);
  BOOST_CHECK_EQUAL(new_random_seed, 2244123448);

  float random_draw = exploration::uniform_random_merand48(new_random_seed);
  BOOST_CHECK_CLOSE(random_draw, 0.99999f, 0.001f);

  const float range_max = 7190.0f;
  const float range_min = -121830.0f;
  const float interval_size = (range_max - range_min) / (32);
  float chosen_value = interval_size * (random_draw + 31) + range_min;
  BOOST_CHECK_CLOSE(chosen_value, range_max, 0.000000001f);
}

BOOST_AUTO_TEST_CASE(check_rand_state_cross_platform)
{
  rand_state random_state;
  random_state.set_random_state(10);
  BOOST_CHECK_CLOSE(random_state.get_and_update_random(), 0.0170166492, FLOAT_TOL);
  BOOST_CHECK_CLOSE(random_state.get_and_update_random(), 0.370730162, FLOAT_TOL);
  BOOST_CHECK_CLOSE(random_state.get_and_update_random(), 0.166691661, FLOAT_TOL);
  BOOST_CHECK_CLOSE(random_state.get_and_update_random(), 0.362691283, FLOAT_TOL);
  BOOST_CHECK_CLOSE(random_state.get_and_update_random(), 0.969445825, FLOAT_TOL);
}


// Copyright 2018 Damian Hofmann - 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_SPARSE_HAMILTONIAN_OPERATOR_HH
#define NETKET_SPARSE_HAMILTONIAN_OPERATOR_HH

#include <Eigen/SparseCore>

#include "abstract_matrix_wrapper.hpp"
#include "Hilbert/hilbert_index.hpp"

namespace netket
{

/**
 * This class stores the matrix elements of a given Operator (AbstractHamiltonian
 * or AbstractObservable) as an Eigen dense matrix.
 */
template<class Operator, class WfType = Eigen::VectorXcd>
class SparseMatrixWrapper : public AbstractMatrixWrapper<Operator, WfType>
{
    using Matrix = Eigen::SparseMatrix<std::complex<double>>;

    Matrix matrix_;
    int dim_;

public:
    explicit SparseMatrixWrapper(const Operator& the_operator)
    {
        InitializeMatrix(the_operator);
    }

    WfType Apply(const WfType& state) const override
    {
        return matrix_ * state;
    }

    int GetDimension() const override
    {
        return dim_;
    }

    const Matrix& GetMatrix() const
    {
        return matrix_;
    }

    /**
     * Computes the eigendecomposition of the given matrix.
     * @param options The options are passed directly to the constructor of SelfAdjointEigenSolver.
     * @return An instance of Eigen::SelfAdjointEigenSolver initialized with the wrapped operator and options.
     */
    Eigen::SelfAdjointEigenSolver<Matrix> ComputeEigendecomposition(int options = Eigen::ComputeEigenvectors) const
    {
        return Eigen::SelfAdjointEigenSolver<Matrix>(matrix_, options);
    }

private:
    void InitializeMatrix(const Operator &the_operator)
    {
        const auto& hilbert = the_operator.GetHilbert();
        const HilbertIndex hilbert_index(hilbert);
        dim_ = hilbert_index.NStates();

        matrix_.resize(dim_, dim_);
        matrix_.setZero();

        for(int i = 0; i < dim_; ++i)
        {
            auto v = hilbert_index.NumberToState(i);

            std::vector<std::complex<double>> matrix_elements;
            std::vector<std::vector<int>> connectors;
            std::vector<std::vector<double>> newconfs;
            the_operator.FindConn(v, matrix_elements, connectors, newconfs);

            for(size_t k = 0; k < connectors.size(); ++k)
            {
                auto vk = v;
                hilbert.UpdateConf(vk, connectors[k], newconfs[k]);
                auto j = hilbert_index.StateToNumber(vk);
                matrix_.insert(i, j) = matrix_elements[k];
            }
        }
    }
};

}

#endif //NETKET_SPARSE_HAMILTONIAN_OPERATOR_HH


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Sqrt_2 generic tag

     Represents the Sqrt_2 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Sqrt_2, double, 1
                                , 0x3FB504F3, 0x3ff6A09E667F3BCCULL
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Sqrt_2, Site> dispatching_Sqrt_2(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Sqrt_2, Site>();
   }
   template<class... Args>
   struct impl_Sqrt_2;
  }
  /*!
    Generates value \f$\sqrt2\f$

    @par Semantic:

    @code
    T r = Sqrt_2<T>();
    @endcode

    is similar to:

    @code
    T r = T(sqrt(as_floating<T>(2)));
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Sqrt_2, Sqrt_2)
} }

#include <boost/simd/constant/common.hpp>

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/splitfactor.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check splitfactor behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::splitfactor;
  using boost::simd::Splitfactor;

  STF_TYPE_IS(decltype(Splitfactor<T>()), T);
  STF_EQUAL(Splitfactor<T>(), T(0));
  STF_EQUAL(splitfactor( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check splitfactor behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::splitfactor;
  using boost::simd::Splitfactor;

  STF_TYPE_IS(decltype(Splitfactor<T>()), T);
  STF_IEEE_EQUAL(Splitfactor<T>(), T(1 << 13));
  STF_IEEE_EQUAL(splitfactor( as(T{}) ), T(1 << 13));
}
STF_CASE_TPL( "Check splitfactor behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::splitfactor;
  using boost::simd::Splitfactor;

  STF_TYPE_IS(decltype(Splitfactor<T>()), T);
  STF_IEEE_EQUAL(Splitfactor<T>(), T(1 << 27));
  STF_IEEE_EQUAL(splitfactor( as(T{}) ), T(1 << 27));
}


/*  $Id: bullcutter.cpp 667 2011-02-14 22:13:45Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "bullcutter.h"
#include "numeric.h"
#include "ellipse.h"

namespace ocl
{

BullCutter::BullCutter() {
    std::cout << " usage: BullCutter( double diameter, double corner_radius, double length ) \n";
    assert(0);
}

BullCutter::BullCutter(double d, double r, double l) {
    diameter = d;               assert( d > 0.0 );
    radius = d/2.0;        // total cutter radius
    radius1 = d/2.0 - r;   // cylindrical middle part radius
    radius2 = r;                assert( radius1 > 0.0 ); // corner radius
    length = l;                 assert( l > 0.0 );
    xy_normal_length = radius1;
    normal_length = radius2;
    center_height = radius2;
}

MillingCutter* BullCutter::offsetCutter(double d) const {
    return new BullCutter(diameter+2*d, radius2+d, length+d) ;
}

// height of cutter at radius r
double BullCutter::height(double r) const {
    if ( r <= radius1 )
        return 0.0; // cylinder
    else if ( r <= radius )
        return radius2 - sqrt( square(radius2) - square(r-radius1) ); // toroid
    else {
        assert(0);
        return -1;
    }
}

// width of cutter at height h
double BullCutter::width(double h) const {
    return ( h >= radius2 ) ? radius : radius1 + sqrt(square(radius2)-square(radius2-h)) ;
}

// drop-cutter: vertex and facet are handled in base-class

// drop-cutter: Toroidal cutter edge-test
CC_CLZ_Pair BullCutter::singleEdgeDropCanonical( const Point& u1, const Point& u2 ) const {
    if ( isZero_tol( u1.z - u2.z ) ) {  // horizontal edge special case
        return CC_CLZ_Pair( 0 , u1.z - height(u1.y) );
    } else { // the general offset-ellipse case
        double b_axis = radius2;                            // short axis of ellipse = radius2
        double theta = atan( (u2.z - u1.z) / (u2.x-u1.x) ); // theta is the slope of the line
        double a_axis = fabs( radius2/sin(theta) );         // long axis of ellipse = radius2/sin(theta)
        Point ellcenter(0,u1.y,0);
        Ellipse e = Ellipse( ellcenter, a_axis, b_axis, radius1);
        int iters = e.solver_brent();
        assert( iters < 200 );
        e.setEllipsePositionHi(u1,u2); // this selects either EllipsePosition1 or EllipsePosition2 and sets it to EllipsePosition_hi
        // pseudo cc-point on the ellipse/cylinder, in the CL=origo system
        Point ell_ccp = e.ePointHi();         assert( fabs( ell_ccp.xyNorm() - radius1 ) < 1E-5); // ell_ccp should be on the cylinder-circle
        Point cc_tmp_u = ell_ccp.closestPoint(u1,u2); // find real cc-point
        return CC_CLZ_Pair( cc_tmp_u.x , e.getCenterZ()-radius2);
    }
}

// push-cutter: vertex and facet handled by base-class

bool BullCutter::generalEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    //std::cout << " BullCutter::generalEdgePush() \n";
    bool result = false;

    if ( isZero_tol( (p2-p1).xyNorm() ) ) { // this would be a vertical edge
        return result;
    }

    if ( isZero_tol( p2.z-p1.z ) ) // this would be a horizontal edge
        return result;
    assert( fabs(p2.z-p1.z) > 0.0 ); // no horiz edges allowed hereafter

    // p1+t*(p2-p1) = f.p1.z+radius2   =>
    double tplane = (f.p1.z + radius2 - p1.z ) / (p2.z-p1.z); // intersect edge with plane at z = ufp1.z
    Point ell_center = p1+tplane*(p2-p1);
    assert( isZero_tol( fabs(ell_center.z - (f.p1.z+radius2)) ) );
    Point major_dir = (p2-p1);
    assert( major_dir.xyNorm() > 0.0 );

    major_dir.z = 0;
    major_dir.xyNormalize();
    Point minor_dir = major_dir.xyPerp();
    double theta = atan( (p2.z - p1.z) / (p2-p1).xyNorm() );
    double major_length = fabs( radius2/sin(theta) ) ;
    double minor_length = radius2;
    AlignedEllipse e(ell_center, major_length, minor_length, radius1,  major_dir, minor_dir );
    if ( e.aligned_solver( f ) ) { // now we want the offset-ellipse point to lie on the fiber
        Point pseudo_cc  = e.ePoint1(); // pseudo cc-point on ellipse and cylinder
        Point pseudo_cc2 = e.ePoint2();
        CCPoint cc  = pseudo_cc.closestPoint(p1,p2);
        CCPoint cc2 = pseudo_cc2.closestPoint(p1,p2);
        cc.type  = EDGE_POS;
        cc2.type = EDGE_POS;
        Point cl  = e.oePoint1() - Point(0,0,center_height);
        assert( isZero_tol( fabs(cl.z - f.p1.z)) );
        Point cl2 = e.oePoint2() - Point(0,0,center_height);
        assert( isZero_tol( fabs(cl2.z - f.p1.z)) );
        double cl_t  = f.tval(cl);
        double cl_t2 = f.tval(cl2);
        if ( i.update_ifCCinEdgeAndTrue( cl_t, cc, p1, p2, true ) )
            result = true;
        if ( i.update_ifCCinEdgeAndTrue( cl_t2, cc2, p1, p2, true ) )
            result = true;
    }
    //std::cout << " BullCutter::generalEdgePush() DONE result= " << result << "\n";
    return result;
}

std::string BullCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, BullCutter c) {
  stream << "BullCutter(d=" << c.diameter << ", r1=" << c.radius1 << " r2=" << c.radius2 << ", L=" << c.length <<  ")";
  return stream;
}

} // end namespace
// end file bullcutter.cpp


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

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

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

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

 /*!
  @ingroup group-trigonometry
    Function object implementing reallog capabilities

    Computes reallog value of its parameter.

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

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

#endif


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <vinecopulib/bicop/fit_controls.hpp>
#include <boost/property_tree/ptree.hpp>

namespace vinecopulib {

// forward declaration of Abstract class
class AbstractBicop;
using BicopPtr = std::shared_ptr<AbstractBicop>;

//! @brief A class for bivariate copula models.
//!
//! The copula model is fully characterized by the family, rotation,
//! and parameters.
class Bicop
{

public:
    // Constructors
    Bicop(const BicopFamily family = BicopFamily::indep,
          const int rotation = 0,
          const Eigen::MatrixXd &parameters = Eigen::MatrixXd());

    Bicop(const Eigen::Matrix<double, Eigen::Dynamic, 2>& data,
          const FitControlsBicop &controls = FitControlsBicop());

    Bicop(const char *filename);

    Bicop(const boost::property_tree::ptree input);

    // Serialize
    boost::property_tree::ptree to_ptree() const;

    void to_json(const char *filename) const;

    // Getters and setters
    BicopFamily get_family() const;

    std::string get_family_name() const;

    int get_rotation() const;

    Eigen::MatrixXd get_parameters() const;

    double get_tau() const;

    double get_loglik() const;
    size_t get_nobs() const;
    double get_aic() const;
    double get_bic() const;
    double get_mbic(const double psi0) const;

    void set_rotation(const int rotation);

    void set_parameters(const Eigen::MatrixXd &parameters);

    // Stats methods
    Eigen::VectorXd
    pdf(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::VectorXd
    cdf(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::VectorXd
    hfunc1(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::VectorXd
    hfunc2(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::VectorXd
    hinv1(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::VectorXd
    hinv2(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    Eigen::Matrix<double, Eigen::Dynamic, 2>
    simulate(const size_t &n,
             const bool qrng = false,
             const std::vector<int>& seeds = std::vector<int>()) const;

    // Methods modifying the family/rotation/parameters
    void fit(const Eigen::Matrix<double, Eigen::Dynamic, 2> &data,
             const FitControlsBicop &controls = FitControlsBicop());

    void select(const Eigen::Matrix<double, Eigen::Dynamic, 2>& data,
                FitControlsBicop controls = FitControlsBicop());

    // Fit statistics
    double loglik(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u =
            Eigen::Matrix<double, Eigen::Dynamic, 2>()) const;

    double aic(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u =
            Eigen::Matrix<double, Eigen::Dynamic, 2>()) const;

    double bic(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u =
            Eigen::Matrix<double, Eigen::Dynamic, 2>()) const;

    double mbic(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u =
            Eigen::Matrix<double, Eigen::Dynamic, 2>(),
        const double psi0 = 0.9) const;

    // Misc
    std::string str() const;

    double calculate_npars() const;

    double parameters_to_tau(const Eigen::MatrixXd &parameters) const;

    Eigen::MatrixXd tau_to_parameters(const double &tau) const;

    void flip();

    Eigen::MatrixXd get_parameters_lower_bounds() const;

    Eigen::MatrixXd get_parameters_upper_bounds() const;

private:

    Eigen::Matrix<double, Eigen::Dynamic, 2> cut_and_rotate(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) const;

    void check_rotation(int rotation) const;

    void check_weights_size(const Eigen::VectorXd& weights,
                            const Eigen::MatrixXd& data) const;

    void check_fitted() const;

    double compute_mbic_penalty(const size_t nobs, const double psi0) const;

    BicopPtr get_bicop() const;

    BicopPtr bicop_;
    int rotation_;
    size_t nobs_;
};
}

#include <vinecopulib/bicop/implementation/class.ipp>