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

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <string>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/strong_components.hpp>

//for debugging only
#include <iostream>
#include <iomanip>
#include <fstream>
#include <time.h>

namespace
{

class compClass
{
public:
	bool operator()(const std::pair<int, int>& a, const std::pair<int, int>& b)
	{
		if (a.second != b.second)
			return !(a.second < b.second);
		else
			return a.first < b.first;
	}
};

bool compareAbs(double a, double b)
{
	return std::abs(a) < std::abs(b);
}

//the index is 1-based, according to the paper
struct Posiform
{
	std::vector<std::pair<std::pair<int, int>, long long int> > quadratic;
	std::vector<std::pair<int, long long int> > linear;
	long long int cst;
	int numVars;
};

struct SC
{
	std::vector<int> original;
	std::vector<int> positive;
	std::vector<int> negative;
};

struct SCRet
{
	std::vector<SC> S;
	compressed_matrix::CompressedMatrix<long long int> G;
};

Posiform BQPToPosiform(const compressed_matrix::CompressedMatrix<long long int>& Q, bool makeRandom = false)
{
	int numVariables = Q.numRows();
	Posiform ret;
	ret.numVars = numVariables;
	std::vector<long long int> q(numVariables);

	//q = diag(Q);
	for (int i = 0; i < numVariables; i++)
		q[i] = Q.get(i, i);

	//tmpQ = triu(Q,1);
	std::vector<int> tmpQRowOffsets(numVariables + 1);
	std::vector<int> tmpQColIndices; tmpQColIndices.reserve(Q.nnz());
	std::vector<long long int> tmpQValues; tmpQValues.reserve(Q.nnz());
	int index = 0;
	for (int i = 0; i < Q.numRows(); i++)
	{
		int start = Q.rowOffsets()[i];
		int end = Q.rowOffsets()[i + 1];
		tmpQRowOffsets[i] = index;
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = Q.colIndices()[j];
			if (r != c)
			{
				tmpQColIndices.push_back(c);
				tmpQValues.push_back(Q.values()[j]); //Q.values()[j] is Q(r, c)
				++index;
			}
		}
	}
	tmpQRowOffsets.back() = index; //number of non-zero elements

	compressed_matrix::CompressedMatrix<long long int> tmpQ(numVariables, numVariables, tmpQRowOffsets, tmpQColIndices, tmpQValues);

	if (!makeRandom) //make a deterministic posiform from Q
	{
		//cPrime = q + sum(tmpQ.*(tmpQ<0),2); and quadratic term
		std::vector<long long int> cPrime = q;
		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] < 0)
				{
					cPrime[i] += tmpQ.values()[j];
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, -(c + 1)), -tmpQ.values()[j])); //+1 makes it 1-based
				}
				else if (tmpQ.values()[j] > 0)
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, c + 1), tmpQ.values()[j])); //+1 makes it 1-based
			}
		}

		//constant term and linear term
		ret.cst = 0;
		for (int i = 0; i < numVariables; i++)
		{
			if (cPrime[i] < 0)
			{
				ret.cst += cPrime[i]; //constant term
				ret.linear.push_back(std::make_pair(-(i + 1), -cPrime[i])); //+1 makes it 1-based
			}
			else if (cPrime[i] > 0)
				ret.linear.push_back(std::make_pair(i + 1, cPrime[i])); //+1 makes it 1-based
		}
	}
	else
	{
		std::vector<long long int> linear = q;
		std::set<std::pair<int, int> > negPairs;
		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] < 0)
				{
					if (rand() % 2 == 0)
						negPairs.insert(std::make_pair(-(r + 1), c + 1)); //+1 makes it 1-based
					else
						negPairs.insert(std::make_pair(r + 1, -(c + 1))); //+1 makes it 1-based
				}
			}
		}

		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] > 0)
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, c + 1), tmpQ.values()[j]));
				else
				{
					if (negPairs.find(std::make_pair(r + 1, -(c + 1)))!=negPairs.end())
						ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, -(c + 1)), -tmpQ.values()[j]));
					else
						ret.quadratic.push_back(std::make_pair(std::make_pair(-(r + 1), c + 1), -tmpQ.values()[j]));
				}
			}
		}

		for (std::set<std::pair<int, int> >::iterator it = negPairs.begin(); it != negPairs.end(); ++it)
		{
			if (it->first > 0)
				linear[it->first - 1] += tmpQ(it->first - 1, -it->second - 1);
			else
				linear[it->second - 1] += tmpQ(-it->first - 1, it->second - 1);
		}

		ret.cst = 0;

		for (int i = 0; i < numVariables; i++)
		{
			if (linear[i] < 0)
			{
				ret.cst += linear[i];
				ret.linear.push_back(std::make_pair(-(i + 1), -linear[i]));
			}
			else if (linear[i] > 0)
				ret.linear.push_back(std::make_pair(i + 1, linear[i]));
		}
	}

	return ret;
}

compressed_matrix::CompressedMatrix<long long int> posiformToImplicationNetwork_1(const Posiform& p)
{
	int n = p.numVars;
	int numVertices = 2 * n + 2;
	int source = 0;
	int sink = n + 1;

	//n = p.numVars
	//0: source;
	//1 to n: x_1 to x_n
	//n+1: sink
	//n+2 to 2*n+1: \overline_x_1 to \overline_x_n
	std::map<std::pair<int, int>, long long int> m;
	for (int i = 0; i < p.linear.size(); i++)
	{
		int v = p.linear[i].first;

		long long int capacity = p.linear[i].second; // originally p.linear[i].second/2
		if (v > 0)
		{
			m[std::make_pair(source, v + n + 1)] = capacity;
			m[std::make_pair(v, sink)] = capacity;
		}
		else
		{
			v = std::abs(v);
			m[std::make_pair(source, v)] = capacity;
			m[std::make_pair(v + n + 1, sink)] = capacity;
		}
	}

	for (int i = 0; i < p.quadratic.size(); i++)
	{
		int v_1 = p.quadratic[i].first.first;
		int v_2 = p.quadratic[i].first.second;

		long long int capacity = p.quadratic[i].second; // originally p.quadratic[i].second/2
		if (v_1 < 0)
		{
			v_1 = std::abs(v_1);
			m[std::make_pair(v_1 + n + 1, v_2 + n + 1)] = capacity;
			m[std::make_pair(v_2, v_1)] = capacity;
		}
		else if (v_2 < 0)
		{
			v_2 = std::abs(v_2);
			m[std::make_pair(v_1, v_2)] = capacity;
			m[std::make_pair(v_2 + n + 1, v_1 + n + 1)] = capacity;
		}
		else
		{
			m[std::make_pair(v_1, v_2 + n + 1)] = capacity;
			m[std::make_pair(v_2, v_1 + n + 1)] = capacity;
		}
	}

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, m);
}

compressed_matrix::CompressedMatrix<long long int> posiformToImplicationNetwork_2(const Posiform& p)
{
	int n = p.numVars;
	int numVertices = 2 * n + 2;
	int source = n;
	int sink = 2 * n + 1;

	//n = p.numVars
	//0 to n-1: x_1 to x_n
	//n: source;
	//n+1 to 2*n: \overline_x_1 to \overline_x_n
	//2*n+1: sink
	std::map<std::pair<int, int>, long long int> m;
	for (int i = 0; i < p.linear.size(); i++)
	{
		int v = p.linear[i].first;
		long long int capacity = p.linear[i].second; // originally p.linear[i].second/2
		if (v > 0)
		{
			m[std::make_pair(source, v + n)] = capacity;
			m[std::make_pair(v - 1, sink)] = capacity;
		}
		else
		{
			v = std::abs(v);
			m[std::make_pair(source, v - 1)] = capacity;
			m[std::make_pair(v + n, sink)] = capacity;
		}
	}

	for (int i = 0; i < p.quadratic.size(); i++)
	{
		int v_1 = p.quadratic[i].first.first;
		int v_2 = p.quadratic[i].first.second;
		long long int capacity = p.quadratic[i].second; // originally p.quadratic[i].second/2
		if (v_1 < 0)
		{
			v_1 = std::abs(v_1);
			m[std::make_pair(v_1 + n, v_2 + n)] = capacity;
			m[std::make_pair(v_2 - 1, v_1 - 1)] = capacity;
		}
		else if (v_2 < 0)
		{
			v_2 = std::abs(v_2);
			m[std::make_pair(v_1 - 1, v_2 - 1)] = capacity;
			m[std::make_pair(v_2 + n, v_1 + n)] = capacity;
		}
		else
		{
			m[std::make_pair(v_1 - 1, v_2 + n)] = capacity;
			m[std::make_pair(v_2 - 1, v_1 + n)] = capacity;
		}
	}

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, m);
}

//this version returns R instead of F
compressed_matrix::CompressedMatrix<long long int> maxFlow(const compressed_matrix::CompressedMatrix<long long int>& A)
{
	int numVertices = A.numRows();
	int numVariables = numVertices / 2 - 1;

	//clock_t curr_1 = clock();
	//clock_t curr_2;

	using namespace boost;

	typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
	typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, long long int, property<edge_residual_capacity_t, long long int, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is long long int
	//typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, double, property<edge_residual_capacity_t, double, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is double

	Graph g;

	property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
	property_map<Graph, edge_reverse_t>::type reverse_edge = get(edge_reverse, g);
	property_map<Graph, edge_residual_capacity_t>::type residual_capacity = get(edge_residual_capacity, g);

	std::vector<Traits::vertex_descriptor> verts(numVertices);
	for (int i = 0; i < numVertices; ++i)
		verts[i] = add_vertex(g);

	Traits::vertex_descriptor s = verts[0];
	Traits::vertex_descriptor t = verts[numVariables + 1];

	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int cap = A.values()[j];

			Traits::edge_descriptor e1, e2;
			bool in1, in2;
			boost::tie(e1, in1) = add_edge(verts[r], verts[c], g);
			boost::tie(e2, in2) = add_edge(verts[c], verts[r], g);

			capacity[e1] = cap;
			capacity[e2] = 0;
			reverse_edge[e1] = e2;
			reverse_edge[e2] = e1;
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_constructing_graph_for_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	push_relabel_max_flow(g, s, t);

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_for_boost_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<int> RRowOffsets(numVertices+1);
	std::vector<int> RColIndices;
	std::vector<long long int> RValues;
	int offset = 0;
	int currRow = 0;
	graph_traits<Graph>::vertex_iterator u_iter, u_end;
	graph_traits<Graph>::out_edge_iterator ei, e_end;
	for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
		for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
		{
			if (capacity[*ei] > 0)
			{
				if (currRow <= *u_iter)
				{
					for (int i = currRow; i <= *u_iter; i++)
						RRowOffsets[i] = offset;
					currRow = static_cast<int>((*u_iter) + 1);
				}

				RColIndices.push_back(static_cast<int>(target(*ei, g)));
				RValues.push_back(residual_capacity[*ei]);
				++offset;
			}
		}
	for (int i = currRow; i < RRowOffsets.size(); i++)
		RRowOffsets[i] = offset;

	compressed_matrix::CompressedMatrix<long long int> R(numVertices, numVertices, RRowOffsets, RColIndices, RValues); //residual

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_get_R: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::map<std::pair<int, int>, long long int> rm = compressed_matrix::compressedMatrixToMap(R);
	std::map<std::pair<int, int>, long long int> rmMissing;

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_copy_R_to_map: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			long long int Arc = A.get(r, c);
			long long int Rcr = R.get(c, r);
			if (Arc != 0 && R.values()[j] + Rcr - Arc != 0) //it->second is: R(r, c); here it means R(r, c)+R(c, r)!=A(r, c), so R(c, r) is missing
				rmMissing.insert(std::make_pair(std::make_pair(c, r), Arc - R.values()[j]));
		}
	}

	for (std::map<std::pair<int, int>, long long int>::iterator it = rmMissing.begin(), end = rmMissing.end(); it != end; ++it)
		rm.insert(*it);

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_get_missing_elements_in_R: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, rm);
}

std::vector<int> bfs_for_method_2(const compressed_matrix::CompressedMatrix<long long int>& g, int s)
{
	std::queue<int> q;
	q.push(s);
	int numVertices = g.numRows();
	std::vector<int> visited(numVertices, -1);
	visited[s] = 1;

	while (!q.empty())
	{
		int curr = q.front(); q.pop();
		int start = g.rowOffsets()[curr];
		int end = g.rowOffsets()[curr + 1];
		for (int j = start; j < end; j++)
		{
			int c = g.colIndices()[j];
			if (g.values()[j] != 0 && visited[c] == -1)
			{
				visited[c] = 1;
				q.push(c);
			}
		}
	}

	return visited;
}

compressed_matrix::CompressedMatrix<long long int> makeResidualSymmetric(const compressed_matrix::CompressedMatrix<long long int>& R)
{
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	int numVertices = R.numRows();
	int numVariables = numVertices / 2 - 1;
	std::map<std::pair<int, int>, long long int> MsymR;
	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			if (r != c)
			{
				MsymR[std::make_pair(r, c)] += R.values()[j];
				int compR;
				if (r <= numVariables)
					compR = r + (numVariables + 1);
				else
					compR = r - (numVariables + 1);
				int compC;
				if (c <= numVariables)
					compC = c + (numVariables + 1);
				else
					compC = c - (numVariables + 1);
				MsymR[std::make_pair(compC, compR)] += R.values()[j];
			}
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside makeResidualSym: Time elapsed_get_MsymR: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//remove extra 0s in MsymR, required because there will possibly be 0 in MsymR and the 0 will add non-exist edges in stronglyConnectedComponents()
	//cause the connected components results incorrct !!!
	std::map<std::pair<int, int>, long long int> MsymRWithoutZero;
	for (std::map<std::pair<int, int>, long long int>::const_iterator it = MsymR.begin(), end = MsymR.end(); it != end; ++it)
	{
		if (it->second != 0 && it->first.first != numVariables + 1 && it->first.second != 0) //add clearing R here !!!
			MsymRWithoutZero.insert(*it);
	}

	//curr_2 = clock();
	//mexPrintf("inside makeResidualSym: Time elapsed_remove_zero_in_MsymR: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, MsymRWithoutZero);
}

SCRet stronglyConnectedComponents(const compressed_matrix::CompressedMatrix<long long int>& R)
{
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	int numVertices = R.numRows();
	int numVariables = numVertices / 2 - 1;

	using namespace boost;

	typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
	typedef adjacency_list < vecS, vecS, directedS,
		property < vertex_name_t, std::string,
		property < vertex_index_t, long,
		property < vertex_color_t, boost::default_color_type,
		property < vertex_distance_t, long,
		property < vertex_predecessor_t, Traits::edge_descriptor > > > > >
	> Graph;

	typedef graph_traits<Graph>::vertex_descriptor Vertex;

	Graph G;

	std::vector<Traits::vertex_descriptor> root(numVertices);
	for (int i = 0; i < numVertices; ++i)
		root[i] = add_vertex(G);

	std::vector<int> component(num_vertices(G)), discover_time(num_vertices(G));
	std::vector<default_color_type> color(num_vertices(G));

	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			add_edge(root[r], root[c], G);
		}
	}

	//curr_2 = clock();
	//mexPrintf("Time elapsed_constructing_graph_for_strongly_connected_components_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//int num = strong_components(G, &component[0], root_map(&root[0]).color_map(&color[0]).discover_time_map(&discover_time[0]));
	int num = strong_components(G, boost::make_iterator_property_map(component.begin(), boost::get(boost::vertex_index, G)),
			                                root_map(boost::make_iterator_property_map(root.begin(), boost::get(boost::vertex_index, G))).color_map(boost::make_iterator_property_map(color.begin(), boost::get(boost::vertex_index, G))).discover_time_map(boost::make_iterator_property_map(discover_time.begin(), boost::get(boost::vertex_index, G))));

	//curr_2 = clock();
	//mexPrintf("Time elapsed_boost_strongly_connected_components_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//here original's content is from 0 to 2*n+1, 0: source, 1 to n, variables, n+1: sink, n+2 to 2*n+1: bar variables
	//positive: from 1 to n (1-based variables index)
	//negative: from 1 to n (1-based variables index)
	std::vector<SC> vsc(num);
	for (int i = 0; i < component.size(); i++)
	{
		int whichComponent = component[i];
		vsc[whichComponent].original.push_back(i);
		if (vsc[whichComponent].original.back() <= numVariables)
			vsc[whichComponent].positive.push_back(vsc[whichComponent].original.back());
		else
			vsc[whichComponent].negative.push_back(vsc[whichComponent].original.back() - (numVariables + 1));
	}

	std::map<std::pair<int, int>, long long int> M;

	//speed up the program a lot !!!
	//build the graph G for strongly connected components
	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			int sc1 = component[r];
			int sc2 = component[c];
			if (sc1 != sc2 && R.values()[j] > 0 && M.find(std::make_pair(sc1, sc2)) == M.end())
				M.insert(std::make_pair(std::make_pair(sc1, sc2), 1));
		}
	}

	//curr_2 = clock();
	//mexPrintf("Time elapsed_after_boost_scc: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	SCRet ret;
	ret.S = vsc;
	ret.G = compressed_matrix::CompressedMatrix<long long int>(num, num, M);

	return ret;
}

std::vector<int> classifyStronglyConnectedComponents(const std::vector<SC>& S)
{
	std::vector<int> ret(S.size()); //0: self-complement, 1: non-self-complement

	for (int i = 0; i < S.size(); i++)
	{
		bool flag = true;
		std::set<int> pos(S[i].positive.begin(), S[i].positive.end());
		for (int j = 0; j < S[i].negative.size(); j++)
		{
			if (pos.find(S[i].negative[j]) != pos.end())
			{
				flag = false;
				break;
			}
		}

		if (flag)
			ret[i] = 1;
		else
			ret[i] = 0;
	}

	return ret;
}

void shortestPath(int src, const compressed_matrix::CompressedMatrix<long long int>& g, std::vector<int>& visited)
{
	std::queue<int> q;
	q.push(src);
	visited.resize(g.numRows(), 0);
	visited[src] = 1;

	while (!q.empty())
	{
		int curr = q.front(); q.pop();

		int start = g.rowOffsets()[curr];
		int end = g.rowOffsets()[curr + 1];
		for (int j = start; j < end; j++)
		{
			int c = g.colIndices()[j];
			if (g.values()[j] != 0 && !visited[c])
			{
				visited[c] = 2;
				q.push(c);
			}
		}
	}
}

std::vector<std::pair<int, int> > fixVariables(const std::vector<int>& classifiedSC, const SCRet& scRet, int numVariables)
{
	std::vector<std::pair<int, int> > ret;

	int x0Location;
	int x0BarLocation;
	std::vector<int> I;

	for (int i = 0; i < classifiedSC.size(); i++)
	{
		if (classifiedSC[i] == 1) //1: non self-complement
		{
			if (scRet.S[i].original[0] == 0)
				x0Location = i;
			else if (scRet.S[i].original[0] == numVariables + 1)
				x0BarLocation = i;
			else
				I.push_back(i);
		}
	}

	//calculate if there is a path from x0 to other nodes
	std::vector<int> visited;
	shortestPath(x0Location, scRet.G, visited); //speed up the program

	//find complement pairs
	std::vector<int> positive(numVariables + 1, -1);
	std::vector<int> negative(numVariables + 1, -1);
	for (int i = 0; i < I.size(); i++)
	{
		for (int k = 0; k < scRet.S[I[i]].positive.size(); k++)
			positive[scRet.S[I[i]].positive[k]] = I[i];
		for (int k = 0; k < scRet.S[I[i]].negative.size(); k++)
			negative[scRet.S[I[i]].negative[k]] = I[i];
	}

	std::map<int, int> complementPairs;
	for (int i = 0; i < positive.size(); i++)
	{
		if (positive[i] != -1 && negative[i] != -1)
		{
			complementPairs[positive[i]] = negative[i];
			complementPairs[negative[i]] = positive[i];
		}
	}

	std::queue<int> q;

	std::map<std::pair<int, int>, long long int> GTransMap;
	for (int i = 0; i < scRet.G.numRows(); i++)
	{
		int start = scRet.G.rowOffsets()[i];
		int end = scRet.G.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
			GTransMap.insert(std::make_pair(std::make_pair(scRet.G.colIndices()[j], i), scRet.G.values()[j]));
	}
	compressed_matrix::CompressedMatrix<long long int> GTrans(scRet.G.numRows(), scRet.G.numRows(), GTransMap);

	std::vector<int> outDegrees(scRet.G.numRows(), -1);
	for (int i = 0; i < classifiedSC.size(); i++)
	{
		if (classifiedSC[i] == 1) //non self-complement components
		{
			outDegrees[i] = scRet.G.rowOffsets()[i + 1] - scRet.G.rowOffsets()[i];
			if (outDegrees[i] == 0 && i != x0Location && i != x0BarLocation) //need to push the original outdegree 0 nodes into q !!!
				q.push(i);
		}
	}
	outDegrees[x0Location] = -1;
	outDegrees[x0BarLocation] = -1;

	for (int i = 0; i < visited.size(); i++)
	{
		if (visited[i] == 2) //exclude x0
		{
			for (int k = 0; k < scRet.S[i].positive.size(); k++)
				ret.push_back(std::make_pair(scRet.S[i].positive[k], 1));

			for (int k = 0; k < scRet.S[i].negative.size(); k++)
				ret.push_back(std::make_pair(scRet.S[i].negative[k], 0));

			int complement = complementPairs[i];

			outDegrees[i] = -1;
			outDegrees[complement] = -1;
		}
	}

	//decrease the outdegrees of node which has outgoing edges to i and to complement
	//push node which has 0 outdegrees into the queue
	for (int i = 0; i < visited.size(); i++)
	{
		if (visited[i] == 2) //exclude x0
		{
			int start = GTrans.rowOffsets()[i];
			int end = GTrans.rowOffsets()[i + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}

			int complement = complementPairs[i];
			start = GTrans.rowOffsets()[complement];
			end = GTrans.rowOffsets()[complement + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}
		}
	}

	while (!q.empty())
	{
		int curr = q.front(); q.pop();
		if (outDegrees[curr] == 0)
		{
			outDegrees[curr] = -1;
			int complement = complementPairs[curr];
			outDegrees[complement] = -1;

			//fixed all variables in component curr
			for (int k = 0; k < scRet.S[curr].positive.size(); k++)
				ret.push_back(std::make_pair(scRet.S[curr].positive[k], 1));

			for (int k = 0; k < scRet.S[curr].negative.size(); k++)
				ret.push_back(std::make_pair(scRet.S[curr].negative[k], 0));

			//decrease the outdegrees of node which has outgoing edges to i and to complement
			//push node which has 0 outdegrees into the queue
			int start = GTrans.rowOffsets()[curr];
			int end = GTrans.rowOffsets()[curr + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}

			start = GTrans.rowOffsets()[complement];
			end = GTrans.rowOffsets()[complement + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}
		}
	}

	std::sort(ret.begin(), ret.end(), compClass());

	return ret;
}

compressed_matrix::CompressedMatrix<double> computeNewQAndOffset(const compressed_matrix::CompressedMatrix<double>& Q, const std::vector<std::pair<int, int> >& fixed, double& offset)
{
	//fixed now is 1-based
	if (!fixed.empty())
	{
		int numVariables = Q.numRows();

		std::map<int, int> mI;
		for (int i = 0; i < fixed.size(); i++)
			mI[fixed[i].first - 1] = i; //-1 to make it 0-based

		std::vector<int> J;
		std::map<int, int> mJ;
		int cnt = 0;
		for (int i = 0; i < numVariables; i++)
		{
			if (mI.find(i) == mI.end())
			{
				J.push_back(i);
				mJ[i] = cnt++;
			}
		}

		std::map<std::pair<int, int>, double> QII;
		std::map<std::pair<int, int>, double> QJJ;
		std::map<std::pair<int, int>, double> QIJ;
		std::map<std::pair<int, int>, double> QJI;

		for (int i = 0; i < Q.numRows(); i++)
		{
			int start = Q.rowOffsets()[i];
			int end = Q.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = Q.colIndices()[j];
				if (mI.find(r) != mI.end() && mI.find(c) != mI.end())
					QII[std::make_pair(mI[r], mI[c])] = Q.values()[j];
				if (mJ.find(r) != mJ.end() && mJ.find(c) != mJ.end())
					QJJ[std::make_pair(mJ[r], mJ[c])] = Q.values()[j];
				if (mI.find(r) != mI.end() && mJ.find(c) != mJ.end())
					QIJ[std::make_pair(mI[r], mJ[c])] = Q.values()[j];
				if (mJ.find(r) != mJ.end() && mI.find(c) != mI.end())
					QJI[std::make_pair(mJ[r], mI[c])] = Q.values()[j];
			}
		}

		//off_set = x0'*Q_II*x0;
		offset = 0;
		for (std::map<std::pair<int, int>, double>::const_iterator it = QII.begin(), end = QII.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			offset += fixed[r].second * it->second * fixed[c].second;
		}

		//x0'*Q_IJ
		std::vector<double> tmp2(numVariables - fixed.size(), 0);
		for (std::map<std::pair<int, int>, double>::const_iterator it = QIJ.begin(), end = QIJ.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			tmp2[c] += it->second * fixed[r].second;
		}

		//Q_JI*x0
		std::vector<double> tmp3(numVariables - fixed.size(), 0);
		for (std::map<std::pair<int, int>, double>::const_iterator it = QJI.begin(), end = QJI.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			tmp3[r] += it->second * fixed[c].second;
		}

		for (int i = 0; i < (int)tmp3.size(); i++)
			tmp2[i] += tmp3[i];

		cnt = 0;
		for (int i = 0; i < (int)mJ.size() * (int)mJ.size(); i += numVariables - static_cast<int>(fixed.size()) + 1)
		{
			int r = i / static_cast<int>(mJ.size());
			int c = i % mJ.size();
			QJJ[std::make_pair(r, c)] += tmp2[cnt++];
		}

		std::map<std::pair<int, int>, double> mfixedQ;
		for (std::map<std::pair<int, int>, double>::const_iterator it = QJJ.begin(), end = QJJ.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			if (it->second != 0)
				mfixedQ[std::make_pair(J[r], J[c])] = it->second;
		}

		return compressed_matrix::CompressedMatrix<double>(numVariables, numVariables, mfixedQ);
	}
	else
	{
		offset = 0;
		return Q;
	}
}

std::vector<std::pair<int, int> > applyImplication(const compressed_matrix::CompressedMatrix<long long int>& A)
{
	int numVertices = A.numRows();
	int numVariables = numVertices / 2 - 1;

	//debuging only
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	using namespace boost;

	typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
	typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, long long int, property<edge_residual_capacity_t, long long int, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is long long int
	//typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, double, property<edge_residual_capacity_t, double, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is double

	Graph g;

	property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
	property_map<Graph, edge_reverse_t>::type reverse_edge = get(edge_reverse, g);
	property_map<Graph, edge_residual_capacity_t>::type residual_capacity = get(edge_residual_capacity, g);

	std::vector<Traits::vertex_descriptor> verts(numVertices);
	for (int i = 0; i < numVertices; ++i)
		verts[i] = add_vertex(g);

	Traits::vertex_descriptor s = verts[numVariables];
	Traits::vertex_descriptor t = verts[2 * numVariables + 1];

	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int cap = A.values()[j];

			Traits::edge_descriptor e1, e2;
			bool in1, in2;
			boost::tie(e1, in1) = add_edge(verts[r], verts[c], g);
			boost::tie(e2, in2) = add_edge(verts[c], verts[r], g);

			capacity[e1] = cap;
			capacity[e2] = 0;
			reverse_edge[e1] = e2;
			reverse_edge[e2] = e1;
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_building_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	push_relabel_max_flow(g, s, t);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//residual
	std::vector<int> RRowOffsets(numVertices+1);
	std::vector<int> RColIndices;
	std::vector<long long int> RValues;
	//flow
	std::vector<int> FRowOffsets(numVertices+1);
	std::vector<int> FColIndices;
	std::vector<long long int> FValues;

	int offset = 0;
	int currRow = 0;
	graph_traits<Graph>::vertex_iterator u_iter, u_end;
	graph_traits<Graph>::out_edge_iterator ei, e_end;
	for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
		for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
		{
			if (capacity[*ei] > 0)
			{
				if (currRow <= *u_iter)
				{
					for (int i = currRow; i <= *u_iter; i++)
					{
						RRowOffsets[i] = offset;
						FRowOffsets[i] = offset;
					}
					currRow = static_cast<int>((*u_iter) + 1);
				}
				RColIndices.push_back(static_cast<int>(target(*ei, g)));
				RValues.push_back(residual_capacity[*ei]);
				FColIndices.push_back(static_cast<int>(target(*ei, g)));
				FValues.push_back(capacity[*ei] - residual_capacity[*ei]);
				++offset;
			}
		}
	for (int i = currRow; i < RRowOffsets.size(); i++)
		RRowOffsets[i] = offset;
	for (int i = currRow; i < FRowOffsets.size(); i++)
		FRowOffsets[i] = offset;

	compressed_matrix::CompressedMatrix<long long int> R(numVertices, numVertices, RRowOffsets, RColIndices, RValues); //residual
	compressed_matrix::CompressedMatrix<long long int> F(numVertices, numVertices, FRowOffsets, FColIndices, FValues); //flow

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_get_RF: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//resid = (A>0).*R + F'.*(A'>0);
	std::map<std::pair<int, int>, long long int> residM;
	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int RValue = R.get(r, c);
			if (A.values()[j] > 0 && RValue != 0 && r != 2 * numVariables + 1 && c != numVariables)
				residM[std::make_pair(r, c)] += RValue;

			long long int FTValue = F.get(r, c); //not F(c, r) !!!
			if (A.values()[j] > 0 && FTValue != 0 && c != 2 * numVariables + 1 && r != numVariables)
				residM[std::make_pair(c, r)] += FTValue;
		}
	}

	compressed_matrix::CompressedMatrix<long long int> resid(numVertices, numVertices, residM);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_finally_get_resid: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<int> forced = bfs_for_method_2(resid, numVariables);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_bfs_for_method_2: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<std::pair<int, int> > fixed;
	for (int i = 0; i < forced.size(); i++)
	{
		if (forced[i] > 0)
		{
			if (i <= numVariables - 1)
				fixed.push_back(std::make_pair(i + 1, 1)); //1-based
			else if (i >= numVariables + 1 && i <= 2 * numVariables)
				fixed.push_back(std::make_pair(i - numVariables, 0)); //1-based
		}
	}

	std::sort(fixed.begin(), fixed.end(), compClass());

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_fix_vars_and_sorting: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return fixed;
}

} // anonymous namespace

namespace fix_variables_
{

FixVariablesResult fixQuboVariables(const compressed_matrix::CompressedMatrix<double>& Q, int method)
{
	//Q needs to be a square matrix
	if (Q.numRows() != Q.numCols())
		throw FixVariablesException("Q's size is not correct.");

	if (!(method == 1 || method == 2))
		throw FixVariablesException("method must be an integer of 1 or 2.");

	FixVariablesResult ret;

	//check if Q is empty
	if (Q.numRows() == 0 || Q.numCols() == 0)
	{
		ret.offset = 0;
		return ret;
	}

	int numVariables = Q.numRows();

	//clock_t curr_1 = clock();
	//clock_t curr_2;

	//uTriQ = triu(Q) + tril(Q,-1)'; //make upper triangular
	std::map<std::pair<int, int>, double> uTriQMap;
	std::set<int> usedVariables;
	for (int i = 0; i < Q.numRows(); i++)
	{
		int start = Q.rowOffsets()[i];
		int end = Q.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = Q.colIndices()[j];
			uTriQMap[std::make_pair(std::min(r, c), std::max(r, c))] += Q.values()[j];
			usedVariables.insert(r);
			usedVariables.insert(c);
		}
	}

	compressed_matrix::CompressedMatrix<double> uTriQ(numVariables, numVariables, uTriQMap);

	double maxAbsValue = 0;

	if (!uTriQ.values().empty())
		maxAbsValue = std::fabs(*std::max_element(uTriQ.values().begin(), uTriQ.values().end(), compareAbs));

	double ratio = 1;

	if (maxAbsValue != 0)
		ratio = static_cast<double>(std::numeric_limits<long long int>::max()) / maxAbsValue;

	ratio /= static_cast<double>(1LL << 10);

	if (ratio < 1)
		ratio = 1;

	std::map<std::pair<int, int>, long long int> uTriQMapLLI;

	for (std::map<std::pair<int, int>, double>::iterator it = uTriQMap.begin(), end = uTriQMap.end(); it != end; ++it)
		uTriQMapLLI[it->first] = static_cast<long long int>(it->second * ratio);

	compressed_matrix::CompressedMatrix<long long int> uTriQLLI(numVariables, numVariables, uTriQMapLLI);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_make upper triangular: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	Posiform p = BQPToPosiform(uTriQLLI);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_BQPToPosiform: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	if (method == 1)
	{
		compressed_matrix::CompressedMatrix<long long int> A = posiformToImplicationNetwork_1(p);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_posiformToImplicationNetwork_1: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		compressed_matrix::CompressedMatrix<long long int> R = maxFlow(A);  //use this

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_maxFlow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		compressed_matrix::CompressedMatrix<long long int> symR = makeResidualSymmetric(R); //use this

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_makeRSym: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		//add clearing R in makeResidualSym, so here just use symR directly !!!
		SCRet scRet = stronglyConnectedComponents(symR);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_stronglyConnectedComponents: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		std::vector<int> classifiedSC = classifyStronglyConnectedComponents(scRet.S);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_classifyStrongComponents: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		ret.fixedVars = fixVariables(classifiedSC, scRet, numVariables);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_fixVarsUsingOutDegree: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;
	}
	else if (method == 2)
	{
		compressed_matrix::CompressedMatrix<long long int> A = posiformToImplicationNetwork_2(p);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_posiformToImplicationNetwork_2: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		ret.fixedVars = applyImplication(A);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_applyImplication: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;
	}

	if (ret.fixedVars.size() > numVariables)
		throw FixVariablesException("ret.fixedVars has wrong size.");

	// remove unused variables from ret.fixedVars
	std::vector<std::pair<int, int> > updatedFixedVars;
	for (int i = 0; i < ret.fixedVars.size(); ++i)
	{
		if (usedVariables.find(ret.fixedVars[i].first - 1) != usedVariables.end()) // -1 to make it 0-based since usedVariables is 0-based
			updatedFixedVars.push_back(ret.fixedVars[i]);
	}

	ret.fixedVars = updatedFixedVars;

	ret.newQ = computeNewQAndOffset(uTriQ, ret.fixedVars, ret.offset);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_computeNewQAndOffset: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return ret;
}

std::vector<std::pair<int,  int> > fixQuboVariablesMap(std::map<std::pair<int, int>, double> QMap, int QSize, int mtd)
{
    compressed_matrix::CompressedMatrix<double> QInput(QSize, QSize, QMap);

    FixVariablesResult ret = fixQuboVariables(QInput, mtd);

    return ret.fixedVars;
}

} // namespace fix_variables_


#pragma once

#include "optimizationtools/indexed_set.hpp"
#include "optimizationtools/utils.hpp"

#include <iostream>
#include <fstream>

#include <boost/filesystem.hpp>
#include <boost/regex.hpp>

/**
 * Time-dependent orienteering problem.
 *
 * Input:
 * - n locations; for each location j = 1..n, a profit pⱼ
 * - a function t(j₁, j₂, s) which returns the time to travel from location j₁
 *   to location j₂ starting at time s
 * - a time limit tₘₐₓ
 * Problem:
 * - Find a path starting at location 1 and ending at location n such that:
 *   - each location is visited at most once
 *   - the arrival at location n is before tₘₐₓ
 * Objective:
 * - Maximize the total profit of the visited locations
 *
 */

namespace orproblems
{

namespace timedependentorienteering
{

using LocationId = int64_t;
using LocationPos = int64_t;
using ArcCategory = int64_t;
using TimePeriod = int64_t;
using Time = double;
using Length = double;
using Profit = double;

struct Location
{
    Length x;
    Length y;
    Profit profit;
};

struct Arc
{
    ArcCategory category;
    Length length;
};

class Instance
{

public:

    Instance(LocationId n):
        locations_(n),
        arcs_(n, std::vector<Arc>(n)) { }
    void set_maximum_duration(Time maximum_duration) { maximum_duration_ = maximum_duration; }

    Instance(std::string instance_path, std::string format = "")
    {
        std::ifstream file(instance_path);
        if (!file.good())
            throw std::runtime_error(
                    "Unable to open file \"" + instance_path + "\".");
        if (format == "" || format == "verbeeck2014") {
            read_verbeeck2014(file, instance_path);
        } else {
            throw std::invalid_argument(
                    "Unknown instance format \"" + format + "\".");
        }
        file.close();
    }

    virtual ~Instance() { }

    inline LocationId number_of_locations() const { return locations_.size(); }
    inline const Location& location(LocationId j) const { return locations_[j]; }
    inline Time maximum_duration() const { return maximum_duration_; }
    inline Time arrival_time(LocationId j1, LocationId j2, Time start) const
    {
        Time current_time = start;
        Length remaining_length = arcs_[j1][j2].length;
        ArcCategory arc_category = arcs_[j1][j2].category;
        TimePeriod time_period =
            (current_time < 9 - 7)? 0:
            (current_time < 17 - 7)? 1:
            (current_time < 19 - 7)? 2:
            3;
        for (;;) {
            Time time_period_end =
                (time_period == 0)? 9 - 7:
                (time_period == 1)? 17 - 7:
                (time_period == 2)? 19 - 7:
                std::numeric_limits<Time>::max();
            double speed = speed_matrix_[arc_category][time_period];
            Time at = current_time + remaining_length / speed;
            if (at <= time_period_end) {
                //std::cout << "j1 " << j1
                //    << " j2 " << j2
                //    << " x1 " << locations_[j1].x
                //    << " y1 " << locations_[j1].y
                //    << " x2 " << locations_[j2].x
                //    << " y2 " << locations_[j2].y
                //    << " length " << arcs_[j1][j2].length
                //    << " cat " << arc_category
                //    << " start " << start
                //    << " dur " << at - start
                //    << " arrival " << at << " / " << time_period_end
                //    << std::endl;
                return at;
            }
            remaining_length -= (time_period_end - current_time) * speed;
            //std::cout << "remaining_length " << remaining_length << std::endl;
            current_time = time_period_end;
            time_period++;
        }
        return -1;
    }

    std::pair<bool, Time> check(
            std::string certificate_path,
            int verbose = 1) const
    {
        // Initial display.
        if (verbose >= 1) {
            std::cout
                << "Checker" << std::endl
                << "-------" << std::endl;
        }

        std::ifstream file(certificate_path);
        if (!file.good())
            throw std::runtime_error(
                    "Unable to open file \"" + certificate_path + "\".");

        LocationId n = number_of_locations();
        LocationId j = -1;
        LocationId j_prec = 0;
        optimizationtools::IndexedSet locations(n);
        locations.add(0);
        locations.add(n - 1);
        LocationPos number_of_duplicates = 0;
        Time current_time = 0;
        Profit profit = location(0).profit;
        while (file >> j) {
            if (locations.contains(j)) {
                number_of_duplicates++;
                if (verbose == 2)
                    std::cout << "Location " << j << " is already scheduled." << std::endl;
            }
            locations.add(j);
            current_time = arrival_time(j_prec, j, current_time);
            profit += location(j).profit;
            if (verbose == 2)
                std::cout << "Location: " << j
                    << "; Time: " << current_time
                    << "; Profit: " << profit << " (" << location(j).profit << ")"
                    << std::endl;
            j_prec = j;
        }
        current_time = arrival_time(j_prec, n - 1, current_time);
        profit += location(n - 1).profit;

        bool feasible
            = (current_time <= maximum_duration())
            && (number_of_duplicates == 0);
        if (verbose == 2)
            std::cout << "---" << std::endl;
        if (verbose >= 1) {
            std::cout << "Number of locations:       " << locations.size() << " / " << n  << std::endl;
            std::cout << "Number of duplicates:      " << number_of_duplicates << std::endl;
            std::cout << "Duraction:                 " << current_time << " / " << maximum_duration() << std::endl;
            std::cout << "Feasible:                  " << feasible << std::endl;
            std::cout << "Profit:                    " << profit << std::endl;
        }
        return {feasible, profit};
    }

private:

    void read_verbeeck2014(
            std::ifstream& file,
            std::string instance_path)
    {
        std::string tmp;
        LocationId n = -1;
        file
            >> tmp >> n
            >> tmp >> tmp
            >> tmp >> maximum_duration_
            ;
        locations_ = std::vector<Location>(n);
        for (LocationId j = 0; j < n; ++j)
            file >> locations_[j].x >> locations_[j].y >> locations_[j].profit;
        arcs_ = std::vector<std::vector<Arc>>(n, std::vector<Arc>(n));
        for (LocationId j1 = 0; j1 < n; ++j1) {
            for (LocationId j2 = 0; j2 < n; ++j2) {
                Length dx = locations_[j1].x - locations_[j2].x;
                Length dy = locations_[j1].y - locations_[j2].y;
                Length dxy = std::sqrt(dx * dx + dy * dy);
                dxy /= 5;
                arcs_[j1][j2].length = dxy;
            }
        }

        // Read speed matrix.
        auto speed_matrix_path = boost::filesystem::path(instance_path)
            .parent_path().parent_path().parent_path() /= "speedmatrix.txt";
        std::ifstream speed_matrix_file(speed_matrix_path.string());
        speed_matrix_ = std::vector<std::vector<double>>(
                5, std::vector<double>(4, 0.0));
        for (ArcCategory arc_category = 0; arc_category < 5; ++arc_category)
            for (TimePeriod time_period = 0; time_period < 4; ++time_period)
                speed_matrix_file >> speed_matrix_[arc_category][time_period];

        // Read arc category matrix.
        boost::filesystem::directory_iterator it_end;
        auto arc_category_directory = boost::filesystem::path(instance_path)
            .parent_path().parent_path();
        const boost::regex filter("arc_cat_.*\\.txt");
        for (boost::filesystem::directory_iterator it(arc_category_directory); it != it_end; ++it) {
            if (!boost::filesystem::is_regular_file(it->status()))
                continue;
            boost::smatch what;
            // For V2:
            //if(!boost::regex_match(it->leaf(), what, filter))
            //    continue;
            // For V3:
            if (!boost::regex_match(it->path().filename().string(), what, filter))
                continue;
            // File matches, store it
            std::ifstream arc_category_file(it->path().string());
            for (LocationId location_id_1 = 0; location_id_1 < number_of_locations(); ++location_id_1)
                for (LocationId location_id_2 = 0; location_id_2 < number_of_locations(); ++location_id_2)
                    arc_category_file >> arcs_[location_id_1][location_id_2].category;
        }
    }

    std::vector<Location> locations_;
    std::vector<std::vector<Arc>> arcs_;
    std::vector<std::vector<double>> speed_matrix_;
    Time maximum_duration_ = 0;

};

static inline std::ostream& operator<<(
        std::ostream &os, const Instance& instance)
{
    os << "number of locations: " << instance.number_of_locations() << std::endl;
    for (LocationId j = 0; j < instance.number_of_locations(); ++j) {
        os << "location " << j
            << " profit " << instance.location(j).profit;
        os << std::endl;
    }
    return os;
}

}

}


/**
 * @file yaml_eigen.h
 * @author Alexander Herzog
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2015-02-27
 *
 * @brief Add support for eigen frm the yaml-cpp standard package.
 */

#pragma once

#include <type_traits>
#include <iomanip>
#include <Eigen/Eigen>
#include <yaml-cpp/yaml.h>

namespace robot_math{
template<class T>
struct MovableEigenVector;
template<class T>
struct MovableEigenMatrix;
}

namespace YAML {

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct EigenVectorConverter{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> VectorType;

  static void resize_if_needed(int rows, int cols, VectorType& rhs){
    if(rhs.size() != rows*cols)
    {
      if (VectorType::SizeAtCompileTime == Eigen::Dynamic &&
          VectorType::MaxSizeAtCompileTime == Eigen::Dynamic)
      {
        rhs.resize(rows*cols);
      }else
      {
        std::ostringstream error;
        error << "ERROR: The fixed sized vector of size (" << rhs.size()
              << ") is of different size than the input yaml data vector of "
              << "size (" << rows * cols << ")." ;
        throw(std::runtime_error(error.str()));
      }
    }
  }

  static Scalar& access_element(int r, int c, VectorType& rhs)
  {
    return rhs(r+c);
  }
};

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct EigenMatrixConverter{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> MatrixType;

  static void resize_if_needed(int rows, int cols, MatrixType& rhs){
    if(rhs.rows() != rows || rhs.cols() != cols)
    {
      if (MatrixType::SizeAtCompileTime == Eigen::Dynamic &&
          MatrixType::MaxSizeAtCompileTime == Eigen::Dynamic)
      {
        rhs.resize(rows, cols);
      }else
      {
        std::ostringstream error;
        error << "ERROR: The fixed sized matrix of dim (" << rhs.rows() << ","
              << rhs.cols() << ") is of different dim than the input yaml "
              << "data matrix of dim (" << rows << "," << cols << ")." ;
        throw(std::runtime_error(error.str()));
      }
    }
  }

  static Scalar& access_element(int r, int c, MatrixType& rhs){
    return rhs(r, c);
  }
};

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct convert<Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> >{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> Eigen_Type_;
  const static bool is_vector_type_ = Rows == 1 || Cols == 1;
  typedef typename std::conditional<is_vector_type_,
      EigenVectorConverter<Scalar, Rows, Cols, Align, RowsAtCompileTime, ColsAtCompileTime>,
      EigenMatrixConverter<Scalar, Rows, Cols, Align, RowsAtCompileTime, ColsAtCompileTime>>::type Converter_;

  static Node encode(const Eigen_Type_& rhs) {
    Eigen::IOFormat yaml_format(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", "[", "]", "[", "]");
    std::stringstream ss;
    ss << std::setprecision (std::numeric_limits<double>::digits10 + 1) << rhs.format(yaml_format);
    return Load(ss.str());
  }

  static bool decode_2d(const Node& node, Eigen_Type_& rhs){
    const size_t n_rows = node.size();
    const size_t n_cols = node[0].size();
    Converter_::resize_if_needed(n_rows, n_cols, rhs);

    for (size_t r=0;r<n_rows;++r){
      const Node& yaml_row = node[r];
      if(yaml_row.size() != n_cols)
        return false;
      for (size_t c=0;c<n_cols;++c)
        Converter_::access_element(r, c, rhs) = yaml_row[c].as<Scalar>();
    }

    return true;
  }

  static bool decode_1d(const Node& node, Eigen_Type_& rhs){
    const size_t n_size = node.size();
    Converter_::resize_if_needed(n_size, 1, rhs);
    for (size_t r=0;r<n_size;++r)
      Converter_::access_element(r, 0, rhs) = node[r].as<Scalar>();
    return true;
  };

  static bool decode(const Node& node, Eigen_Type_& rhs){
    if(!node.IsSequence())
      return false;
    if(node.size() > 0)
    {
      if(!node[0].IsSequence())
        return decode_1d(node, rhs);
      else
        return decode_2d(node, rhs);
    }
    else
    {
      Converter_::resize_if_needed(0, 0, rhs);
      return true;
    }
  }
};

template<class T>
struct convert<robot_math::MovableEigenVector<T>> {
  static Node encode(const robot_math::MovableEigenVector<T>& mp){
    return convert<T>::encode(static_cast<const T&>(mp));
  }
  static bool decode(const Node& node, robot_math::MovableEigenVector<T>& mp){
    return convert<T>::decode(node, static_cast<T&>(mp));
  }
};

template<>
struct convert<Eigen::Quaterniond> {
  static Node encode(const Eigen::Quaterniond& mp){
    return convert<Eigen::Vector4d>::encode(Eigen::Vector4d(mp.w(), mp.x(), mp.y(), mp.z()));
  }
  static bool decode(const Node& node, Eigen::Quaterniond& mp){
    Eigen::Vector4d v4;
    if(!convert<Eigen::Vector4d>::decode(node, v4)) return false;
    mp.w() = v4[0];
    mp.x() = v4[1];
    mp.y() = v4[2];
    mp.z() = v4[3];
    return true;
  }
};

} // namespace YAML


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

// Copyright (c) 2010 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/geometry.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>

#include <boost/geometry/geometries/adapted/boost_polygon/point.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon/box.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon/ring.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon/polygon.hpp>

#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

#include <iostream>

template <typename T>
void fill_polygon_with_two_holes(boost::polygon::polygon_with_holes_data<T>& boost_polygon_polygon)
{
    std::vector<boost::polygon::point_data<T> > point_vector;
    point_vector.push_back(boost::polygon::point_data<T>(0, 0));
    point_vector.push_back(boost::polygon::point_data<T>(0, 10));
    point_vector.push_back(boost::polygon::point_data<T>(10, 10));
    point_vector.push_back(boost::polygon::point_data<T>(10, 0));
    point_vector.push_back(boost::polygon::point_data<T>(0, 0));
    boost_polygon_polygon.set(point_vector.begin(), point_vector.end());

    std::vector<boost::polygon::polygon_data<T> > holes;
    holes.resize(2);

    {
        std::vector<boost::polygon::point_data<T> > point_vector;
        point_vector.push_back(boost::polygon::point_data<T>(1, 1));
        point_vector.push_back(boost::polygon::point_data<T>(2, 1));
        point_vector.push_back(boost::polygon::point_data<T>(2, 2));
        point_vector.push_back(boost::polygon::point_data<T>(1, 2));
        point_vector.push_back(boost::polygon::point_data<T>(1, 1));
        holes[0].set(point_vector.begin(), point_vector.end());
    }

    {
        std::vector<boost::polygon::point_data<T> > point_vector;
        point_vector.push_back(boost::polygon::point_data<T>(3, 3));
        point_vector.push_back(boost::polygon::point_data<T>(4, 3));
        point_vector.push_back(boost::polygon::point_data<T>(4, 4));
        point_vector.push_back(boost::polygon::point_data<T>(3, 4));
        point_vector.push_back(boost::polygon::point_data<T>(3, 3));
        holes[1].set(point_vector.begin(), point_vector.end());
    }
    boost_polygon_polygon.set_holes(holes.begin(), holes.end());
}

template <typename T>
void test_coordinate_type()
{
    // 1a: Check if Boost.Polygon's point fulfills Boost.Geometry's point concept
    bg::concept::check<boost::polygon::point_data<T> >();

    // 1b: use a Boost.Polygon point in Boost.Geometry, calc. distance with two point types
    boost::polygon::point_data<T> boost_polygon_point(1, 2);

    typedef bg::model::point<T, 2, bg::cs::cartesian> bg_point_type;
    bg_point_type boost_geometry_point(3, 4);
    BOOST_CHECK_EQUAL(bg::distance(boost_polygon_point, boost_geometry_point),
                    2 * std::sqrt(2.0));

    // 2a: Check if Boost.Polygon's box fulfills Boost.Geometry's box concept
    bg::concept::check<boost::polygon::rectangle_data<T> >();

    // 2b: use a Boost.Polygon rectangle in Boost.Geometry, compare with boxes
    boost::polygon::rectangle_data<T> boost_polygon_box;
    bg::model::box<bg_point_type> boost_geometry_box;

    bg::assign_values(boost_polygon_box, 0, 1, 5, 6);
    bg::assign_values(boost_geometry_box, 0, 1, 5, 6);
    T boost_polygon_area = bg::area(boost_polygon_box);
    T boost_geometry_area = bg::area(boost_geometry_box);
    T boost_polygon_area_by_boost_polygon = boost::polygon::area(boost_polygon_box);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_geometry_area);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_polygon_area_by_boost_polygon);

    // 3a: Check if Boost.Polygon's polygon fulfills Boost.Geometry's ring concept
    bg::concept::check<boost::polygon::polygon_data<T> >();

    // 3b: use a Boost.Polygon polygon (ring)
    boost::polygon::polygon_data<T> boost_polygon_ring;
    {
        // Filling it is a two-step process using Boost.Polygon
        std::vector<boost::polygon::point_data<T> > point_vector;
        point_vector.push_back(boost::polygon::point_data<T>(0, 0));
        point_vector.push_back(boost::polygon::point_data<T>(0, 3));
        point_vector.push_back(boost::polygon::point_data<T>(4, 0));
        point_vector.push_back(boost::polygon::point_data<T>(0, 0));
        boost_polygon_ring.set(point_vector.begin(), point_vector.end());
    }

    // Boost-geometry ring
    bg::model::ring<bg_point_type> boost_geometry_ring;
    {
        boost_geometry_ring.push_back(bg_point_type(0, 0));
        boost_geometry_ring.push_back(bg_point_type(0, 3));
        boost_geometry_ring.push_back(bg_point_type(4, 0));
        boost_geometry_ring.push_back(bg_point_type(0, 0));
    }
    boost_polygon_area = bg::area(boost_polygon_ring);
    boost_geometry_area = bg::area(boost_geometry_ring);
    boost_polygon_area_by_boost_polygon = boost::polygon::area(boost_polygon_ring);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_geometry_area);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_polygon_area_by_boost_polygon);

    // Check mutable ring
    std::string wkt = "POLYGON((0 0,0 10,10 10,10 0,0 0))";
    bg::read_wkt(wkt, boost_polygon_ring);
    bg::read_wkt(wkt, boost_geometry_ring);
    boost_polygon_area = bg::area(boost_polygon_ring);
    boost_geometry_area = bg::area(boost_geometry_ring);
    boost_polygon_area_by_boost_polygon = boost::polygon::area(boost_polygon_ring);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_geometry_area);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_polygon_area_by_boost_polygon);

    // 4a: Boost.Polygon's polygon with holes
    boost::polygon::polygon_with_holes_data<T> boost_polygon_polygon;
    fill_polygon_with_two_holes(boost_polygon_polygon);

    // Using Boost.Polygon
    boost_polygon_area = bg::area(boost_polygon_polygon);
    boost_polygon_area_by_boost_polygon = boost::polygon::area(boost_polygon_polygon);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_polygon_area_by_boost_polygon);

    wkt = "POLYGON((0 0,0 10,10 10,10 0,0 0),(1 1,2 1,2 2,1 2,1 1),(3 3,4 3,4 4,3 4,3 3))";

    bg::model::polygon<bg_point_type> boost_geometry_polygon;
    bg::read_wkt(wkt, boost_geometry_polygon);

    boost_geometry_area = bg::area(boost_geometry_polygon);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_geometry_area);

    bg::clear(boost_polygon_polygon);
    bg::read_wkt(wkt, boost_polygon_polygon);
    boost_geometry_area = bg::area(boost_polygon_polygon);
    BOOST_CHECK_EQUAL(boost_polygon_area, boost_geometry_area);

    std::ostringstream out;
    out << bg::wkt(boost_polygon_polygon);
    BOOST_CHECK_EQUAL(wkt, out.str());

}

int test_main(int, char* [])
{
    test_coordinate_type<int>();
    //test_coordinate_type<float>(); // compiles, but "BOOST_CHECK_EQUAL" fails
    test_coordinate_type<double>();
    return 0;
}


/// @file metropolis_hastings.hpp A generic templated Metropolis-Hastings sampler implementation

#ifndef BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP
#define BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP

#include <cmath>
#include <boost/tuple/tuple.hpp>

#include "../partition_sampler_items.hpp"
#include "../detail/random.hpp"
#include "../detail/fun.hpp"
#include "../mh_moves/mh_moves.hpp"

namespace biggles { namespace sampling {

/// \brief attributes of the current sampling state
struct mh_state_observer {
    /// @brief The log probability density of the current sample.
    ///
    /// this is either last_sample_log_density (if proposal was rejected)
    /// or last_proposal_log_density (if proposal was accepted)
    float sample_log_density;

    /// @brief last log(alpha) where alpha is uniformly distributed between 0 and 1
    float last_log_alpha;

    /// @brief log density of the last proposed move
    float last_proposal_log_density;

    /// \brief the density of the last sample p(T|theta, y)
    float last_sample_log_density;

    /// @brief the last proposal density ratio Q(T|T*)/Q(T*|T)
    float last_pdr;

    /// @brief The number of samples which have been accepted during the lifetime of this sampler. This will always be
    /// less than or equal to \c n_proposed_.
    size_t n_accepted;

    /// @brief The number of samples which have been proposed during the lifetime of this sampler.
    size_t n_proposed;

    /// @brief was the last proposal accepted?
    bool accepted;

    mh_state_observer() :
        sample_log_density(-std::numeric_limits<float>::max()),
        last_log_alpha(-std::numeric_limits<float>::max()),
        last_proposal_log_density(-std::numeric_limits<float>::max()),
        last_sample_log_density(-std::numeric_limits<float>::max()),
        last_pdr(-std::numeric_limits<float>::max()),
        n_accepted(1),
        n_proposed(1),
        accepted(false) {}

    mh_state_observer(const mh_state_observer& o) :
        sample_log_density(o.sample_log_density),
        last_log_alpha(o.last_log_alpha),
        last_proposal_log_density(o.last_proposal_log_density),
        last_sample_log_density(o.last_sample_log_density),
        last_pdr(o.last_pdr),
        n_accepted(o.n_accepted),
        n_proposed(o.n_proposed),
        accepted(o.accepted) {}

    const mh_state_observer& operator = (const mh_state_observer& o) {
        if (&o == this) return *this;
        sample_log_density = o.sample_log_density;
        last_log_alpha = o.last_log_alpha;
        last_proposal_log_density = o.last_proposal_log_density;
        last_sample_log_density = o.last_sample_log_density;
        last_pdr = o.last_pdr;
        n_accepted = o.n_accepted;
        n_proposed = o.n_proposed;
        accepted = o.accepted;
        return *this;
    }

    /// \brief the total accptance rate
    float acceptance_rate() const { return static_cast<float>(n_accepted) / static_cast<float>(n_proposed); }

};

/// @brief A Metropolis-Hastings sampler.
///
/// Metropolis-Hastings is an algorithm which can sample from an arbitrary probability density function, \f$ P(x) \f$,
/// given only that the PDF can be evaluated. In fact it is even more general: the function may return some constant
/// multiple of the true PDF meaning that M-H is exceptionally useful when the PDF is known up to some normalisation
/// factor. The PDF \f$ P(x) \f$ is termed the target distribution.
///
/// In addition to being able to evaluate the target distribution, one must be able to draw samples from a proposal
/// distribution, \f$ Q(x' | x) \f$. In this case \f$ x' \f$ is a new sample and \f$ x \f$ is the sample we currently
/// are at.
///
/// To use the sampler you must provide at a minimum a functor which can evaluate the logarithm of the target PDF at a
/// given sample and a proposal functor. The proposal functor takes a current sample, \f$ x \f$, and returns a new
/// sample, \f$ x' \f$, conditioned on it. In addition, it returns
///
/// \f[
/// \log\left(\frac{Q(x|x')}{Q(x'|x)}\right) = \log(Q(x|x')) - \log(Q(x'|x)).
/// \f]
///
/// This logarithm may of course be zero to indicate a symmetric proposal distribution.
///
/// In addition you may optionally provide the C++ type of the sample and a functor which can sample uniformly from the
/// interval [0,1]. The boost library is used to provide a Mersenne twister-based uniform sampler if you do not specify
/// an alternative.
///
/// As an example of use, consider sampling the Rosenbrock function:
///
/// @code
/// typedef boost::tuples::tuple<float, float> point;
///
/// inline float rosenbrock(const point& p)
/// {
///     float x, y;
///     boost::tuples::tie(x,y) = p;
///     return (1.f-x)*(1.f-x) + 100.f*(y-x*x)*(y-x*x);
/// }
/// @endcode
///
/// Firstly, we need to convert this function which has a minimum at (1,1) to a log-PDF with a maximum at (1,1). Since
/// we do not need to worry about normalisation, this is quite easy:
///
/// @code
/// // a cooked-up PDF which has a maximum where the Rosenbrock function has a minimum.
/// inline float rosenbrock_log_pdf(const point& p)
/// {
///     return -rosenbrock(p);
/// }
/// @endcode
///
/// We also need a proposal function. The proposal function should accept a sample, in this case a \c point, and return
/// a tuple containing a new sample and the log proposal density ration outlined above. We shall use a simple Guassian
/// proposal function which moves the sample according to a Gaussian distribution centred on the current sample with a
/// standard deviation of 0.2. Since this proposal distribution is symmetric, we can return 0 as the log proposal ratio:
///
/// @code
/// typedef boost::tuples::tuple<point, float> proposal_result_t;
///
/// inline proposal_result_t propose_gaussian(const point& p)
/// {
///     static boost::mt19937 rng;
///     static boost::normal_distribution<float> norm(0.f, 0.2f);
///     static boost::variate_generator<boost::mt19937&, boost::normal_distribution<float> > variate(rng, norm);
///
///     point new_p(p.get<0>() + variate(), p.get<1>() + variate());
///
///     return proposal_result_t(new_p, 0.f);
/// }
/// @endcode
///
/// Since these functions are plain C++ functions, we shall wrap them in the functor classes available in the standard
/// library's \c functor header. We firstly need to define a type for them:
///
/// @code
/// typedef std::pointer_to_unary_function<const point&, float> pdf_t;
/// typedef std::pointer_to_unary_function<const point&, proposal_result_t> proposal_func_t;
/// @endcode
///
/// Using the standard library \c std::ptr_fun function, we can now declare our sampler.
///
/// @code
/// // MH sampler for Rosenbrock function. An example of using a function pointer directly.
/// sampler<pdf_t, proposal_func_t> s(std::ptr_fun(rosenbrock_log_pdf),
///                                   std::ptr_fun(propose_gaussian));
/// @endcode
///
/// In this case, the sample type has been inferred directly from the types of the proposal and target functions. You
/// may also specify it directly if necessary.
///
/// Calling draw() on \c s will now draw point locations from the Rosenbrock PDF we defined earlier. In real-world use,
/// you should keep an eye on the value returned from acceptance_rate() to check it is around 0.25.
///
/// @sa http://en.wikipedia.org/wiki/Metropolis%E2%80%93Hastings_algorithm
///
class metropolis_hastings_sampler {
public:
    /// @brief Initialise the sampler.
    ///
    /// @param target A functor representing the target distribution. This is copied internally.
    /// @param propose A functor for proposing new samples. This is copied internally.
    /// @param initial_sample An initial starting sample.
    /// @param uniform A functor for generating uniform reals on the interval [0,1].
    metropolis_hastings_sampler(const partition_distribution& target, const partition_proposal& propose,
            const partition_sampler_sample& initial_sample )
        : target_(target)
        , propose_(propose)
        , sample_(initial_sample)
        , last_proposal_(partition_proposal::result_type(initial_sample, -std::numeric_limits<float>::max()))
        , temperature_(1.0f)
        , acceptance_seq_(5000)
        , internal_records_ptr_(new capability_recorder(initial_sample.partition_sample_ptr->first_time_stamp(),
                            initial_sample.partition_sample_ptr->last_time_stamp(),
                            initial_sample.partition_sample_ptr->tracks()))
    {
        //state_.sample_log_density = target(initial_sample);
    }

    /// @brief Draw the next sample from the sampler.
    ///
    /// @return A reference to the new sample. This can be retrieved before the next call to draw() via last_sample().
    const partition_sampler_sample& draw();

    /// @brief Obtain a reference to the last sample drawn from the sampler.
    const partition_sampler_sample& last_sample() const { return sample_; }

    /// @brief Obtain the log probability density of the last sample drawn from the sampler. log(P(x| theta, D)
    float current_sample_log_density() const { return state_.sample_log_density; }

    /// @brief Obtain the current acceptance rate of the sampler.
    ///
    /// The acceptance rate is defined as the ratio of accepted proposals to total number of proposals. The usual rule
    /// of thumb for Metropolis-Hastings is that this value, often termed \f$ \alpha \f$, should be around 0.25.
    float acceptance_rate() const { return state_.acceptance_rate(); }

    /// @brief Obtain the recent acceptance rate of the sampler.
    ///
    /// This acceptance rate only takes the last samples into account instead of the whole sampling history
    float recent_acceptance_rate() const {
        return std::accumulate(acceptance_seq_.begin(), acceptance_seq_.end(), 0.f)/float(acceptance_seq_.size());
    }

    /// @brief The last proposed sample
    partition_sampler_sample proposed_sample() const { return last_proposal_.partition_sample; }

    /*
    /// @brief The last proposed move
    mh_moves::move_type proposed_move() const { return proposed_sample().executed_move; }
    */

    /// @brief last log alpha
    float last_log_alpha() const { return state_.last_log_alpha; }

    /// @brief log density of the last proposed partition log(P(x'|theta, D))
    float last_log_density() const { return state_.last_proposal_log_density; }

    /// @brief last proposal density ratio Q(T|T*)/Q(T*|T)
    float last_pdr() const {return state_.last_pdr; }

    bool last_proposal_accepted() const { return state_.accepted; }

    mh_state_observer current_state() const { return state_; }
    internal_containers_observer current_internals() const { return intern_; }

    void set_temperature(float temperature) {
        if (temperature > 1.f or temperature <= 0.f) {
            throw std::runtime_error("temperature must be in (0, 1].");
        }
        temperature_ = temperature;
    }

protected:
    /// @brief The target distribution functor.
    partition_distribution target_;

    /// @brief The proposal function and PDF. See class documentation.
    partition_proposal propose_;

    /// @brief The current sample.
    partition_sampler_sample sample_;

    /// @brief The last (possibly not accepted) proposal
    partition_proposal::result_type last_proposal_;

    mh_state_observer state_;
    internal_containers_observer intern_;

    float temperature_;

    fqueue<int> acceptance_seq_;

    capability_recorder_ptr internal_records_ptr_;

};

} }

#endif // BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <vector>
#include <string>
#include <iostream>
#include <boost/graph/stanford_graph.hpp>
#include <boost/graph/topological_sort.hpp>

int main()
{
    using namespace boost;
    const int n_vertices = 7;
    Graph* sgb_g = gb_new_graph(n_vertices);

    const char* tasks[]
        = { "pick up kids from school", "buy groceries (and snacks)",
              "get cash at ATM", "drop off kids at soccer practice",
              "cook dinner", "pick up kids from soccer", "eat dinner" };
    const int n_tasks = sizeof(tasks) / sizeof(char*);

    gb_new_arc(sgb_g->vertices + 0, sgb_g->vertices + 3, 0);
    gb_new_arc(sgb_g->vertices + 1, sgb_g->vertices + 3, 0);
    gb_new_arc(sgb_g->vertices + 1, sgb_g->vertices + 4, 0);
    gb_new_arc(sgb_g->vertices + 2, sgb_g->vertices + 1, 0);
    gb_new_arc(sgb_g->vertices + 3, sgb_g->vertices + 5, 0);
    gb_new_arc(sgb_g->vertices + 4, sgb_g->vertices + 6, 0);
    gb_new_arc(sgb_g->vertices + 5, sgb_g->vertices + 6, 0);

    typedef graph_traits< Graph* >::vertex_descriptor vertex_t;
    std::vector< vertex_t > topo_order;
    topological_sort(sgb_g, std::back_inserter(topo_order),
        vertex_index_map(get(vertex_index, sgb_g)));
    int n = 1;
    for (std::vector< vertex_t >::reverse_iterator i = topo_order.rbegin();
         i != topo_order.rend(); ++i, ++n)
        std::cout << n << ": " << tasks[get(vertex_index, sgb_g)[*i]]
                  << std::endl;

    gb_recycle(sgb_g);
    return EXIT_SUCCESS;
}


/*
 * Imu.hpp
 *
 *  Created on: Apr 30, 2021
 *      Author: jelavice
 */

#pragma once
#include <Eigen/Dense>
#include "icp_localization/common/time.hpp"

#include <sensor_msgs/Imu.h>

namespace icp_loco {

template<typename FloatType>
class ImuReading
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Vector = Eigen::Matrix<FloatType, 3, 1>;
  using Quaternion = Eigen::Quaternion<FloatType>;
  ImuReading()
      : ImuReading(Vector::Zero(), Vector::Zero(), Quaternion::Identity())
  {
  }
  ImuReading(const Vector& lin, const Vector& ang)
      : ImuReading(lin, ang, Quaternion::Identity())
  {
  }

  ImuReading(const Vector& lin, const Vector& ang, const Quaternion &q)
      : acc_(lin),
        angVel_(ang),
        q_(q)
  {
  }

  template<typename OtherType>
  ImuReading<OtherType> cast() const
  {
    return ImuReading<OtherType>(acc_.template cast<OtherType>(),
                                 angVel_.template cast<OtherType>(), q_.template cast<OtherType>());
  }

  const Vector& acceleration() const
  {
    return acc_;
  }
  const Vector& angularVelocity() const
  {
    return angVel_;
  }
  const Quaternion& rotation() const
  {
    return q_;
  }

  Vector& acceleration()
  {
    return acc_;
  }
  Vector& angularVelocity()
  {
    return angVel_;
  }
  Quaternion& rotation()
  {
    return q_;
  }

  std::string asString() const{
    const double kRadToDeg = 180.0 / M_PI;

    const std::string acc  =  string_format("acc:[%f, %f, %f]", acc_.x(), acc_.y(), acc_.z());
    const std::string angVel  =  string_format("angVel:[%f, %f, %f]", angVel_.x(), angVel_.y(), angVel_.z());
    const std::string rot = string_format("q:[%f, %f, %f, %f]",q_.x(), q_.y(), q_.z(), q_.w());
    const auto rpy = toRPY(q_) * kRadToDeg;
    const std::string rpyString = string_format("rpy (deg):[%f, %f, %f]",rpy.x(), rpy.y(),rpy.z());
    return acc + " ; " +angVel + " ; " + rot + " ; " + rpyString;
  }

 private:
  Vector acc_;
  Vector angVel_;
  Quaternion q_;
};

using ImuReadingd = ImuReading<double>;
using ImuReadingf = ImuReading<float>;

struct TimestampedImuReading
{
  Time time_;
  ImuReadingd imu_;
};

TimestampedImuReading interpolate(const TimestampedImuReading& start, const TimestampedImuReading& end,
                             const Time &time);

TimestampedImuReading fromRos(const sensor_msgs::Imu &msg);

} // namespace icp_loco


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

// 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)

//[boost_polygon_ring
//`Shows how to use Boost.Polygon polygon_data within Boost.Geometry

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon.hpp>

int main()
{
    typedef boost::polygon::polygon_data<int> polygon;
    typedef boost::polygon::polygon_traits<polygon>::point_type point;

    point pts[5] = {
        boost::polygon::construct<point>(0, 0),
        boost::polygon::construct<point>(0, 10),
        boost::polygon::construct<point>(10, 10),
        boost::polygon::construct<point>(10, 0),
        boost::polygon::construct<point>(0, 0)
    };

    polygon poly;
    boost::polygon::set_points(poly, pts, pts+5);

    std::cout << "Area (using Boost.Geometry): "
        << boost::geometry::area(poly) << std::endl;
    std::cout << "Area (using Boost.Polygon): "
        << boost::polygon::area(poly) << std::endl;

    return 0;
}

//]

//[boost_polygon_ring_output
/*`
Output:
[pre
Area (using Boost.Geometry): 100
Area (using Boost.Polygon): 100
]
*/
//]


// vi: set et ts=4 sw=2 sts=2:

#ifndef HMAT_HMATRIX_LOW_RANK_DATA_HPP
#define HMAT_HMATRIX_LOW_RANK_DATA_HPP

#include "common.hpp"
#include "hmatrix_data.hpp"
#include <armadillo>

namespace hmat {

template <typename ValueType>
class HMatrixLowRankData : public HMatrixData<ValueType> {

public:
  void apply(const arma::Mat<ValueType> &X, arma::Mat<ValueType> &Y,
             TransposeMode trans, ValueType alpha, ValueType beta) const
      override;

  void apply(const arma::subview<ValueType> &X, arma::subview<ValueType> &Y,
             TransposeMode trans, ValueType alpha, ValueType beta) const
      override;

  const arma::Mat<ValueType> &A() const;
  arma::Mat<ValueType> &A();

  const arma::Mat<ValueType> &B() const;
  arma::Mat<ValueType> &B();

  int rows() const override;
  int cols() const override;
  int rank() const override;

  typename ScalarTraits<ValueType>::RealType frobeniusNorm() const override;

  double memSizeKb() const override;

private:
  arma::Mat<ValueType> m_A;
  arma::Mat<ValueType> m_B;
};
}

#include "hmatrix_low_rank_data_impl.hpp"

#endif