/***************************************************************************
                          treealgs.cc  -  description
                             -------------------
    begin                : Tue Jan 31 2006
    copyright            : (C) 2005 by Knut-Helge Vik
    email                : knuthelv@ifi.uio.no
 ***************************************************************************/
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>
#include <functional>
#include <fstream>

#include "treestructure.h"
#include "treealgs.h"
#include "../simdefs.h"
#include "../network/vertex_set.h"

using namespace std;
using namespace boost;

namespace TreeAlgorithms
{
ostream& operator<<(ostream& ostr, const pair<int, int> id)
{
	ostr << "( " << id.first << "," << id.second << " )" ;
	return ostr;
}

bool isVertex(const GraphN &g, int u)
{
	bool isV = true;
	if(num_vertices(g) > u)
	{
		if(g[u].id == -1)  isV = false;

		if(!isV)
		{
			//cerr << WRITE_PRETTY_FUNCTION << " v: " << u << " prop : " << g[u] << endl;
			ASSERTING(out_degree(u, g) <= 0);
		}
	}
	else
		isV = false;

	return isV;
}

bool isEdge(const GraphN &g, int u, int v)
{
	if(!isVertex(g,u) || !isVertex(g,v)) return false;

	pair<edge_descriptorN, bool> ep = edge(u,v,g);
	if(!ep.second) return false;

	return true;
}

int getOutDegree(const GraphN &g, int u)
{
	int od = 0;
	if(isVertex(g, u))
	{
		od = out_degree(u, g);
	}

	return od;
}

int getOutDegree(int u, const GraphN &g)
{
	int od = 0;
	if(isVertex(g, u))
	{
		od = out_degree(u, g);
	}

	return od;
}

int getDegreeConstraint(const GraphN &g, int u)
{
	//cerr << WRITE_FUNCTION << u << ":" << g[u] << endl;
	if(g[u].degree_limit > -1)
	{
		//cerr << WRITE_FUNCTION << " returning node specific degree limit : " << g[u] << endl;
		return g[u].degree_limit;
	}

	if(g[u].vertexFunction == CLIENT)
		return (GlobalSimArgs::getDegreeConstraint());
	else if(g[u].vertexFunction == WELL_CONNECTED)
		return (GlobalSimArgs::getDegreeConstraintSP());
	else
		ASSERTING(0);

	return max(GlobalSimArgs::getDegreeConstraint(), GlobalSimArgs::getDegreeConstraintSP());
}

int getDegreeConstraint(int u, const GraphN &g)
{
	return getDegreeConstraint(g, u);
}

int getDegreeConstraint(const GraphN &g, int u, int leverage)
{
	return (getDegreeConstraint(g,u) + leverage);
}

std::ostream& operator<<(std::ostream& ostr, const TreeStructure &T)
{
	T.print(ostr);
	return ostr;
}
/*-----------------------------------------------------------------------
	class TreeStructure:
				Functions to class TreeStructure
------------------------------------------------------------------------- */
TreeStructure::TreeStructure(const GraphN &gin) : timeStamp_(0.0)
{
    insertGraph(gin);
}

void
TreeStructure::insertGraph(const GraphN &gin)
{
	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(gin); vit != vit_end; ++vit)
	{
		if(num_vertices(gin) > *vit && gin[*vit].id > -1)
		{
			if(out_degree(*vit, gin) > 0)
				insertEdges(*vit, gin);
		}
	}
}

void
TreeStructure::meshify(int v)
{
	for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		if(v == *vit) continue;
		insertEdge(v, *vit);
	}
}

int
TreeStructure::meshifyGraph()
{
	int vertex_degree = V.size() - 1;
	int num_edges_added = 0;
	for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		int src = *vit;
		VertexSet outV;
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(src, g); oit != oit_end; ++oit)
		{
			int targ = target(*oit, g);
			ASSERTING(targ != src);
			outV.insert(targ);

			if(g[*oit].weight <= 0.0001) g[*oit].weight = 1.0f;
		}

		VertexSet remV = V - outV - src;

		for(VertexSet::iterator it = remV.begin(), it_end = remV.end(); it != it_end; ++it)
		{
			int targ = *it;

			if(isEdge(src, targ)) continue;
			if(src == targ) continue;

			pair<edge_descriptorN, bool> e = add_edge(src, targ, g);
			ASSERTING(e.second);

			if(g[src].id < 0)
			{
				g[src].id = src;
				g[src].vertexFunction = CLIENT;
			}
			if(g[targ].id < 0)
			{
				g[targ].id = targ;
				g[targ].vertexFunction = CLIENT;
			}

			num_edges_added++;

			EdgeProp ep;
			ep.id = pair<int, int>(src, targ);
			ep.delay = 1.0f;
			ep.weight = 1.0f;
			ep.bw = 1000.0;
			ep.cost = 1.0;
			ep.length = 1.0;
			ep.hops = 1;

			g[e.first] = ep;
		}

		ASSERTING(getOutDegree(src) == vertex_degree);
	}

	return num_edges_added;
}

VertexFunction
TreeStructure::vertexFunction(int v) const
{
	if(!isVertex(v)) return NO_FUNCTION;

	return g[v].vertexFunction;
}

VertexState
TreeStructure::vertexState(int v) const
{
	if(!isVertex(v)) return NO_STATE;
	return g[v].vertexState;
}

bool
TreeStructure::isWCN(int v) const
{
	return vertexFunction(v) == WELL_CONNECTED;
}

bool
TreeStructure::isGroupMember(int v) const
{
	return vertexState(v) == GROUP_MEMBER;
}

bool
TreeStructure::isSteinerPoint(int v) const
{
	return vertexState(v) == STEINER_POINT;
}

void TreeStructure::makeMember(int v)
{
	g[v].vertexState = GROUP_MEMBER;

	S.erase(v);
}
void TreeStructure::makeSteiner(int v)
{
	g[v].vertexState = STEINER_POINT;

	S.insert(v);
}

void TreeStructure::makeSteinerAvailable(int v)
{
	g[v].vertexState = STEINER_POINT_AVAILABLE;
	S.insert(v);
}

void TreeStructure::clearEdges()
{
	EdgeList::iterator oit = E.begin(), oit_end = E.end();
	for( ; oit != oit_end; ++oit)
	{
		int src = source(*oit, g), targ = target(*oit, g);
		pair<edge_descriptorN, bool> ep = edge(src, targ, g);
		if(ep.second) g[ep.first] = EdgeProp();

		remove_edge(src, targ, g);
		remove_edge(src, targ, g);
	}

	// -- debug --
	edge_iteratorN ei, ei_end;
    boost::tuples::tie(ei, ei_end) = edges(g);
	ASSERTING(ei == ei_end);
	// -- debug end --

	E.clear();
	Ep.clear();
}
void TreeStructure::clearSteiner()
{
	//cerr << WRITE_FUNCTION << "removing steiner : " << endl;

	V = V - S;
	VertexSet::iterator sit = S.begin(), sit_end = S.end();
	for( ; sit != sit_end; ++sit)
	{
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(*sit, g); oit != oit_end; ++oit) removeEdge(*oit);

		//cerr << *sit << " " ;
		clear_vertex(*sit, g);
		g[*sit] = VertexProp();
	}
	S.clear();
	//cerr << endl;
}

void TreeStructure::removeEdge(edge_descriptorN e)
{
	int src = source(e, g), targ = target(e, g);
	this->removeEdge(src, targ);
}

void TreeStructure::removeEdge(int src, int targ)
{
	if(isEdge(src, targ))
	{
		pair<edge_descriptorN, bool> ep = edge(src, targ, g);
		ASSERTING(ep.second);
		g[ep.first] = EdgeProp();
		remove_edge(src, targ, g);
		remove_edge(targ, src, g);
		Ep.removeEdge(g[src].id, g[targ].id);
	}

	E.removeEdge(g, src, targ);
}

void TreeStructure::removeEdges(int u, const VertexSet &V, const GraphN &global_g)
{
	for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
		removeEdge(u, *vit);
}

void TreeStructure::removeVertex(int v)
{
	if(!isVertex(v)) return;

	if((g[v].vertexState == STEINER_POINT) || (g[v].vertexState == STEINER_POINT_AVAILABLE))
		S.erase(v);

	//cerr << WRITE_FUNCTION << "id: " << v << " removing edges " << endl;
	vertex_descriptorN v_src = vertex(v, g);
	ASSERTING(v_src == v);

	//EdgePair epList;
	out_edge_iteratorN oit, oit_end;
    for(boost::tuples::tie(oit, oit_end) = out_edges(v_src, g); oit != oit_end; ++oit)
	{
		vertex_descriptorN src = source(*oit, g), targ = target(*oit, g);

		// -- debug --
		//cerr << *oit << " " << " prop: " << g[*oit] << " " ;
		//epList.push_back(EdgeId(src, targ));
		ASSERTING(src == v_src);
		ASSERTING(V.contains(targ));
		// -- end debug --

		E.removeEdge(g, src, targ);
		Ep.removeEdge(g[src].id, g[targ].id);
	}

	// -- debug remove --
	//for(EdgePair::iterator it = epList.begin(), it_end = epList.end(); it != it_end; ++it)
	//{
	//	cerr << *it << " ";
	//	int src = it->first, targ = it->second;
	//	if(isEdge(src, targ)) remove_edge(src, targ, g);
	//}
	// -- end debug remove --
	//cerr << endl;

	V.erase(v_src);
	clear_vertex(v_src, g);
	g[v_src] = VertexProp(); // reset vertex
}

void TreeStructure::insertVertex(int v, const GraphN &global_g)
{
	this->insertVertex(v, global_g[v]);
}

void TreeStructure::addVertex(int v)
{
	int num = num_vertices(g);
	if(num <= v)
	{
		ASSERTING(num <= v);
		int ret = -1;

		for(int i = num; i <= v; i++)
			ret = add_vertex(g);

		ASSERTING(ret == v);
		ASSERTING(num_vertices(g) == v + 1);
	}
}

void TreeStructure::insertVertex(int v, const VertexProp &vp)
{
	addVertex(v);

	if(vp.vertexState == STEINER_POINT || vp.vertexState == STEINER_POINT_AVAILABLE)
	{
		S.insert(v);
		g[v] = VertexProp(vp, STEINER_POINT);
	}
	else g[v] = VertexProp(vp);

	V.insert(v);

	ASSERTING(isVertex(v));
}
void TreeStructure::insertVertex(int v, const VertexProp &vp, const VertexState &state)
{
	addVertex(v);
	if(state == STEINER_POINT_AVAILABLE || state == STEINER_POINT) S.insert(v);

	V.insert(v);
	g[v] = VertexProp(vp, state);
	ASSERTING(isVertex(v));
}

void TreeStructure::insertVertex(int v, const VertexState &state, const VertexFunction &func)
{
	addVertex(v);

	if(state == STEINER_POINT_AVAILABLE || state == STEINER_POINT) S.insert(v);

	V.insert(v);
	g[v] = VertexProp(v, state, func);

	ASSERTING(isVertex(v));
}

bool TreeStructure::isVertex(int u) const
{
	ASSERTING(u > -1);

	bool isV = true;
	if(num_vertices(g) > u) // && u > -1)
	{
		vertex_descriptorN src = vertex(u, g);
		ASSERTING(src == u);

		if(g[u].id == -1)  isV = false;

		if(!isV)
		{
			//cerr << WRITE_PRETTY_FUNCTION << " v: " << u << " prop : " << g[u] << endl;
			ASSERTING(out_degree(u, g) <= 0);
		}
		else ASSERTING(g[u].id == u);
	}
	else
	{
		isV = false;
        ASSERTING(V.contains(u));
        ASSERTING(S.contains(u));
	}

	return isV;
}

bool TreeStructure::isEdge(int u, int v) const
{
	if(!isVertex(u) || !isVertex(v)) return false;

	pair<edge_descriptorN, bool> ep = edge(u,v,g);
	if(!ep.second) return false;

	return true;
}

int TreeStructure::getOutDegree(int u) const
{
	int od = 0;
	if(isVertex(u)) od = out_degree(u, g);

	return od;
}

int TreeStructure::getAvOutDegree(int u) const
{
	return (getDegreeConstraint(g, u) - getOutDegree(u));
}

void TreeStructure::insertEdge(edge_descriptorN e, const GraphN &global_g)
{
	int src = source(e, global_g), targ = target(e, global_g);
	this->insertEdge(src, targ, global_g);
}

void TreeStructure::insertEdge(int src, int targ, const GraphN &global_g)
{
	vertex_descriptorN u = vertex(src, global_g);
	vertex_descriptorN v = vertex(targ, global_g);

	if(u != src) cerr << WRITE_FUNCTION << u << " != " << src << endl;
	if(v != targ) cerr << WRITE_FUNCTION << v << " != " << targ << endl;
	ASSERTING(u == src);
	ASSERTING(v == targ);

	pair<edge_descriptorN, bool> e = edge(src, targ, global_g);
	ASSERTING(e.second);

	if(!(isVertex(src)))  insertVertex(src, global_g[src]);
	if(!(isVertex(targ))) insertVertex(targ, global_g[targ]);

	this->insertEdge(src, targ, global_g[e.first]);
}

void TreeStructure::insertEdge(int src, int targ, const EdgeProp &ep)
{
	//cerr << WRITE_FUNCTION << " src " << src << " targ " << targ << " ep: " << ep << endl;
	if(!isEdge(src, targ))
	{
		ASSERTING(num_vertices(g) > src || num_vertices(g) > targ);

		pair<edge_descriptorN, bool> new_e = add_edge(src, targ, g);
		ASSERTING(new_e.second);
		g[new_e.first] = ep;
		g[new_e.first].id = ep.id;
		g[new_e.first].weight = ep.weight;

		E.push_back(new_e.first);
		Ep.insertEdge(g[src].id, g[targ].id);

		// -- debug --
		ASSERTING(g[src].id == src);
		ASSERTING(g[targ].id == targ);
		int u = source(new_e.first, g), v = target(new_e.first, g);
		ASSERTING(src == u || src == v);
		ASSERTING(targ == u || targ == v);
		// -- debug end --
	}
	else // exists -> update edge properties
	{
		ASSERTING(num_vertices(g) > src || num_vertices(g) > targ);
		pair<edge_descriptorN, bool> e = edge(src, targ, g);
		ASSERTING(e.second);
		g[e.first] = ep;

		g[e.first].id = ep.id;
		g[e.first].weight = ep.weight;

		// -- debug --
		ASSERTING(g[src].id == src);
		ASSERTING(g[targ].id == targ);
		// -- end debug --
	}
}

void TreeStructure::updateEdge(const EdgeId &id, const EdgeProp &ep)
{
	insertEdge(id.first, id.second, ep);
}

void TreeStructure::insertEdges(int u, const VertexSet &V, const GraphN &global_g)
{
	for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
		insertEdge(u, *vit, global_g);
}

void
TreeStructure::insertEdges(int v, const GraphN &gin)
{
	out_edge_iteratorN oit, oit_end;
    for(boost::tuples::tie(oit, oit_end) = out_edges(v, gin); oit != oit_end; ++oit)
		insertEdge(*oit, gin);
}

void TreeStructure::mergeTrees(const TreeStructure &subTree)
{
	VertexSet::const_iterator vit = subTree.V.begin(), vit_end = subTree.V.end();
	for( ; vit != vit_end; ++vit)
	{
		insertVertex(*vit, subTree.g);
	}

	edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = edges(subTree.g); eit != eit_end; ++eit)
	{
		int src = source(*eit, subTree.g), targ = target(*eit, subTree.g);
		if(isEdge(src, targ)) continue;

		insertEdge(src, targ, subTree.g);
	}
}

void TreeStructure::mergeTrees(const TreeStructure &subTree, const GraphN &global_g)
{
	VertexSet::const_iterator vit = subTree.V.begin(), vit_end = subTree.V.end();
	for( ; vit != vit_end; ++vit)
	{
		insertVertex(*vit, global_g);
	}

	edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = edges(subTree.g); eit != eit_end; ++eit)
	{
		int src = source(*eit, subTree.g), targ = target(*eit, subTree.g);
		if(isEdge(src, targ)) continue;

		insertEdge(src, targ, global_g);
	}
}
void TreeStructure::diffTrees(const TreeStructure &subTree)
{
	edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = edges(subTree.g); eit != eit_end; ++eit)
	{
		int src = source(*eit, subTree.g), targ = target(*eit, subTree.g);
		if(isEdge(src, targ))
		{
			removeEdge(src, targ);
		}
	}

	VertexSet::const_iterator vit = subTree.V.begin(), vit_end = subTree.V.end();
	for( ; vit != vit_end; ++vit)
	{
		if(getOutDegree(*vit) <= 0) removeVertex(*vit);
	}
}

void TreeStructure::removeUnusedVertices()
{
	VertexSet remV;
	VertexSet::iterator vit = V.begin(), vit_end = V.end();
	for( ; vit != vit_end; ++vit)
	{
		if(getOutDegree(*vit) <= 0)
		{
			if(g[*vit].vertexState == GROUP_MEMBER)
			{
				print();
				printGraph();
				printVertexState(cerr);
			}

			ASSERTING(g[*vit].vertexState != GROUP_MEMBER);
			remV.insert(*vit);
		}
	}

	vit = remV.begin();
	vit_end = remV.end();
	for( ; vit != vit_end; ++vit)
		removeVertex(*vit);
}

void TreeStructure::getNeighbors(int u, VertexSet &N) const
{
	if(!isVertex(u)) return;

	out_edge_iteratorN oit, oit_end;
    for(boost::tuples::tie(oit, oit_end) = out_edges(u, g); oit != oit_end; ++oit)
	{
		int v = target(*oit, g);
		N.insert(v);
	}
}

void TreeStructure::vertexDistance(int u, int v)
{
	// TODO
	ASSERTING(0);
}

void TreeStructure::clearVertex(int u)
{
	if(getOutDegree(u) <= 0) return;

	// gather edges
	EdgePair remEdges;
	out_edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = out_edges(u, g); eit != eit_end; ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		remEdges.push_back(EdgeId(src,targ));
	}

	// remove edges
	for(EdgePair::iterator eit = remEdges.begin(), eit_end = remEdges.end(); eit != eit_end; ++eit)
		removeEdge(eit->first, eit->second);

	ASSERTING(getOutDegree(u) <= 0);
}

void TreeStructure::print() const
{
    iVertexMapConst idmap = get(&VertexProp::id, g);

	VertexSet_cit vit;
	cerr << "V:";
	for(vit = V.begin(); vit != V.end(); ++vit)
		cerr << *vit << ",";

	if(!S.empty())
	{
		cerr << " S:";
		for(vit = S.begin(); vit != S.end(); ++vit)
			cerr << *vit << ",";
	}

	if(!C.empty())
	{
		cerr << " C:";
		for(vit = C.begin(); vit != C.end(); ++vit)
			cerr << *vit << ",";
	}

	cerr << " E: ";
	EdgeList_cit eit;
	for(eit = E.begin(); eit != E.end(); ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		cerr << " (" << src << "," << targ << ")";
	}
	cerr << endl;

	cerr << " Ep : " ;
	EdgePair::const_iterator peit;
	for(peit = Ep.begin(); peit != Ep.end(); ++peit)
	{
		cerr << " (" << peit->first << ", " << peit->second << ")" ;
	}
	cerr << endl;
}
void TreeStructure::print(ostream &ostr) const
{
    iVertexMapConst idmap = get(&VertexProp::id, g);

	VertexSet_it vit;
	ostr << "V:";
	for(vit = V.begin(); vit != V.end(); ++vit)
		ostr << *vit << ",";

	if(!S.empty())
	{
		ostr << " S:";
		for(vit = S.begin(); vit != S.end(); ++vit)
			ostr << *vit << ",";
	}

	if(!C.empty())
	{
		ostr << " C:";
		for(vit = C.begin(); vit != C.end(); ++vit)
			ostr << *vit << ",";
	}

	ostr << " E: ";
	EdgeList_cit eit;
	for(eit = E.begin(); eit != E.end(); ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		ostr << " (" << src << "," << targ << ")";
	}
	//ostr << endl;

	ostr << " Ep : " ;
	EdgePair::const_iterator peit;
	for(peit = Ep.begin(); peit != Ep.end(); ++peit)
	{
		ostr << " (" << peit->first << ", " << peit->second << ")" ;
	}
	//ostr << endl;
}

void TreeStructure::printGraph() const
{
	// get the property map for vertex indices
    typedef property_map<const GraphN, vertex_index_t>::type IndexMap;
    IndexMap index = get(vertex_index, g);
    iVertexMapConst idmap = get(&VertexProp::id, g);

    cerr << "vertices(g) = (";
    pair<vertex_iteratorN, vertex_iteratorN> vp;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first)
    {
		if(idmap[*vp.first] >= 0 )
			cerr << index[*vp.first] << " "; // << " id: " << name[idmap[*vp.first]] << " sp: " << zmap[*vp.first] << ") ";
    }
    cerr << ")" << endl;

    cerr << "edges(g) = ";
    dEdgeMapConst wm = get(&EdgeProp::weight, g);
    graph_traits<GraphN>::edge_iterator ei, ei_end;
    for (boost::tuples::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    {
        cerr << "((" << index[source(*ei, g)] << "," << index[target(*ei, g)] << ") " ; // w(" << ei.weight << ") " ;
		//EdgeProp ep = get(boost::edge_bundle, g)[*ei];
		cerr << " w: " << wm[*ei] << ") "; // << ep.weight << " ";
    }
    cerr << endl;
}
void TreeStructure::printGraph(ostream &ostr) const
{
	// get the property map for vertex indices
    typedef property_map<const GraphN, vertex_index_t>::type IndexMap;
    IndexMap index = get(vertex_index, g);
    iVertexMapConst idmap = get(&VertexProp::id, g);

    ostr << "vertices(g) = (";
    pair<vertex_iteratorN, vertex_iteratorN> vp;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first)
    {
		if(idmap[*vp.first] >= 0 )
			ostr << index[*vp.first] << " "; // << " id: " << name[idmap[*vp.first]] << " sp: " << zmap[*vp.first] << ") ";
    }
    ostr << ")" << endl;

    ostr << "edges(g) = ";
    dEdgeMapConst wm = get(&EdgeProp::weight, g);
    graph_traits<GraphN>::edge_iterator ei, ei_end;
    for (boost::tuples::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    {
        ostr << "((" << index[source(*ei, g)] << "," << index[target(*ei, g)] << ") " ; // w(" << ei.weight << ") " ;
		//EdgeProp ep = get(boost::edge_bundle, g)[*ei];
		ostr << " w: " << wm[*ei] << ") "; // << ep.weight << " ";
    }
    ostr << endl;
}
void TreeStructure::printVertexState(ostream& ostr) const
{
	vsVertexMapConst vsmap = get(&VertexProp::vertexState, g);
	iVertexMapConst idmap  = get(&VertexProp::id, g);

	ostr << "Graph VertexState: " << endl;

	VertexSet::const_iterator it,end;
	for(it = V.begin(), end = V.end(); it != end; it++)
	{
		ostr << g[*it] << endl;
	}
}

void TreeStructure::clearVE()
{
	V.clear();
	E.clear();
}

void TreeStructure::clear()
{
	V.clear();
	S.clear();
	E.clear();
	Ep.clear();
	g.clear();
}

void TreeStructure::insertEdge(int src, int targ)
{
	if(!(isVertex(src)))  insertVertex(src);
	if(!(isVertex(targ))) insertVertex(targ);

	EdgeProp ep;
	ep.id = pair<int, int>(src, targ);
	ep.delay = 1.0f; //(std::numeric_limits<double>::max)() - 2;
	ep.weight = 1.0f; //(std::numeric_limits<double>::max)() - 2;
	ep.bw = 1000.0; //(std::numeric_limits<double>::max)() - 2;
	ep.cost = 1.0;
	ep.length = 1.0;
	ep.hops = 1;

	this->insertEdge(src, targ, ep);
}

void TreeStructure::insertVertex(int v)
{
	insertVertex(v, VertexProp(v, NO_STATE, NO_FUNCTION) );
}

}; // namespace TreeAlgorithms

/*
void TreeStructure::createSimVIndex()
{
	ASSERTING(V.size() != 0);

	VertexSet::iterator vit, vit_end;
	for(vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		VertexProp vp = g[*vit];
		simV.insertVertex(vp);
	}
}

void TreeStructure::createSimVId()
{
	ASSERTING(V.size() != 0);

	iVertexMap idmap = get(&VertexProp::id, g);

	VertexSet::iterator vit, vit_end;
	for(vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		VertexProp vp = g[idmap[*vit]];
		simV.insertVertex(vp);
	}
}

void TreeStructure::createSimEIndex()
{
	ASSERTING(E.size() != 0);

	EdgeList::iterator eit, eit_end;
	for(eit = E.begin(), eit_end = E.end(); eit != eit_end; ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		pair<edge_descriptorN, bool> e = edge(src, targ, g);
		ASSERTING(e.second);
		simE.insertEdge(src, targ, g[e.first]);
	}
}

void TreeStructure::createSimEId()
{
	ASSERTING(Ep.size() != 0);

	EdgePair::iterator peit, peit_end;
	for(peit = Ep.begin(), peit_end = Ep.end(); peit != peit_end; ++peit)
	{
		pair<edge_descriptorN, bool> e = edge(peit->first, peit->second, g);
		assert(e.second);
		EdgeProp ep = g[e.first];
		simE.insertEdge(peit->first, peit->second, ep);
	}
}
void TreeStructure::createSimVEIndex()
{
	createSimVIndex();
	createSimEIndex();
}

void TreeStructure::createSimVEId()
{
	createSimVId();
	createSimEId();
}

void TreeStructure::createSimGraphIndex()
{
	ASSERTING(V.size() != 0 || E.size() != 0);

	vertex_iteratorN vit, vit_end;
	for(tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		VertexProp vp = g[*vit];
		simG.insertVertex(vp);
	}

	edge_iteratorN eit, eit_end;
	for(tie(eit, eit_end) = edges(g); eit != eit_end; ++eit)
	{
		EdgeProp ep = g[*eit];
		int src = source(*eit, g), targ = target(*eit, g);
		simG.insertEdge(src, targ, ep);
	}
}

void TreeStructure::createSimGraphId()
{
	ASSERTING(V.size() != 0 || E.size() != 0);
	iVertexMap idmap = get(&VertexProp::id, g);

	vertex_iteratorN vit, vit_end;
	for(tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		VertexProp vp = g[idmap[*vit]];
		simG.insertVertex(vp);
	}

	edge_iteratorN eit, eit_end;
	for(tie(eit, eit_end) = edges(g); eit != eit_end; ++eit)
	{
		EdgeProp ep = g[*eit];
		int src = source(*eit, g), targ = target(*eit, g);
		simG.insertEdge(idmap[src], idmap[targ], ep);
	}
}

void TreeStructure::createSimTreeIndex()
{
	ASSERTING(V.size() != 0 || E.size() != 0);

	VertexSet::iterator vit, vit_end;
	for(vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		VertexProp vp = g[*vit];
		simTree.insertVertex(vp);
	}

	EdgeList::iterator eit, eit_end;
	for(eit = E.begin(), eit_end = E.end(); eit != eit_end; ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		pair<edge_descriptorN, bool> e = edge(src, targ, g);
		ASSERTING(e.second);
		simTree.insertEdge(src, targ, g[e.first]);
	}
}

void TreeStructure::createSimTreeId()
{
	assert(0);

	ASSERTING(V.size() != 0 || Ep.size() != 0);
	iVertexMap idmap = get(&VertexProp::id, g);

	VertexSet::iterator vit, vit_end;
	for(vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
	{
		VertexProp vp = g[idmap[*vit]];
		simTree.insertVertex(vp);
	}

	EdgePair::iterator peit, peit_end;
	for(peit = Ep.begin(), peit_end = Ep.end(); peit != peit_end; ++peit)
	{
		pair<edge_descriptorN, bool> e = edge(peit->first, peit->second, g);
		assert(e.second);
		EdgeProp ep = g[e.first];
		simTree.insertEdge(peit->first, peit->second, ep);
	}
}
*/


#include <iostream>

//--- SpatialOps includes ---//
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/structured/IntVec.h>
#include <spatialops/structured/MemoryWindow.h>
#include <spatialops/structured/stencil/FVStaggeredOperatorTypes.h>
#include <spatialops/OperatorDatabase.h>
#include <spatialops/structured/stencil/StencilBuilder.h>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/Nebo.h>

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace po = boost::program_options;

using namespace SpatialOps;

#define RUN_TEST(TEST,							\
		 TYPE)							\
  boost::posix_time::ptime start( boost::posix_time::microsec_clock::universal_time() ); \
  boost::posix_time::ptime end( boost::posix_time::microsec_clock::universal_time() ); \
  int ii = 0;								\
  start = boost::posix_time::microsec_clock::universal_time();		\
									\
  for(; ii < number_of_runs; ii++) {					\
    TEST;								\
  };									\
									\
  end = boost::posix_time::microsec_clock::universal_time();		\
  std::cout << TYPE;							\
  std::cout << " runs: ";						\
  std::cout << number_of_runs;						\
  std::cout << " result: ";						\
  std::cout << (end - start).total_microseconds()*1e-6;			\
  std::cout << std::endl;

template<typename FieldType>
inline void evaluate_square(FieldType & result,
                            FieldType const & phi,
                            int const number_of_runs) {
  RUN_TEST(result <<= square(phi),
           "square                  ");
};

template<typename FieldType>
inline void evaluate_pow_double(FieldType & result,
                                FieldType const & phi,
                                double const exp,
                                int const number_of_runs) {
  RUN_TEST(result <<= pow(phi, exp),
           "original pow with double");
};

template<typename FieldType>
inline void evaluate_pow_int(FieldType & result,
                             FieldType const & phi,
                             int const exp,
                             int const number_of_runs) {
  RUN_TEST(result <<= pow(phi, exp),
           "pow_int                 ");
};

int main(int iarg, char* carg[]) {
    typedef SVolField Field;

    int nx, ny, nz;
    int number_of_runs;
    int exp = 2;
#ifdef ENABLE_THREADS
  int thread_count;
#endif

    // parse the command line options input describing the problem
    {
        po::options_description desc("Supported Options");
	desc.add_options()
	  ( "help", "print help message" )
	  ( "nx", po::value<int>(&nx)->default_value(10), "Grid in x" )
	  ( "ny", po::value<int>(&ny)->default_value(10), "Grid in y" )
	  ( "nz", po::value<int>(&nz)->default_value(10), "Grid in z" )
#ifdef ENABLE_THREADS
          ( "tc", po::value<int>(&thread_count)->default_value(NTHREADS), "Number of threads for Nebo")
#endif
	  ( "runs", po::value<int>(&number_of_runs)->default_value(1), "Number of iterations of each test");

	po::variables_map args;
	po::store( po::parse_command_line(iarg,carg,desc), args );
	po::notify(args);

	if (args.count("help")) {
	    std::cout << desc << "\n";
	    return 1;
	}

#ifdef ENABLE_THREADS
    set_hard_thread_count(thread_count);
#endif
    }

    const GhostData ghost(1);
    const BoundaryCellInfo bc = BoundaryCellInfo::build<Field>(false,false,false,false,false,false);
    const MemoryWindow window( get_window_with_ghost(IntVec(nx,ny,nz),ghost,bc) );

    Field a( window, bc, ghost, NULL );
    Field r( window, bc, ghost, NULL );

    Field::iterator ia = a.begin();
    for(size_t kk = 0; kk < window.glob_dim(2); kk++)
      for(size_t jj = 0; jj < window.glob_dim(1); jj++)
        for(size_t ii = 0; ii < window.glob_dim(0); ii++, ++ia)
          *ia = ii + jj * 2 + kk * 4;

    evaluate_square(r, a, number_of_runs);
    evaluate_pow_int(r, a, exp, number_of_runs);
    evaluate_pow_double(r, a, exp, number_of_runs);

    return 0;
};


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

#include <vital/types/homography.h>

#include <Eigen/Core>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

namespace py=pybind11;
namespace kwiver {
namespace vital  {
namespace python {

namespace kv=kwiver::vital;

// Easy way to automate bindings of templated classes.
// For more information, see below link
// https://stackoverflow.com/questions/47487888/pybind11-template-class-of-many-types
template< typename T >
void declare_homogaphy( py::module &m, std::string const& typestr )
{
  using Class = kv::homography_< T >;
  using matrix_t = Eigen::Matrix< T, 3, 3>;
  const std::string pyclass_name = std::string( "Homography" ) + typestr;

  py::class_< Class, std::shared_ptr< Class >, kv::homography >(m, pyclass_name.c_str())
  .def(py::init())
  .def(py::init< matrix_t const& >())
  .def_static("random", [] ()
  {
    return Class(matrix_t::Random(3, 3));
  })
  .def("matrix", (matrix_t& (Class::*) ()) &Class::get_matrix)
  .def("inverse", &Class::inverse)
  .def("map", &Class::map,
    py::arg("point"))
  .def("normalize", &Class::normalize)
  .def("__mul__", &Class::operator*)
  .def_property_readonly( "type_name", [] ( Class const& self )
  {
    return self.data_type().name()[0];
  })
  ;
}

}
}
}

using namespace kwiver::vital::python;
PYBIND11_MODULE(homography, m)
{
  py::module::import("kwiver.vital.types.transform_2d");
  py::class_< kv::homography,
              kv::transform_2d,
              std::shared_ptr< kv::homography > >(m, "BaseHomography");
  declare_homogaphy< float  >(m, "F");
  declare_homogaphy< double >(m, "D");
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/config.hpp>

#include <algorithm>
#include <vector>
#include <utility>
#include <iostream>

#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/graph_utility.hpp>

template<class NewGraph, class Tag>
struct graph_copier
: public boost::base_visitor<graph_copier<NewGraph, Tag> >
{
  typedef Tag event_filter;

  graph_copier(NewGraph& graph)
      : new_g(graph) {
  }

  template<class Edge, class Graph>
  void operator()(Edge e, Graph& g) {
    boost::add_edge(boost::source(e, g), boost::target(e, g), new_g);
  }
 private:
  NewGraph& new_g;
};

template<class NewGraph, class Tag>
inline graph_copier<NewGraph, Tag>
copy_graph(NewGraph& g, Tag) {
  return graph_copier<NewGraph, Tag>(g);
}

int main(int , char* [])
{
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

  Graph G(5);
  boost::add_edge(0, 2, G);
  boost::add_edge(1, 1, G);
  boost::add_edge(1, 3, G);
  boost::add_edge(1, 4, G);
  boost::add_edge(2, 1, G);
  boost::add_edge(2, 3, G);
  boost::add_edge(2, 4, G);
  boost::add_edge(3, 1, G);
  boost::add_edge(3, 4, G);
  boost::add_edge(4, 0, G);
  boost::add_edge(4, 1, G);

  Graph G_copy(5);

  // The source vertex
  boost::depth_first_search(G,
                            boost::visitor(boost::make_dfs_visitor(copy_graph(G_copy, boost::on_tree_edge()))));

  boost::print_graph(G);
  boost::print_graph(G_copy);

  return 0;
}


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/homogeneous_coordinates.hpp>
#include <sm/serialization_macros.hpp>
#include <sm/string_routines.hpp>

namespace aslam {
namespace cameras {

template <typename CAMERA_GEOMETRY_T>
class CameraGeometryTestHarness {
  public:
    typedef CAMERA_GEOMETRY_T camera_geometry_t;
    typedef typename camera_geometry_t::keypoint_t keypoint_t;
    typedef typename camera_geometry_t::jacobian_homogeneous_t jacobian_homogeneous_t;
    typedef typename camera_geometry_t::jacobian_t jacobian_t;
    typedef typename camera_geometry_t::inverse_jacobian_t inverse_jacobian_t;
    typedef typename camera_geometry_t::inverse_jacobian_homogeneous_t inverse_jacobian_homogeneous_t;
    typedef typename camera_geometry_t::projection_t::jacobian_intrinsics_t jacobian_intrinsics_t;
    typedef Eigen::Vector3d point_t;
    typedef Eigen::Vector4d pointh_t;
    typedef typename camera_geometry_t::projection_t projection_t;

    camera_geometry_t _geometry;
    double _tolerance;

    CameraGeometryTestHarness() : _tolerance(1e-2) { _geometry = camera_geometry_t::getTestGeometry(); }

    CameraGeometryTestHarness(const camera_geometry_t& geometry, double tolerance)
        : _geometry(geometry), _tolerance(tolerance) {}

    CameraGeometryTestHarness(const camera_geometry_t& geometry) : _geometry(geometry), _tolerance(1e-2) {}

    CameraGeometryTestHarness(double tolerance) : _tolerance(tolerance) {
        _geometry = camera_geometry_t::getTestGeometry();
    }

    virtual ~CameraGeometryTestHarness() {}

    void testKeypointToEuclidean() {
        SCOPED_TRACE(__FUNCTION__);

        for (int i = 0; i < 20; i++) {
            keypoint_t y = _geometry.createRandomKeypoint();
            // Test that we can go from a keypoint to Euclidean and back to within tolerance.
            inverse_jacobian_t J;
            point_t p;
            _geometry.keypointToEuclidean(y, p, J);

            sm::eigen::assertFinite(p, SM_SOURCE_FILE_POS);
            sm::eigen::assertFinite(J, SM_SOURCE_FILE_POS);

            keypoint_t yhat;
            _geometry.euclideanToKeypoint(p, yhat);

            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");

            pointh_t ph = sm::kinematics::toHomogeneous(p);
            keypoint_t yhat2;
            _geometry.homogeneousToKeypoint(ph, yhat2);
            sm::eigen::assertFinite(yhat2, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat2, _tolerance, "");

            ph = -ph;
            _geometry.homogeneousToKeypoint(ph, yhat2);
            sm::eigen::assertFinite(yhat2, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat2, _tolerance, "");

            inverse_jacobian_t Jest;
            _geometry.keypointToEuclideanFiniteDifference(y, p, Jest);
            ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");
        }
    }

    void testKeypointToHomogeneous() {
        SCOPED_TRACE(__FUNCTION__);
        for (int i = 0; i < 20; i++) {
            point_t p = _geometry.createRandomVisiblePoint();
            pointh_t ph = sm::kinematics::toHomogeneous(p);
            keypoint_t y;
            _geometry.euclideanToKeypoint(p, y);
            // Test that we can go from a keypoint to Euclidean and back to within tolerance.
            inverse_jacobian_homogeneous_t J;
            pointh_t ph2;
            _geometry.keypointToHomogeneous(y, ph2, J);

            sm::eigen::assertFinite(ph, SM_SOURCE_FILE_POS);
            sm::eigen::assertFinite(J, SM_SOURCE_FILE_POS);

            keypoint_t yhat;
            _geometry.homogeneousToKeypoint(ph, yhat);
            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");

            // point_t p = sm::kinematics::fromHomogeneous(ph);
            // keypoint_t yhat2;
            // _geometry.euclideanToKeypoint(p,yhat2);
            // sm::eigen::assertFinite(yhat2,SM_SOURCE_FILE_POS);
            // ASSERT_DOUBLE_MX_EQ(y,yhat2,_tolerance,"");

            inverse_jacobian_homogeneous_t Jest;
            // std::cout << ph.transpose() << "\n";
            _geometry.keypointToHomogeneousFiniteDifference(y, ph2, Jest);
            ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");
            ph = -ph;
            _geometry.homogeneousToKeypoint(ph, yhat);
            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");
        }
    }

    void testEuclideanToKeypoint() {
        SCOPED_TRACE(__FUNCTION__);
        for (int i = 0; i < 20; i++) {
            keypoint_t y = _geometry.createRandomKeypoint();

            jacobian_t J;
            point_t p;
            _geometry.keypointToEuclidean(y, p);
            sm::eigen::assertFinite(p, SM_SOURCE_FILE_POS);

            keypoint_t yhat;
            _geometry.euclideanToKeypoint(p, yhat, J);
            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            sm::eigen::assertFinite(J, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");

            pointh_t ph = sm::kinematics::toHomogeneous(p);
            keypoint_t yhat2;
            _geometry.homogeneousToKeypoint(ph, yhat2);
            sm::eigen::assertFinite(yhat2, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat2, _tolerance, "");

            ph = -ph;
            _geometry.homogeneousToKeypoint(ph, yhat2);
            sm::eigen::assertFinite(yhat2, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat2, _tolerance, "");

            if (_geometry.isProjectionInvertible()) {
                // If the sensor model is invertible, we should get the same point in space.
                pointh_t ph2;
                _geometry.keypointToHomogeneous(y, ph2);
                point_t p2 = sm::kinematics::fromHomogeneous(ph2);
                ASSERT_DOUBLE_MX_EQ(p, p2, _tolerance, "");
            }

            jacobian_t Jest;
            _geometry.euclideanToKeypointFiniteDifference(p, yhat, Jest);
            ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");
        }
    }

    void testHomogeneousToKeypoint() {
        SCOPED_TRACE(__FUNCTION__);
        for (int i = 0; i < 20; i++) {
            keypoint_t y = _geometry.createRandomKeypoint();

            jacobian_homogeneous_t J;
            pointh_t ph;
            _geometry.keypointToHomogeneous(y, ph);

            // having the fourth component be zero can be a singularity
            // for the Jacobian of some camera models.
            // Setting this to 1.0 seems to work and is still testing what
            // we want.
            if (ph[3] == 0.0) ph[3] = 1.0;
            sm::eigen::assertFinite(ph, SM_SOURCE_FILE_POS);

            keypoint_t yhat;
            _geometry.homogeneousToKeypoint(ph, yhat, J);
            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            sm::eigen::assertFinite(J, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");
            jacobian_homogeneous_t Jest;
            _geometry.homogeneousToKeypointFiniteDifference(ph, y, Jest);
            ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");

            if (_geometry.isProjectionInvertible()) {
                point_t pp = sm::kinematics::fromHomogeneous(ph);
                keypoint_t yhat2;
                _geometry.euclideanToKeypoint(pp, yhat2);
                sm::eigen::assertFinite(yhat2, SM_SOURCE_FILE_POS);
                ASSERT_DOUBLE_MX_EQ(y, yhat2, _tolerance, "");

                // If the sensor model is invertible, we should get the same point in space.p
                point_t p2;
                _geometry.keypointToEuclidean(y, p2);
                ASSERT_DOUBLE_MX_EQ(pp, p2, _tolerance, "");
            }
            // This should still work
            ph = -ph;

            _geometry.homogeneousToKeypoint(ph, yhat, J);
            sm::eigen::assertFinite(yhat, SM_SOURCE_FILE_POS);
            sm::eigen::assertFinite(J, SM_SOURCE_FILE_POS);
            ASSERT_DOUBLE_MX_EQ(y, yhat, _tolerance, "");
            _geometry.homogeneousToKeypointFiniteDifference(ph, y, Jest);
            ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");
        }
    }

    void testIntrinsicsJacobian() {
        SCOPED_TRACE(__FUNCTION__);
        if (_geometry.projection().minimalDimensions() > 0) {
            for (int i = 0; i < 20; i++) {
                point_t p = _geometry.createRandomVisiblePoint(2);

                jacobian_intrinsics_t J;
                _geometry.euclideanToKeypointIntrinsicsJacobian(p, J);

                jacobian_intrinsics_t Jest;
                _geometry.euclideanToKeypointIntrinsicsJacobianFiniteDifference(p, Jest);
                ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");

                Eigen::Vector4d ph = sm::kinematics::toHomogeneous(p);
                _geometry.homogeneousToKeypointIntrinsicsJacobian(ph, J);
                _geometry.homogeneousToKeypointIntrinsicsJacobianFiniteDifference(ph, Jest);
                ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");

                ph = -ph;
                _geometry.homogeneousToKeypointIntrinsicsJacobian(ph, J);
                _geometry.homogeneousToKeypointIntrinsicsJacobianFiniteDifference(ph, Jest);
                ASSERT_DOUBLE_MX_EQ(J, Jest, _tolerance, "");
            }
        }
    }

    void testDistortionJacobian() {
        SCOPED_TRACE(__FUNCTION__);
        if (_geometry.projection().distortion().minimalDimensions() > 0) {
            // An initial test along the optical axis.
            point_t p(0.0, 0.0, 10.0);
            if (_geometry.isEuclideanVisible(p)) {
                SCOPED_TRACE(" P: " + sm::s_tuple(p(0), p(1), p(2)));
                // std::cout << "Optical axis\n";
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> J;
                _geometry.euclideanToKeypointDistortionJacobian(p, J);
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> Jest;

                _geometry.euclideanToKeypointDistortionJacobianFiniteDifference(p, Jest);
                sm::eigen::assertNear(J, Jest, 1e-5, SM_SOURCE_FILE_POS, "Distortion Jacobian");
                // This relative tolerance test was too strict for small values.
                // ASSERT_DOUBLE_MX_EQ(J,Jest,_tolerance,"");
            }

            p << 0.0, 0.0, -10.0;
            if (_geometry.isEuclideanVisible(p)) {
                SCOPED_TRACE(" P: " + sm::s_tuple(p(0), p(1), p(2)));
                // std::cout << "Negative optical axis\n";
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> J;
                _geometry.euclideanToKeypointDistortionJacobian(p, J);
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> Jest;

                _geometry.euclideanToKeypointDistortionJacobianFiniteDifference(p, Jest);
                sm::eigen::assertNear(J, Jest, 1e-5, SM_SOURCE_FILE_POS, "Distortion Jacobian");
                // This relative tolerance test was too strict for small values.
                // ASSERT_DOUBLE_MX_EQ(J,Jest,_tolerance,"");
            }

            // For some reason this case is difficult for the omni camera
            p << 1e-1, 1e-1, 10.0;
            if (_geometry.isEuclideanVisible(p)) {
                SCOPED_TRACE(" P: " + sm::s_tuple(p(0), p(1), p(2)));
                // std::cout << "Negative optical axis\n";
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> J;
                _geometry.euclideanToKeypointDistortionJacobian(p, J);
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> Jest;

                _geometry.euclideanToKeypointDistortionJacobianFiniteDifference(p, Jest);
                // This relative tolerance test was too strict for small values.
                // ASSERT_DOUBLE_MX_EQ(J,Jest,_tolerance,"");
                sm::eigen::assertNear(J, Jest, 1e-5, SM_SOURCE_FILE_POS, "Distortion Jacobian");
            }

            for (int i = 0; i < 200; i++) {
                point_t p = _geometry.createRandomVisiblePoint(10);
                SCOPED_TRACE("Trial " + boost::lexical_cast<std::string>(i) + " P: " + sm::s_tuple(p(0), p(1), p(2)));
                // SM_OUT(p);
                // keypoint_t k;
                // bool valid = _geometry.euclideanToKeypoint(p,k);
                // SM_OUT(k);
                // SM_OUT(valid);

                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> J;
                _geometry.euclideanToKeypointDistortionJacobian(p, J);
                Eigen::Matrix<double, 2, projection_t::distortion_t::IntrinsicsDimension> Jest;

                _geometry.euclideanToKeypointDistortionJacobianFiniteDifference(p, Jest);

                // This relative tolerance test was too strict for small values.
                // ASSERT_DOUBLE_MX_EQ(J,Jest,_tolerance,"");
                sm::eigen::assertNear(J, Jest, 1e-5, SM_SOURCE_FILE_POS, "Distortion Jacobian");
            }
        }
    }

    void testSerialization() {
        SCOPED_TRACE(__FUNCTION__);
        ASSERT_TRUE(_geometry.isBinaryEqual(_geometry));

        sm::boost_serialization::save(_geometry, "test.ba");

        camera_geometry_t G2;

        ASSERT_FALSE(_geometry.isBinaryEqual(G2));

        sm::boost_serialization::load(G2, "test.ba");

        ASSERT_TRUE(_geometry.isBinaryEqual(G2));
    }

    void testAll() {
        {
            SCOPED_TRACE("EuclideanToKeypoint");
            testEuclideanToKeypoint();
        }
        {
            SCOPED_TRACE("HomogeneousToKeypoint");
            testHomogeneousToKeypoint();
        }

        {
            SCOPED_TRACE("KeypointToEuclidean");
            testKeypointToEuclidean();
        }
        {
            SCOPED_TRACE("KeypointToHomogeneous");
            testKeypointToHomogeneous();
        }
        {
            SCOPED_TRACE("IntrinsicsJacobian");
            testIntrinsicsJacobian();
        }
        {
            SCOPED_TRACE("DistortionJacobian");
            testDistortionJacobian();
        }
        {
            SCOPED_TRACE("Serialization");
            testSerialization();
        }
    }
};

}  // namespace cameras
}  // namespace aslam


/**
 * Copyright (c) 2015-2016, The Kovri I2P Router Project
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be
 *    used to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * 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
 * THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 */

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include <limits>

#include "crypto/Rand.h"

/// TODO(unassigned): unit-tests for all crypto::Rand* functions

BOOST_AUTO_TEST_SUITE(RandInRange)

template <class T>
struct Range {
  T repeated, count;
  T min, max, result;
  T Test();
  Range()
      : repeated(0),
        count(0),
        min(0),
        max(std::numeric_limits<T>::max()) {
          result = Test();
        }
};

template <class T>
T Range<T>::Test() {
  do {
    repeated = result;
    result = i2p::crypto::RandInRange<T>(min, max);
    count++;
  } while ((count != 100));  // Arbitrary number
  return ((result >= min) &&
          (result <= max) &&
          (result != repeated));  // A bit harsh?
}

BOOST_AUTO_TEST_CASE(_uint8_t) {
  Range<uint8_t> test;
  BOOST_CHECK(test.result);
}

BOOST_AUTO_TEST_CASE(_uint16_t) {
  Range<uint16_t> test;
  BOOST_CHECK(test.result);
}

BOOST_AUTO_TEST_CASE(_uint32_t) {
  Range<uint32_t> test;
  BOOST_CHECK(test.result);
}

BOOST_AUTO_TEST_CASE(_uint64_t) {
  Range<uint64_t> test;
  BOOST_CHECK(test.result);
}

// Signed, so test for negative results
// regardless of initialized lowerbound
BOOST_AUTO_TEST_CASE(_int) {
  Range<int> test;
  BOOST_CHECK(test.result);
}

BOOST_AUTO_TEST_CASE(_long) {
  Range<long> test;
  BOOST_CHECK(test.result);
}

BOOST_AUTO_TEST_SUITE_END()


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file data_expression_test.cpp
/// \brief Basic regression test for data expressions.

#include <boost/test/minimal.hpp>
#include <iostream>

#include "mcrl2/core/identifier_string.h"
#include "mcrl2/data/abstraction.h"
#include "mcrl2/data/application.h"
#include "mcrl2/data/assignment.h"
#include "mcrl2/data/bag.h"
#include "mcrl2/data/basic_sort.h"
#include "mcrl2/data/data_expression.h"
#include "mcrl2/data/exists.h"
#include "mcrl2/data/forall.h"
#include "mcrl2/data/function_sort.h"
#include "mcrl2/data/function_symbol.h"
#include "mcrl2/data/lambda.h"
#include "mcrl2/data/list.h"
#include "mcrl2/data/nat.h"
#include "mcrl2/data/set.h"
#include "mcrl2/data/variable.h"
#include "mcrl2/data/where_clause.h"

using namespace mcrl2;
using namespace mcrl2::data;

void variable_test()
{
  basic_sort s("S");
  variable x("x", s);
  BOOST_CHECK(pp(x.name()) == "x");
  BOOST_CHECK(x.sort() == s);

  core::identifier_string y_name("y");
  variable y(y_name, s);
  BOOST_CHECK(pp(y.name()) == "y");
  BOOST_CHECK(y.sort() == s);

  variable y_("y", s);
  BOOST_CHECK(pp(y_.name()) == "y");
  BOOST_CHECK(y.sort() == s);

  BOOST_CHECK(x != y);
  BOOST_CHECK(x != y_);
  BOOST_CHECK(y == y_);

  const data_expression& y_e(y);
  variable y_e_(y_e);
  BOOST_CHECK(y_e_ == y);
  BOOST_CHECK(y_e_.name() == y.name());
  BOOST_CHECK(y_e_.sort() == y.sort());
}

void function_symbol_test()
{
  basic_sort s0("S0");
  basic_sort s1("S1");
  basic_sort s("S");

  sort_expression_vector s01;
  s01.push_back(s0);
  s01.push_back(s1);
  function_sort fs(s01, s);

  data::function_symbol f("f", fs);
  BOOST_CHECK(pp(f.name()) == "f");
  BOOST_CHECK(f.sort() == fs);

  data::function_symbol g("g", s0);
  BOOST_CHECK(pp(g.name()) == "g");
  BOOST_CHECK(g.sort() == s0);

  core::identifier_string g_name("g");
  data::function_symbol g_(g_name, s0);
  BOOST_CHECK(pp(g_.name()) == "g");
  BOOST_CHECK(g_.sort() == s0);

  BOOST_CHECK(f != g);
  BOOST_CHECK(f != g_);
  BOOST_CHECK(g == g_);

  const data_expression& f_e(f);
  data::function_symbol f_e_(f_e);
  BOOST_CHECK(f_e == f);
  BOOST_CHECK(f_e_.name() == f.name());
  BOOST_CHECK(f_e_.sort() == f.sort());
}

void application_test()
{
  basic_sort s0("S0");
  basic_sort s1("S1");
  basic_sort s("S");
  sort_expression_vector s01;
  s01.push_back(sort_expression(s0));
  s01.push_back(sort_expression(s1));
  function_sort s01s(s01, s);

  data::function_symbol f("f", s01s);
  data_expression x(variable("x", s0));
  data_expression y(variable("y", s1));
  data_expression_list xy = { x, y };
  application fxy(f, xy);
  BOOST_CHECK(fxy.sort() == s);
  BOOST_CHECK(fxy.head() == f);
  BOOST_CHECK(data_expression_list(fxy.begin(), fxy.end()) == xy);
  BOOST_CHECK(*(fxy.begin()) == x);
  BOOST_CHECK(*(++fxy.begin()) == y);

  const data_expression& fxy_e(fxy);
  application fxy_e_(fxy_e);
  BOOST_CHECK(fxy == fxy_e_);
  BOOST_CHECK(fxy.sort() == fxy_e_.sort());
  BOOST_CHECK(fxy.head() == fxy_e_.head());
  BOOST_CHECK(fxy == f(x,y));
}

void abstraction_test()
{
  basic_sort s("S");

  variable x("x", s);
  variable_list xl = { x };
  abstraction I(lambda_binder(), xl, x);
  BOOST_CHECK(I.binding_operator() == lambda_binder());
  BOOST_CHECK(I.variables() == xl);
  BOOST_CHECK(I.body() == x);

  const data_expression& I_e(I);
  abstraction I_e_(I_e);
  BOOST_CHECK(I_e_ == I);
  BOOST_CHECK(I_e_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_e_.variables() == I.variables());
  BOOST_CHECK(I_e_.body() == I.body());
}

void lambda_test()
{
  basic_sort s("S");

  variable x("x", s);
  variable_list xl = { x };
  lambda I(xl, x);
  BOOST_CHECK(I.binding_operator() == lambda_binder());
  BOOST_CHECK(is_lambda(I));
  BOOST_CHECK(I.variables() == xl);
  BOOST_CHECK(I.body() == x);
  const sort_expression& s_(s);
  sort_expression_vector s_l {s_};
  BOOST_CHECK(!s_l.empty());
  function_sort fs(s_l, s);
  BOOST_CHECK(I.sort() == fs);

  const data_expression& I_e(I);
  lambda I_e_(I_e);
  BOOST_CHECK(I_e_ == I);
  BOOST_CHECK(I_e_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_e_.variables() == I.variables());
  BOOST_CHECK(I_e_.body() == I.body());
  abstraction I_(lambda_binder(), xl, x);
  BOOST_CHECK(I_ == I);
  BOOST_CHECK(I_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_.variables() == I.variables());
  BOOST_CHECK(I_.body() == I.body()) ;
}

void forall_test()
{
  basic_sort s("S");

  variable x("x", s);
  variable_list xl = { x };
  forall I(xl, x);
  BOOST_CHECK(I.binding_operator() == forall_binder());
  BOOST_CHECK(is_forall(I));
  BOOST_CHECK(I.variables() == xl);
  BOOST_CHECK(I.body() == x);
  const sort_expression& s_(s);
  sort_expression_vector s_l {s_};

  // TODO Check sort

  const data_expression& I_e(I);
  forall I_e_(I_e);
  BOOST_CHECK(I_e_ == I);
  BOOST_CHECK(I_e_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_e_.variables() == I.variables());
  BOOST_CHECK(I_e_.body() == I.body());
  abstraction I_(forall_binder(), xl, x);
  BOOST_CHECK(I_ == I);
  BOOST_CHECK(I_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_.variables() == I.variables());
  BOOST_CHECK(I_.body() == I.body()) ;
}

void exists_test()
{
  basic_sort s("S");

  variable x("x", s);
  variable_list xl = { x };
  exists I(xl, x);
  BOOST_CHECK(I.binding_operator() == exists_binder());
  BOOST_CHECK(is_exists(I));
  BOOST_CHECK(I.variables() == xl);
  BOOST_CHECK(I.body() == x);
  const sort_expression& s_(s);
  sort_expression_vector s_l {s_};

  // TODO Check sort

  const data_expression& I_e(I);
  exists I_e_(I_e);
  BOOST_CHECK(I_e_ == I);
  BOOST_CHECK(I_e_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_e_.variables() == I.variables());
  BOOST_CHECK(I_e_.body() == I.body());
  abstraction I_(exists_binder(), xl, x);
  BOOST_CHECK(I_ == I);
  BOOST_CHECK(I_.binding_operator() == I.binding_operator());
  BOOST_CHECK(I_.variables() == I.variables());
  BOOST_CHECK(I_.body() == I.body()) ;
}

void set_comprehension_test()
{
  basic_sort s("S");
  variable x("x", make_function_sort(s,sort_bool::bool_()));
  data::function_symbol f("f", make_function_sort(s, sort_bool::bool_()));
  data_expression e(sort_set::set_comprehension(s, x));
  BOOST_CHECK(e.sort() == sort_set::set_(s));
}

void bag_comprehension_test()
{
  basic_sort s("S");
  data::function_symbol f("f", make_function_sort(s, sort_nat::nat()));
  data_expression e(sort_bag::bag_comprehension(s, f));
  BOOST_CHECK(e.sort() == sort_bag::bag(s));
}

void where_declaration_test()
{
  basic_sort s("S");
  variable x("x", s);
  variable y("y", s);

  assignment xy(x,y);
  assignment_expression_list xyl = { xy };
  where_clause wxy(x, xyl);
  BOOST_CHECK(wxy.body() == x);
  BOOST_CHECK(wxy.declarations() == xyl);

  const data_expression& wxy_e(wxy);
  where_clause wxy_e_(wxy_e);
  BOOST_CHECK(wxy_e_ == wxy);
  BOOST_CHECK(wxy_e_.body() == wxy.body());
  BOOST_CHECK(wxy_e_.declarations() == wxy.declarations());
  BOOST_CHECK(wxy.sort() == x.sort());
}

void assignment_test()
{
  basic_sort s("S");
  variable x("x", s);
  variable y("y", s);

  assignment xy(x, y);
  BOOST_CHECK(xy.lhs() == x);
  BOOST_CHECK(xy.rhs() == y);
}

void system_defined_check()
{
  BOOST_CHECK(sort_list::empty(sort_pos::pos()) != sort_list::empty(sort_nat::nat()));
}

int test_main(int argc, char** argv)
{
  variable_test();

  function_symbol_test();

  application_test();

  abstraction_test();

  lambda_test();

  forall_test();

  exists_test();

  set_comprehension_test();

  bag_comprehension_test();

  where_declaration_test();

  assignment_test();

  return EXIT_SUCCESS;
}


#include <expression/dualtime/FixedPointBDFDualTimeIntegrator.h>
#include <expression/dualtime/VariableImplicitBDFDualTimeIntegrator.h>
#include <expression/dualtime/BlockImplicitBDFDualTimeIntegrator.h>

#include <spatialops/structured/Grid.h>
#include <spatialops/structured/FVStaggered.h>
#include <spatialops/structured/FieldComparisons.h>

#include <test/TestHelper.h>
#include "ExpDecay.h"
#include <expression/Functions.h>
#include <expression/ExpressionTree.h>
#include <expression/ExprPatch.h>

#include <expression/matrix-assembly/SparseMatrix.h>
#include <expression/matrix-assembly/ScaledIdentityMatrix.h>

#include <boost/program_options.hpp>

#include <iostream>
#include <fstream>

using std::cout;
using std::endl;

namespace po = boost::program_options;
namespace so = SpatialOps;

typedef so::SVolField FieldT;

enum IntegratorType{
  FIXED_POINT,
  VARIABLE_IMPLICIT,
  BLOCK_IMPLICIT
};

#define ASSEMBLER(MatrixT, FieldT, name) \
  boost::shared_ptr<MatrixT<FieldT> > name = boost::make_shared<MatrixT<FieldT> >();

bool test( const IntegratorType integratorType )
{
  /* ---------------------------------------------------------------
   * domain, operators, field managers, etc setup
   * ---------------------------------------------------------------
   */
  Expr::ExprPatch patch(1);
  SpatialOps::OperatorDatabase sodb;
  Expr::FieldManagerList& fml = patch.field_manager_list();

  /* ---------------------------------------------------------------
   * variable and rhs names, jacobian, preconditioner
   * ---------------------------------------------------------------
   */
  std::string phiName = "phi";
  const Expr::Tag phiTag   ( phiName,          Expr::STATE_NONE );
  const Expr::Tag phiNTag  ( phiName,          Expr::STATE_N    );
  const Expr::Tag phiRHSTag( phiName + "_rhs", Expr::STATE_NONE );

  /* ---------------------------------------------------------------
   * build integrator, set constant physical and dual time step
   * ---------------------------------------------------------------
   */
  const unsigned bdfOrder = 1;
  boost::shared_ptr<Expr::DualTime::BDFDualTimeIntegrator> integrator;

  const Expr::Tag dtTag( "timestep"     , Expr::STATE_NONE );
  const Expr::Tag dsTag( "dual_timestep", Expr::STATE_NONE );

  double ds; // dual time step size

  switch( integratorType ){
    case FIXED_POINT:{
      ds = 0.01;
      integrator = boost::make_shared<Expr::DualTime::FixedPointBDFDualTimeIntegrator<FieldT> >( 0,
                                                                                "Fixed Point BDF",
                                                                                dtTag,
                                                                                dsTag,
                                                                                bdfOrder );
      break;
    }
    case VARIABLE_IMPLICIT:{
      ds = 1e16;
      integrator = boost::make_shared<Expr::DualTime::VariableImplicitBDFDualTimeIntegrator<FieldT> >( 0,
                                                                                "Variable Implicit BDF",
                                                                                dtTag,
                                                                                dsTag,
                                                                                bdfOrder );
      break;
    }
    case BLOCK_IMPLICIT:{
      ds = 1e16;
      integrator = boost::make_shared<Expr::DualTime::BlockImplicitBDFDualTimeIntegrator<FieldT> >( 0,
                                                                                "Block Implicit BDF",
                                                                                dtTag,
                                                                                dsTag,
                                                                                bdfOrder );
      break;
    }
  }

  const double dt = 0.01;
  typedef Expr::ConstantExpr<so::SingleValueField>::Builder ConstantSingleValueT;
  integrator->set_physical_time_step_expression( new ConstantSingleValueT( dtTag, dt ) );
  integrator->set_dual_time_step_expression    ( new ConstantSingleValueT( dsTag, ds ) );

  /* ---------------------------------------------------------------
   * register variables and RHS at STATE_NONE to the integrator
   * ---------------------------------------------------------------
   */
  Expr::ExpressionFactory& factory = integrator->factory();

  const double rateConst = 1.3;
  factory.register_expression( new ExpDecayRHS<FieldT>::Builder( rateConst, phiRHSTag, phiTag ) );
  integrator->add_variable<FieldT>( phiTag.name(), phiRHSTag );

  if( integratorType == BLOCK_IMPLICIT ){
    // because we have a generic integrator type, we need to cast to block implicit type to continue
    typedef Expr::DualTime::BlockImplicitBDFDualTimeIntegrator<FieldT> BIType;
    auto biPtr = boost::dynamic_pointer_cast<BIType>( integrator );

    // need to call this before indices can be obtained
    biPtr->done_adding_variables();

    // these are maps from tags to indices
    std::map<Expr::Tag,int> varIndices = biPtr->variable_tag_index_map();
    std::map<Expr::Tag,int> rhsIndices = biPtr->rhs_tag_index_map();

    const int phiVarIdx = varIndices[phiTag];
    const int phiRhsIdx = rhsIndices[phiRHSTag];

    // build an assembler to compute the Jacobian matrix of the rhs to the governed variable
    ASSEMBLER( Expr::matrix::DenseSubMatrix, FieldT, jacobian )
    jacobian->element<FieldT>(phiRhsIdx,phiVarIdx) = Expr::matrix::sensitivity( phiRHSTag, phiTag );
    jacobian->finalize(); // need to finalize before using

    // build an identity matrix preconditioner
    ASSEMBLER( Expr::matrix::ScaledIdentityMatrix, FieldT, preconditioner )
    preconditioner->finalize(); // need to finalize before using

    // set the Jacobian and preconditioner assemblers
    biPtr->set_jacobian_and_preconditioner( jacobian, preconditioner );
  }

  /* ---------------------------------------------------------------
   * final integrator prep, must happen before setting initial conditions, and tree output
   * ---------------------------------------------------------------
   */
  integrator->prepare_for_integration( fml, sodb, patch.field_info() );

  integrator->lock_field<FieldT>( phiNTag );
  integrator->lock_all_execution_fields();
  integrator->copy_from_initial_condition_to_execution<FieldT>( phiNTag.name() );

  {
    std::ofstream f("dualtimeExpDecayTest-integrator-tree.dot");
    integrator->get_tree().write_tree( f );
  }

  /* ---------------------------------------------------------------
   * set initial conditions with STATE_N
   * ---------------------------------------------------------------
   */
  Expr::ExpressionFactory icFactory;

  const double phi0 = 1.0;
  const Expr::ExpressionID id = icFactory.register_expression( new Expr::ConstantExpr<FieldT>::Builder( phiNTag, phi0 ) );
  Expr::ExpressionTree icTree( id, icFactory, patch.id() );

  icTree.register_fields( fml );
  icTree.bind_fields( fml );
  icTree.execute_tree();

  /* ---------------------------------------------------------------
   * set iteration counts, prep solution history file
   * ---------------------------------------------------------------
   */

  const unsigned maxIterPerStep = 100;

  unsigned totalIter = 0;
  unsigned stepCount = 0;
  unsigned dualIter  = 0;
  bool     converged = false;

  std::ofstream fout("dualtimeExpDecayTest-solution-history.txt");
  fout << "      t \t              c-exact \t              c\n";

  /* ---------------------------------------------------------------
   * perform the integration
   * ---------------------------------------------------------------
   */
  const double tend = 2.0;

  double t = 0;
  while( t<tend ){

    dualIter = 0;
    integrator->begin_time_step();

    do{
      integrator->advance_dualtime( converged );
      dualIter++;
      totalIter++;
    } while( !converged && dualIter <= maxIterPerStep );

    integrator->end_time_step();
    stepCount++;
    t += dt;

    FieldT& phiN = fml.field_ref<FieldT>(phiNTag);
    phiN.add_device(CPU_INDEX);
    fout << std::scientific << std::setprecision(10)
         << t                      << "\t"
         << phi0*exp(-rateConst*t) << "\t"
         << phiN[0]                << std::endl;
  }
  fout.close();

  /* ---------------------------------------------------------------
   * process the test
   * ---------------------------------------------------------------
   */
  TestHelper status(false);

  if( integratorType != FIXED_POINT ){
    status( integrator->get_tree().computes_field( sens_tag( phiRHSTag, phiTag ) ),
            "Computes d phi_rhs / d phi" );
    status( integrator->get_tree().computes_field( sens_tag(phiTag,phiTag) ),
            "Computes d phi / d phi" );
  }

  const FieldT& phiPredicted      = fml.field_ref<FieldT>( phiNTag );
  const double  phiFromMatlabCode = 0.075528511873306;
  SpatialOps::SpatFldPtr<FieldT> tmp = SpatialOps::SpatialFieldStore::get<FieldT>( phiPredicted );
  *tmp <<= phiFromMatlabCode;
  status( SpatialOps::field_equal_abs( phiPredicted, *tmp, 1e-6 ), "phi value-check" );

  const double meanStepPerIter = ( (double) totalIter ) / ( (double) stepCount );
  switch( integratorType ){
    case FIXED_POINT:{
      const double meanStepPerIterFromMatlabCode = 27;
      status( std::abs( meanStepPerIter - meanStepPerIterFromMatlabCode ) < 1e-8, "fixed-point convergence rate check" );
      break;
    }
    case VARIABLE_IMPLICIT:{
      const double meanStepPerIterFromMatlabCode = 2;
      status( std::abs( meanStepPerIter - meanStepPerIterFromMatlabCode ) < 1e-8, "variable-implicit convergence rate check" );
      break;
    }
    case BLOCK_IMPLICIT:{
      const double meanStepPerIterFromMatlabCode = 2;
      status( std::abs( meanStepPerIter - meanStepPerIterFromMatlabCode ) < 1e-8, "block-implicit convergence rate check" );
      break;
    }
  }
  return status.ok();
}

int main( int iarg, char* carg[] )
{
  /* ---------------------------------------------------------------
   * initialization from command line options
   * ---------------------------------------------------------------
   */
  bool fixedPoint       = false;
  bool variableImplicit = false;
  bool blockImplicit    = false;

  {
    po::options_description desc("Supported Options");
    desc.add_options()
        ( "help", "print help message" )
        ( "fixed-point", "Use the fixed point dualtime integration scheme")
        ( "variable-implicit", "Use the variable implicit dualtime integration scheme")
        ( "block-implicit", "Use the block implicit dualtime integration scheme");;
    po::variables_map args;
    try{
      po::store( po::parse_command_line(iarg,carg,desc), args );
      po::notify(args);
    }
    catch( std::exception& err ){
      cout << "\nError parsing command line options.\n\n"
          << err.what() << "\n\n" << desc << "\n";
      return -1;
    }
    if( args.count("help") ){
      cout << desc << "\n";
      return 1;
    }
    fixedPoint       = args.count("fixed-point");
    variableImplicit = args.count("variable-implicit");
    blockImplicit    = args.count("block-implicit");

    if( !fixedPoint && !variableImplicit && !blockImplicit ){
      std::cout << "Error: no dualtime integration method selected.\n"
                << desc << std::endl;
        return -1;
    }
  }

  try{
    TestHelper status( true );
    if( fixedPoint       ) status( test( FIXED_POINT       ), "Fixed Point"       );
    if( variableImplicit ) status( test( VARIABLE_IMPLICIT ), "Variable Implicit" );
    if( blockImplicit    ) status( test( BLOCK_IMPLICIT    ), "Block Implicit" );

    if( status.ok() ){
      std::cout << "PASS\n";
      return 0;
    }

  }
  catch( std::exception& err ){
    std::cout << err.what() << std::endl;
  }
  std::cout << "FAIL\n";
  return -1;
}


//
// GibbsSamplerFromHDP.hpp
//
// Copyright (c) 2018 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#ifndef HDP
#define HDP

#include<stdlib.h>
#include<math.h>
#include<cmath>
#include<iostream>
#include<vector>
#include<numeric>
#include<memory>
#include<random>
#include<iomanip>
#include<fstream>
#include<limits>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include"BOWFileParser.hpp"
#include"utils.hpp"

class GibbsSamplerFromHDP{
protected:
    const std::vector<std::vector<unsigned int> > &_frequencyMatrix;
    const std::vector<std::vector<unsigned int> > &_docVoca;
    const unsigned int _J, _V, _iterationNumber, _burnIn, _samplingInterval;
    const double _alpha, _beta, _gamma;
    unsigned int _K, _samplingCount;
    std::vector<unsigned int> _Tj;
    std::vector<std::vector<unsigned int> >  _xji, _tji, _kjt;
    std::vector<std::vector<double> > _thetaEx, _phiEx;
    std::vector<unsigned int> _nj, _nk, _mk;
    std::vector<double> _sumOfNkSampled;
    std::vector<std::vector<unsigned int> > _njt, _nkv;
    std::vector<std::vector<double> > _sumOfNjkSampled, _sumOfNkvSampled;
    std::vector<std::vector<std::vector<unsigned int> > > _njtv;
    std::vector<double> _logLikelihood;
public:
    GibbsSamplerFromHDP(const BOWFileParser &parser, double alpha, double beta, double gamma, unsigned int iterationNumber, unsigned int burnIn, unsigned int samplingInterval);
    ~GibbsSamplerFromHDP();
    virtual void initializeParameters();
    virtual void sampleTji();
    virtual void sampleKjt(unsigned int j, unsigned int t, bool isNewT=false);
    virtual void countN();
    virtual void addT(unsigned int j, unsigned int i);
    virtual void addK();
    virtual void deleteT(unsigned int j, unsigned int t);
    virtual void deleteK(unsigned int k);
    virtual void calculateExpectations(bool isUseOnlyLastSample=false);
    virtual void calculateLogLikelihood();
    virtual void checkNumbers();
    virtual void writeParameters(std::string thetaFilename, std::string phiFilename)const;
    virtual void writeLogLikelihood(std::string logLikelihoodFilename)const;
    virtual void storeSamples();
    virtual void runIteraions();
};

#endif


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_data_structure/sources/ai/priority_queue_tmpl_impl.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(priority_queue)

BOOST_AUTO_TEST_CASE(priority_queue_basic)
{
    typedef cor::data_structure::PriorityQueueTmpl<cor::RFloat, cor::RInt32> Q;

    Q q;

    q.set_max_size(4);

    q.enqueue(0.1, 1);
    q.enqueue(0.3, 3);
    q.enqueue(0.5, 5);
    q.enqueue(0.4, 4);
    q.enqueue(0.2, 2);

    BOOST_CHECK_EQUAL(q.size(), 4);

    cor::RInt32 i;
    i = 1;
    while(!q.empty())
    {
        auto p = q.dequeue();

        BOOST_CHECK_EQUAL(p.second, i);

        i++;
    }

    BOOST_CHECK_EQUAL(q.size(), 0);

}

BOOST_AUTO_TEST_SUITE_END()


/*************************************************************************
    > File Name: src/jointPointCloud.cpp
    > Author: Xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > Created Time: 2015年07月22日 星期三 20时46分08秒
 ************************************************************************/

#include<iostream>
using namespace std;

#include "slamBase.h"

#include <opencv2/core/eigen.hpp>

#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

// Eigen !
#include <Eigen/Core>
#include <Eigen/Geometry>

int main( int argc, char** argv )
{
    //本节要拼合data中的两对图像
    ParameterReader pd;
    // 声明两个帧，FRAME结构请见include/slamBase.h
    FRAME frame1, frame2;

    //读取图像
    frame1.rgb = cv::imread( "./data/rgb1.png" );
    frame1.depth = cv::imread( "./data/depth1.png", -1);
    frame2.rgb = cv::imread( "./data/rgb2.png" );
    frame2.depth = cv::imread( "./data/depth2.png", -1 );

    // 提取特征并计算描述子
    cout<<"extracting features"<<endl;
    string detecter = pd.getData( "detector" );
    string descriptor = pd.getData( "descriptor" );

    computeKeyPointsAndDesp( frame1, detecter, descriptor );
    computeKeyPointsAndDesp( frame2, detecter, descriptor );

    // 相机内参
    CAMERA_INTRINSIC_PARAMETERS camera;
    camera.fx = atof( pd.getData( "camera.fx" ).c_str());
    camera.fy = atof( pd.getData( "camera.fy" ).c_str());
    camera.cx = atof( pd.getData( "camera.cx" ).c_str());
    camera.cy = atof( pd.getData( "camera.cy" ).c_str());
    camera.scale = atof( pd.getData( "camera.scale" ).c_str() );

    cout<<"solving pnp"<<endl;
    // 求解pnp
    RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );

    cout<<result.rvec<<endl<<result.tvec<<endl;

    // 处理result
    // 将旋转向量转化为旋转矩阵
    cv::Mat R;
    cv::Rodrigues( result.rvec, R );
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);

    // 将平移向量和旋转矩阵转换成变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    Eigen::AngleAxisd angle(r);
    cout<<"translation"<<endl;
    Eigen::Translation<double,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2));
    T = angle;
    T(0,3) = result.tvec.at<double>(0,0);
    T(1,3) = result.tvec.at<double>(0,1);
    T(2,3) = result.tvec.at<double>(0,2);

    // 转换点云
    cout<<"converting image to clouds"<<endl;
    PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
    PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );

    // 合并点云
    cout<<"combining clouds"<<endl;
    PointCloud::Ptr output (new PointCloud());
    pcl::transformPointCloud( *cloud1, *output, T.matrix() );
    *output += *cloud2;
    pcl::io::savePCDFile("data/result.pcd", *output);
    cout<<"Final result saved."<<endl;

    pcl::visualization::CloudViewer viewer( "viewer" );
    viewer.showCloud( output );
    while( !viewer.wasStopped() )
    {

    }
    return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#if defined(HAS_GMP)

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "test.hpp"
#include <boost/multiprecision/gmp.hpp>

#if defined(HAS_MPFR)
#include <boost/multiprecision/mpfr.hpp>
#endif
#if defined(HAS_MPFI)
#include <boost/multiprecision/mpfi.hpp>
#endif
#ifdef HAS_TOMMATH
#include <boost/multiprecision/tommath.hpp>
#endif
#ifdef HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace boost::multiprecision;

#ifdef BOOST_MSVC
#pragma warning(disable : 4127)
#endif

template <class T>
T generate_random_int(unsigned bits_wanted)
{
   static boost::random::mt19937               gen;
   typedef boost::random::mt19937::result_type random_type;

   T        max_val;
   unsigned digits;
   if (std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits  = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits  = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while ((random_type(1) << bits_per_r_val) > (gen.max)())
      --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for (unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class T>
T generate_random(unsigned bits_wanted)
{
   typedef typename component_type<T>::type int_type;
   T                                        val(generate_random_int<int_type>(bits_wanted), generate_random_int<int_type>(bits_wanted));
   return val;
}

template <class From, class To>
void test_convert_neg_val(From from, const boost::mpl::true_&)
{
   from                                       = -from;
   typename component_type<From>::type answer = numerator(from) / denominator(from);
   To                                  t3(from);
   To                                  t4 = from.template convert_to<To>();
   BOOST_CHECK_EQUAL(answer.str(), t3.str());
   BOOST_CHECK_EQUAL(answer.str(), t4.str());
}
template <class From, class To>
void test_convert_neg_val(From const&, const boost::mpl::false_&)
{
}

template <class From, class To>
void test_convert_imp(boost::mpl::int_<number_kind_rational> const&, boost::mpl::int_<number_kind_integer> const&)
{
   int bits_wanted = (std::min)((std::min)(std::numeric_limits<From>::digits, std::numeric_limits<To>::digits), 2000);

   for (unsigned i = 0; i < 100; ++i)
   {
      From                                from   = generate_random<From>(bits_wanted);
      typename component_type<From>::type answer = numerator(from) / denominator(from);
      To                                  t1(from);
      To                                  t2 = from.template convert_to<To>();
      BOOST_CHECK_EQUAL(answer.str(), t1.str());
      BOOST_CHECK_EQUAL(answer.str(), t2.str());
      test_convert_neg_val<From, To>(from, boost::mpl::bool_ < std::numeric_limits<From>::is_signed && std::numeric_limits<To>::is_signed > ());
   }
}

template <class From, class To>
void test_convert_neg_float_val(From from, To const& tol, const boost::mpl::true_&)
{
   from      = -from;
   To answer = To(numerator(from)) / To(denominator(from));
   To t3(from);
   To t4 = from.template convert_to<To>();
   BOOST_CHECK_CLOSE_FRACTION(answer, t3, tol);
   BOOST_CHECK_CLOSE_FRACTION(answer, t4, tol);
}
template <class From, class To>
void test_convert_neg_float_val(From const&, To const&, const boost::mpl::false_&)
{
}

template <class From, class To>
void test_convert_imp(boost::mpl::int_<number_kind_rational> const&, boost::mpl::int_<number_kind_floating_point> const&)
{
   int bits_wanted = (std::min)((std::min)(std::numeric_limits<From>::digits, std::numeric_limits<To>::digits), 2000);

   for (unsigned i = 0; i < 100; ++i)
   {
      From from   = generate_random<From>(bits_wanted);
      To   answer = To(numerator(from)) / To(denominator(from));
      To   t1(from);
      To   t2  = from.template convert_to<To>();
      To   tol = std::numeric_limits<To>::is_specialized ? std::numeric_limits<To>::epsilon() : ldexp(To(1), 1 - bits_wanted);
      tol *= 2;
      BOOST_CHECK_CLOSE_FRACTION(answer, t1, tol);
      BOOST_CHECK_CLOSE_FRACTION(answer, t2, tol);
      test_convert_neg_float_val<From, To>(from, tol, boost::mpl::bool_ < std::numeric_limits<From>::is_signed && std::numeric_limits<To>::is_signed > ());
   }
}

template <class From, class To>
void test_convert_neg_rat_val(From from, const boost::mpl::true_&)
{
   from = -from;
   To t3(from);
   To t4 = from.template convert_to<To>();
   BOOST_CHECK_EQUAL(from.str(), t3.str());
   BOOST_CHECK_EQUAL(from.str(), t4.str());
}
template <class From, class To>
void test_convert_neg_rat_val(From const&, const boost::mpl::false_&)
{
}

template <class From, class To>
void test_convert_imp(boost::mpl::int_<number_kind_rational> const&, boost::mpl::int_<number_kind_rational> const&)
{
   int bits_wanted = (std::min)((std::min)(std::numeric_limits<From>::digits, std::numeric_limits<To>::digits), 2000);

   for (unsigned i = 0; i < 100; ++i)
   {
      From from = generate_random<From>(bits_wanted);
      To   t1(from);
      To   t2 = from.template convert_to<To>();
      BOOST_CHECK_EQUAL(from.str(), t1.str());
      BOOST_CHECK_EQUAL(from.str(), t2.str());
      test_convert_neg_rat_val<From, To>(from, boost::mpl::bool_ < std::numeric_limits<From>::is_signed && std::numeric_limits<To>::is_signed > ());
   }
}

template <class From, class To>
void test_convert()
{
   test_convert_imp<From, To>(typename number_category<From>::type(), typename number_category<To>::type());
}

int main()
{
   test_convert<mpq_rational, cpp_int>();
   test_convert<mpq_rational, int128_t>();
   test_convert<mpq_rational, uint128_t>();
   test_convert<mpq_rational, cpp_rational>();

   test_convert<mpq_rational, cpp_bin_float_50>();

   test_convert<mpq_rational, cpp_dec_float_50>();

   test_convert<mpq_rational, mpz_int>();
   test_convert<mpq_rational, mpf_float_50>();
#if defined(HAS_MPFR)
   test_convert<mpq_rational, mpfr_float_50>();
#endif
#if defined(HAS_MPFI)
   test_convert<mpq_rational, mpfi_float_50>();
#endif
#ifdef HAS_TOMMATH
   test_convert<mpq_rational, tom_int>();
   test_convert<mpq_rational, tom_rational>();
#endif
   return boost::report_errors();
}

#else

int main() { return 0; }

#endif


//          Copyright Maksym V. Bilinets 2015 - 2020.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt )

#include <dst/binary_tree/mixin/ordering.h>

#include <dst/allocator/counter_allocator.h>
#include <dst/allocator/wary_allocator.h>
#include <dst/binary_tree/balanced_tree.h>

#include <boost/test/unit_test.hpp>

namespace dst_test
{

#if defined(_LIBCPP_VERSION) && _LIBCPP_STD_VER > 17 // We use libc++ && C++20
template <typename T>
using test_allocator = dst::counter_allocator<T, dst::wary_allocator<T>>;
#else
template <typename T> using test_allocator = dst::counter_allocator<T>;
#endif

using ordering_tree =
  dst::binary_tree::balanced_tree<int,
                                  test_allocator<int>,
                                  dst::binary_tree::AVL,
                                  dst::binary_tree::Ordering>;

BOOST_AUTO_TEST_SUITE(test_binary_tree_ordering)

BOOST_AUTO_TEST_CASE(test_allocator_used_by_internal_vectors)
{
  // $      |      $
  // $      3      $
  // $    /   \    $
  // $   2     4   $
  // $  /       \  $
  // $ 1         5 $
  ordering_tree tree;

  const auto it_3 = tree.insert_left(tree.nil(), 3);
  const auto it_2 = tree.insert_left(it_3, 2);
  const auto it_4 = tree.insert_right(it_3, 4);
  const auto it_1 = tree.insert_left(it_2, 1);
  const auto it_5 = tree.insert_right(it_4, 5);

  const auto allocated_memory_1 = tree.get_allocator().allocated();

  BOOST_TEST(allocated_memory_1 > 0u);

  BOOST_TEST(tree.order(it_2, it_4));

  const auto allocated_memory_2 = tree.get_allocator().allocated();

  BOOST_TEST(allocated_memory_2 > allocated_memory_1);

  BOOST_TEST(!tree.order(it_5, it_1));

  const auto allocated_memory_3 = tree.get_allocator().allocated();

  BOOST_TEST(allocated_memory_3 > allocated_memory_2);

  BOOST_TEST(tree.order(it_2, it_5));

  BOOST_TEST(tree.get_allocator().allocated() == allocated_memory_3);
}

BOOST_AUTO_TEST_SUITE_END()
}


#include <iostream>
#include <string>
#include <boost/format.hpp>
#include "slide_algorithm/algorithm.hpp"
#include "data_type.hpp"

int main(int argc, char* argv[])
{
	if (argc == 1) {
		std::cout << "使用方法: " << argv[0] << " <ヨコ> <タテ> <選択可能回数> <選択コスト> <交換コスト>" << std::endl;
		std::cout << "標準入力から行列を与えてください" << std::endl;
		return -1;
	}
	int const width = std::stoi(argv[1]);
	int const height = std::stoi(argv[2]);
	int const selectable = std::stoi(argv[3]);
	int const cost_select = std::stoi(argv[4]);
	int const cost_change = std::stoi(argv[5]);

	std::vector<std::vector<point_type>> matrix;
	int point_int;
	std::string point_str;
	for (int y = 0; y < height; ++y) {
		std::vector<point_type> row;
		for (int x = 0; x < width; ++x) {
			std::cin >> point_str;
			point_int = std::stoi(point_str, nullptr, 16);
			row.push_back({point_int / 16, point_int % 16});
		}
		matrix.push_back(std::move(row));
	}

	auto question = question_data(0, "", {width, height}, selectable, cost_select, cost_change, matrix);
	algorithm algo;
	algo.reset(question);
	algo.get();

	return 0;
}

// vim: set ts=4 sw=4:


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/generate_integer_sequence.hpp>
#include <boost/hana/detail/std/integer_sequence.hpp>

#include <cstddef>
#include <type_traits>
using namespace boost::hana;

// `generate_integer_sequence` without function is equivalent to
// `make_integer_sequence`
template <std::size_t i>
struct without_function {
    static_assert(std::is_same<
        detail::generate_integer_sequence<int, i>,
        detail::std::make_integer_sequence<int, i>
    >::value, "");
};
template struct without_function<0>;
template struct without_function<1>;
template struct without_function<2>;
template struct without_function<3>;
template struct without_function<4>;
template struct without_function<5>;

// `generate_integer_sequence` uses the provided function to create the
// `integer_sequence`.
struct ones {
    template <typename Array>
    constexpr auto operator()(Array result) const {
        for (auto i = 0; i != result.size(); ++i)
            result[i] = 1;
        return result;
    }
};
static_assert(std::is_same<
    detail::generate_integer_sequence<int, 0, ones>,
    detail::std::integer_sequence<int>
>::value, "");

static_assert(std::is_same<
    detail::generate_integer_sequence<int, 1, ones>,
    detail::std::integer_sequence<int, 1>
>::value, "");

static_assert(std::is_same<
    detail::generate_integer_sequence<int, 2, ones>,
    detail::std::integer_sequence<int, 1, 1>
>::value, "");

static_assert(std::is_same<
    detail::generate_integer_sequence<int, 3, ones>,
    detail::std::integer_sequence<int, 1, 1, 1>
>::value, "");

static_assert(std::is_same<
    detail::generate_integer_sequence<int, 4, ones>,
    detail::std::integer_sequence<int, 1, 1, 1, 1>
>::value, "");

int main() {

}


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/pack.hpp>
#include <algorithm>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns he number of elements in the range [first,last)
    for which pred returns true.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param pred  predicate to apply to each element.

    @par Requirement

      - @c first, @c last and @c out must be pointers to Vectorizable type.

    @par Example:

      @snippet count_if.cpp count_if

    @par Possible output:

      @snippet count_if.txt count_if

  **/

  template<typename T, typename Pred>
  typename std::iterator_traits<const T*>::difference_type
  count_if(T const* first, T const* last, Pred pred)
  {
    auto pr = segmented_aligned_range(first,last);

    auto c  = std::count_if(pr.head.begin(), pr.head.end(), pred);
    for(pack<T> x : pr.body) c+= nbtrue(pred(x));
         c += std::count_if(pr.tail.begin(), pr.tail.end(), pred);

    return c;
  }
} }

#endif


/***********************************************************************************
 * Copyright (c) 2017, UT-Battelle
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * 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 the xacc nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * 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 <COPYRIGHT HOLDER> 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.
 *
 * Contributors:
 *   Initial API and implementation - Alex McCaskey
 *
 **********************************************************************************/
#ifndef TASK_STATEPREPARATIONEVALUATOR_HPP_
#define TASK_STATEPREPARATIONEVALUATOR_HPP_

#include <Eigen/Dense>
#include <boost/math/constants/constants.hpp>
#include "exprtk.hpp"
#include <boost/algorithm/string.hpp>
#include "IRProvider.hpp"
#include "XACC.hpp"

namespace xacc {
namespace vqe {

class StatePreparationEvaluator {

protected:

	static constexpr double pi = boost::math::constants::pi<double>();

	using symbol_table_t = exprtk::symbol_table<double>;
	using expression_t = exprtk::expression<double>;
	using parser_t = exprtk::parser<double>;

public:

	static std::shared_ptr<Function> evaluateCircuit(
			const std::shared_ptr<Function> statePrep, const int nParameters,
			const Eigen::VectorXd& x) {
		auto pi = boost::math::constants::pi<double>();
		std::vector<std::string> variableNames;
		for (int i = 0; i < nParameters; i++) {
			variableNames.push_back(
					boost::get<std::string>(statePrep->getParameter(i)));
		}

		auto gateRegistry = xacc::getService<IRProvider>("gate");
		auto evaluatedStatePrep = gateRegistry->createFunction("evaled_"+statePrep->name(), {}, {});

		for (auto inst : statePrep->getInstructions()) {
			if (inst->isParameterized()
					&& inst->getParameter(0).which() == 3) {
				int idx = -1;
				auto expression = boost::get<std::string>(
						inst->getParameter(0));
				for (int i = 0; i < nParameters; i++) {

					if (boost::contains(expression, variableNames[i])) {
						idx = i;
					}
				}

				std::string varName = variableNames[idx];
				double val;
				symbol_table_t symbol_table;
				symbol_table.add_variable(varName, val);
				symbol_table.add_constants();
				expression_t expr;
				expr.register_symbol_table(symbol_table);
				parser_t parser;
				parser.compile(expression, expr);
				val = x(idx);
				auto res = expr.value();
//				if (res < 0.0) {
//					res = 4 * pi + res;
//				}
				InstructionParameter p(res);
				auto updatedInst = gateRegistry->createInstruction(
						inst->name(), inst->bits());
				updatedInst->setParameter(0, p);
				evaluatedStatePrep->addInstruction(updatedInst);
			} else {
				evaluatedStatePrep->addInstruction(inst);
			}
		}

		return evaluatedStatePrep;
	}
};
}
}

#endif /* TASK_STATEPREPARATIONEVALUATOR_HPP_ */


#include <boost/range/algorithm.hpp>
#include <boost/range/irange.hpp>
#include <iostream>

int main()
{
  boost::integer_range<int> ir = boost::irange(0, 3);
  boost::copy(ir, std::ostream_iterator<int>{std::cout, ","});
}