#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <exception>
#include <vector>
#include <time.h>
#include <utility>
#include <limits>
#include <tuple>
#include <list>

#include <boost/math/special_functions/prime.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

//typedegs for graph and edges
typedef adjacency_list < listS, vecS, undirectedS,no_property,
	property < edge_weight_t, int > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef std::pair<int, int> Edge;

//edge with weight
typedef std::tuple<int,int,int> Edge_wW;

// Graph Definitions
//typedef boost::adjacency_list<boost::vecS,boost::vecS,boost::directedS> Graph;
typedef boost::graph_traits<graph_t>::vertex_descriptor Vertex;
typedef boost::graph_traits<graph_t>::edge_descriptor EdgeDes;

// Visitor that throw an exception when finishing the destination vertex
class my_visitor : boost::default_bfs_visitor{
protected:
  Vertex destination_vertex_m;
public:
  my_visitor(Vertex destination_vertex_l)
    : destination_vertex_m(destination_vertex_l) {};

  void initialize_vertex(const Vertex &s, const graph_t &g) const {}
  void discover_vertex(const Vertex &s, const graph_t &g) const {}
  void examine_vertex(const Vertex &s, const graph_t &g) const {}
  void examine_edge(const EdgeDes &e, const graph_t &g) const {}
  void edge_relaxed(const EdgeDes &e, const graph_t &g) const {}
  void edge_not_relaxed(const EdgeDes &e, const graph_t &g) const {}
  void finish_vertex(const Vertex &s, const graph_t &g) const {
    if (destination_vertex_m == s)
      throw(2);
  }
};

//find the prime vertexes
int findPrimes(std::vector<int> &primes, int vertNumb){

		std::ifstream file;
		file.open("primes1.txt");
		int searchingPrimes=true;

		int primesNumb=0;

		std::string str;
		getline(file, str,'\r');

	while(searchingPrimes){

		int numsInLine=8;
		getline(file, str,'\r');

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

			int nextPrime = std::stoi(str.substr(10*i,10+10*i));

			if(nextPrime<=vertNumb){
				primes.push_back(nextPrime);
				primesNumb++;
			}
			else{
				searchingPrimes=false;
				break;
			}

		}
		getline(file, str,'\r');

	}
	file.close();

	return primesNumb;
}

// fills the edge array with all the edges from the node array
void createEdgeArray(std::vector<Edge>* nodes,Edge* edge_array,int* weights, int vertNumb ){

	int count=0;
	for(int i=0;i<vertNumb;i++){
		int size=nodes[i].size();
		for(int j=0;j<size;j++){
			Edge edg=nodes[i].at(j);

			edge_array[count]=Edge(i,edg.first);
			weights[count]=edg.second;
			count++;
		}
	}
}

// reads in and initialize the edges
void initializeEdges(std::vector<Edge>* nodes,std::vector<int> primes, int vertNumb,int arcNumb,int primeNumb,std::ifstream &file){

	try {

		int count=0;

		//edges from the source node to 2 with weithg 0
		nodes[0].push_back(Edge(2,0));
		count++;

		std::string str;

		while (count<arcNumb-primeNumb+1) {

			getline(file, str, ' ');
			int from = std::stoi(str);

			getline(file, str, ' ');
			int to = std::stoi(str);

			getline(file,str);
			int dist=std::stoi(str);

			if(from>to){
				int help=from;
				from = to;
				to = help;
			}

			nodes[from].push_back(Edge(to,dist));

			count++;
		}

		//Edges from all primes except 2 to the sink
		for(int i=1;i<primeNumb;i++){

			int prime=primes.at(i);
			nodes[prime].push_back(Edge(vertNumb-1,0));
		}
	}
	catch (...) {
	}
}

// removes an Edge out of nodes from vertex to parent
void removeEdge(std::vector<Edge>* nodes, int vertex, int parent){

	if(parent<vertex){
		int help=vertex;
		vertex=parent;
		parent=help;
	}

	std::vector<Edge>::iterator it=nodes[vertex].begin();

	for(it;it!=nodes[vertex].end();it++){
		if((*it).first==parent){
			nodes[vertex].erase(it);
			break;
		}
	}
}

// uses boost dijkstra and returns the distance from the graph to the new prime
int dijkstra(graph_t &g,int vertNumb, bool* steinTree, std::vector<Edge>* nodes){

	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	std::vector<vertex_descriptor> p(num_vertices(g));
	std::vector<int> d(num_vertices(g));
	vertex_descriptor s = vertex(0, g);
	my_visitor vis(vertNumb-1);

   	//clock_t tStartd1 = clock();

   	try{
		dijkstra_shortest_paths(g, s,
	                          predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
	                          distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))).
	                          visitor(vis));
	}
	catch(...){
	}

	//std::cout <<"INFORM TIME DIJKSTRA: "<< (double)(clock() - tStartd1) / CLOCKS_PER_SEC << " seconds" << std::endl;

	///////////// remove edges
	//clock_t rem=clock();
	int vertex=vertNumb-1;
	int parent=p[vertex];

	remove_edge(vertex,parent,g);
	removeEdge(nodes,vertex,parent);

	while(!steinTree[parent]){

	 		steinTree[parent]=true;

	 		// create a new Edge from source node to parent node with weight 0
	 		add_edge(0,parent,0,g);

	 		//removes the Edge from vertex to parent
	 		remove_edge(vertex,parent,g);
	 		removeEdge(nodes,vertex,parent);

	 		vertex=parent;
	 		parent=p[parent];
	}

	remove_edge(vertex,parent,g);
	removeEdge(nodes,vertex,parent);

	//std::cout <<"INFORM TIME REMOVE EDGES: "<< (double)(clock() - rem) / CLOCKS_PER_SEC << " seconds" << std::endl;
	//std::cout<< "RESULT Parent  "<< p[vertNumb-1]<<std::endl;

	return d[vertNumb-1];
}

// TODO is not working
void simplifyGraph(graph_t &g,std::vector<Edge>* nodes,bool* steinTree,int vertNumb){
	bool hasEdge[vertNumb]={false};

	for(int i = 1;i<vertNumb;i++){

		std::vector<Edge>::iterator it=nodes[i].begin();
		if(steinTree[i]){

			for(it;it!=nodes[i].end();it++){

				std::cout<<i<<std::endl;
				std::cout<<((*it).first)<<std::endl;  //This gives out some strange values
				std::cout<<(nodes[i].size())<<std::endl;
				if(steinTree[(*it).first]){		//segmentation fault in this line

					std::cout<<"lalala"<<std::endl;
					remove_edge(i,(*it).first,g);
					it=nodes[i].erase(it);

				}
				else{
					std::cout<<"lululu"<<std::endl;
					hasEdge[(*it).first]=true;
				}
				std::cout<<"hehehehe"<<std::endl;
			}
		}
		else{

			std::cout<<"huhuhuhu"<<std::endl;
			for(it;it!=nodes[i].end();it++){
				hasEdge[(*it).first]=true;
			}
		}

		if(nodes[i].size()==0 && !hasEdge){
			std::cout<<"weweweew"<<std::endl;
			remove_vertex(i,g);
		}
	}
}

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

	clock_t c=clock();
	//////// read input arguments

	std::ifstream file;
	file.open(argv[1]);

	if (!file.is_open()) {
			std::cerr << "there is no file of the name \"" << argv[1] << "\" in the directory" << std::endl;
			return 0;
	}
	////////

	// get the Number of vertexes
  	std::string str;
	getline(file, str, ' ');
	int vertNumb = std::stoi(str)+2;

	// get the prime nodes
	std::vector<int> primes;
	int primeNumb=findPrimes(primes,vertNumb-2);

	//vector for the Edges to the nodes, first node is source last node is sink( this is needed for simplify graph)
	std::vector<Edge> nodes[vertNumb];

	// get the Number of arcs
	getline(file, str);
	int arcNumb=std::stoi(str)+primeNumb;

	//the vertexes in the steiner tree
	bool steinTree[vertNumb]={false};
	steinTree[0]=true;
	steinTree[2]=true;

	//////// create the graph
	initializeEdges(nodes, primes, vertNumb, arcNumb, primeNumb,file);

	Edge *edge_array;
	edge_array = (Edge*)malloc((arcNumb)* sizeof(Edge));

	int *weights;
	weights = (int*)malloc((arcNumb)* sizeof(int));

	createEdgeArray(nodes,edge_array,weights,vertNumb);

	graph_t g(edge_array, edge_array + arcNumb, weights, vertNumb);
	////////

	//clock_t cl[primeNumb];
	//cl[0]=clock();

	//weight of steinertree
	long long weightST=0;

	for(int i =1; i<primeNumb;i++){

		weightST= weightST+dijkstra(g, vertNumb, steinTree,nodes);

	/*
	 	if(i%100==0){
	 		simplifyGraph(g,nodes,steinTree,vertNumb);
	 	}

	*/

		//cl[i]=clock();
		//std::cout <<"INFORM TIME STEP: "<<  i<<"   "<< (double)(cl[i] - cl[i-1]) / CLOCKS_PER_SEC << " seconds" << std::endl;

	}

		clock_t tStart5 = clock();
		std::cout <<"INFORM WEIGHT OF STEINER TREE: "<< weightST << std::endl;
		std::cout <<"INFORM TIME: "<< (double)(tStart5 - c) / CLOCKS_PER_SEC << " seconds" << std::endl;

 	return 0;
}


/*
@copyright Louis Dionne 2015
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_HANA_TEST_LAWS_SEARCHABLE_HPP
#define BOOST_HANA_TEST_LAWS_SEARCHABLE_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/searchable.hpp>

#include <laws/base.hpp>
#include <test/numeric.hpp>

namespace boost { namespace hana { namespace test {
    template <typename S, typename = when<true>>
    struct TestSearchable : TestSearchable<S, laws> {
        using TestSearchable<S, laws>::TestSearchable;
    };

    template <typename S>
    struct TestSearchable<S, laws> {
        static_assert(_models<Searchable, S>{}, "");

        template <typename Searchables, typename Keys>
        TestSearchable(Searchables searchables, Keys keys) {
            auto check = [](auto p, auto key, auto xs, auto ys) {
                // any_of(xs, p) <=> !all_of(xs, negated p)
                //               <=> !none_of(xs, p)
                BOOST_HANA_CHECK(
                    hana::any_of(xs, p) ^iff^
                    hana::not_(hana::all_of(xs, hana::compose(not_, p)))
                );

                BOOST_HANA_CHECK(
                    hana::any_of(xs, p) ^iff^
                    hana::not_(hana::none_of(xs, p))
                );

                // any(xs)  <=> any_of(xs, id)
                // all(xs)  <=> all_of(xs, id)
                // none(xs) <=> none_of(xs, id)
                only_when_(hana::all_of(xs, models<Logical>), [=](auto _) {
                    auto xs_ = _(xs);

                    BOOST_HANA_CHECK(
                        hana::any(xs_) ^iff^ hana::any_of(xs_, id)
                    );

                    BOOST_HANA_CHECK(
                        hana::all(xs_) ^iff^ hana::all_of(xs_, id)
                    );

                    BOOST_HANA_CHECK(
                        hana::none(xs_) ^iff^ hana::none_of(xs_, id)
                    );
                });

                // find_if(xs, always(false_)) == nothing
                BOOST_HANA_CONSTANT_CHECK(equal(
                    hana::find_if(xs, hana::always(false_)),
                    nothing
                ));

                // subset(xs, ys) <=> all_of(xs, [](auto x) { return elem(ys, x); })
                BOOST_HANA_CHECK(
                    hana::subset(xs, ys) ^iff^ hana::all_of(xs, [=](auto x) {
                        return elem(ys, x);
                    })
                );

                // find(xs, x) == find_if(xs, [](auto y) { return y == x; })
                BOOST_HANA_CHECK(hana::equal(
                    hana::find(xs, key),
                    hana::find_if(xs, equal.to(key))
                ));

                // elem(xs, x) <=> any_of(xs, [](auto y) { return y == x; })
                BOOST_HANA_CHECK(
                    hana::elem(xs, key) ^iff^
                    hana::any_of(xs, equal.to(key))
                );

                return 1;
            };

            auto predicates = hana::concat(
                hana::to<hana::Tuple>(hana::transform(keys, equal.to)),
                hana::make_tuple(
                    hana::always(false_),
                    hana::always(true_)
                )
            );
            hana::for_each(predicates, [=](auto pred) {
            hana::for_each(keys, [=](auto key) {
            hana::for_each(searchables, [=](auto xs) {
            hana::for_each(searchables, [=](auto ys) {
                check(pred, key, xs, ys);
            });});});});
        }
    };

    template <typename S>
    struct TestSearchable<S, when<_models<Sequence, S>{}>> : TestSearchable<S, laws> {
        template <int i>
        using x = _constant<i>;

        template <int = 0>
        struct invalid { };

        template <typename Xs, typename Keys>
        TestSearchable(Xs xs, Keys keys) : TestSearchable<S, laws>{xs, keys} {
            constexpr auto list = make<S>;

            BOOST_HANA_CONSTEXPR_LAMBDA auto is_even = [](auto x) {
                return x % 2 == 0;
            };

            auto c = numeric; // a minimal comparable
            auto logical = numeric;

            //////////////////////////////////////////////////////////////////
            // any_of
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::any_of(list(), equal.to(x<9>{})))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::any_of(list(x<0>{}), equal.to(x<9>{})))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}), equal.to(x<0>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, invalid<1>{}), equal.to(x<0>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, invalid<1>{}, invalid<2>{}), equal.to(x<0>{}))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::any_of(list(x<0>{}, x<1>{}), equal.to(x<9>{})))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}), equal.to(x<1>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}, invalid<2>{}), equal.to(x<1>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}, invalid<2>{}, invalid<3>{}), equal.to(x<1>{}))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::any_of(list(x<0>{}, x<1>{}, x<2>{}), equal.to(x<9>{})))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}, x<2>{}), equal.to(x<2>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}, x<2>{}, nothing), equal.to(x<2>{}))
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::any_of(list(x<0>{}, x<1>{}, x<2>{}, nothing, nothing), equal.to(x<2>{}))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::any_of(list(), invalid<>{}))
            );

            BOOST_HANA_CONSTEXPR_CHECK(
                hana::any_of(list(c(0)), equal.to(c(0)))
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::not_(hana::any_of(list(c(0)), equal.to(c(1))))
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::not_(hana::any_of(list(1), is_even))
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::any_of(list(2), is_even)
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::any_of(list(1, 2), is_even)
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::not_(hana::any_of(list(1, 3), is_even))
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                hana::any_of(list(1, 3, 4), is_even)
            );

            //////////////////////////////////////////////////////////////////
            // any
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::any(list())));
            BOOST_HANA_CONSTEXPR_CHECK(hana::any(list(logical(true))));
            BOOST_HANA_CONSTEXPR_CHECK(hana::not_(hana::any(list(logical(false)))));

            //////////////////////////////////////////////////////////////////
            // all_of
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(
                all_of(list(), invalid<>{})
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                all_of(list(c(0)), equal.to(c(0)))
            );
            BOOST_HANA_CONSTEXPR_CHECK(
                not_(all_of(list(c(0)), equal.to(c(1))))
            );
            BOOST_HANA_CONSTEXPR_CHECK(not_(
                all_of(list(c(0), c(1)), equal.to(c(0)))
            ));
            BOOST_HANA_CONSTEXPR_CHECK(
                all_of(list(c(0), c(0)), equal.to(c(0)))
            );

            BOOST_HANA_CONSTEXPR_CHECK(not_(all_of(list(1), is_even)));
            BOOST_HANA_CONSTEXPR_CHECK(all_of(list(2), is_even));
            BOOST_HANA_CONSTEXPR_CHECK(all_of(list(2, 4), is_even));
            BOOST_HANA_CONSTEXPR_CHECK(not_(all_of(list(1, 2), is_even)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(all_of(list(1, 3), is_even)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(all_of(list(1, 3, 4), is_even)));

            //////////////////////////////////////////////////////////////////
            // all
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(all(list()));
            BOOST_HANA_CONSTEXPR_CHECK(all(list(logical(true))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(all(list(logical(false)))));
            BOOST_HANA_CONSTEXPR_CHECK(all(list(logical(true), logical(true))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(all(list(logical(true), logical(false)))));

            //////////////////////////////////////////////////////////////////
            // none_of
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(none_of(list(), invalid<>{}));
            BOOST_HANA_CONSTEXPR_CHECK(none_of(list(c(0)), equal.to(c(1))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(none_of(list(c(0)), equal.to(c(0)))));

            BOOST_HANA_CONSTEXPR_CHECK(none_of(list(1), is_even));
            BOOST_HANA_CONSTEXPR_CHECK(not_(none_of(list(2), is_even)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(none_of(list(1, 2), is_even)));
            BOOST_HANA_CONSTEXPR_CHECK(none_of(list(1, 3), is_even));
            BOOST_HANA_CONSTEXPR_CHECK(not_(none_of(list(1, 3, 4), is_even)));

            //////////////////////////////////////////////////////////////////
            // none
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(none(list()));
            BOOST_HANA_CONSTEXPR_CHECK(none(list(logical(false))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(none(list(logical(true)))));

            //////////////////////////////////////////////////////////////////
            // find_if
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(), equal.to(x<9>{})),
                nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}), equal.to(x<9>{})),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}), equal.to(x<0>{})),
                just(x<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, invalid<1>{}), equal.to(x<0>{})),
                just(x<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, invalid<1>{}, invalid<2>{}), equal.to(x<0>{})),
                just(x<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}), equal.to(x<9>{})),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}), equal.to(x<1>{})),
                just(x<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, invalid<2>{}), equal.to(x<1>{})),
                just(x<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, invalid<2>{}, invalid<3>{}), equal.to(x<1>{})),
                just(x<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, x<2>{}), equal.to(x<9>{})),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, x<2>{}), equal.to(x<2>{})),
                just(x<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, x<2>{}, nothing), equal.to(x<2>{})),
                just(x<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find_if(list(x<0>{}, x<1>{}, x<2>{}, nothing, nothing), equal.to(x<2>{})),
                just(x<2>{})
            ));

            //////////////////////////////////////////////////////////////////
            // find
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(), invalid<>{}),
                nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}), x<9>{}),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}), x<0>{}),
                just(x<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, invalid<1>{}), x<0>{}),
                just(x<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, invalid<1>{}, invalid<2>{}), x<0>{}),
                just(x<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}), x<9>{}),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}), x<1>{}),
                just(x<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, invalid<2>{}), x<1>{}),
                just(x<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, invalid<2>{}, invalid<3>{}), x<1>{}),
                just(x<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, x<2>{}), x<9>{}),
                nothing
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, x<2>{}), x<2>{}),
                just(x<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, x<2>{}, nothing), x<2>{}),
                just(x<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(list(x<0>{}, x<1>{}, x<2>{}, nothing, nothing), x<2>{}),
                just(x<2>{})
            ));

            //////////////////////////////////////////////////////////////////
            // elem
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTEXPR_CHECK(elem(list(c(0)), c(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(elem(list(c(0)), c(1))));

            //////////////////////////////////////////////////////////////////
            // in
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTEXPR_CHECK(c(0) ^in^ list(c(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(c(1) ^in^ list(c(0))));

            //////////////////////////////////////////////////////////////////
            // subset
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(subset(
                list(), list(c(1)))
            );

            BOOST_HANA_CONSTEXPR_CHECK(subset(
                list(c(0)), list(c(0))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(subset(
                list(c(0)), list(c(0), c(1))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(not_(subset(
                list(c(0)), list(c(1)))
            ));
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_SEARCHABLE_HPP


#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <tuple>
#include <unordered_map>

#ifndef UTIL_GRAPH_HH_
#define UTIL_GRAPH_HH_

using UGraph = boost::adjacency_list<boost::listS,       //
                                     boost::vecS,        //
                                     boost::undirectedS, //
                                     boost::no_property, //
                                     boost::no_property>;

template <typename TripleVec, typename T>
void
build_boost_graph(const TripleVec &data, UGraph &G, const T cutoff)
{
    using Vertex = boost::graph_traits<UGraph>::vertex_descriptor;
    using Edge = UGraph::edge_descriptor;

    Vertex max_vertex = 0;
    auto find_max_n = [&max_vertex](const auto &tt) {
        if (std::get<0>(tt) > max_vertex)
            max_vertex = std::get<0>(tt);
        if (std::get<1>(tt) > max_vertex)
            max_vertex = std::get<1>(tt);
    };

    std::for_each(data.begin(), data.end(), find_max_n);
    for (Vertex u = boost::num_vertices(G); u <= max_vertex; ++u)
        boost::add_vertex(G);

    auto add_edge = [&G, &cutoff](const auto &tt) {
        if (std::get<2>(tt) <= cutoff)
            boost::add_edge(std::get<0>(tt), std::get<1>(tt), G);
    };

    std::for_each(data.begin(), data.end(), add_edge);
}

#endif


#include <iostream>
#include <ctime>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>

#include <boost/thread/thread.hpp>
#include <boost/thread/thread_time.hpp>

using namespace std;
#define BOOST_DATE_TIME_SOURCE

void time_elapsed_boost()
{
    double sum = 1.0;

    boost::posix_time::ptime start_;
    boost::posix_time::ptime stop_;

    // 获取当前时间
    start_ = boost::posix_time::microsec_clock::local_time();

    for (size_t i = 0; i < 1e7; i++)
        for (size_t i = 0; i < 1e3; i++)
            sum += 2.0/1.5;

    stop_ = boost::posix_time::microsec_clock::local_time();

    // 返回毫秒数
    float elapsed_milliseconds_ = (stop_ - start_).total_milliseconds();

    // 返回微秒数
    float elapsed_microseconds_ = (stop_ - start_).total_microseconds();

    cout << "boost takes " << elapsed_milliseconds_ << " ms"<< endl;
    cout << "boost takes " << elapsed_microseconds_ << " μs"<< endl;

}

#define CLOCKS_PER_MILSEC (CLOCKS_PER_SEC/1000)

void time_elapsed_buildin()
{
    time_t start_;
    time_t stop_;

    double sum = 1.0;

    // 获取当前时间
    start_ = clock();

    for (size_t i = 0; i < 1e7; i++)
        for (size_t i = 0; i < 1e3; i++)
            sum += 2.0/1.5;

    stop_ = clock();

    // 返回毫秒数
    double elapsed_milliseconds_ = (double )(stop_ - start_) / CLOCKS_PER_MILSEC;

    cout << "buildin takes " << elapsed_milliseconds_ << " ms"<< endl;

}

void get_date_time()
{
    std::string str_time = boost::gregorian::to_iso_string(
                                    boost::gregorian::day_clock::local_day());

    std::cout << str_time.c_str() << std::endl;

    str_time = boost::posix_time::to_iso_string(
                                boost::posix_time::second_clock::local_time());

    // str_time 里存放时间的格式是YYYYMMDDTHHMMSS，日期和时间用大写字母T隔开了
    std::cout << str_time.c_str() << std::endl;

    int pos = str_time.find('T');
    str_time.replace(pos, 1, std::string("-"));
    str_time.replace(pos + 3, 0, std::string(":"));
    str_time.replace(pos + 6, 0, std::string(":"));

    std::cout << str_time.c_str() << std::endl;
}

int main(int argc, char const *argv[])
{
    for(int i = 0; i < 20; i++){

        if (i % 2== 0){

            time_elapsed_buildin();
            time_elapsed_boost();
        }
        else {

            time_elapsed_boost();
            time_elapsed_buildin();
        }

        std::cout << std::endl;
        boost::thread::sleep(boost::get_system_time() + boost::posix_time::seconds(1));
    }

    get_date_time();

    return 0;
}


//
// Created by przestaw on 04.12.2020.
//

#ifndef NBC4GPU_GPULEARNCOLUMN_HPP
#define NBC4GPU_GPULEARNCOLUMN_HPP

#include <Defines.h>
#include <error/Execeptions.h>
DIAGNOSTIC_PUSH
#include <boost/compute/algorithm/accumulate.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/core.hpp>
#include <boost/compute/lambda.hpp>
DIAGNOSTIC_POP
#include <mutex>

namespace nbc4gpu {
  template <typename ValueType> class GPULearnColumn {
  public:
    using AvgAndVariance = std::pair<ValueType, ValueType>;

    /**
     * Constructor
     * @param col column of values
     * @param queue device column
     */
    GPULearnColumn(std::vector<ValueType> &col,
                   boost::compute::command_queue &queue);

    /**
     * Calculates or returns average and standard deviation
     * @return average and standard deviation
     */
    AvgAndVariance operator()();

  private:
    AvgAndVariance learn();

    bool resSet_;
    AvgAndVariance result_;
    std::vector<ValueType> &col_;
    boost::compute::command_queue &queue_;
    std::mutex calculationGuard;
  };

  template <typename ValueType>
  GPULearnColumn<ValueType>::GPULearnColumn(
      std::vector<ValueType> &col,
      boost::compute::command_queue &queue)
      : resSet_(false), col_(col), queue_(queue) {}

  template <typename ValueType>
  typename GPULearnColumn<ValueType>::AvgAndVariance
  GPULearnColumn<ValueType>::operator()() {
    if (!resSet_) {
      std::unique_lock lock(calculationGuard);
      if (!resSet_) {
        result_ = learn();
        resSet_ = true;
      }
    }
    return result_;
  }

  template <typename ValueType>
  typename GPULearnColumn<ValueType>::AvgAndVariance
  GPULearnColumn<ValueType>::learn() {
    if (col_.empty()) {
      throw nbc4gpu::error::ZeroValuesProvided(" column summary");
    }
    // transfer the values to the device
    boost::compute::vector<ValueType> avgVector(col_.size(),
                                                queue_.get_context());
    boost::compute::future<void> fAvg = boost::compute::copy_async(
        col_.begin(), col_.end(), avgVector.begin(), queue_);
    // reduce is destructive, need to copy twice
    boost::compute::vector<ValueType> varianceVector(col_.size(),
                                                     queue_.get_context());
    boost::compute::future<void> fStdDev = boost::compute::copy_async(
        col_.begin(), col_.end(), varianceVector.begin(), queue_);

    ValueType avg = 0.0;
    fAvg.wait(); // Wait for data to be copied

    boost::compute::reduce(avgVector.begin(), avgVector.end(), &avg, queue_);
    avg = avg / static_cast<ValueType>(col_.size());

    fStdDev.wait(); // Wait for data to be copied
    boost::compute::transform(varianceVector.begin(),
                              varianceVector.end(),
                              varianceVector.begin(),
                              boost::compute::lambda::_1 - avg,
                              queue_);

    boost::compute::transform(varianceVector.begin(),
                              varianceVector.end(),
                              varianceVector.begin(),
                              boost::compute::lambda::_1
                                  * boost::compute::lambda::_1,
                              queue_);

    ValueType variance = 0.0;
    boost::compute::reduce(
        varianceVector.begin(), varianceVector.end(), &variance, queue_);
    variance = variance * (1 / static_cast<ValueType>(col_.size() - 1));

    return std::make_pair(avg, variance);
  }
} // namespace nbc4gpu

#endif // NBC4GPU_GPULEARNCOLUMN_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

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

#include <iostream>
#include <string>
#include <vector>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>

// To register the 'geographic' distance function to calculate distance over the earth:
#include <boost/geometry/extensions/gis/geographic/strategies/andoyer.hpp>
#include <boost/geometry/extensions/algorithms/parse.hpp>

// Define a GPS point with coordinates in latitude/longitude and some additional values
struct gps_point
{
    double latitude, longitude, height;
    double speed;
    // Date/time, heading, etc could be added

    // The default constructor is required if being used in a vector
    gps_point() {}

    // Define a constructor to create the point in one line. Order of latitude/longitude
    // does not matter as long as "E", "N", etc are included
    gps_point(std::string const& c1, std::string const& c2, double h, double s)
        : height(h)
        , speed(s)
    {
        boost::geometry::parse(*this, c1, c2);
    }
};

// Declare a custom linestring which will have the GPS points
struct gps_track : std::vector<gps_point>
{
    std::string owner;
    int route_identifier;
    // etc

    gps_track(int i, std::string const& o)
        : owner(o)
        , route_identifier(i)
    {}
};

// Register this point as being a recognizable point by Boost.Geometry
BOOST_GEOMETRY_REGISTER_POINT_2D(gps_point, double, cs::geographic<degree>, longitude, latitude)

// Register the track as well, as being a "linestring"
BOOST_GEOMETRY_REGISTER_LINESTRING(gps_track)

int main()
{
    // Declare a "GPS Track" and add some GPS points
    gps_track track(23, "Mister G");
    track.push_back(gps_point("52 22 23 N", "4 53 32 E", 50, 180));
    track.push_back(gps_point("52 10 00 N", "4 59 59 E", 110, 170));
    track.push_back(gps_point("52 5 20 N", "5 6 56 E", 0, 90));

    std::cout
        << "track:  " << track.route_identifier << std::endl
        << "from:   " << track.owner << std::endl
        << "as wkt: " << boost::geometry::dsv(track) << std::endl
        << "length: " << boost::geometry::length(track)/1000.0 << " km" << std::endl;

    // Above gives the idea, shows that custom linestrings can be useful.
    // We could of course do anything with this track which the library can handle, e.g.:
    // - simplify it
    // - calculate distance of point-to-line
    // - project it to UTM, then transform it to a GIF image (see p03_example)

    return 0;
}


//============================================================================
// Name        : minimacro.cpp
// Author      : wilfeli
// Version     :
// Copyright   : Your copyright notice
// Description : Minimacro model in C++, Ansi-style
//============================================================================

#include <iostream>
#include <fstream>
#include <string>
#include <map>
#include <Eigen/Dense>

#include "W.h"
#include "Parameters.h"
#include "H.h"
#include "F.h"
#include "Market.h"

using namespace std;

void
save_history(W& w, int N, std::string file_path){
	//saves history
	//parameters of the simulation

	std::string file_path_default;
	std::string file_name;

    if (file_path.empty()){
        file_path = file_path_default;
    };

	int i = 0;
	//read current number of iterations
	file_name = file_path + "HF_i.txt";
	std::ifstream in_file(file_name);

	std::string I_NUMBER;

    if (in_file){
        std::getline(in_file, I_NUMBER);
    }else{
        I_NUMBER = "0";
    };
	in_file.close();

	std::ofstream out_file(file_name);
	out_file << std::to_string(std::stoi(I_NUMBER) + 1) << std::endl;
    out_file.close();

	file_name = file_path + "HF_param_" + I_NUMBER + ".txt";
	out_file.open(file_name);

	if (out_file){
		//save number of H
		//save number of F
		out_file << "number of humans: "<< w.NH << std::endl;
		out_file << "number of firms: "<< w.NFGC << std::endl;
		out_file << "F wm length: " << w.param->F_wm_LENGTH << std::endl;
		out_file << "H wm length: " << w.param->H_wm_LENGTH << std::endl;
		out_file << "F type: " << w.Fs.back()->param->opt_TYPE << std::endl;
		out_file << "H type: " << w.Hs.back()->param->opt_TYPE << std::endl;
		out_file << "seed: " << w.param->W_SEED << std::endl;
		out_file << "F forecasting length: " << w.param->F_T_MAX << std::endl;
		out_file << "H forecasting length: " << w.param->H_T_MAX << std::endl;
		out_file << "F production function parameters: " << w.param->F_F_F_theta << std::endl;
		out_file << "H utility function parameters: " << w.param->H_GOAL_T_theta << std::endl;
		out_file << "F mRe Phi: " << w.param->F_mRE_PHI << std::endl;
		out_file << "H mRe Phi: " << w.param->H_mRE_PHI << std::endl;
		out_file << "F QL L: " << w.param->F_QL_L << std::endl;
		out_file << "H QL L: " << w.param->H_QL_L << std::endl;
		out_file << "F Grid: " << w.param->F_GRID << std::endl;
		out_file << "H Grid: " << w.param->H_GRID << std::endl;

		i = 0;

		for (auto a:w.Fs){
			if (a->status > 0.0){
				i++;
			};
		};

		out_file << "End number of F:" << i << std::endl;

	};

	out_file.close();

    Eigen::IOFormat CleanFmt(Eigen::StreamPrecision, 0, ", ", "\n");

	file_name = file_path + "HF_data_" + I_NUMBER + ".txt";
	out_file.open(file_name);
	//save prices of the market
	Eigen::MatrixXd data_all(N, 5);
	data_all = Eigen::MatrixXd::Zero(N, 5);
	//employment, sales on goods market, production, price on labor market,
	//price on goods market

	for (auto a:w.Hs){
		data_all.col(0) += Eigen::VectorXd::Map(a->account->asCHK_q.data(),(long)a->account->asCHK_q.size());
		data_all.col(1) += Eigen::VectorXd::Map(a->account->asGC_q.data(),(long)a->account->asGC_q.size());
	};

	for (auto a:w.Fs){
        if (a->status > 0.0){
            data_all.col(2) += Eigen::VectorXd::Map(a->account->production_q.data(),(long)a->account->production_q.size());
        };
	};

	for (int j = 0; j < N; ++j){
		data_all(j,3) = w.ml->market_price[j]->p;
		data_all(j,4) = w.mc->market_price[j]->p;

	};
	if (out_file){
		out_file << data_all.format(CleanFmt);
	};

//	cout << data_all;
	out_file.close();

	file_name = file_path + "HF_data_all_H_" + I_NUMBER + ".txt";
	Eigen::MatrixXd h_all(N, w.NH);
	i = 0;
	for (auto a:w.Hs){
		h_all.col(i) = Eigen::VectorXd::Map(a->account->g_t.data(),(long)a->account->g_t.size());
		i++;

	};

    out_file.open(file_name);
	if (out_file){
		out_file << h_all.format(CleanFmt);;
	};
	out_file.close();

	file_name = file_path + "HF_data_all_F_" + I_NUMBER + ".txt";
	Eigen::MatrixXd f_all(N, w.NFGC);
	i = 0;
	for (auto a:w.Fs){
		f_all.col(i) = Eigen::VectorXd::Map(a->account->profit.data(),(long)a->account->profit.size());
		i++;

	};
    out_file.open(file_name);
	if (out_file){
		out_file << f_all.format(CleanFmt);
	};
	out_file.close();

};

void
read_init(double& seed_, int& N_, std::map <std::string, std::string> &ini, std::string file_name){
	std::string file_path_default;
	std::string file_name_default;

	//open ini file
	file_name_default = "minimacro.ini";

    if (file_name.empty()){
        file_name = file_name_default;
    };

	std::ifstream in_file(file_name);

	std::string s, key, value;

    while (std::getline( in_file, s )){
        // Extract the key value
        std::string::size_type begin = s.find_first_not_of( " \f\t\v" );
        std::string::size_type end = s.find( '=', begin );
        key = s.substr( begin, end - begin );

        // (No leading or trailing whitespace allowed)
        key.erase( key.find_last_not_of( " \f\t\v" ) + 1 );

        // No blank keys allowed
        if (key.empty()) continue;

        // Extract the value (no leading or trailing whitespace allowed)
        begin = s.find_first_not_of( " \f\n\r\t\v", end + 1 );
        end   = s.find_last_not_of(  " \f\n\r\t\v" ) + 1;

        value = s.substr( begin, end - begin );
        ini[key] = value;

//        std::cout << s << " " << key << " " << value << std::endl;
	};
    in_file.close();

    //set ini values
    N_ = std::stoi(ini["N"]);
    seed_ = std::stoi(ini["seed"]);

};

int main(int argc, char *argv[]) {
    //path to ini file
    //argv[1] path to ini file
    //argv[2] path to save directory

    std::string file_ini_name;
    std::string file_path;
    //path to save directory
    if (argc > 2){
        file_path = argv[2];
    };

    if (argc > 1){
        file_ini_name = argv[1];
    };

	//seed
	double seed = 2013;

    //number of steps
    int N = 100;
    std::map <std::string, std::string> simulation_ini;

    read_init(seed, N, simulation_ini, file_ini_name);

	//create world
	W w(seed);

	w.NH = 10;
	w.NFGC = 3;
	w.NCB = 1;

//	w.param->F_F_F_theta << ?, ?;
//	w.param->H_GOAL_T_theta << ?, ?;

    w.param->SIMULATION_MODE = simulation_ini["SIMULATION_MODE"];
    w.param->F_T_MAX = std::stoi(simulation_ini["F_T_MAX"]);
    w.param->H_T_MAX = std::stoi(simulation_ini["H_T_MAX"]);
    w.param->F_opt_CS_N = std::stoi(simulation_ini["F_opt_CS_N"]);
    w.param->H_opt_CS_N = std::stoi(simulation_ini["H_opt_CS_N"]);

    std::string wm_length;
    wm_length = simulation_ini["F_wm_LENGTH"];
    if (wm_length == "inf"){
        w.param->F_wm_LENGTH = std::numeric_limits<double>::infinity();;
    }else{
        w.param->F_wm_LENGTH = std::stod(simulation_ini["F_wm_LENGTH"]);
    };
    wm_length = simulation_ini["H_wm_LENGTH"];
    if (wm_length == "inf"){
        w.param->H_wm_LENGTH = std::numeric_limits<double>::infinity();;
    }else{
        w.param->H_wm_LENGTH = std::stod(simulation_ini["H_wm_LENGTH"]);
    };

    w.param->F_GRID = simulation_ini["F_GRID"];
    w.param->H_GRID = simulation_ini["H_GRID"];

    //decision type
    w.param->F_opt_TYPE = simulation_ini["F_opt_TYPE"];
    w.param->H_opt_TYPE = simulation_ini["H_opt_TYPE"];

    //QL parameters
    w.param->F_QL_L = std::stod(simulation_ini["F_QL_L"]);
    w.param->H_QL_L = std::stod(simulation_ini["H_QL_L"]);

    //mRe parameters
//    w.param->F_mRE_EPSILON = std::stod(simulation_ini["F_mRE_EPSILON"]);
//    w.param->H_mRE_EPSILON = std::stod(simulation_ini["H_mRE_EPSILON"]);
//    w.param->F_mRE_T = std::stod(simulation_ini["F_mRE_T"]);
//    w.param->H_mRE_T = std::stod(simulation_ini["H_mRE_T"]);
    w.param->F_mRE_PHI = std::stod(simulation_ini["F_mRE_PHI"]);
    w.param->H_mRE_PHI = std::stod(simulation_ini["H_mRE_PHI"]);

    w.param->F_ACCOUNTING_TYPE = simulation_ini["F_ACCOUNTING_TYPE"];

    if (simulation_ini.find("NH") != simulation_ini.end()){
        w.NH = std::stoi(simulation_ini["NH"]);
    };

    if (simulation_ini.find("NFGC") != simulation_ini.end()){
        w.NFGC = std::stoi(simulation_ini["NFGC"]);
    };

    if (simulation_ini.find("NCB") != simulation_ini.end()){
        w.NCB = std::stoi(simulation_ini["NCB"]);
    };

	w.init();

	for (int i=0; i<N; ++i){
		w.step();
//        std::cout << i << " ";
//        w.print_step();
	};

//	w.print_step();

	save_history(w, N, file_path);

	return 0;
};


//==================================================================================================
/*
  Copyright 2017 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/algorithm/lexicographical_compare.hpp>
#include <boost/simd/function/compare_greater.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check unary simd::lexicographical_compare", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values1(2*N+1), values2(2*N+1);;
  std::iota(values1.begin(), values1.end(),T(1));
  std::iota(values2.begin(), values2.end(),T(2));

  {
    bool b1 = boost::simd::lexicographical_compare( values2.data(), values2.data()+2*N+1, values2.data(), values2.data()+2*N+1);
    bool sb1 = std::lexicographical_compare( values2.data(), values2.data()+2*N+1, values2.data(), values2.data()+2*N+1);
    STF_EQUAL(b1, sb1);
    bool b2 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N+1, values2.data(), values2.data()+2*N+1);
    bool sb2 = std::lexicographical_compare( values1.data(), values1.data()+2*N+1, values2.data(), values2.data()+2*N+1);
    STF_EQUAL(b2, sb2);
    bool b3 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N+1, values1.data(), values1.data()+2*N-1);
    bool sb3 = std::lexicographical_compare( values1.data(), values1.data()+2*N+1, values1.data(), values1.data()+2*N-1);
    STF_EQUAL(b3, sb3);
    bool b4 = boost::simd::lexicographical_compare( values2.data(), values2.data()+2*N+1, values1.data(), values1.data()+2*N-1);
    bool sb4 = std::lexicographical_compare( values2.data(), values2.data()+2*N+1, values1.data(), values1.data()+2*N-1);
    STF_EQUAL(b4, sb4);
    bool b5 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N-1, values1.data(), values1.data()+2*N+1);
    bool sb5 = std::lexicographical_compare( values1.data(), values1.data()+2*N-1, values1.data(), values1.data()+2*N+1);
    STF_EQUAL(b5, sb5);
  }
  {
    bool b1 = boost::simd::lexicographical_compare( values2.data(), values2.data()+2*N+1, values2.data(), values2.data()+2*N+1, bs::compare_greater);
    bool sb1 = std::lexicographical_compare( values2.data(), values2.data()+2*N+1, values2.data(), values2.data()+2*N+1, bs::compare_greater);
    STF_EQUAL(b1, sb1);
    bool b2 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N+1, values2.data(), values2.data()+2*N+1, bs::compare_greater);
    bool sb2 = std::lexicographical_compare( values1.data(), values1.data()+2*N+1, values2.data(), values2.data()+2*N+1, bs::compare_greater);
    STF_EQUAL(b2, sb2);
    bool b3 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N+1, values1.data(), values1.data()+2*N-1, bs::compare_greater);
    bool sb3 = std::lexicographical_compare( values1.data(), values1.data()+2*N+1, values1.data(), values1.data()+2*N-1, bs::compare_greater);
    STF_EQUAL(b3, sb3);
    bool b4 = boost::simd::lexicographical_compare( values2.data(), values2.data()+2*N+1, values1.data(), values1.data()+2*N-1, bs::compare_greater);
    bool sb4 = std::lexicographical_compare( values2.data(), values2.data()+2*N+1, values1.data(), values1.data()+2*N-1, bs::compare_greater);
    STF_EQUAL(b4, sb4);
    bool b5 = boost::simd::lexicographical_compare( values1.data(), values1.data()+2*N-1, values1.data(), values1.data()+2*N+1, bs::compare_greater);
    bool sb5 = std::lexicographical_compare( values1.data(), values1.data()+2*N-1, values1.data(), values1.data()+2*N+1, bs::compare_greater);
    STF_EQUAL(b5, sb5);
  }
}


#pragma once

#include <Eigen/Dense>
#include <algorithm>

// own includes -------------------------------------------------
#include "base/array_buffer.hpp"
#include "h2n_1d.hpp"

namespace boltzmann {
template <typename BASIS, typename NUMERIC_T = double>
class Hermite2Nodal
{
 public:
  typedef NUMERIC_T numeric_t;
  typedef Eigen::Matrix<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> matrix_t;

 public:
  /**
   *
   *
   * @param basis  Hermite basis
   * @param K      max poly. degree
   * @param init   functor to initialize the 1d trafo matrix (initializers can
   * be found in
   * h2n_1d.hpp)
   */
  template <typename INITIALIZER>
  Hermite2Nodal(const BASIS &basis, const int K, const INITIALIZER &init);

  /**
   * @brief Transform from Nodal to Hermite basis.
   *
   * Also see @ref to_nodal ,
   *
   * @param dst Hermite coefficients ordered according to @ref basis.
   * @param src Nodal coefficients, \f$ src(i,j) = f(x_j, y_i) \f$.
   * @param transpose defaults to false, true is ``experimental''
   *
   *
   */
  template <typename MATRIX>
  void to_hermite(numeric_t *dst, const MATRIX &src, bool transpose = false) const;

  template <typename DERIVED, typename DERIVED2>
  void to_hermite(Eigen::DenseBase<DERIVED> &dst,
                  const Eigen::DenseBase<DERIVED2> &src,
                  bool transpose = false) const;

  /**
   * @brief Transform from Hermite to Nodal basis.
   *
   * @param dst Output Nodal coefficients dst(i,j) correpsonds to nodes \f$ x_i,
   * y_j\f$.
   * @param c   Hermite coefficients in @ref basis
   * @param tranpose: defaults to false, true is ``experimental''
   *
   *
   * Internally the Hermite coefficients are arranged like so:
   * @remark{
   *
   *     H[deg(y), deg(x)] = c_(iy, ix)
   * }
   *
   */
  template <typename MATRIX>
  void to_nodal(MATRIX &dst, const numeric_t *c, bool transpose = false) const;

  template <typename DERIVED, typename DERIVED2>
  void to_nodal(Eigen::DenseBase<DERIVED> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  const matrix_t &get_n2h() const { return N2H_; }
  const matrix_t &get_h2n() const { return H2N_; }

 private:
  std::vector<unsigned int> perm_;
  int K_;

  matrix_t N2H_;
  matrix_t H2N_;
  thread_local static ::ArrayBuffer<> buf_;

  BASIS hbasis_;
};

template <typename BASIS, typename NUMERIC_T>
thread_local ::ArrayBuffer<> Hermite2Nodal<BASIS, NUMERIC_T>::buf_;

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename INITIALIZER>
Hermite2Nodal<BASIS, NUMERIC_T>::Hermite2Nodal(const BASIS &basis,
                                               const int K,
                                               const INITIALIZER &init)
    : K_(K)
    , hbasis_(basis)
{
  buf_.reserve(K * K);
  // initialize the 1d transformation matrix
  init(H2N_, N2H_);

  typedef typename BASIS::elem_t elem_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type hy_t;

  typename elem_t::Acc::template get<hx_t> get_hx;
  typename elem_t::Acc::template get<hy_t> get_hy;

  // max degree
  unsigned int max_kx =
      get_hx(*std::max_element(basis.begin(),
                               basis.end(),
                               [&](const elem_t &e1, const elem_t &e2) {
                                 return get_hx(e1).get_id().k < get_hx(e2).get_id().k;
                               }))
          .get_id()
          .k;
  unsigned int max_ky =
      get_hy(*std::max_element(basis.begin(),
                               basis.end(),
                               [&](const elem_t &e1, const elem_t &e2) {
                                 return get_hy(e1).get_id().k < get_hy(e2).get_id().k;
                               }))
          .get_id()
          .k;

  assert(max_kx == max_ky);

  unsigned int stride = K;
  perm_.resize(hbasis_.n_dofs());
  // build permutation vector
  unsigned int i = 0;
  for (auto elem = basis.begin(); elem < basis.end(); ++elem, ++i) {
    unsigned int kx = get_hx(*elem).get_id().k;
    unsigned int ky = get_hy(*elem).get_id().k;
    perm_[i] = kx * stride + ky;
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename MATRIX>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_nodal(MATRIX &dst, const numeric_t *c, bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);

  if (!transpose) {
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    //  const auto& T = h2n_.get_matrix();
    const auto &T = H2N_;
    dst = T * TMP * T.transpose();
  } else {
    // ATTENTION: untested!
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    const auto &T = H2N_;
    dst = T.transpose() * TMP * T;
  }
}

// -----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename DERIVED, typename DERIVED2>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_nodal(Eigen::DenseBase<DERIVED> &dst,
                                          const Eigen::DenseBase<DERIVED2> &c,
                                          bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);

  if (!transpose) {
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    //  const auto& T = h2n_.get_matrix();
    const auto &T = H2N_;
    dst = T * TMP * T.transpose();
  } else {
    // ATTENTION: untested!
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    const auto &T = H2N_;
    dst = T.transpose() * TMP * T;
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename MATRIX>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_hermite(numeric_t *dst, const MATRIX &src, bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);
  if (!transpose) {
    const auto &T = N2H_;
    TMP = T * src * T.transpose();
    // undo permuation
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = TMP.data()[perm_[i]];
    }
  } else {
    // ATTENTION: untested!
    const auto &T = N2H_;
    TMP = T.transpose() * src * T;

    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = data[perm_[i]];
    }
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename DERIVED, typename DERIVED2>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_hermite(Eigen::DenseBase<DERIVED> &dst,
                                            const Eigen::DenseBase<DERIVED2> &src,
                                            bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);
  if (!transpose) {
    const auto &T = N2H_;
    TMP = T * src.derived() * T.transpose();
    // undo permuation
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = TMP.data()[perm_[i]];
    }
  } else {
    // ATTENTION: untested!
    const auto &T = N2H_;
    TMP = T.transpose() * src.derived() * T;

    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = data[perm_[i]];
    }
  }
}

}  // end namespace boltzmann


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.arithmetic toolbox - remquo/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 01/12/2010
///
#include <boost/simd/toolbox/arithmetic/include/functions/remquo.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/remainder.hpp>
#include <boost/simd/include/functions/idivround.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/toolbox/constant/constant.hpp>
#include <cmath>
#include <nt2/include/constants/nan.hpp>

#include <boost/simd/sdk/math.hpp>

NT2_TEST_CASE_TPL ( remquo_real__2_0,  (double))
{
  using boost::simd::remquo;
  using boost::simd::tag::remquo_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<remquo_(T,T)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef boost::fusion::tuple<T,typename boost::dispatch::meta::as_integer<T,signed>::type> wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  iT n = 0;
  T r;

  remquo(boost::simd::Nan<double>(), T(1), r, n);
  NT2_TEST_EQUAL(r, boost::simd::Nan<double>());
  NT2_TEST_EQUAL(n, 0);
  remquo(T(1), boost::simd::Nan<double>(), r, n);
  NT2_TEST_EQUAL(r, boost::simd::Nan<double>());
  NT2_TEST_EQUAL(n, 0);

#ifdef BOOST_SIMD_HAS_REMQUO
  T a[10] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
  T b[9] = {2, 3, 4, 5, 6, 7, 8, 9, 10};

  int32_t n1;
  T r1;

  for(int i=0; i < 10; ++i)
  {
    for(int j=0; j < 9; ++j)
    {
      remquo(a[i], b[j], r, n);
      r1 = ::remquo(a[i], b[j], &n1);
      NT2_TEST_EQUAL(r, r1);
      NT2_TEST_EQUAL(n&3, n1&3);
    }
  }
#endif

}// end of test for double

NT2_TEST_CASE_TPL ( remquo_real__2_0f,  (float))
{
  using boost::simd::remquo;
  using boost::simd::tag::remquo_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<remquo_(T,T)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef boost::fusion::tuple<T,typename boost::dispatch::meta::as_integer<T,signed>::type> wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  iT n = 0;
  T r;

  remquo(boost::simd::Nan<float>(), T(1), r, n);
  NT2_TEST_EQUAL(r, boost::simd::Nan<float>());
  NT2_TEST_EQUAL(n, 0);
  remquo(T(1), boost::simd::Nan<float>(), r, n);
  NT2_TEST_EQUAL(r, boost::simd::Nan<float>());
  NT2_TEST_EQUAL(n, 0);

#ifdef BOOST_SIMD_HAS_REMQUO
  T a[10] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
  T b[9] = {2, 3, 4, 5, 6, 7, 8, 9, 10};

  int32_t n1;
  T r1;

  for(int i=0; i < 10; ++i)
  {
    for(int j=0; j < 9; ++j)
    {
      remquo(a[i], b[j], r, n);
      r1 = ::remquo(a[i], b[j], &n1);
      std::cout << "nt2 : a " << a[i] << " --- b " << b[j] << " --> quo = " << n << " rem = " << r << std::endl;
      std::cout << "C   : a " << a[i] << " --- b " << b[j] << " --> quo = " << n1<< " rem = " << r1<< std::endl;
      NT2_TEST_EQUAL(r, r1);
      NT2_TEST_EQUAL(n&3, n1&3);
    }
  }
#endif
}


// Boost.Polygon library voronoi_test_helper.hpp file

//          Copyright Andrii Sydorchuk 2010-2011.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef VORONOI_TEST_HELPER
#define VORONOI_TEST_HELPER

#include <algorithm>
#include <iostream>
#include <iterator>
#include <fstream>
#include <map>
#include <vector>
#include <utility>

#include <boost/polygon/polygon.hpp>
using namespace boost::polygon;

namespace voronoi_test_helper {

enum kOrientation {
  RIGHT = -1,
  COLLINEAR = 0,
  LEFT = 1
};

template <typename VERTEX>
kOrientation get_orientation(
    const VERTEX& v1, const VERTEX& v2, const VERTEX& v3) {
  typename VERTEX::coordinate_type lhs = (v2.x() - v1.x()) * (v3.y() - v2.y());
  typename VERTEX::coordinate_type rhs = (v2.y() - v1.y()) * (v3.x() - v2.x());
  if (lhs == rhs) {
    return COLLINEAR;
  }
  return (lhs < rhs) ? RIGHT : LEFT;
}

template <typename OUTPUT>
bool verify_cell_convexity(const OUTPUT& output) {
  typename OUTPUT::const_cell_iterator cell_it;
  for (cell_it = output.cells().begin();
       cell_it != output.cells().end(); cell_it++) {
    const typename OUTPUT::edge_type* edge = cell_it->incident_edge();
    if (edge)
      do {
        if (edge->next()->prev() != edge) {
          return false;
        }
        if (edge->cell() != &(*cell_it)) {
          return false;
        }
        if (edge->vertex1() != edge->next()->vertex0()) {
          return false;
        }
        if (edge->vertex0() != NULL &&
            edge->vertex1() != NULL &&
            edge->next()->vertex1() != NULL) {
          if (get_orientation(*edge->vertex0(),
                              *edge->vertex1(),
                              *edge->next()->vertex1()) != LEFT) {
            return false;
          }
        }
        edge = edge->next();
      } while (edge != cell_it->incident_edge());
  }
  return true;
}

template <typename OUTPUT>
bool verify_incident_edges_ccw_order(const OUTPUT& output) {
  typedef typename OUTPUT::edge_type voronoi_edge_type;
  typename OUTPUT::const_vertex_iterator vertex_it;
  for (vertex_it = output.vertices().begin();
       vertex_it != output.vertices().end(); vertex_it++) {
    if (vertex_it->is_degenerate())
      continue;
    const voronoi_edge_type* edge = vertex_it->incident_edge();
    do {
      const voronoi_edge_type* next_edge = edge->rot_next();
      if (edge->vertex0() != next_edge->vertex0()) {
        return false;
      }
      if (edge->vertex1() != NULL && next_edge->vertex1() != NULL &&
          get_orientation(*edge->vertex1(),
                          *edge->vertex0(),
                          *next_edge->vertex1()) == LEFT) {
        return false;
      }
      edge = edge->rot_next();
    } while (edge != vertex_it->incident_edge());
  }
  return true;
}

template <typename VERTEX>
struct cmp {
  bool operator()(const VERTEX& v1, const VERTEX& v2) const {
    if (v1.x() != v2.x())
      return v1.x() < v2.x();
    return v1.y() < v2.y();
  }
};

template <typename Output>
bool verfiy_no_line_edge_intersections(const Output &output) {
  // Create map from edges with first point less than the second one.
  // Key is the first point of the edge, value is a vector of second points
  // with the same first point.
  typedef typename Output::vertex_type vertex_type;
  cmp<vertex_type> comparator;
  std::map< vertex_type, std::vector<vertex_type>, cmp<vertex_type> > edge_map;
  typename Output::const_edge_iterator edge_it;
  for (edge_it = output.edges().begin();
       edge_it != output.edges().end(); edge_it++) {
    if (edge_it->is_finite()) {
      if (comparator(*edge_it->vertex0(), *edge_it->vertex1())) {
        edge_map[*edge_it->vertex0()].push_back(*edge_it->vertex1());
      }
    }
  }
  return !intersection_check(edge_map);
}

template <typename Point2D>
bool intersection_check(
    const std::map< Point2D, std::vector<Point2D>, cmp<Point2D> > &edge_map) {
  // Iterate over map of edges and check if there are any intersections.
  // All the edges are stored by the low x value. That's why we iterate
  // left to right checking for intersections between all pairs of edges
  // that overlap in the x dimension.
  // Complexity. Approximately N*sqrt(N). Worst case N^2.
  typedef Point2D point_type;
  typedef typename point_type::coordinate_type coordinate_type;
  typedef typename std::map<point_type, std::vector<point_type>, cmp<Point2D> >::const_iterator
      edge_map_iterator;
  typedef typename std::vector<point_type>::size_type size_type;
  edge_map_iterator edge_map_it1, edge_map_it2, edge_map_it_bound;
  for (edge_map_it1 = edge_map.begin();
       edge_map_it1 != edge_map.end(); edge_map_it1++) {
    const point_type &point1 = edge_map_it1->first;
    for (size_type i = 0; i < edge_map_it1->second.size(); i++) {
      const point_type &point2 = edge_map_it1->second[i];
      coordinate_type min_y1 = (std::min)(point1.y(), point2.y());
      coordinate_type max_y1 = (std::max)(point1.y(), point2.y());

      // Find the first edge with greater or equal first point.
      edge_map_it_bound = edge_map.lower_bound(point2);

      edge_map_it2 = edge_map_it1;
      edge_map_it2++;
      for (; edge_map_it2 != edge_map_it_bound; edge_map_it2++) {
        const point_type &point3 = edge_map_it2->first;
        for (size_type j = 0; j < edge_map_it2->second.size(); j++) {
          const point_type &point4 = edge_map_it2->second[j];
          coordinate_type min_y2 = (std::min)(point3.y(), point4.y());
          coordinate_type max_y2 = (std::max)(point3.y(), point4.y());

          // In most cases it is enought to make
          // simple intersection check in the y dimension.
          if (!(max_y1 > min_y2 && max_y2 > min_y1))
            continue;

          // Intersection check.
          if (get_orientation(point1, point2, point3) *
              get_orientation(point1, point2, point4) == RIGHT &&
              get_orientation(point3, point4, point1) *
              get_orientation(point3, point4, point2) == RIGHT)
            return true;
        }
      }
    }
  }
  return false;
}

enum kVerification {
  CELL_CONVEXITY = 1,
  INCIDENT_EDGES_CCW_ORDER = 2,
  NO_HALF_EDGE_INTERSECTIONS = 4,
  FAST_VERIFICATION = 3,
  COMPLETE_VERIFICATION = 7
};

template <typename Output>
bool verify_output(const Output &output, kVerification mask) {
  bool result = true;
  if (mask & CELL_CONVEXITY)
    result &= verify_cell_convexity(output);
  if (mask & INCIDENT_EDGES_CCW_ORDER)
    result &= verify_incident_edges_ccw_order(output);
  if (mask & NO_HALF_EDGE_INTERSECTIONS)
    result &= verfiy_no_line_edge_intersections(output);
  return result;
}

template <typename PointIterator>
void save_points(
    PointIterator first, PointIterator last, const char* file_name) {
  std::ofstream ofs(file_name);
  ofs << std::distance(first, last) << std::endl;
  for (PointIterator it = first; it != last; ++it) {
    ofs << it->x() << " " << it->y() << std::endl;
  }
  ofs.close();
}

template <typename SegmentIterator>
void save_segments(
     SegmentIterator first, SegmentIterator last, const char* file_name) {
  std::ofstream ofs(file_name);
  ofs << std::distance(first, last) << std::endl;
  for (SegmentIterator it = first; it != last; ++it) {
    ofs << it->low().x() << " " << it->low().y() << " ";
    ofs << it->high().x() << " " << it->high().y() << std::endl;
  }
  ofs.close();
}

template <typename T>
void clean_segment_set(std::vector< segment_data<T> >& data) {
  typedef T Unit;
  typedef typename scanline_base<Unit>::Point Point;
  typedef typename scanline_base<Unit>::half_edge half_edge;
  typedef int segment_id;
  std::vector<std::pair<half_edge, segment_id> > half_edges;
  std::vector<std::pair<half_edge, segment_id> > half_edges_out;
  segment_id id = 0;
  half_edges.reserve(data.size());
  for (typename std::vector< segment_data<T> >::iterator it = data.begin();
       it != data.end(); ++it) {
    Point l = it->low();
    Point h = it->high();
    half_edges.push_back(std::make_pair(half_edge(l, h), id++));
  }
  half_edges_out.reserve(half_edges.size());
  // Apparently no need to pre-sort data when calling validate_scan.
  line_intersection<Unit>::validate_scan(
      half_edges_out, half_edges.begin(), half_edges.end());
  std::vector< segment_data<T> > result;
  result.reserve(half_edges_out.size());
  for (std::size_t i = 0; i < half_edges_out.size(); ++i) {
    id = half_edges_out[i].second;
    Point l = half_edges_out[i].first.first;
    Point h = half_edges_out[i].first.second;
    segment_data<T> orig_seg = data[id];
    if (orig_seg.high() < orig_seg.low())
      std::swap(l, h);
    result.push_back(segment_data<T>(l, h));
  }
  std::swap(result, data);
}
}  // voronoi_test_helper

#endif