/**
 * @file   DL_MPL.cpp
 * @author Wei Li
 * @date   Oct 2019
 */
#include <algorithm>
#include <vector>
#include <boost/timer/timer.hpp>
#include"DL_MPL.h"

SIMPLEMPL_BEGIN_NAMESPACE

std::vector<std::list<Edge_Simple> >  Read_Graph_File(std::string filename, int & vertex_numbers, int & edge_numbers)
{
	std::ifstream filein(filename.c_str());
	// std::cout << "filename : " << filename << std::endl;
	filein >> vertex_numbers >> edge_numbers; // >> mask_numbers;
	/*
	std::cout << "vertex_numbers : " << vertex_numbers << std::endl;
	std::cout << "edge_numbers : " << edge_numbers << std::endl;
	std::cout << "mask_numbers : " << mask_numbers << std::endl;
	*/

	int source, target;
	std::vector<std::list<Edge_Simple> > edge_list;
	edge_list.resize(vertex_numbers + 1);
	for (int i = 1; i <= edge_numbers; i++)
	{
		filein >> source >> target;
		edge_list[source].push_back(Edge_Simple{ target, i });
		edge_list[target].push_back(Edge_Simple{ source, i });
	}
	filein.close();
	return edge_list;
}

std::vector<std::list<Edge_Simple> >  Read_Stitch_Graph_File(std::string filename, int & vertex_numbers, int & edge_numbers)
{
	std::ifstream filein(filename.c_str());
	int total_vertex_numbers;
	// std::cout << "filename : " << filename << std::endl;
	filein >> total_vertex_numbers; // >> mask_numbers;
	/*
	std::cout << "vertex_numbers : " << vertex_numbers << std::endl;
	std::cout << "edge_numbers : " << edge_numbers << std::endl;
	std::cout << "mask_numbers : " << mask_numbers << std::endl;
	*/

	int source, target;
	double weight;
	std::vector<Vertex*> node_list;
	node_list.resize(total_vertex_numbers);
	vertex_numbers = 0;
	edge_numbers = 0;
	while (filein >> source >> target >> weight){
		//if node source and node target is not created. New one/
		if(!node_list[source]){
			Vertex* source_vertex = new Vertex;
			assert(source_vertex->Conflicts.empty());
			node_list[source] = source_vertex;
			vertex_numbers ++;
		}
		if(!node_list[target]){
			Vertex* target_vertex = new Vertex;
			node_list[target] = target_vertex;
			vertex_numbers ++;
		}

		//if two nodes are stitch relationships and both of them are parent
		if(weight < 0){
			Vertex* parent_vertex = new Vertex;
			assert(parent_vertex->Childs.empty());
			parent_vertex->parentOf(node_list[source]);
			parent_vertex->parentOf(node_list[target]);
			vertex_numbers -= 1;
			continue;
		}
		node_list[source]->Conflicts.insert(node_list[target]);
	}
	filein.close();

	//need to store node list, which does not contain child node
	std::set<Vertex*> node_wo_stitch_list;
	int index = 1;
	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		//if the node in parent node, means it has no stitch relations
		if((*it)->Is_Parent){
			node_wo_stitch_list.insert((*it));
			(*it)->updateConflicts();
			(*it)->No = index;
			index ++;
		}
		//else, add its parent node if it has not been added into node_wo_stitch_list
		else{
			assert((*it)->parent->Is_Parent);
			if(node_wo_stitch_list.find((*it)->parent) == node_wo_stitch_list.end()){
				node_wo_stitch_list.insert((*it)->parent);
				(*it)->parent->updateConflicts();
				(*it)->parent->No = index;
				index ++;
			}

		}
	}
    mplAssert(node_wo_stitch_list.size() == (unsigned int)vertex_numbers);

	std::vector<std::list<Edge_Simple> >  edge_list;
	edge_list.resize(vertex_numbers + 1);
	for(std::set<Vertex*>::iterator it = node_wo_stitch_list.begin(); it != node_wo_stitch_list.end(); ++it) {
		assert((*it)->No != 0);
		for(std::set<Vertex*>::iterator itconflict = (*it)->Conflicts_in_LG.begin(); itconflict != (*it)->Conflicts_in_LG.end(); ++itconflict) {
			if((*itconflict)->Is_Parent){
				assert((*itconflict)->No != 0);
				edge_numbers++;
				edge_list[(*it)->No].push_back(Edge_Simple{ (*itconflict)->No, edge_numbers });
				edge_list[(*itconflict)->No].push_back(Edge_Simple{ (*it)->No, edge_numbers });
			}
			else{
				assert((*itconflict)->parent->Is_Parent);
				//avoid repeat (conflicts of stitch realation nodes may represent same conflict)
				edge_numbers++;
				edge_list[(*it)->No].push_back(Edge_Simple{ (*itconflict)->parent->No, edge_numbers });
				edge_list[(*itconflict)->parent->No].push_back(Edge_Simple{ (*it)->No, edge_numbers });
			}
		}
		}
	std::cout<<"EDGE list generated with size: "<<edge_numbers<<std::endl;
	return edge_list;
}

std::vector<int> BFS_Order_no_stitch_first(std::vector<std::list<Edge_Simple> >  & edge_list,DancingLink & dl ){
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	//int first_vertex = find_max_degree_node(edge_list);
	calcualte_degree_of_each_node(edge_list,node_degree);
	int max_degree = find_max_degree(edge_list);
	std::vector<int> rows_num;
	rows_num.assign(edge_list.size(),0);
	int max_rows = 0;
	int min_rows = INT_MAX;
	for(auto i = 1; i<(int)edge_list.size();i++){
		rows_num[i] = dl.Col_Header_Table[i].Children_Number;
		if(max_rows < dl.Col_Header_Table[i].Children_Number){max_rows = dl.Col_Header_Table[i].Children_Number;}
		if(min_rows > dl.Col_Header_Table[i].Children_Number){min_rows = dl.Col_Header_Table[i].Children_Number;}
	}
	// std::cout<<"rows_num ";
	// for (auto i = rows_num.begin(); i != rows_num.end(); ++i)
	// 	std::cout << *i << ' ';
	// std::cout<<std::endl;
	//we calculate the case of each node as the root and return the smallest calculation costs
	int smallest_cost = INT_MAX;
	int smallest_cost_node = -1;
	std::vector<int> tmp_result_vector;
	for(auto root_node = 1; root_node < (int)edge_list.size(); root_node++){
		tmp_result_vector.clear();
		nonexistent.clear();
		intermediate_queue.push(root_node);
		tmp_result_vector.push_back(root_node);
		while (!intermediate_queue.empty())
		{
			int next = intermediate_queue.front();
			intermediate_queue.pop();
			if (nonexistent.find(next) == nonexistent.end())
			{
				nonexistent.insert(next);
				tmp_result_vector.push_back(next);
				// for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
				// 	intermediate_queue.push(i->target);
				//push the nodes to the queue by small-degree firstly order
				for(int row_num = min_rows; row_num <= max_rows; row_num++){
					for(int d = 1; d<= max_degree;d++){
						for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
							if(node_degree[i->target] == d && rows_num[i->target] == row_num){
								intermediate_queue.push(i->target);
							}
						}
					}
			}

				continue;
			}
			else
				continue;
		}
		//calculate the cost of each node : calculation function is \sum rows_num[node] * (node_num - index) (largest rows_num, we want to put it into the final location)
		int total_cost = 0;
		for(auto index = 1; index <edge_list.size();index++){
			total_cost += rows_num[tmp_result_vector[index]] * (edge_list.size() - index);
		}
		if(total_cost < smallest_cost){
			result_vector= tmp_result_vector;
			smallest_cost = total_cost;
			smallest_cost_node = root_node;
		}
		// std::cout<<"total_cost " <<total_cost<<std::endl;
		// std::cout<<"TMP RESULT IS"<<std::endl;
		// for (auto i = tmp_result_vector.begin(); i != tmp_result_vector.end(); ++i)
    	// 	std::cout << *i << ' ';
		// std::cout<<std::endl;
	}
	return result_vector;
}
std::vector<int> BFS_Order(std::vector<std::list<Edge_Simple> >  & edge_list)
{
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	//int first_vertex = find_max_degree_node(edge_list);
	int first_vertex = calcualte_degree_of_each_node(edge_list,node_degree);
	intermediate_queue.push(first_vertex);
	result_vector.push_back(first_vertex);

	int max_degree = find_max_degree(edge_list);
	while (!intermediate_queue.empty())
	{
		int next = intermediate_queue.front();
		intermediate_queue.pop();
		if (nonexistent.find(next) == nonexistent.end())
		{
			nonexistent.insert(next);
			result_vector.push_back(next);
			// for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
			// 	intermediate_queue.push(i->target);
			//push the nodes to the queue by small-degree firstly order
			for(int d = 1; d<= max_degree;d++){
				for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
					if(node_degree[i->target] == d){
						intermediate_queue.push(i->target);
					}
				}
			}

			continue;
		}
		else
			continue;
	}
	return result_vector;
}

std::vector<int> BFS_Order_max_first(std::vector<std::list<Edge_Simple> >  & edge_list)
{
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	int first_vertex = find_max_degree_node(edge_list);
	//int first_vertex = calcualte_degree_of_each_node(edge_list,node_degree);
	intermediate_queue.push(first_vertex);
	result_vector.push_back(first_vertex);
	//int first_vertex = find_max_degree_node(edge_list);
	calcualte_degree_of_each_node(edge_list,node_degree);
	int max_degree = find_max_degree(edge_list);
	while (!intermediate_queue.empty())
	{
		int next = intermediate_queue.front();
		intermediate_queue.pop();
		if (nonexistent.find(next) == nonexistent.end())
		{
			nonexistent.insert(next);
			result_vector.push_back(next);
			for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
				intermediate_queue.push(i->target);
			//push the nodes to the queue by max-degree firstly order

			// for(int d = max_degree; d>= 1;d--){
			// 	for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
			// 		if(node_degree[i->target] == d){
			// 			intermediate_queue.push(i->target);
			// 		}
			// 	}
			// }

			// for(int d = 1; d<= max_degree;d++){
			// 	for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
			// 		if(node_degree[i->target] == d){
			// 			intermediate_queue.push(i->target);
			// 		}
			// 	}
			// }
			continue;
		}
		else
			continue;
	}
	return result_vector;
}
int  find_max_degree_node(std::vector<std::list<Edge_Simple> >  & edge_list){
	int max_degree = 0;
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()> max_degree){
			max_degree = edge_list[it].size();
			vertex_node = it;
		}
	}
	return vertex_node;
}

int  find_max_degree(std::vector<std::list<Edge_Simple> >  & edge_list){
	int max_degree = 0;
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()> max_degree){
			max_degree = edge_list[it].size();
			vertex_node = it;
		}
	}
	return max_degree;
}

int calcualte_degree_of_each_node(std::vector<std::list<Edge_Simple> >  & edge_list, std::vector<int> & node_degree){
	int min_degree = std::numeric_limits<int>::max();
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()< min_degree){
			min_degree = edge_list[it].size();
			vertex_node = it;
		}
		node_degree[it] = edge_list[it].size();
	}
	return vertex_node;
}
std::vector<int> Simple_Order(int size)
{
	std::vector<int> result_vector;
	result_vector.reserve(size);
	result_vector.push_back(1);
	for (int i = 1; i < size; i++)
	{
		result_vector.push_back(i);
	}
	return result_vector;
}

int Sorting_Queue(DancingLink & dl)
{
	int col_no = std::numeric_limits<int>::max();
	int max = std::numeric_limits<int>::max();
	for (auto c = dl.DL_Header.Right; c != &dl.DL_Header; c = c->Right)
	{
		if (c->Children_Number < max)
		{
			max = c->Children_Number;
			col_no = c->Col;
		}
	}
	return col_no;
}

void Convert_to_Exat_Cover(int & row_numbers, int & col_numbers, std::string infilename, std::string Exact_Cover_Filename, bool whether_BFS,
	int & vertex_numbers, int & edge_numbers, int & mask_numbers, std::vector<int> & MPLD_search_vector)
{
	std::ofstream fileout(Exact_Cover_Filename);

	std::vector<std::list<Edge_Simple> >  edge_list = Read_Stitch_Graph_File(infilename, vertex_numbers, edge_numbers); //, mask_numbers);

	if (whether_BFS)
		MPLD_search_vector = BFS_Order(edge_list);
	else
		MPLD_search_vector = Simple_Order(edge_list.size());
	row_numbers = vertex_numbers * mask_numbers + 1;
	col_numbers = edge_numbers * mask_numbers + vertex_numbers;
	fileout << row_numbers << std::endl;
	fileout << col_numbers << std::endl;
	int count = 0;
	int total_edge = 0;
	for (auto it = edge_list.begin(); it != edge_list.end(); it++)
	{
		if(it == edge_list.begin()){continue;}
		std::cout<<it->size()<<std::endl;
		total_edge += it->size();
 		int temp = (it->size() + 1) * mask_numbers;
		count += temp;
	}
	std::cout<<total_edge<<std::endl;
	assert(total_edge == 2 * edge_numbers );
	count += edge_numbers*mask_numbers;

	fileout << count << std::endl;
	for (unsigned int it = 1; it < edge_list.size(); ++it)
	{
		for (int i = 1; i <= mask_numbers; ++i)
		{
			fileout << (it - 1)*mask_numbers + i << " " << it << std::endl;
			for (auto j = edge_list[it].begin(); j != edge_list[it].end(); ++j)
			{
				fileout << (it - 1)*mask_numbers + i << " " << vertex_numbers + (j->No - 1)*mask_numbers + i << std::endl;
			}
		}
	}
	for (int i = 0; i < edge_numbers; ++i)
	{
		for (int j = 1; j <= mask_numbers; ++j)
			fileout << vertex_numbers * mask_numbers + 1 << " " << vertex_numbers + i * mask_numbers + j << std::endl;
	}
	fileout.close();
	return;
}

int Next_Column(std::vector<int> & MPLD_search_vector, int depth)
{
	return MPLD_search_vector[depth];
}

int Next_Column_stitch(DancingLink & dl,std::vector<int> & MPLD_search_vector)
{
	bool not_find_last_uncovered_col = true;
	int last_col = 0;
	for(unsigned int i = 0; i< MPLD_search_vector.size(); i++){
		Cell *col = &dl.Col_Header_Table[MPLD_search_vector[i]];
		//if the column(vertex) has been removed(selected)
		//if(col_cover_vector[MPLD_search_vector[i]])	continue;
		if(col->Left->Right != col) continue;
		else{
			mplAssert(col->InDLX);
			if(not_find_last_uncovered_col){
				last_col = MPLD_search_vector[i];
				not_find_last_uncovered_col = false;
			}
		}

		if(col-> Children_Number == 1){
			return MPLD_search_vector[i];
		}
	}
	assert(not_find_last_uncovered_col == false);
	return last_col;
}
bool Vertices_All_Covered(DancingLink & dl, int& vertex_numbers)
{
	if (dl.DL_Header.Right->Col > vertex_numbers)
		return true;
	else
		return false;
}

void store_intermediate_process(DancingLink & dl, int this_col, std::set<int> & row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col,std::vector<int>  &  conflict_col_table,std::vector<int>  &last_rows)
{
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it) {
		Delete_the_Row_in_which_Col[*row_it] = this_col;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{	//std::cout<<"row deleted is "<<*row_it<<"in column "<<this_col<<std::endl;
							Order_of_Row_Deleted_in_Col[row_ele->Col].push_back(*row_it);
			conflict_col_table[row_ele->Col] = this_col;
			last_rows[row_ele->Col] = (*row_it);}
	}
}

void efficient_store_intermediate_process(DancingLink & dl, int this_col, std::set<int> & row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col)
{
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it) {
		Delete_the_Row_in_which_Col[*row_it] = this_col;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{	//std::cout<<"row deleted is "<<*row_it<<"in column "<<this_col<<std::endl;
							Order_of_Row_Deleted_in_Col[row_ele->Col].push_back(*row_it);}
	}
}

void recover_intermediate_process(DancingLink & dl, int this_col, std::set<int>& row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col,std::vector<int>  &  conflict_col_table,std::vector<int>  &last_rows)
{
	(void)this_col;
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it)
	{
		 Delete_the_Row_in_which_Col[*row_it] = 0;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{Order_of_Row_Deleted_in_Col[row_ele->Col].pop_back();
			conflict_col_table[row_ele->Col] = Order_of_Row_Deleted_in_Col[row_ele->Col].back();
			last_rows[row_ele->Col] =  0;}
	}
}

void efficient_recover_intermediate_process(DancingLink & dl, int this_col, std::set<int>& row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col)
{
	(void)this_col;
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it)
	{
		 Delete_the_Row_in_which_Col[*row_it] = 0;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{Order_of_Row_Deleted_in_Col[row_ele->Col].pop_back();}
	}
}

bool MPLD_X_Solver(DancingLink & dl, std::vector<int8_t>& color_vector,std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, int mask_numbers,
			std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector, const char* result_file,
			std::vector<bool>& col_cover_vector,std::vector<int>& row_select_vector,
			std::vector<int> & partial_conflict_col_table,
			std::vector<int>  &  conflict_col_table,std::vector<int>  &partial_last_rows,std::vector<int>  &last_rows)
{
	// If there is no columns left or all the verteices are covered, then the algorithm terminates.
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		//Decode_OpenMPL(vertex_numbers, mask_numbers,color_vector,result_vec, conflict_pair, result_file);
		return true;
	}
	/*
	if (dl.DL_Header.Right->Col > vertex_numbers )
	{
		this_col = Sorting_Queue(dl);
	}
	else
	{
	*/
	Cell *col;
	//TODO: this depth should be a bug cause sometimes it is not by order (need a bool vector to record the col cover information)
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_cover_vector[this_col] = true;
	if (col->Children_Number == 0)
	{
		// int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		// int conflict_col = Delete_the_Row_in_which_Col[last_row];
		//std::cout<<"last row is "<<last_row<<std::endl;
		//mplAssert(conflict_col == conflict_col_table[this_col]);
		conflict_pair = std::make_pair(conflict_col_table[this_col], this_col);
		//Also,  record the partial results (rows) in the last detected conflict
		row_select_vector.assign(result_vec.begin(),result_vec.end());
		row_select_vector.push_back(last_rows[this_col]);
		//We should should push back the last row(coloring result of this conflict col)
		partial_last_rows.assign(last_rows.begin(), last_rows.end());
		partial_conflict_col_table.assign(conflict_col_table.begin(), conflict_col_table.end());
		// if (MPLD_X_Solver(dl, color_vector,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1, MPLD_search_vector, result_file))
		// 	return true;
	}
	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col,conflict_col_table,last_rows);

		Remove_Rows_Cols(dl, row_set, col_set);
		if (MPLD_X_Solver(dl, color_vector,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector, result_file,col_cover_vector,row_select_vector,partial_conflict_col_table,conflict_col_table,partial_last_rows,last_rows))
			return true;

		result_vec.pop_back();
		recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col,conflict_col_table,last_rows);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
	col_cover_vector[this_col] = false;

	return false;
}

bool Efficient_MPLD_X_Solver(DancingLink & dl,std::vector<int8_t>& color_vector, std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, int mask_numbers,
			std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector, const char* result_file,
			std::vector<int> & partial_row_results, std::vector<int> & partial_col_results,std::vector<int> & col_results)
{
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		//Decode_OpenMPL(vertex_numbers, mask_numbers,color_vector,result_vec, conflict_pair, result_file);
		return true;
	}

	Cell *col;
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);
#ifdef DEBUG_DANCINGLINKCOLORING
    mplPrint(kDEBUG, "BEGIN: this_col : %d depth : %d\n", this_col, depth);
#endif

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_results.push_back(this_col);
	if (col->Children_Number == 0)
	{
		int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		int conflict_col = Delete_the_Row_in_which_Col[last_row];
		conflict_pair = std::make_pair(conflict_col, this_col);
		partial_row_results.assign(result_vec.begin(),result_vec.end());
		partial_row_results.push_back(last_row);
		partial_col_results.assign(col_results.begin(), col_results.end());
	}

	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		efficient_store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Remove_Rows_Cols(dl, row_set, col_set);
		if (Efficient_MPLD_X_Solver(dl,color_vector, result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector, result_file,partial_row_results,partial_col_results,col_results))
			return true;

		result_vec.pop_back();
		efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
#ifdef DEBUG_DANCINGLINKCOLORING
    mplPrint(kDEBUG, "END: this_col : %d depth : %d\n", this_col, depth);
#endif
	col_results.pop_back();
	return false;
}

bool Efficient_MPLD_X_Solver_v2(DancingLink & dl, std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector,
			std::vector<int> & partial_row_results, std::vector<int> & partial_col_results,std::vector<int> & col_results,
			std::vector<std::vector<int>> &early_quit_count,bool & early_quit)
{
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		return true;
	}

	Cell *col;
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_results.push_back(this_col);
	if (col->Children_Number == 0)
	{
		int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		int conflict_col = Delete_the_Row_in_which_Col[last_row];
		conflict_pair = std::make_pair(conflict_col, this_col);
		partial_row_results.assign(result_vec.begin(),result_vec.end());
		partial_row_results.push_back(last_row);
		partial_col_results.assign(col_results.begin(), col_results.end());
		early_quit_count[conflict_col][this_col] ++;
		early_quit_count[this_col][conflict_col] ++;
		if(early_quit_count[this_col][conflict_col] > 500 || early_quit_count[conflict_col][this_col]> 500){
			early_quit = true;
			LR_recover(*col);
			col_results.pop_back();
			return false;
		}
	}

	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		efficient_store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Remove_Rows_Cols(dl, row_set, col_set);
		if (Efficient_MPLD_X_Solver_v2(dl, result_vec, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector,partial_row_results,partial_col_results,col_results,early_quit_count,early_quit))
			return true;
		else{
			if(early_quit){
				result_vec.pop_back();
				efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
				Recover_Rows_Cols(dl, row_set, col_set);
				LR_recover(*col);
				col_results.pop_back();
#ifdef DEBUG_DANCINGLINKCOLORING
                mplPrint(kDEBUG, "END by early stop: this_col : %d depth : %d\n", this_col, depth);
#endif
				return false;
			}
		}

		result_vec.pop_back();
		efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
	col_results.pop_back();
	return false;
}

/***
 * The core function to solve one dl.
 * Typically, this function may run MPLD_X_Solver  multiple times due to the existence of some exact conflicts
 * Return : selected row
 * **/
std::vector<int> core_solve_dl(DancingLink & dl, std::vector<std::list<Edge_Simple> > & edge_list,  int  row_numbers,  int  col_numbers,
 int  vertex_numbers, int mask_number){
	//the total conflict pairs
	// boost::timer::cpu_timer dancing_link_timer;
	// //dancing_link_timer.start();
	std::pair<int, int>  conflict_pair;
	//the conflict pair if MPLD_X_Solver has NO a conflict-free solution
	std::set<std::pair<int, int> >  final_conflict;
	//selected result rows if MPLD_X_Solver has a conflict-free solution
	std::vector<int> selected_rows;
	//the final selected cols;
	std::vector<int> selected_cols;
	//the partial selected rows(coloring results actually) if MPLD_X_Solver has NO a conflict-free solution
	std::vector<int> partial_selected_rows;
	//the partial selected cols(selected )  if MPLD_X_Solver has NO a conflict-free solution
	std::vector<int> partial_selected_cols;
	//the final selected rows
	std::vector<int> final_result;
	//middle information for recover partial results
	std::vector<int> Delete_the_Row_in_which_Col;
	std::vector<std::list<int> >  Order_of_Row_Deleted_in_Col;
	Order_of_Row_Deleted_in_Col.resize(col_numbers + 1);
	Delete_the_Row_in_which_Col.resize(row_numbers + 1);
	std::vector<int> MPLD_search_vector;
	//MPLD_search_vector = BFS_Order(edge_list);
	//MPLD_search_vector = Simple_Order(edge_list.size());
	MPLD_search_vector = BFS_Order_no_stitch_first(edge_list,dl);
	//MPLD_search_vector = BFS_Order_max_first(edge_list);
	// for(auto i = 0; i<MPLD_search_vector.size(); i++){
	// 	std::cout<<MPLD_search_vector[i]<<" ";
	// }
	// std::cout<<std::endl;
	int depth = 1;
	std::vector<std::vector<int>> early_quit_count;
	for(auto i = 0; i <= vertex_numbers; i++){
		std::vector<int> row_early_quite_count;
		row_early_quite_count.assign(vertex_numbers+1,0);
		early_quit_count.push_back(row_early_quite_count);
	}
	bool early_quit = false;
	bool result = Efficient_MPLD_X_Solver_v2(dl, selected_rows, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth,
		MPLD_search_vector,partial_selected_rows,partial_selected_cols,selected_cols,early_quit_count, early_quit);
	int iteration = 1;
	//std::cout<< "IN MPL:::::::::::::::::::::first try solve " << dancing_link_timer.format(6)<<std::endl;
	//dancing_link_timer.start();
	while(result==false && partial_selected_rows.size()< vertex_numbers){
		mplAssert(partial_selected_rows.size() == partial_selected_cols.size());
		iteration++;
#ifdef DEBUG_DANCINGLINKCOLORING
        mplPrint(kDEBUG, "iteration %d\n", iteration);
#endif
		final_conflict.insert(conflict_pair);
		int col_edge = -1;
		for(std::list<Edge_Simple>::iterator conflict_edge = edge_list[conflict_pair.first].begin();conflict_edge != edge_list[conflict_pair.first].end();++conflict_edge){
			if((*conflict_edge).target == conflict_pair.second){
				col_edge  = (*conflict_edge).No;
				break;
			}
		}
		if(col_edge==-1){
			std::cout<<"bug found!"<<std::endl;
		}
		mplAssert(col_edge!=-1);
		mplAssert(selected_rows.size() == 0);
		for(int i = 1;i<=mask_number;i++){
			int this_col = vertex_numbers + (col_edge -1)*(mask_number) + i;
			//Cell *col = &dl.Col_Header_Table[this_col];
			Remove_Single_Col(dl,this_col);
		}
		//std::cout<< "IN MPL:::::::::::::::::::::remove conflict cols " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
		final_result.insert( final_result.end(), partial_selected_rows.begin(), partial_selected_rows.end() );
		//NOTE that the order between the two parts (remove edges and recover partial results are important! Cause some rows won't be removed this time due to the exact conflict edge is removed)
		for(int i = 0; i < partial_selected_rows.size(); i++){
			//if the row is selected ( in the partial result), recover the partial results
				std::set<int> row_set;
				std::set<int> col_set;
				Select_All_Rows_Cols(dl, partial_selected_rows[i], row_set, col_set);
				efficient_store_intermediate_process(dl, partial_selected_cols[i], row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
				Remove_Rows_Cols(dl, row_set, col_set);
		}
		//std::cout<< "IN MPL:::::::::::::::::::::recover partial results " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
		partial_selected_rows.clear();
		partial_selected_cols.swap(selected_cols);
		selected_cols.clear();
		depth = 1;
		for (auto &v: early_quit_count) {
			std::fill(v.begin(), v.end(), 0);
		}
		early_quit = false;
		result = Efficient_MPLD_X_Solver_v2(dl, selected_rows, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth,
		MPLD_search_vector,partial_selected_rows,partial_selected_cols,selected_cols,early_quit_count, early_quit);
		//std::cout<< "IN MPL:::::::::::::::::::::second try solve " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
	}
	if(selected_rows.size() != 0){
		final_result.insert( final_result.end(), selected_rows.begin(), selected_rows.end() );
		mplAssert(final_result.size() == vertex_numbers);
	}
	else final_result.insert( final_result.end(), partial_selected_rows.begin(), partial_selected_rows.end() );
	return final_result;
}
/***
 * The function to decode and calculate the cost of the results of DL.
 * Args: selected rows
 * Return : selected row
 * **/

void decode_row_results(std::vector<int> & final_result, std::vector<int8_t>& color_vector, int vertex_number,
int  mask_number, std::vector<std::vector<std::pair<uint32_t,uint32_t>>>& decode_mat, std::vector<Vertex*> & node_list ){
	std::vector<int8_t> color_results_wo_stitch (vertex_number);
	for (auto i = final_result.begin(); i != final_result.end(); i++)
	{
		// if the selected row is in the range of parent node (no stitch)
		// std::cout << *i << std::endl;
		if((*i) < vertex_number * mask_number + 1){
		int No = ((*i) - 1) / mask_number + 1;
		int mask = (*i + 2) % mask_number;
		color_results_wo_stitch[No-1] = mask; }
		// std::cout<<"selected rows: "<<(*i)<<std::endl;
		//if the selected_rows represent some stitches rows
		if((*i) > (vertex_number * (uint32_t)(mask_number) + (uint32_t)1)){
			std::vector<std::pair<uint32_t,uint32_t>>& row_decoder = decode_mat[(*i)- vertex_number * mask_number -2];
			for(std::vector<std::pair<uint32_t,uint32_t>>::iterator it = row_decoder.begin(); it != row_decoder.end(); ++it) {
				color_vector[(*it).first] = (*it).second;
				// std::cout<<"Corresponding color "<<(*it).first<<" "<<(*it).second<<std::endl;
			}
		}
	}
	// std::cout<<"Finished"<<std::endl;

	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		if(color_vector[(*it)->Stitch_No]!= -1){continue;}
		if((*it)->No == 0){
			mplAssert((*it)->Is_Parent == false);
			mplAssert((*it)->parent->Stitch_No == -1);
			mplAssert(color_results_wo_stitch[(*it)->parent->No -1]!= -1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->parent->No -1];
		}
		else{
			mplAssert(color_results_wo_stitch[(*it)->No -1]!=-1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->No -1];
		}
	}
	return;
}

void decode_row_results_wo_skeleton(std::vector<int> & final_result, std::vector<int8_t>& color_vector, int vertex_number,
int  mask_number, std::vector<std::vector<std::pair<uint32_t,uint32_t>>>& decode_mat, std::vector<Vertex*> & node_list ){
	std::vector<int8_t> color_results_wo_stitch (vertex_number);
	for (auto i = final_result.begin(); i != final_result.end(); i++)
	{
		// if the selected row is in the range of parent node (no stitch)
		// std::cout << *i << std::endl;
		if((*i) < vertex_number * mask_number + 1){
		int No = ((*i) - 1) / mask_number + 1;
		int mask = (*i + 2) % mask_number;
		color_results_wo_stitch[No-1] = mask; }

		//if the selected_rows represent some stitches rows
		if((*i) > (vertex_number * (uint32_t)(mask_number))){
			std::vector<std::pair<uint32_t,uint32_t>>& row_decoder = decode_mat[(*i)- vertex_number * mask_number -1];
			for(std::vector<std::pair<uint32_t,uint32_t>>::iterator it = row_decoder.begin(); it != row_decoder.end(); ++it) {
				color_vector[(*it).first] = (*it).second;
			}
		}
	}

	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		if(color_vector[(*it)->Stitch_No]!= -1){continue;}
		if((*it)->No == 0){
			mplAssert((*it)->Is_Parent == false);
			mplAssert((*it)->parent->Stitch_No == -1);
			mplAssert(color_results_wo_stitch[(*it)->parent->No -1]!= -1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->parent->No -1];
		}
		else{
			mplAssert(color_results_wo_stitch[(*it)->No -1]!=-1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->No -1];
		}
	}
	return;
}
void Decode(int vertex_numbers, int mask_numbers, std::vector<int> result_vec, std::pair<int, int> conflict_pair, std::string filename)
{
	(void)conflict_pair;
	(void)filename;
	(void)vertex_numbers;
	std::map<int, int> Final_Color;
	std::ofstream fileout(filename);
	fileout << "Solution : " << std::endl;
	for (auto i = result_vec.begin(); i != result_vec.end(); i++)
	{
		// std::cout << *i << std::endl;
		int No = ((*i) - 1) / mask_numbers + 1;
		int mask = (*i + 2) % mask_numbers + 1;
		Final_Color.insert(std::make_pair(No, mask));
	}
	for (auto i = Final_Color.begin(); i != Final_Color.end(); i++)
	{
		fileout << "Vertex " << i->first << " \t : " << i->second << std::endl;
	}
	fileout << "=============================================" << std::endl;

	// if (conflict_pair!=nullptr)
	// {
	// 	fileout << "Conflicts: " << std::endl;
	// 	fileout << "No\t Source\t Target" << std::endl;
	// 	fileout << count++ << "\t " << conflict_pair->first << "\t " <<conflict_pair->second << std::endl;
	// 	fileout << "=============================================" << std::endl;
	// }

	fileout.close();
}

void Decode_OpenMPL(int vertex_numbers, int mask_numbers, std::vector<int8_t>& color_vector,std::vector<int> result_vec, std::pair<int, int> conflict_pair,  char* filename)
{
	(void)filename;
	(void)conflict_pair;
	for (auto i = result_vec.begin(); i != result_vec.end(); i++)
	{
		//std::cout << *i << std::endl;
		if((*i) < vertex_numbers * mask_numbers + 1){
		int No = ((*i) - 1) / mask_numbers + 1;
		int mask = (*i + 2) % mask_numbers;
		color_vector[No-1] = mask; }
	}

	// if (!conflict_pair.empty())
	// {
	// 	std::cout << "Conflicts: " << std::endl;
	// 	std::cout << "No\t Source\t Target" << std::endl;
	// 	int count = 1;
	// 	for (auto i = conflict_pair.begin(); i != conflict_pair.end(); i++)
	// 		{color_vector[i->second-1] = color_vector[i->first-1];
	// 		std::cout << count++ << "\t " << i->first << "\t " << i->second << std::endl;}
	// 	std::cout << "=============================================" << std::endl;
	// }
}

void MPLD_Solver(std::string Graph_Filename, std::string Exact_Cover_Filename, bool whether_BFS, int mask_numbers,  const char* result_file)
{
	(void)result_file;
	int vertex_numbers;
	int edge_numbers;
	int row_numbers;
	int col_numbers;
	std::vector<int8_t> color_vec;
	std::vector<int> result_vec;
	std::pair<int, int>  conflict_pair;
	std::vector<int> Delete_the_Row_in_which_Col;
	std::vector<std::list<int> >  Order_of_Row_Deleted_in_Col;
	std::vector<int> MPLD_search_vector;
	DancingLink dl;
	std::cout<<"Ready to generate cover matrix"<<std::endl;
	Convert_to_Exat_Cover(row_numbers, col_numbers, Graph_Filename, Exact_Cover_Filename, whether_BFS, vertex_numbers, edge_numbers, mask_numbers, MPLD_search_vector);
	Order_of_Row_Deleted_in_Col.resize(col_numbers + 1);
	Delete_the_Row_in_which_Col.resize(row_numbers + 1);
	DL_Load(dl, Exact_Cover_Filename);
	//int depth = 1;
	//MPLD_X_Solver(dl, color_vec,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth, MPLD_search_vector, result_file,col_cover_vector);
}

SIMPLEMPL_END_NAMESPACE


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Hannes Roest $
// $Authors: Hannes Roest $
// --------------------------------------------------------------------------

#include <OpenMS/ANALYSIS/OPENSWATH/OPENSWATHALGO/ALGO/Scoring.h>
#include <OpenMS/ANALYSIS/OPENSWATH/OPENSWATHALGO/Macros.h>
#include <cmath>

#include <boost/numeric/conversion/cast.hpp>

namespace OpenSwath
{
  namespace Scoring
  {

    void normalize_sum(double x[], unsigned int n)
    {
      double sumx = std::accumulate(&x[0], &x[0] + n, 0.0);
      if (sumx == 0.0)
      {
        return;
      } // do not divide by zero
      for (unsigned int i = 0; i < n; i++)
      {
        x[i] = x[i] / sumx;
      }
    }

    double NormalizedManhattanDist(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double delta_ratio_sum = 0;
      normalize_sum(x, n);
      normalize_sum(y, n);
      for (int i = 0; i < n; i++)
      {
        delta_ratio_sum += std::fabs(x[i] - y[i]);
      }
      return delta_ratio_sum / n;
    }

    double RootMeanSquareDeviation(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double result = 0;
      for (int i = 0; i < n; i++)
      {

        result += (x[i] - y[i]) * (x[i] - y[i]);
      }
      return std::sqrt(result / n);
    }

    double SpectralAngle(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double dotprod = 0;
      double x_len = 0;
      double y_len = 0;
      for (int i = 0; i < n; i++)
      {
        dotprod += x[i] * y[i];
        x_len += x[i] * x[i];
        y_len += y[i] * y[i];
      }
      x_len = std::sqrt(x_len);
      y_len = std::sqrt(y_len);

      return std::acos(dotprod / (x_len * y_len));
    }

    XCorrArrayType::iterator xcorrArrayGetMaxPeak(XCorrArrayType& array)
    {
      OPENSWATH_PRECONDITION(array.size() > 0, "Cannot get highest apex from empty array.");

      XCorrArrayType::iterator max_it = array.begin();
      double max = array.begin()->second;
      for (XCorrArrayType::iterator it = array.begin(); it != array.end(); ++it)
      {
        if (it->second > max)
        {
          max = it->second;
          max_it = it;
        }
      }
      return max_it;
    }

    void standardize_data(std::vector<double>& data)
    {
      OPENSWATH_PRECONDITION(data.size() > 0, "Need non-empty array.");

      // subtract the mean and divide by the standard deviation
      double mean = std::accumulate(data.begin(), data.end(), 0.0) / (double) data.size();
      double sqsum = 0;
      for (std::vector<double>::iterator it = data.begin(); it != data.end(); ++it)
      {
        sqsum += (*it - mean) * (*it - mean);
      }
      double std = sqrt(sqsum / data.size()); // standard deviation

      for (std::size_t i = 0; i < data.size(); i++)
      {
        data[i] = (data[i] - mean) / std;
      }
    }

    XCorrArrayType normalizedCrossCorrelation(std::vector<double>& data1,
                                              std::vector<double>& data2, int maxdelay, int lag = 1)
    {
      OPENSWATH_PRECONDITION(data1.size() != 0 && data1.size() == data2.size(), "Both data vectors need to have the same length");

      // normalize the data
      standardize_data(data1);
      standardize_data(data2);
      std::map<int, double> result = calculateCrossCorrelation(data1, data2, maxdelay, lag);
      for (std::map<int, double>::iterator it = result.begin(); it != result.end(); ++it)
      {
        it->second = it->second / data1.size();
      }
      return result;
    }

    XCorrArrayType calculateCrossCorrelation(std::vector<double>& data1,
                                             std::vector<double>& data2, int maxdelay, int lag)
    {
      OPENSWATH_PRECONDITION(data1.size() != 0 && data1.size() == data2.size(), "Both data vectors need to have the same length");

      XCorrArrayType result;
      int datasize = boost::numeric_cast<int>(data1.size());
      int i, j, delay;

      for (delay = -maxdelay; delay <= maxdelay; delay = delay + lag)
      {
        double sxy = 0;
        for (i = 0; i < datasize; ++i)
        {
          j = i + delay;
          if (j < 0 || j >= datasize)
          {
            continue;
          }
          sxy += (data1[i]) * (data2[j]);
        }
        result[delay] = sxy;
      }
      return result;
    }

    XCorrArrayType calcxcorr_legacy_mquest_(std::vector<double>& data1,
                                            std::vector<double>& data2, bool normalize)
    {
      OPENSWATH_PRECONDITION(!data1.empty() && data1.size() == data2.size(), "Both data vectors need to have the same length");
      int maxdelay = boost::numeric_cast<int>(data1.size());
      int lag = 1;

      XCorrArrayType result;
      double mean1 = std::accumulate(data1.begin(), data1.end(), 0.) / (double)data1.size();
      double mean2 = std::accumulate(data2.begin(), data2.end(), 0.) / (double)data2.size();
      double denominator = 1.0;
      int datasize = boost::numeric_cast<int>(data1.size());
      int i, j, delay;

      // Normalized cross-correlation = subtract the mean and divide by the standard deviation
      if (normalize)
      {
        double sqsum1 = 0;
        double sqsum2 = 0;
        for (std::vector<double>::iterator it = data1.begin(); it != data1.end(); ++it)
        {
          sqsum1 += (*it - mean1) * (*it - mean1);
        }

        for (std::vector<double>::iterator it = data2.begin(); it != data2.end(); ++it)
        {
          sqsum2 += (*it - mean2) * (*it - mean2);
        }
        // sigma_1 * sigma_2 * n
        denominator = sqrt(sqsum1 * sqsum2);
      }

      for (delay = -maxdelay; delay <= maxdelay; delay = delay + lag)
      {
        double sxy = 0;
        for (i = 0; i < datasize; i++)
        {
          j = i + delay;
          if (j < 0 || j >= datasize)
          {
            continue;
          }
          if (normalize)
          {
            sxy += (data1[i] - mean1) * (data2[j] - mean2);
          }
          else
          {
            sxy += (data1[i]) * (data2[j]);
          }
        }

        if (denominator > 0)
        {
          result[delay] = sxy / denominator;
        }
        else
        {
          // e.g. if all datapoints are zero
          result[delay] = 0;
        }
      }
      return result;
    }

  } //end namespace Scoring
}


#include "CompliantStaticSolver.h"

#include <sofa/core/ObjectFactory.h>

#include <boost/math/tools/minima.hpp>
#include <tuple>

#include "../utils/nr.h"
#include "../utils/scoped.h"

#include "../constraint/Constraint.h"

namespace sofa {
namespace component {
namespace odesolver {

// TODO use std::numeric_limits

CompliantStaticSolver::CompliantStaticSolver()
    : epsilon(initData(&epsilon, (SReal)1e-16, "epsilon", "division by zero threshold")),
      line_search(initData(&line_search, unsigned(LS_SECANT), "line_search",
                           "line search method, 0: none (use dt), 1: brent, 2: secant (default). (warning: brent does not work with constraints.")),
      conjugate(initData(&conjugate, true, "conjugate", "conjugate descent directions")),
      ls_precision(initData(&ls_precision, (SReal)1e-8, "ls_precision", "line search precision")),
      ls_iterations(initData(&ls_iterations, unsigned(10), "ls_iterations",
                             "line search iterations")),
      ls_step(initData(&ls_step, (SReal)1e-8, "ls_step",
                       "line search bracketing step (should not cross any extrema from current position"))
{

}

struct CompliantStaticSolver::helper {

    simulation::common::VectorOperations vec;
    simulation::common::MechanicalOperations mec;

    core::behavior::MultiVecDeriv dx;
    core::behavior::MultiVecDeriv f;

    helper(const core::ExecParams* params,
           core::objectmodel::BaseContext* ctx) : vec(params, ctx),
                                                  mec(params, ctx),
                                                  dx( &vec, core::VecDerivId::dx() ),
                                                  f( &vec, core::VecDerivId::force() )    {
    }

    // TODO const args ?
    SReal dot(core::MultiVecDerivId lhs, core::MultiVecDerivId rhs)  {
        vec.v_dot(lhs, rhs);
        return vec.finish();
    }

    // arg can't be dx !
    void K(core::MultiVecDerivId res, core::MultiVecDerivId arg) {
        vec.v_eq(dx, arg);
        vec.v_clear( res );

        mec.addMBKdx( res , 0, 0, -1, true, true);
        mec.projectResponse( res );
    }

    void forces(core::MultiVecDerivId res) {
        mec.computeForce( res );
        mec.projectResponse( res );
    }

    template<class Vec>
    void realloc(Vec& res) {
        res.realloc( &vec, false, true );
    }

    template<class Res, class A, class B>
    void set(Res& res,
             A& a,
             B& b,
             SReal lambda) {
        vec.v_op(res, a, b, lambda);
    }

    template<class A, class B>
    void set(A& res,
             B& b) {
        vec.v_eq(res, b);
    }

};

CompliantStaticSolver::ls_info::ls_info()
    : eps(0),
      iterations(15),
      precision(1e-7),
      fixed_step(1e-5),
      bracket_step(1e-8) {

}

// minimizes (implicit) potential function in direction dir) by
// zeroing g = grad^T dir using secant method

// precond:
// - op.f contains forces for current pos
void CompliantStaticSolver::ls_secant(helper& op,
                                      core::MultiVecCoordId pos,
                                      core::MultiVecDerivId dir,
                                      const ls_info& info) {
    scoped::timer step("ls_secant");

    SReal dg = 0;
    SReal dx = 0;

    SReal g_prev = 0;
    SReal total = 0;

    // slightly damp newton iterations TODO hardcode
    static const SReal damping = 1e-14;

    SReal fixed = info.fixed_step;

    for(unsigned k = 0, n = info.iterations; k < n; ++k) {

        if( k ) {
            // update forces
            op.mec.propagateX(pos, true);
            op.forces( op.f );
        }

        const SReal g = op.dot(op.f, dir);

        // std::cout << "line search (secant) " << k << " "  << total << " " << g << std::endl;

        // are we done ?
        if( std::abs(g) <= info.precision ) break;

        dg = g - g_prev;

        const SReal dx_prev = dx;

        // fallback on fixed step
        dx = fixed;

        // (damped) secant method
        if( k && (std::abs(dg) > info.eps)) {
            dx = -(dx_prev / (dg + damping)) * g;
        } else {

            // try to move more to change function
            fixed *= 2;

        }

        total += dx;

        // move dx along dir
        op.set(pos, pos, dir, dx);

        // next
        g_prev = g;
    }

}

struct CompliantStaticSolver::potential_energy {

    // WARNING: backup/restore pos on scope entry/exit
    potential_energy(helper& op,
                     const core::MultiVecCoordId& pos,
                     const core::MultiVecDerivId& dir,
                     const core::MultiVecCoordId& tmp,
                     bool restore = true)
        : op(op),
          pos(pos),
          dir(dir),
          tmp(tmp),
          restore(restore) {

        // backup start position
        op.set(tmp, pos);
    }

    helper& op;

    const core::MultiVecCoordId& pos;
    const core::MultiVecDerivId& dir;
    const core::MultiVecCoordId& tmp;

    // do we restore pos in dtor ?
    bool restore;

    SReal operator()(SReal x) const {

        // move to x along dir
        op.set(pos, tmp, dir, x);

        // update forces/energy
        op.mec.propagateX(pos, true);

        // TODO apparently, this is not needed
        // op.forces(op.f);

        // note: potential energy only works for pos !
        SReal dummy, result;
        op.mec.computeEnergy(dummy, result);

        // std::cout << "potential energy: " << x << ", " << result << std::endl;

        return result;
    }

    ~potential_energy() {
        if( restore ) {
            op.set(pos, tmp);
        }
    }

};

void CompliantStaticSolver::ls_brent(helper& op,
                                     core::MultiVecCoordId pos,
                                     core::MultiVecDerivId dir,
                                     const ls_info& info,
                                     core::MultiVecCoordId tmp) {
    scoped::timer step("ls_brent");
    typedef utils::nr::optimization<SReal> opt;

    opt::func_call a, b, c;

    a.x = 0;
    b.x = info.bracket_step;

    opt::func_call res;

    {
        // we need the scope because f might reset pos on scope exit
        const potential_energy f(op, pos, dir, tmp, false);

        opt::minimum_bracket(a, b, c, f);

        // std::cout << "bracketing: " << a.x << ", " << c.x << std::endl;

        // TODO compute this from precision
        const int bits = 32;
        {
            boost::uintmax_t iter = info.iterations;
            std::tie(res.x, res.f) = boost::math::tools::brent_find_minima(f,
                                                               a.x, c.x,
                                                               bits,
                                                               iter);
            // std::cout << "brent: " << res.x << std::endl;
        }

    }

    // TODO make sure the last function call is the closest to the optimium
    // op.set(pos, pos, dir, res.x);

    // TODO do we want to do this ?
    // op.mec.propagateX(pos, true);
    // op.forces(op.f);

}

// accumulate force in an external vector
class AugmentedLagrangianVisitor : public simulation::MechanicalVisitor {

public:
    AugmentedLagrangianVisitor(const sofa::core::MechanicalParams* mparams,
                               core::MultiVecId id)
        : MechanicalVisitor(mparams),
          id(id) { }

    core::MultiVecId id;

    Result mstate(simulation::Node* node,
                  core::behavior::BaseMechanicalState* mm) {

        linearsolver::Constraint* c = node->get<linearsolver::Constraint>(core::objectmodel::BaseContext::Local);

        if( c ) {
            // TODO project ?

            // add force to external force
            mm->vOp(params, id.getId(mm), core::ConstVecId::force() );
        }

        return RESULT_CONTINUE;
    }

    virtual Result fwdMappedMechanicalState(simulation::Node* node,
                                            core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

    virtual Result fwdMechanicalState(simulation::Node* node,
                                      core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

};

// somehow setting external force directly does not work
class WriteExternalForceVisitor : public simulation::MechanicalVisitor {
public:
    WriteExternalForceVisitor(const sofa::core::MechanicalParams* mparams,
                              core::MultiVecId id)
        : MechanicalVisitor(mparams),
          id(id) { }

    core::MultiVecId id;

    Result mstate(simulation::Node* /*node*/,
                  core::behavior::BaseMechanicalState* mm) {

        // add force to external force

        // we need to add to externalForce (gravity)
        mm->vOp(params, core::VecId::externalForce(), id.getId(mm));
        // core::VecId::externalForce(),
        // , 1.0 );

        return RESULT_CONTINUE;
    }

    virtual Result fwdMappedMechanicalState(simulation::Node* node,
                                            core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

    virtual Result fwdMechanicalState(simulation::Node* node,
                                      core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

};

SOFA_DECL_CLASS(CompliantStaticSolver)
int CompliantStaticSolverClass = core::RegisterObject("Static solver")
    .add< CompliantStaticSolver >();

    void CompliantStaticSolver::solve(const core::ExecParams* params,
                                SReal dt,
                                core::MultiVecCoordId pos,
                                core::MultiVecDerivId vel) {

        helper op(params, getContext() );

        // mparams setup
        op.mec.mparams.setImplicit(false);
        op.mec.mparams.setEnergy(true);

        // descent direction
        op.realloc(dir);

        // lagrange multipliers
        op.realloc(lambda);

        // some work vector for postions
        op.realloc(tmp);

        // first iteration
        if(!iteration) {
            op.vec.v_clear( lambda );
            previous = 0;

        }

        // why on earth does this dot work ?!

        // core::behavior::MultiVecDeriv ext(&op.vec, core::VecDerivId::externalForce() );
        // op.set(ext, lambda);
        {
            WriteExternalForceVisitor vis(&op.mec.mparams, lambda.id());
            getContext()->executeVisitor(&vis, true);

            // core::behavior::MultiVecDeriv ext(&op.vec, core::VecDerivId::externalForce() );
            // std::cout << ext << std::endl;
        }

        // obtain (projected) gradient
        op.forces( op.f );

        // note: we *could* skip the above when line-search is on
        // after the first iteration, but we would miss any change in
        // the scene (e.g. mouse interaction)

        // polar-ribiere
        const SReal current = op.dot(op.f, op.f);

        if(!iteration) {
            // something large at first ?

            // TODO figure out a reasonable default
            augmented = std::sqrt(current) / 2.0;
        }

        SReal beta = 0;

        const SReal eps = epsilon.getValue();
        if( conjugate.getValue() && std::abs(previous) > eps ) {

            {
                if(iteration > 0) {
                    // polak-ribiere
                    beta = (current - op.dot(vel, op.f) ) / previous;

                    // dai-yuan
                    // beta = current / (op.dot(dir, vel) - op.dot(dir, op.f) );
                }

                // direction reset
                // beta = std::max(0.0, beta);
            }

        }

        // conjugation
        op.set(dir, op.f, dir, beta);

        // polak-ribiere
        {
            // backup previous f to vel
            op.set(vel, op.f);
        }

        // line search
        const unsigned ls = line_search.getValue();
        if( ls ) {
            ls_info info;

            info.eps = eps;
            info.precision = ls_precision.getValue();
            info.iterations = ls_iterations.getValue();
            info.fixed_step = dt;
            info.bracket_step = ls_step.getValue();

            switch( ls ) {

            case LS_SECANT:
                ls_secant(op, pos, dir.id(), info);
                break;

            case LS_BRENT:
                ls_brent(op, pos, dir.id(), info, tmp.id());
                break;

            default:
                throw std::runtime_error("bad line-search");
            }

        } else {
            // fixed step
            op.set(pos, pos, dir.id(), dt);
        }

        const SReal error = std::sqrt( op.dot(op.f, op.f) );

        if( f_printLog.getValue() ) {
            sout << "forces norm: " << error << sendl;
        }

        // augmented lagrangian
        if( error <= augmented ) {

            // TODO don't waste time if we have no constraints
            op.mec.propagateX(pos, true);
            op.forces(op.f);

            AugmentedLagrangianVisitor vis(&op.mec.mparams, lambda.id() );
            getContext()->executeVisitor( &vis, true );

            // TODO should we reset CG ?
            // op.vec.v_clear(dir);

            augmented /= 2;

            if( f_printLog.getValue() ) {
                sout << "augmented lagrangian threshold: " << augmented << sendl;
            }

        }

        // next iteration
        previous = current;

        ++iteration;
    }

    void CompliantStaticSolver::reset() {
        iteration = 0;

    }

    void CompliantStaticSolver::init() {

    }

}
}
}


#include <merely3d/mesh.hpp>

#include <Eigen/Dense>

#include <array>

using Eigen::Vector3f;

namespace merely3d
{
    StaticMesh::StaticMesh(std::vector<float> vertices_and_normals, std::vector<unsigned int> faces)
            : _data(new detail::StaticMeshData(std::move(vertices_and_normals),
                                               std::move(faces)))
    {
        if (_data->faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }
        if (_data->vertices_and_normals.size() % 6 != 0)
        {
            throw std::invalid_argument("Vertices and normals must have size divisible by 6");
        }
    }

    StaticMesh::StaticMesh(std::vector<float> vertices,
                                  std::vector<float> normals,
                                  std::vector<unsigned int> faces)
    {
        if (vertices.size() % 3 != 0)
        {
            throw std::invalid_argument("Vertices must have size divisible by 3");
        }
        if (normals.size() % 3 != 0)
        {
            throw std::invalid_argument("Normals must have size divisible by 3");
        }
        if (faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }
        if (vertices.size() != normals.size())
        {
            throw std::invalid_argument("Number of vertices and normals must be the same.");
        }

        const auto num_vertices = vertices.size() / 3;
        decltype(vertices) vertices_and_normals;
        auto & vn = vertices_and_normals;

        for (size_t i = 0; i < num_vertices; ++i)
        {
            const auto vbegin = vertices.begin() + 3 * i;
            const auto nbegin = normals.begin() + 3 * i;
            vn.insert(vn.end(), vbegin, vbegin + 3);
            vn.insert(vn.end(), nbegin, nbegin + 3);
        }
        _data = std::make_shared<detail::StaticMeshData>(std::move(vertices_and_normals), std::move(faces));
    }

    StaticMesh StaticMesh::with_angle_weighted_normals(std::vector<float> vertices, std::vector<unsigned int> faces)
    {
        if (vertices.size() % 3 != 0)
        {
            throw std::invalid_argument("Vertices must have size divisible by 3");
        }
        if (faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }

        std::vector<float> normals(vertices.size(), 0.0f);

        for (size_t i = 0; i < faces.size(); i += 3)
        {
            const auto triangle = std::array<unsigned int, 3> { faces[i], faces[i + 1], faces[i + 2] };

            const auto v0 = Vector3f(vertices[3 * triangle[0]], vertices[3 * triangle[0] + 1], vertices[3 * triangle[0] + 2]);
            const auto v1 = Vector3f(vertices[3 * triangle[1]], vertices[3 * triangle[1] + 1], vertices[3 * triangle[1] + 2]);
            const auto v2 = Vector3f(vertices[3 * triangle[2]], vertices[3 * triangle[2] + 1], vertices[3 * triangle[2] + 2]);

            const auto face_vertices = std::array<Vector3f, 3> { v0, v1, v2 };
            const Vector3f face_normal = (v1 - v0).cross(v2 - v0);

            // For each vertex in the face, compute contribution to angle-weighted vertex normals
            for (size_t j = 0; j < 3; ++j)
            {
                // Compute left and right edge vectors that meet in local vertex (j + 1)
                const Vector3f left = (face_vertices[j] - face_vertices[(j + 1) % 3]).normalized();
                const Vector3f right = (face_vertices[(j + 2) % 3] - face_vertices[(j + 1) % 3]).normalized();
                const float dot = left.dot(right);
                // Rounding errors may push the dot product beyond the domain of acos, so we clamp the result
                // to the allowed range [-1, 1]
                const float angle = std::acos(std::max(-1.0f, std::min(dot, 1.0f)));
                const auto vertex_index = triangle[(j + 1) % 3];
                assert(vertex_index < vertices.size() / 3);
                assert(std::isfinite(angle));
                normals[3 * vertex_index + 0] += angle * face_normal[0];
                normals[3 * vertex_index + 1] += angle * face_normal[1];
                normals[3 * vertex_index + 2] += angle * face_normal[2];
            }
        }

        for (size_t i = 0; i < normals.size(); i += 3)
        {
            const Vector3f normal = Vector3f(normals[i], normals[i + 1], normals[i + 2]).normalized();
            normals[i + 0] = normal[0];
            normals[i + 1] = normal[1];
            normals[i + 2] = normal[2];
        }

        return StaticMesh(std::move(vertices), std::move(normals), std::move(faces));
    }
}


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_PLAYGROUND_OPERATORS_RECONSTRUCTIONS_HH
#define DUNE_GDT_PLAYGROUND_OPERATORS_RECONSTRUCTIONS_HH

#include <boost/numeric/conversion/cast.hpp>

#include <dune/gdt/playground/localevaluation/swipdg.hh>
#include <dune/gdt/operators/fluxreconstruction.hh>

namespace Dune {
namespace GDT {
namespace Operators {

template< class GridViewType, class DiffusionFactorType, class DiffusionTensorType >
class DiffusiveFluxReconstruction
{
  static_assert(GridViewType::dimension == 2, "Only implemented for dimDomain 2 at the moment!");
  static_assert(std::is_base_of< Stuff::IsLocalizableFunction, DiffusionFactorType >::value,
                "DiffusionFactorType has to be tagged as Stuff::IsLocalizableFunction!");
  static_assert(std::is_base_of< Stuff::IsLocalizableFunction, DiffusionTensorType >::value,
                "DiffusionTensorType has to be tagged as Stuff::IsLocalizableFunction!");
public:
  typedef typename GridViewType::template Codim< 0 >::Entity EntityType;
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;
  typedef typename DiffusionFactorType::RangeFieldType FieldType;
  typedef typename DiffusionFactorType::DomainType DomainType;

private:
  static_assert(dimDomain == 2, "Not implemented!");

public:
  DiffusiveFluxReconstruction(const GridViewType& grid_view,
                              const DiffusionFactorType& diffusion_factor,
                              const DiffusionTensorType& diffusion_tensor,
                              const size_t over_integrate = 0)
    : grid_view_(grid_view)
    , diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
    , over_integrate_(over_integrate)
  {}

  template< class GV, class V >
  void apply(const Stuff::LocalizableFunctionInterface< EntityType, DomainFieldType, dimDomain, FieldType, 1 >& source,
             DiscreteFunction< Spaces::RT::PdelabBased< GV, 0, FieldType, dimDomain >, V >& range) const
  {
    const auto& rtn0_space = range.space();
    auto& range_vector = range.vector();
    const FieldType infinity = std::numeric_limits< FieldType >::infinity();
    for (size_t ii = 0; ii < range_vector.size(); ++ii)
      range_vector[ii] = infinity;
    const LocalEvaluation::SWIPDG::Inner< DiffusionFactorType, DiffusionTensorType >
        inner_evaluation(diffusion_factor_, diffusion_tensor_);
    const LocalEvaluation::SWIPDG::BoundaryLHS< DiffusionFactorType, DiffusionTensorType >
        boundary_evaluation(diffusion_factor_, diffusion_tensor_);
    const Stuff::Functions::Constant< EntityType, DomainFieldType, dimDomain, FieldType, 1 > constant_one(1);
    DomainType normal(0);
    DomainType xx_entity(0);
    DynamicMatrix< FieldType > tmp_matrix(1, 1, 0);
    DynamicMatrix< FieldType > tmp_matrix_en_en(1, 1, 0);
    DynamicMatrix< FieldType > tmp_matrix_en_ne(1, 1, 0);
    std::vector< typename Spaces::RT::PdelabBased< GV, 0, FieldType, dimDomain >::BaseFunctionSetType::RangeType >
        basis_values(rtn0_space.mapper().maxNumDofs(),
                     typename Spaces::RT::PdelabBased< GV, 0, FieldType, dimDomain >::BaseFunctionSetType::RangeType(0));
    // walk the grid
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto local_DoF_indices = rtn0_space.local_DoF_indices(entity);
      const auto global_DoF_indices = rtn0_space.mapper().globalIndices(entity);
      assert(global_DoF_indices.size() == local_DoF_indices.size());
      const auto local_diffusion_factor = diffusion_factor_.local_function(entity);
      const auto local_diffusion_tensor = diffusion_tensor_.local_function(entity);
      const auto local_source = source.local_function(entity);
      const auto local_basis = rtn0_space.base_function_set(entity);
      const auto local_constant_one = constant_one.local_function(entity);
      // walk the intersections
      const auto intersection_it_end = grid_view_.iend(entity);
      for (auto intersection_it = grid_view_.ibegin(entity);
           intersection_it != intersection_it_end;
           ++intersection_it) {
        const auto& intersection = *intersection_it;
        if (intersection.neighbor() && !intersection.boundary()) {
          const auto neighbor_ptr = intersection.outside();
          const auto& neighbor = *neighbor_ptr;
          if (grid_view_.indexSet().index(entity) < grid_view_.indexSet().index(neighbor)) {
            const auto local_diffusion_factor_neighbor = diffusion_factor_.local_function(neighbor);
            const auto local_diffusion_tensor_neighbor = diffusion_tensor_.local_function(neighbor);
            const auto local_source_neighbor = source.local_function(neighbor);
            const auto local_constant_one_neighbor = constant_one.local_function(neighbor);
            const size_t local_intersection_index = intersection.indexInInside();
            const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
            // do a face quadrature
            FieldType lhs = 0;
            FieldType rhs = 0;
            const size_t integrand_order = inner_evaluation.order(*local_diffusion_factor,
                                                                  *local_diffusion_tensor,
                                                                  *local_diffusion_factor_neighbor,
                                                                  *local_diffusion_tensor_neighbor,
                                                                  *local_constant_one,
                                                                  *local_source,
                                                                  *local_constant_one_neighbor,
                                                                  *local_source_neighbor);
            const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain - 1 >::rule(
                  intersection.type(), boost::numeric_cast< int >(integrand_order + over_integrate_));
            const auto quadrature_it_end = quadrature.end();
            for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
              const auto& xx_intersection = quadrature_it->position();
              xx_entity = intersection.geometryInInside().global(xx_intersection);
              normal = intersection.unitOuterNormal(xx_intersection);
              const FieldType integration_factor = intersection.geometry().integrationElement(xx_intersection);
              const FieldType weigth = quadrature_it->weight();
              // evalaute
              local_basis.evaluate(xx_entity, basis_values);
              const auto& basis_value = basis_values[local_DoF_index];
              tmp_matrix *= 0.0;
              tmp_matrix_en_en *= 0.0;
              tmp_matrix_en_ne *= 0.0;
              inner_evaluation.evaluate(*local_diffusion_factor,
                                        *local_diffusion_tensor,
                                        *local_diffusion_factor_neighbor,
                                        *local_diffusion_tensor_neighbor,
                                        *local_constant_one,
                                        *local_source,
                                        *local_constant_one_neighbor,
                                        *local_source_neighbor,
                                        intersection,
                                        xx_intersection,
                                        tmp_matrix_en_en, // <- we are interested in this one
                                        tmp_matrix,
                                        tmp_matrix_en_ne, // <- and this one
                                        tmp_matrix);
              // compute integrals
              assert(tmp_matrix_en_en.rows() >= 1);
              assert(tmp_matrix_en_en.cols() >= 1);
              assert(tmp_matrix_en_ne.rows() >= 1);
              assert(tmp_matrix_en_ne.cols() >= 1);
              lhs += integration_factor * weigth * (basis_value * normal);
              rhs += integration_factor * weigth * (tmp_matrix_en_en[0][0] + tmp_matrix_en_ne[0][0]);
            } // do a face quadrature
            // set DoF
            const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
            // and make sure we are the first to do so
            assert(!(range_vector[global_DoF_index] < infinity));
            range_vector[global_DoF_index] = rhs / lhs;
          }
        } else if (intersection.boundary() && !intersection.neighbor()) {
          const size_t local_intersection_index = intersection.indexInInside();
          const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
          // do a face quadrature
          FieldType lhs = 0;
          FieldType rhs = 0;
          const size_t integrand_order = boundary_evaluation.order(*local_diffusion_factor,
                                                                   *local_diffusion_tensor,
                                                                   *local_source,
                                                                   *local_constant_one);
          const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain - 1 >::rule(
                intersection.type(), boost::numeric_cast< int >(integrand_order + over_integrate_));
          const auto quadrature_it_end = quadrature.end();
          for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
            const auto xx_intersection = quadrature_it->position();
            normal = intersection.unitOuterNormal(xx_intersection);
            const FieldType integration_factor = intersection.geometry().integrationElement(xx_intersection);
            const FieldType weigth = quadrature_it->weight();
            xx_entity = intersection.geometryInInside().global(xx_intersection);
            // evalaute
            local_basis.evaluate(xx_entity, basis_values);
            const auto& basis_value = basis_values[local_DoF_index];
            tmp_matrix *= 0.0;
            boundary_evaluation.evaluate(*local_diffusion_factor,
                                         *local_diffusion_tensor,
                                         *local_constant_one,
                                         *local_source,
                                         intersection,
                                         xx_intersection,
                                         tmp_matrix);
            // compute integrals
            assert(tmp_matrix.rows() >= 1);
            assert(tmp_matrix.cols() >= 1);
            lhs += integration_factor * weigth * (basis_value * normal);
            rhs += integration_factor * weigth * tmp_matrix[0][0];
          } // do a face quadrature
          // set DoF
          const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
          assert(!(range_vector[global_DoF_index] < infinity));
          // and make sure we are the first to do so
          range_vector[global_DoF_index] = rhs / lhs;
        } else
          DUNE_THROW(Stuff::Exceptions::internal_error, "Unknown intersection type!");
      } // walk the intersections
    } // walk the grid
  } // ... apply(...)

private:
  const GridViewType& grid_view_;
  const DiffusionFactorType& diffusion_factor_;
  const DiffusionTensorType& diffusion_tensor_;
  const size_t over_integrate_;
}; // class DiffusiveFluxReconstruction

template< class GV, class DF, class DT >
DiffusiveFluxReconstruction< GV, DF, DT > make_diffusive_flux_reconstruction(const GV& grid_view,
                                                                             const DF& diffusion_factor,
                                                                             const DT& diffusion_tensor,
                                                                             const size_t over_integrate = 0)
{
  return DiffusiveFluxReconstruction< GV, DF, DT >(grid_view, diffusion_factor, diffusion_tensor, over_integrate);
}

} // namespace Operators
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_PLAYGROUND_OPERATORS_RECONSTRUCTIONS_HH


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <jsoncpp/json/json.h>
#include <NTL/ZZ.h>
#include "GACDDecrypter.h"

NTL::ZZ GACDDecrypter::decrypt(NTL::ZZ& ciphertext){
	return ciphertext/k;
}

void GACDDecrypter::readSecretsFromJSON(std::string& secrets){
	Json::Value root;
	Json::Reader reader;
	bool parsingSuccessful = reader.parse(secrets,root);
	if (parsingSuccessful){
		k = NTL::conv<NTL::ZZ>(root["key"].asCString());
	}
}


/// @brief Eigen wrappers for different CCD methods

#pragma once

#include <array>

#include <Eigen/Core>

namespace ccd {

/// Methods of continuous collision detection.
enum CCDMethod {
    /// Etienne Vouga's CCD using a root finder in floating points
    FLOATING_POINT_ROOT_FINDER = 0,
    /// Floating-point root-finder minimum separation CCD of [Lu et al. 2018]
    MIN_SEPARATION_ROOT_FINDER,
    /// Root parity method of [Brochu et al. 2012]
    ROOT_PARITY,
    /// Teseo's reimplementation of [Brochu et al. 2012] using rationals
    RATIONAL_ROOT_PARITY,
    /// Root parity with and fixes
    FLOATING_POINT_ROOT_PARITY,
    /// Rational root parity with fixes
    RATIONAL_FIXED_ROOT_PARITY,
    /// Bernstein sign classification method of [Tang et al. 2014]
    BSC,
    /// TightCCD method of [Wang et al. 2015]
    TIGHT_CCD,
    // SafeCCD
    SAFE_CCD,
    /// Interval based CCD of [Redon et al. 2002]
    UNIVARIATE_INTERVAL_ROOT_FINDER,
    /// Interval based CCD of [Redon et al. 2002] solved using [Snyder 1992]
    MULTIVARIATE_INTERVAL_ROOT_FINDER,
    /// Custom inclusion based CCD of [Wang et al. 2020]
    TIGHT_INCLUSION,
    /// WARNING: Not a method! Counts the number of methods.
    NUM_CCD_METHODS
};

static const char* method_names[CCDMethod::NUM_CCD_METHODS] = {
    "FloatingPointRootFinder",
    "MinSeparationRootFinder",
    "RootParity",
    "RationalRootParity",
    "FloatingPointRootParity",
    "RationalFixedRootParity",
    "BSC",
    "TightCCD",
    "SafeCCD",
    "UnivariateIntervalRootFinder",
    "MultivariateIntervalRootFinder",
    "TightInclusion",
};

/// Minimum separation distance used when looking for 0 distance collisions.
static const double DEFAULT_MIN_DISTANCE = 1e-8;

/**
 * @brief Detect collisions between a vertex and a triangular face.
 *
 * Looks for collisions between a point and triangle as they move linearily
 * with constant velocity. Returns true if the vertex and face collide.
 *
 * @param[in]  vertex_start        Start position of the vertex.
 * @param[in]  face_vertex0_start  Start position of the first vertex of the
 *                                 face.
 * @param[in]  face_vertex1_start  Start position of the second vertex of the
 *                                 face.
 * @param[in]  face_vertex2_start  Start position of the third vertex of the
 *                                 face.
 * @param[in]  vertex_end          End position of the vertex.
 * @param[in]  face_vertex0_end    End position of the first vertex of the
 *                                 face.
 * @param[in]  face_vertex1_end    End position of the second vertex of the
 *                                 face.
 * @param[in]  face_vertex2_end    End position of the third vertex of the
 *                                 face.
 * @param[in]  method              Method of exact CCD.
 *
 * @returns  True if the vertex and face collide.
 */
bool vertexFaceCCD(
    const Eigen::Vector3d& vertex_start,
    const Eigen::Vector3d& face_vertex0_start,
    const Eigen::Vector3d& face_vertex1_start,
    const Eigen::Vector3d& face_vertex2_start,
    const Eigen::Vector3d& vertex_end,
    const Eigen::Vector3d& face_vertex0_end,
    const Eigen::Vector3d& face_vertex1_end,
    const Eigen::Vector3d& face_vertex2_end,
    const CCDMethod method,
    const double tolerance = 1e-6,
    const long max_iter = 1e6,
    const std::array<double, 3>& err = { { -1, 0, 0 } });

/**
 * @brief Detect collisions between two edges as they move.
 *
 * Looks for collisions between edges as they move linearly with constant
 * velocity. Returns true if the edges collide.
 *
 * @param[in]  edge0_vertex0_start  Start position of the first edge's first
 *                                  vertex.
 * @param[in]  edge0_vertex1_start  Start position of the first edge's second
 *                                  vertex.
 * @param[in]  edge1_vertex0_start  Start position of the second edge's first
 *                                  vertex.
 * @param[in]  edge1_vertex1_start  Start position of the second edge's second
 *                                  vertex.
 * @param[in]  edge0_vertex0_end    End position of the first edge's first
 *                                  vertex.
 * @param[in]  edge0_vertex1_end    End position of the first edge's second
 *                                  vertex.
 * @param[in]  edge1_vertex0_end    End position of the second edge's first
 *                                  vertex.
 * @param[in]  edge1_vertex1_end    End position of the second edge's second
 *                                  vertex.
 * @param[in]  method               Method of exact CCD.
 *
 * @returns True if the edges collide.
 */
bool edgeEdgeCCD(
    const Eigen::Vector3d& edge0_vertex0_start,
    const Eigen::Vector3d& edge0_vertex1_start,
    const Eigen::Vector3d& edge1_vertex0_start,
    const Eigen::Vector3d& edge1_vertex1_start,
    const Eigen::Vector3d& edge0_vertex0_end,
    const Eigen::Vector3d& edge0_vertex1_end,
    const Eigen::Vector3d& edge1_vertex0_end,
    const Eigen::Vector3d& edge1_vertex1_end,
    const CCDMethod method,
    const double tolerance = 1e-6,
    const long max_iter = 1e6,
    const std::array<double, 3>& err = { { -1, 0, 0 } });

/**
 * @brief Detect proximity collisions between a vertex and a triangular face.
 *
 * Looks for collisions between a point and triangle as they move linearily
 * with constant velocity. Returns true if the vertex and face collide.
 *
 * @param[in]  vertex_start        Start position of the vertex.
 * @param[in]  face_vertex0_start  Start position of the first vertex of the
 *                                 face.
 * @param[in]  face_vertex1_start  Start position of the second vertex of the
 *                                 face.
 * @param[in]  face_vertex2_start  Start position of the third vertex of the
 *                                 face.
 * @param[in]  vertex_end          End position of the vertex.
 * @param[in]  face_vertex0_end    End position of the first vertex of the
 *                                 face.
 * @param[in]  face_vertex1_end    End position of the second vertex of the
 *                                 face.
 * @param[in]  face_vertex2_end    End position of the third vertex of the
 *                                 face.
 * @param[in]  method              Method of minimum separation CCD.
 *
 * @returns  True if the vertex and face collide.
 */
bool vertexFaceMSCCD(
    const Eigen::Vector3d& vertex_start,
    const Eigen::Vector3d& face_vertex0_start,
    const Eigen::Vector3d& face_vertex1_start,
    const Eigen::Vector3d& face_vertex2_start,
    const Eigen::Vector3d& vertex_end,
    const Eigen::Vector3d& face_vertex0_end,
    const Eigen::Vector3d& face_vertex1_end,
    const Eigen::Vector3d& face_vertex2_end,
    const double min_distance,
    const CCDMethod method,
    const double tolerance = 1e-6,
    const long max_iter = 1e6,
    const std::array<double, 3>& err = { { -1, 0, 0 } });

/**
 * @brief Detect proximity collisions between two edges as they move.
 *
 * Looks for collisions between edges as they move linearly with constant
 * velocity. Returns true if the edges collide.
 *
 * @param[in]  edge0_vertex0_start  Start position of the first edge's first
 *                                  vertex.
 * @param[in]  edge0_vertex1_start  Start position of the first edge's second
 *                                  vertex.
 * @param[in]  edge1_vertex0_start  Start position of the second edge's first
 *                                  vertex.
 * @param[in]  edge1_vertex1_start  Start position of the second edge's second
 *                                  vertex.
 * @param[in]  edge0_vertex0_end    End position of the first edge's first
 *                                  vertex.
 * @param[in]  edge0_vertex1_end    End position of the first edge's second
 *                                  vertex.
 * @param[in]  edge1_vertex0_end    End position of the second edge's first
 *                                  vertex.
 * @param[in]  edge1_vertex1_end    End position of the second edge's second
 *                                  vertex.
 * @param[in]  method               Method of minimum separation CCD.
 *
 * @returns True if the edges collide.
 */
bool edgeEdgeMSCCD(
    const Eigen::Vector3d& edge0_vertex0_start,
    const Eigen::Vector3d& edge0_vertex1_start,
    const Eigen::Vector3d& edge1_vertex0_start,
    const Eigen::Vector3d& edge1_vertex1_start,
    const Eigen::Vector3d& edge0_vertex0_end,
    const Eigen::Vector3d& edge0_vertex1_end,
    const Eigen::Vector3d& edge1_vertex0_end,
    const Eigen::Vector3d& edge1_vertex1_end,
    const double min_distance,
    const CCDMethod method,
    const double tolerance = 1e-6,
    const long max_iter = 1e6,
    const std::array<double, 3>& err = { { -1, 0, 0 } });

inline bool is_minimum_separation_method(const CCDMethod& method)
{
    switch (method) {
    case CCDMethod::MIN_SEPARATION_ROOT_FINDER:
    case CCDMethod::TIGHT_INCLUSION:
        return true;
    default:
        return false;
    }
}

inline bool is_conservative_method(const CCDMethod& method)
{
    switch (method) {
    // MIN_SEPARATION_ROOT_FINDER is conservative because minimum separation
    // distance of zero does not work well.
    case CCDMethod::MIN_SEPARATION_ROOT_FINDER:
    case CCDMethod::TIGHT_CCD:
    case CCDMethod::UNIVARIATE_INTERVAL_ROOT_FINDER:
    case CCDMethod::MULTIVARIATE_INTERVAL_ROOT_FINDER:
    case CCDMethod::TIGHT_INCLUSION:
        return true;
    default:
        return false;
    }
}

inline bool is_time_of_impact_computed(const CCDMethod& method)
{
    switch (method) {
    case CCDMethod::FLOATING_POINT_ROOT_FINDER:
    case CCDMethod::MIN_SEPARATION_ROOT_FINDER:
    case CCDMethod::UNIVARIATE_INTERVAL_ROOT_FINDER:
    case CCDMethod::MULTIVARIATE_INTERVAL_ROOT_FINDER:
    case CCDMethod::TIGHT_INCLUSION:
        return true;
    default:
        return false;
    }
}

inline bool is_method_enabled(const CCDMethod& method)
{
    switch (method) {
    case FLOATING_POINT_ROOT_FINDER:
        return CCD_WRAPPER_WITH_FPRF;

    case MIN_SEPARATION_ROOT_FINDER:
        return CCD_WRAPPER_WITH_MSRF;

    case ROOT_PARITY:
        return CCD_WRAPPER_WITH_RP;

    case RATIONAL_ROOT_PARITY:
        return CCD_WRAPPER_WITH_RRP;

    case FLOATING_POINT_ROOT_PARITY:
        return CCD_WRAPPER_WITH_FPRP;

    case RATIONAL_FIXED_ROOT_PARITY:
        return CCD_WRAPPER_WITH_RFRP;

    case BSC:
        return CCD_WRAPPER_WITH_BSC;

    case TIGHT_CCD:
        return CCD_WRAPPER_WITH_TIGHT_CCD;

    case SAFE_CCD:
        return CCD_WRAPPER_WITH_SAFE_CCD;

    case UNIVARIATE_INTERVAL_ROOT_FINDER:
    case MULTIVARIATE_INTERVAL_ROOT_FINDER:
        return CCD_WRAPPER_WITH_INTERVAL;

    case TIGHT_INCLUSION:
        return CCD_WRAPPER_WITH_TIGHT_INCLUSION;

    default:
        return false;
    }
}
} // namespace ccd