/***************************************************************************
                          graphalgs.cc  -  description
                             -------------------
    begin                : Tue Feb 28 2006
    copyright            : (C) 2005 by Knut-Helge Vik
    email                : knuthelv@ifi.uio.no
 ***************************************************************************/
#include "graphalgs.h"
#include "../boostprop.h"
//#include "../simtime.h"
//#include "../treealgs/treealgs.h"
//#include "../treealgs/dijkstra_sp.h"
//#include "../boostprop.h"
//#include "../network/node_base.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>
#include <functional>
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;
using namespace boost;
using namespace TreeAlgorithms;

namespace GraphAlgorithms
{

/*-----------------------------------------------------------------------
						generate
------------------------------------------------------------------------- */
int generateRandomNumber(double max)
{
	return (int) (max * rand() / (RAND_MAX + 1.0));
}

int generateSizeOfSteinerSet(int members)
{
	int numToAdd = 0;

	// if fixed then return number
	if(GlobalSimArgs::getSteinerMemberSize() > 0) return GlobalSimArgs::getSteinerMemberSize();

	// else calculate with regards to degree limit and number of members
	if(ADD_CORE_LINKS_OPTIMIZED == GlobalSimArgs::getSimPruneAlgo())
	{
		if(isSteinerAlgo(GlobalSimArgs::getSimTreeAlgo()) || (GlobalSimArgs::getSimGraphAlgo() != COMPLETE_MEMBER_GRAPH))
		{
			numToAdd = (int) (ceil( (double) members/ (double) GlobalSimArgs::getDegreeConstraintSP())); // - 2) * 2) + 2;
			//numToAdd = numToAdd + (int) (ceil( (double) ((numToAdd * 2) + 2)/ (double) GlobalSimArgs::getDegreeConstraintSP()));
			numToAdd = numToAdd + (int) (ceil( (double) (numToAdd + 2)/ (double) GlobalSimArgs::getDegreeConstraintSP()));
		}
		else // not steiner algo
		{
			//cerr << WRITE_FUNCTION << " ERRORRERRORREERRRROROROEOREOREOREORO!!!!!!!! -> wrongly calculated steiner ste size " << endl;
			numToAdd = (int) (ceil( (double) members/ (double) GlobalSimArgs::getDegreeConstraint())); // - 2) * 2) + 2;
			numToAdd = numToAdd + (int) (ceil( (double) (numToAdd + 2)/ (double) GlobalSimArgs::getDegreeConstraint()));
		}
	}
	else // K_BEST_LINKS ...
	{
		if((GlobalSimArgs::getSteinerMemberRatio() > 0) && ((isSteinerAlgo(GlobalSimArgs::getSimTreeAlgo()) || (GlobalSimArgs::getSimGraphAlgo() != COMPLETE_MEMBER_GRAPH))))
		{
			numToAdd = int(members * GlobalSimArgs::getSteinerMemberRatio());
		}
		else if(isSteinerAlgo(GlobalSimArgs::getSimTreeAlgo()) || (GlobalSimArgs::getSimGraphAlgo() != COMPLETE_MEMBER_GRAPH))
		{
			//cerr << WRITE_FUNCTION << " ERRORRERRORREERRRROROROEOREOREOREORO!!!!!!!! -> wrongly calculated steiner ste size " << endl;
			numToAdd = (int) (ceil( (double) members/ (double) GlobalSimArgs::getDegreeConstraintSP())); // - 2) * 2) + 2;
			numToAdd = numToAdd + (int) (ceil( (double) (numToAdd + 2)/ (double) GlobalSimArgs::getDegreeConstraintSP()));
			//numToAdd = numToAdd + (int) (ceil( (double) ((numToAdd * 2) + 2)/ (double) GlobalSimArgs::getDegreeConstraintSP()));
		}
		else
		{
			numToAdd = (int) (ceil( (double) members/ (double) GlobalSimArgs::getDegreeConstraint())); // - 2) * 2) + 2;
			numToAdd = numToAdd + (int) (ceil( (double) (numToAdd + 2)/ (double) GlobalSimArgs::getDegreeConstraint()));
		}
	}
	//cerr << WRITE_FUNCTION << " num sp to add " << numToAdd << endl;

	if(numToAdd <= 1) numToAdd = 2;
	return numToAdd;
}

int generateSizeOfSteinerSet(int members, int steiner)
{
	return max((generateSizeOfSteinerSet(members) - steiner), 0);
}

int generateSizeNonSteiner(int members)
{
	// if fixed then return number
	if(GlobalSimArgs::getSteinerMemberSize() > 0) return GlobalSimArgs::getSteinerMemberSize();

	int numToAdd = (int) (ceil( (double) members/ (double) GlobalSimArgs::getDegreeConstraint())); // - 2) * 2) + 2;
	numToAdd = numToAdd + (int) (ceil( (double) ((numToAdd * 2) + 2)/ (double) GlobalSimArgs::getDegreeConstraint()));
	return numToAdd;
}

double generateDelayLimit(int members)
{
	double delayLimit = (members + generateSizeOfSteinerSet(members) ) * 0.75;
	ASSERTING(delayLimit > 0);

	return delayLimit;
}

/*-----------------------------------------------------------------------
Observation: Minimum number of edges to avoid partitioning a graph is:
	partitions = 2;
	(|V|/|partitions|) = n;
	nC2 = |e|; // w/ 2 partitions
	|min_edges| = |e| + 1; // w/ no partitions
------------------------------------------------------------------------- */

int generateMinEdgeCountNoPartition(int num_vertices)
{
	int partitions = 2; 		// minimum partitions in a graph

	int n = (int) (num_vertices/partitions) + 1;
	cerr << "n " << n << endl;

	int r = 2;
	int nfac = n;
	for(int i = nfac-1; i > 0; i--)
		nfac = nfac * i;

	cerr << "nfac " << nfac << endl;

	int nminrfac = (n-r);
	for(int i = nminrfac - 1; i > 0; i--)
		nminrfac = nminrfac * i;

	cerr << "nminrfac " << nminrfac << endl;

	int e = nfac/(r * nminrfac);

	cerr << WRITE_FUNCTION  << num_vertices << " vertices needs " << (e + 1) << " edges " << endl;
	return (e + 1);

	//int numberV = generateSizeOfSteinerSet(groupInfo.vertexMap_.size()) + groupInfo.vertexMap_.size();
	//int minEdgeCount = generateMinEdgeCountNoPartition(numberV);
	//double kfloat = (double) (minEdgeCount/numberV);
	//double integer;
	//double frac = modf( (double) kfloat, &integer);
	//double k = ceil(kfloat);
	//cerr << "minEdgeCount " << minEdgeCount << " kfloat " << kfloat << " frac " << frac << " integer " << integer << " k " << k << endl;
	//char c = getchar();
}

/*-----------------------------------------------------------------------
	Produces a graph tha only has vertices with out edges, and,
	the vertex/edge index != vertex/edge id
------------------------------------------------------------------------- */
void densifyGraph(const GraphN &g, GraphN& dg)
{
	//cerr << WRITE_FUNCTION << endl; print_graph(g_in, get(&VertexProp::id, g_in));

	// Old sparse Graph
	//iVertexMap idmap_g 		= get(&VertexProp::id, &g);
	//vsVertexMap vsmap_g     = get(&VertexProp::vertexState, &g);

	// new Dense Graph
	iVertexMap 	idmap_dg	= get(&VertexProp::id, dg);
    vertex_iteratorN vit, vit_end, vit_in, vit_in_end;

	// -- Adding vertices and properties --
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
    {
		if(getOutDegree(g, *vit) > 0)
    	{
    		vertex_descriptorN u = add_vertex(g[*vit], dg);
    		//cerr << "Added: "  << u << " id: " << idmap_dg[u] << " VertexState: " << dg[u].vertexState << endl;
    		//ASSERTING(dg[u].vertexState == GROUP_MEMBER || dg[u].vertexState == STEINER_POINT);
    	}
    }

    // -- Adding edges and properties --
    boost::tuples::tie(vit_in, vit_in_end) = vertices(dg);
    boost::tuples::tie(vit, vit_end) = vertices(dg);
    for(  ; vit != vit_end; ++vit)
    {
    	//cerr << "u: " << *vit ;
   		for( vit_in = vit; vit_in != vit_in_end; ++vit_in)
		{
			if(*vit != *vit_in)
			{
				pair<edge_descriptorN, bool> ep = edge(idmap_dg[*vit], idmap_dg[*vit_in], g);
				if(ep.second)
				{
					ASSERTING(ep.second);
					pair<edge_descriptorN, bool> e = add_edge(*vit, *vit_in, dg);
					ASSERTING(e.second);
					dg[e.first] = g[ep.first];

					// -- debug --
					//cerr << " v: " << *vit_in << endl;
					//cerr << "Found e(g): "  << ep.first << " (" << g[ep.first].id.first << "," << g[ep.first].id.second << ")" << endl;
					//cerr << "Added e(dg): "  << e.first << " (" << dg[e.first].id.first << "," << dg[e.first].id.second << ")" << endl;
					// -- debug end --
				}
			}
		}
    }

	// -- debug --
	cerr << WRITE_FUNCTION << endl;
	//print_graph(dg, get(&VertexProp::id, dg));
	//dumpGraph(dg);
	checkGraph(dg);
	//char c = getchar();
}

void densifyGraph(const TreeStructure &T, GraphN& dg)
{
	// -- Adding vertices and properties --
	for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
		if(getOutDegree(T.g, *vit) > 0)
    		vertex_descriptorN u = add_vertex(T.g[*vit], dg);

    // -- Adding edges and properties --
	iVertexMap 	idmap_dg	= get(&VertexProp::id, dg);
	vertex_iteratorN vit, vit_end, vit_in, vit_in_end;

    boost::tuples::tie(vit_in, vit_in_end) = vertices(dg);
    boost::tuples::tie(vit, vit_end) = vertices(dg);
    for(  ; vit != vit_end; ++vit)
    {
    	//cerr << "u: " << *vit ;
   		for( vit_in = vit; vit_in != vit_in_end; ++vit_in)
		{
			if(*vit != *vit_in)
			{
				pair<edge_descriptorN, bool> ep = edge(idmap_dg[*vit], idmap_dg[*vit_in], T.g);
				if(ep.second)
				{
					ASSERTING(ep.second);
					pair<edge_descriptorN, bool> e = add_edge(*vit, *vit_in, dg);
					ASSERTING(e.second);
					dg[e.first] = T.g[ep.first];

					// -- debug --
					//cerr << " v: " << *vit_in << endl;
					//cerr << "Found e(T.g): "  << ep.first << " (" << T.g[ep.first].id.first << "," << T.g[ep.first].id.second << ")" << endl;
					//cerr << "Added e(dg): "  << e.first << " (" << dg[e.first].id.first << "," << dg[e.first].id.second << ")" << endl;
					// -- debug end --
				}
			}
		}
    }

	// -- debug --
	//cerr << WRITE_FUNCTION << endl;
	//dumpGraph(dg);
	checkGraph(dg);
}

/*-----------------------------------------------------------------------
	Produces a graph that may have vertices with no out-edges, and,
	that has vertex/edge index == vertex/edge id
------------------------------------------------------------------------- */
void sparsifyGraph(const GraphN &g, GraphN& dg)
{
	//cerr << WRITE_FUNCTION << endl; print_graph(g_in, get(&VertexProp::id, g_in));

	// Old dense Graph
	iVertexMapConst idmap_g 		= get(&VertexProp::id, g);
	vsVertexMapConst vsmap_g     = get(&VertexProp::vertexState, g);

	// new sparse Graph
	iVertexMap 	idmap_dg	= get(&VertexProp::id, dg);
    vertex_iteratorN vit, vit_end, vit_in, vit_in_end;

    // -- Adding vertices, edges and properties --
    boost::tuples::tie(vit_in, vit_in_end) = vertices(g);
    boost::tuples::tie(vit, vit_end) = vertices(g);
    for(  ; vit != vit_end; ++vit)
    {
    	//cerr << "u: " << *vit ;
   		for( vit_in = vit; vit_in != vit_in_end; ++vit_in)
		{
			if(*vit != *vit_in)
			{
				//pair<edge_descriptorN, bool> ep = edge(idmap_dg[*vit], idmap_dg[*vit_in], g);
				pair<edge_descriptorN, bool> ep = edge(*vit, *vit_in, g);
				if(ep.second)
				{
					ASSERTING(ep.second);
					pair<edge_descriptorN, bool> e = add_edge(idmap_g[*vit], idmap_g[*vit_in], dg);
					ASSERTING(e.second);
					dg[e.first] = g[ep.first];

					// adding vertex properties
					dg[idmap_g[*vit]] = g[*vit];
					dg[idmap_g[*vit_in]] = g[*vit_in];

					// -- debug --
					//cerr << " v: " << *vit_in << endl;
					//cerr << "Found v(g): " << *vit << ", " << *vit_in << endl;
					//cerr << "Added v(dg): " << idmap_g[*vit] << ", " << idmap_g[*vit_in] << endl;
					//cerr << "Found e(g): "  << ep.first << " (" << g[ep.first].id.first << "," << g[ep.first].id.second << ")" << endl;
					//cerr << "Added e(dg): "  << e.first << " (" << dg[e.first].id.first << "," << dg[e.first].id.second << ")" << endl;
					// -- debug end --
				}
			}
		}
    }

	// -- debug --
	//cerr << WRITE_FUNCTION << endl;
	//print_graph(dg, get(&VertexProp::id, dg));
	//dumpGraph(dg);
	checkGraph(dg);
	//char c = getchar();
}
/*-----------------------------------------------------------------------
				checks the validity of the graph
------------------------------------------------------------------------- */
void checkGraph(const GraphN &g)
{

#ifndef NDEBUG

	ASSERTING(num_edges(g) > 0);
	ASSERTING(num_vertices(g) > 1);

	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		if(getOutDegree(g, *vit) > 0)
		{
			if(g[*vit].vertexFunction == NO_FUNCTION || g[*vit].vertexState == VERTEX_STATE_ERROR)
			{
				cerr << WRITE_FUNCTION << "Node : " << *vit << " " << g[*vit] << endl;
				//dumpGraph(g);
			}
			ASSERTING(g[*vit].vertexState != VERTEX_STATE_ERROR);
			ASSERTING(g[*vit].vertexFunction != NO_FUNCTION);
			ASSERTING(g[*vit].id > -1);

			out_edge_iteratorN oit, oit_end;
            for(boost::tuples::tie(oit, oit_end) = out_edges(*vit, g); oit != oit_end; ++oit)
			{
				if(g[*oit].weight <= 0 ||
					g[*oit].cost <= 0 ||
					g[*oit].delay <= 0 ||
					g[*oit].id.first == g[*oit].id.second)
					//g[*oit].weight > GlobalSimArgs::maxEdgeDelay_)
				{

					vertex_descriptorN src = source(*oit, g), targ = target(*oit, g);
					pair<edge_descriptorN, bool> ep = edge(src, targ, g);
					if(ep.second) cerr << "(" << src << "," << targ << ") edge prop : " << g[ep.first] << endl;

					//dumpGraph(g);
					cerr << WRITE_FUNCTION << "node: " << *vit << g[*vit] << " edge : " << *oit << " : " <<  g[*oit] << endl;
					cerr << WRITE_FUNCTION << " out degree : " << getOutDegree(*vit, g) << " edges : " << endl;

					out_edge_iteratorN oit_in, oit_in_end;
                    for(boost::tuples::tie(oit_in, oit_in_end) = out_edges(*vit, g); oit_in != oit_in_end; ++oit_in)
						cerr << "(" << *oit_in << "," << g[*oit_in] << ") " ;

					cerr << endl;
				}
				ASSERTING(g[*oit].weight > 0);
				ASSERTING(g[*oit].cost  > 0);
				ASSERTING(g[*oit].delay > 0);
				ASSERTING(g[*oit].id.first != g[*oit].id.second);
				ASSERTING(g[*oit].weight <= GlobalSimArgs::maxEdgeDelay_);
			}

			/*if(g[*vit].obj != NULL)
			{
				NodeBase *nob = g[*vit].obj;
				if(nob->getId() != *vit)
				{
					cerr << WRITE_FUNCTION << "Node : " << g[*vit] << endl;
				}

				ASSERTING(nob->getId() == *vit);
			}*/
		}
		else if(getOutDegree(g, *vit) <= 0)
		{
			if(g[*vit].vertexState == STEINER_POINT || g[*vit].vertexState == GROUP_MEMBER)
			{
				cerr << WRITE_FUNCTION << "Node : " << g[*vit] << endl;
				//dumpGraph(g);
			}
			ASSERTING(g[*vit].vertexState != STEINER_POINT);
			ASSERTING(g[*vit].vertexState != GROUP_MEMBER);
		}
	}

	checkIsPartitioned(g);
	//checkMaxDistance(g);
#endif

}

void checkGraphEqualSP(const GraphN &g)
{
#ifndef NDEBUG
	ASSERTING(num_edges(g) > 0);
	ASSERTING(num_vertices(g) > 1);

	vertex_iteratorN vit, vit_end, vit_in, vit_in_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(*vit, g); oit != oit_end; ++oit)
		{
			double shortest_path_weight = g[*oit].weight;
			unsigned int targ = target(*oit, g);

            for(boost::tuples::tie(vit_in, vit_in_end) = vertices(g); vit_in != vit_in_end; ++vit_in)
			{
				if(*vit_in == *vit) continue;
				pair<edge_descriptorN, bool> ep = edge(*vit, *vit_in, g);
				ASSERTING(ep.second);

				if(*vit_in == targ) continue;

				pair<edge_descriptorN, bool> ep_in = edge(*vit_in, targ, g);
				ASSERTING(ep_in.second);

				// (*vit -> targ) == (*vit -> *vit_in -> targ)?
				double indirect_path_weight = g[ep.first].weight + g[ep_in.first].weight;
				if(shortest_path_weight == indirect_path_weight)
				{
					cerr << WRITE_PRETTY_FUNCTION << " shortest_path_weight : " << shortest_path_weight << " == indirect_path_weight " << indirect_path_weight << " Edge : " << g[*oit] << " == path: " << g[ep.first] << " " << g[ep_in.first] << endl;
				}
				else if(shortest_path_weight > indirect_path_weight)
				{
					cerr << WRITE_PRETTY_FUNCTION << "ERROR!! shortest_path_weight : " << shortest_path_weight << " > indirect_path_weight " << indirect_path_weight << " Edge : " << g[*oit] << " == path: " << g[ep.first] << " " << g[ep_in.first] << endl;
				}

			}
		}
	}
#endif
}

int cntSPViolations(const GraphN &g)
{
	ASSERTING(num_edges(g) > 0);
	ASSERTING(num_vertices(g) > 1);

	int num_of_violations = 0;

	vertex_iteratorN vit, vit_end, vit_in, vit_in_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(*vit, g); oit != oit_end; ++oit)
		{
			double shortest_path_weight = (double)g[*oit].weight;
			unsigned int targ = target(*oit, g);

            //for(boost::tuples::tie(vit_in, vit_in_end) = vertices(g); vit_in != vit_in_end; ++vit_in)
			for(vit_in = vit, vit_in_end = vit_end; vit_in != vit_in_end; ++vit_in)
			{
				if(*vit_in == *vit) continue;
				if(*vit_in == targ) continue;

				pair<edge_descriptorN, bool> ep = edge(*vit, *vit_in, g);
				if(!ep.second) continue;

				pair<edge_descriptorN, bool> ep_in = edge(*vit_in, targ, g);
				if(!ep_in.second) continue;

				// (*vit -> targ) == (*vit -> *vit_in -> targ)?
				double indirect_path_weight = (double) g[ep.first].weight + (double) g[ep_in.first].weight;

				//if((float) indirect_path_weight < (float) shortest_path_weight)
				if(shortest_path_weight - indirect_path_weight > 0.0001 && shortest_path_weight < 2)
				{
					double diff = (float)indirect_path_weight - (float)shortest_path_weight;

					num_of_violations++;
					//cerr << WRITE_PRETTY_FUNCTION << "not sp edge: " << *oit << " sp weight : " << shortest_path_weight << " > indirect " << indirect_path_weight << " Edge : " << g[*oit] << " > path: " << g[ep.first] << " " << g[ep_in.first] << endl;
					//printf(" shortest_path_weight : %f indirect_path_weight %f\n", shortest_path_weight,  indirect_path_weight);
					//ASSERTING( indirect_path_weight < shortest_path_weight );
					//ASSERTING( indirect_path_weight == shortest_path_weight );

					break;
				}
			}
		}
	}

	return num_of_violations;
}

int cntInvalidEdgeWeights(const GraphN &g, int invalid_weight)
{
	int counter = 0;
	edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = edges(g); eit !=eit_end; ++eit)
		if(g[*eit].weight >= invalid_weight) counter++;

	return counter;
}

int cntInvalidEdgeWeights(const GraphN &g, const VertexSet &invalidV, int invalid_weight)
{
	int counter = 0;
	edge_iteratorN eit, eit_end;
    for(boost::tuples::tie(eit, eit_end) = edges(g); eit !=eit_end; ++eit)
	{
		int src = source(*eit, g), targ = target(*eit, g);
		if(invalidV.contains(src) || invalidV.contains(targ)) continue;

		if(g[*eit].weight >= invalid_weight) counter++;
	}

	return counter;
}

int cntInvalidVertices(const GraphN &g, VertexSet &invalidVertices, int invalid_weight)
{
	int counter = 0;
	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		int cnt_invalid = 0;
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(*vit, g); oit != oit_end; ++oit)
		{
			if(g[*oit].weight >= invalid_weight)
				cnt_invalid++;
		}

		if(cnt_invalid >= (out_degree(*vit, g) - 4))
		{
			counter++;
			invalidVertices.insert(*vit);
		}
	}

	return counter;
}

/*-----------------------------------------------------------------------
	connects a partitioned graph through shortest paths
------------------------------------------------------------------------- */
void connectPartitionedGraph(const TreeStructure &inputT, TreeStructure &newT)
{
	// ensure graph is connected
	VertexSet partV;
	while(findPartition(newT.g, partV))
	{
		bool inserted_edge = false;
		for(VertexSet::const_iterator vi = partV.begin(), vi_end = partV.end(); vi != vi_end; ++vi)
		{
			int src = *vi;
			std::multimap<double, edge_descriptorN> addEdges;
			out_edge_iteratorN oit, oit_end;
            for(boost::tuples::tie(oit, oit_end) = out_edges(src, inputT.g); oit != oit_end; ++oit)
				addEdges.insert(pair<double, edge_descriptorN>(get(&EdgeProp::weight, inputT.g)[*oit], *oit));

			for(std::multimap<double, edge_descriptorN>::iterator mit = addEdges.begin(), mit_end = addEdges.end(); mit != mit_end; ++mit)
			{
				int targ = target(mit->second, inputT.g);
				ASSERTING(src != targ);

				if(partV.contains(targ)) continue;

				if(newT.isEdge(src, targ) == false)
				{
					newT.insertEdge(src, targ, inputT.g);
					inserted_edge = true;
					break;
				}
			}

			if(inserted_edge == true) break;
		}

		partV.clear();
	}
}

/*-----------------------------------------------------------------------
	finds out whether the graph is partitioned
------------------------------------------------------------------------- */
bool findPartition(const GraphN &g, VertexSet &partV)
{
	vector<bool> visited(num_vertices(g));

	// init
	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		visited[*vit] = false;
	}

	// start walk
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		if(getOutDegree(g, *vit) > 0)
		{
			walkGraph(g, *vit, visited);
			break;
		}
	}

	// check
	bool partition = false;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		if(g[*vit].id > -1 && visited[*vit] == false)
		{
			//if(!partition) cerr << WRITE_FUNCTION << " Partition (" << *vit ;
			//else cerr << ", " << *vit;

			partV.insert(*vit);
			partition = true;
		}
	}

	//if(partition) cerr << ")" << endl;

	return partition;
}

/*-----------------------------------------------------------------------
	finds out whether the graph is partitioned
------------------------------------------------------------------------- */
bool checkIsPartitioned(const GraphN &g)
{
	vector<bool> visited(num_vertices(g));

	// init
	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		visited[*vit] = false;
	}

	// start walk
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		if(getOutDegree(g, *vit) > 0)
		{
			walkGraph(g, *vit, visited);
			break;
		}
	}

	// check
	bool connected = true;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		//if(getOutDegree(*vit, g) > 0)
		//{
			if(g[*vit].id > -1 && visited[*vit] == false)
			{
				if(connected) cerr << WRITE_FUNCTION << " Partition (" << *vit ;
				else cerr << ", " << *vit;

				connected = false;
			}
			//ASSERTING(visited[*vit] == true);
		//}
	}

	if(!connected)
		cerr << ")" << endl;

	return connected;
}

void walkGraph(const GraphN &g, vertex_descriptorN u, vector<bool> &visited)
{
	out_edge_iteratorN oit, oit_end;
    for(boost::tuples::tie(oit, oit_end) = out_edges(u, g); oit != oit_end; ++oit)
	{
		vertex_descriptorN v = target(*oit, g);
		ASSERTING(u != v);
		if(visited[v] == false)
		{
			visited[v] = true;
			walkGraph(g, v, visited);
		}
	}
}

void checkMaxDistance(const GraphN &g)
{
#ifndef NDEBUG
	double maxDistance = 0;
	dEdgeMapConst wmap = get(&EdgeProp::weight, g);

	vertex_iteratorN vit, vit_end;
    for(boost::tuples::tie(vit, vit_end) = vertices(g); vit != vit_end; ++vit)
	{
		out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(*vit, g); oit != oit_end; ++oit)
		{
			if(wmap[*oit] > maxDistance) maxDistance = wmap[*oit];
			//if(g[*oit].weight > maxDistance) maxDistance = g[*oit].weight;

		}
	}

	//cerr << "Max Distance " << maxDistance << endl;
	GlobalSimArgs::maxEdgeDelay_ = maxDistance;
#endif // NDEBUG
}

}; // namespace GraphAlgorithms


#include "rbfcore.h"
#include "utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <unordered_map>
#include <ctime>
#include <chrono>
#include <iomanip>
#include <algorithm>
#include <queue>
#include "readers.h"
//#include "mymesh/UnionFind.h"
//#include "mymesh/tinyply.h"

typedef std::chrono::high_resolution_clock Clock;
double randomdouble() {return static_cast <double> (rand()) / static_cast <double> (RAND_MAX);}
double randomdouble(double be,double ed) {return be + randomdouble()*(ed-be);	}

void RBF_Core::NormalRecification(double maxlen, vector<double>&nors){

    double maxlen_r = -1;
    auto p_vn = nors.data();
    int  np = nors.size()/3;
    if(1){
        for(int i=0;i<np;++i){
            maxlen_r = max(maxlen_r,MyUtility::normVec(p_vn+i*3));
        }

        cout<<"maxlen_r: "<<maxlen_r<<endl;
        double ratio = maxlen / maxlen_r;
        for(auto &a:nors)a*=ratio;
    }else{
        for(int i=0;i<np;++i){
            MyUtility::normalize(p_vn+i*3);
        }

    }

}

bool RBF_Core::Write_Hermite_NormalPrediction(string fname, int mode){

//    vector<uchar>labelcolor(npt*4);
//    vector<uint>f2v;
//    uchar red[] = {255,0,0, 255};
//    uchar green[] = {0,255,0, 255};
//    uchar blue[] = {0,0,255, 255};
//    for(int i=0;i<labels.size();++i){
//        uchar *pcolor;
//        if(labels[i]==0)pcolor = green;
//        else if(labels[i]==-1)pcolor = blue;
//        else if(labels[i]==1)pcolor = red;
//        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
//    }
    //fname += mp_RBF_METHOD[curMethod];

//    for(int i=0;i<npt;++i){
//        uchar *pcolor = green;
//        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
//    }

    vector<double>nors;
    if(mode ==0)nors=initnormals;
    else if(mode == 1)nors=newnormals;
    else if(mode == 2)nors = initnormals_uninorm;
    NormalRecification(1.,nors);

    //for(int i=0;i<npt;++i)if(randomdouble()<0.5)MyUtility::negVec(nors.data()+i*3);
    //cout<<pts.size()<<' '<<f2v.size()<<' '<<nors.size()<<' '<<labelcolor.size()<<endl;
    //writePLYFile(fname,pts,f2v,nors,labelcolor);

//    writeObjFile_vn(fname,pts,nors);
    writePLYFile_VN(fname,pts,nors);

    return 1;
}

void RBF_Core::Set_HermiteRBF(vector<double>&pts){

    cout<<"Set_HermiteRBF"<<endl;
    //for(auto a:pts)cout<<a<<' ';cout<<endl;
    isHermite = true;

    a.set_size(npt*4);
    M.set_size(npt*4,npt*4);
    double *p_pts = pts.data();
    for(int i=0;i<npt;++i){
        for(int j=i;j<npt;++j){
            M(i,j) = M(j,i) = Kernal_Function_2p(p_pts+i*3, p_pts+j*3);
        }
    }

    //if(User_Lamnbda!=0)for(int i=0;i<npt;++i)M(i,i) += User_Lamnbda;

    double G[3];
    for(int i=0;i<npt;++i){
        for(int j=0;j<npt;++j){

            Kernal_Gradient_Function_2p(p_pts+i*3, p_pts+j*3, G);
            //            int jind = j*3+npt;
            //            for(int k=0;k<3;++k)M(i,jind+k) = -G[k];
            //            for(int k=0;k<3;++k)M(jind+k,i) = G[k];

            for(int k=0;k<3;++k)M(i,npt+j+k*npt) = G[k];
            for(int k=0;k<3;++k)M(npt+j+k*npt,i) = G[k];

        }
    }

    double H[9];
    for(int i=0;i<npt;++i){
        for(int j=i;j<npt;++j){

            Kernal_Hessian_Function_2p(p_pts+i*3, p_pts+j*3, H);
            //            int iind = i*3+npt;
            //            int jind = j*3+npt;
            //            for(int k=0;k<3;++k)
            //                for(int l=0;l<3;++l)
            //                    M(jind+l,iind+k) = M(iind+k,jind+l) = -H[k*3+l];

            for(int k=0;k<3;++k)
                for(int l=0;l<3;++l)
                    M(npt+j+l*npt,npt+i+k*npt) = M(npt+i+k*npt,npt+j+l*npt) = -H[k*3+l];
        }
    }

    //cout<<std::setprecision(5)<<std::fixed<<M<<endl;

    bsize= 4;
    N.zeros(npt*4,4);
    b.set_size(4);

    for(int i=0;i<npt;++i){
        N(i,0) = 1;
        for(int j=0;j<3;++j)N(i,j+1) = pts[i*3+j];
    }
    for(int i=0;i<npt;++i){
        //        int ind = i*3+npt;
        //        for(int j=0;j<3;++j)N(ind+j,j+1) = 1;

        for(int j=0;j<3;++j)N(npt+i+j*npt,j+1) = -1;
    }

    //cout<<N<<endl;
    //arma::vec eigval = eig_sym( M ) ;
    //cout<<eigval.t()<<endl;

    if(!isnewformula){
        cout<<"start solve M: "<<endl;
        auto t1 = Clock::now();
        if(isinv)Minv = inv(M);
        else {
            arma::mat Eye;
            Eye.eye(npt*4,npt*4);
            Minv = solve(M,Eye);
        }
        cout<<"solved M: "<<(invM_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;

        t1 = Clock::now();
        if(isinv)bprey = inv_sympd(N.t() * Minv * N) * N.t() * Minv;
        else {
            arma::mat Eye2;
            Eye2.eye(bsize,bsize);
            bprey = solve(N.t() * Minv * N, Eye2) * N.t() * Minv;
        }
        cout<<"solved bprey "<<std::chrono::nanoseconds(Clock::now() - t1).count()/1e9<<endl;
    }else{

    }
}

double Gaussian_2p(const double *p1, const double *p2, double sigma){

    return exp(-MyUtility::vecSquareDist(p1,p2)/(2*sigma*sigma));
}

void RBF_Core::Set_Actual_User_LSCoef(double user_ls){

    User_Lamnbda = User_Lamnbda_inject = user_ls > 0 ?  user_ls : 0;

}

void RBF_Core::Set_Actual_Hermite_LSCoef(double hermite_ls){

    ls_coef = Hermite_ls_weight_inject = hermite_ls > 0?hermite_ls:0;
}

void RBF_Core::Set_SparsePara(double spa){
    sparse_para = spa;
}

void RBF_Core::Set_User_Lamnda_ToMatrix(double user_ls){

    {
        Set_Actual_User_LSCoef(user_ls);
        auto t1 = Clock::now();
        cout<<"setting K, HermiteApprox_Lamnda"<<endl;
        if(User_Lamnbda>0){
            arma::sp_mat eye;
            eye.eye(npt,npt);

            dI = inv(eye + User_Lamnbda*K00);
            saveK_finalH = K = K11 - (User_Lamnbda)*(K01.t()*dI*K01);

        }else saveK_finalH = K = K11;
        cout<<"solved: "<<(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;
    }

    finalH = saveK_finalH;

}

void RBF_Core::Set_HermiteApprox_Lamnda(double hermite_ls){

    {
        Set_Actual_Hermite_LSCoef(hermite_ls);
        auto t1 = Clock::now();
        cout<<"setting K, HermiteApprox_Lamnda"<<endl;
        if(ls_coef>0){
            arma::sp_mat eye;
            eye.eye(npt,npt);

            if(ls_coef > 0){
                arma:: mat tmpdI = inv(eye + (ls_coef+User_Lamnbda)*K00);
                K = K11 - (ls_coef+User_Lamnbda)*(K01.t()*tmpdI*K01);
            }else{
                K = saveK_finalH;
            }
        }
        cout<<"solved: "<<(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;
    }

}

void RBF_Core::Set_Hermite_PredictNormal(vector<double>&pts){

    Set_HermiteRBF(pts);

    auto t1 = Clock::now();
    cout<<"setting K"<<endl;

    if(!isnewformula){
        arma::mat D = N.t()*Minv;
        K = Minv - D.t()*inv(D*N)*D;
        K = K.submat( npt, npt, npt*4-1, npt*4-1 );
        finalH = saveK_finalH = K;

    }else{
        cout<<"using new formula"<<endl;
        bigM.zeros((npt+1)*4,(npt+1)*4);
        bigM.submat(0,0,npt*4-1,npt*4-1) = M;
        bigM.submat(0,npt*4,(npt)*4-1, (npt+1)*4-1) = N;
        bigM.submat(npt*4,0,(npt+1)*4-1, (npt)*4-1) = N.t();

        //for(int i=0;i<4;++i)bigM(i+(npt)*4,i+(npt)*4) = 1;

        auto t2 = Clock::now();
        bigMinv = inv(bigM);
        cout<<"bigMinv: "<<(setK_time= std::chrono::nanoseconds(Clock::now() - t2).count()/1e9)<<endl;
		bigM.clear();
        Minv = bigMinv.submat(0,0,npt*4-1,npt*4-1);
        Ninv = bigMinv.submat(0,npt*4,(npt)*4-1, (npt+1)*4-1);

        bigMinv.clear();
        //K = Minv - Ninv *(N.t()*Minv);
        K = Minv;
        K00 = K.submat(0,0,npt-1,npt-1);
        K01 = K.submat(0,npt,npt-1,npt*4-1);
        K11 = K.submat( npt, npt, npt*4-1, npt*4-1 );

        M.clear();N.clear();
        cout<<"K11: "<<K11.n_cols<<endl;

        //Set_Hermite_DesignedCurve();

        Set_User_Lamnda_ToMatrix(User_Lamnbda_inject);

//		arma::vec eigval, ny;
//		arma::mat eigvec;
//		ny = eig_sym( eigval, eigvec, K);
//		cout<<ny<<endl;

        cout<<"K: "<<K.n_cols<<endl;
    }

    //K = ( K.t() + K )/2;
    cout<<"solve K total: "<<(setK_time= std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;
    return;

}

void RBF_Core::SetInitnormal_Uninorm(){

    initnormals_uninorm = initnormals;
    for(int i=0;i<npt;++i)MyUtility::normalize(initnormals_uninorm.data()+i*3);

}

int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm(){

    arma::vec eigval, ny;
    arma::mat eigvec;

    if(!isuse_sparse){
        ny = eig_sym( eigval, eigvec, K);
    }else{
//		cout<<"use sparse eigen"<<endl;
//        int k = 4;
//        do{
//            ny = eigs_sym( eigval, eigvec, sp_K, k, "sa" );
//            k+=4;
//        }while(ny(0)==0);
    }

    cout<<"eigval(0): "<<eigval(0)<<endl;

    int smalleig = 0;

    initnormals.resize(npt*3);
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    for(int i=0;i<npt*3;++i)y(i+npt) = eigvec(i,smalleig);
    for(int i=0;i<npt;++i){
        initnormals[i*3]   = y(npt+i);
        initnormals[i*3+1] = y(npt+i+npt);
        initnormals[i*3+2] = y(npt+i+npt*2);
        //MyUtility::normalize(normals.data()+i*3);
    }

    SetInitnormal_Uninorm();
    cout<<"Solve_Hermite_PredictNormal_UnitNorm finish"<<endl;
    return 1;
}

/***************************************************************************************************/
/***************************************************************************************************/
double acc_time;

static int countopt = 0;
double optfunc_Hermite(const vector<double>&x, vector<double>&grad, void *fdata){

    auto t1 = Clock::now();
    RBF_Core *drbf = reinterpret_cast<RBF_Core*>(fdata);
    int n = drbf->npt;
    arma::vec arma_x(n*3);

    //(  sin(a)cos(b), sin(a)sin(b), cos(a)  )  a =>[0, pi], b => [-pi, pi];
    vector<double>sina_cosa_sinb_cosb(n * 4);
    for(int i=0;i<n;++i){
        int ind = i*4;
        sina_cosa_sinb_cosb[ind] = sin(x[i*2]);
        sina_cosa_sinb_cosb[ind+1] = cos(x[i*2]);
        sina_cosa_sinb_cosb[ind+2] = sin(x[i*2+1]);
        sina_cosa_sinb_cosb[ind+3] = cos(x[i*2+1]);
    }

    for(int i=0;i<n;++i){
        auto p_scsc = sina_cosa_sinb_cosb.data()+i*4;
        //        int ind = i*3;
        //        arma_x(ind) = p_scsc[0] * p_scsc[3];
        //        arma_x(ind+1) = p_scsc[0] * p_scsc[2];
        //        arma_x(ind+2) = p_scsc[1];
        arma_x(i) = p_scsc[0] * p_scsc[3];
        arma_x(i+n) = p_scsc[0] * p_scsc[2];
        arma_x(i+n*2) = p_scsc[1];
    }

    arma::vec a2;
    //if(drbf->isuse_sparse)a2 = drbf->sp_H * arma_x;
    //else
    a2 = drbf->finalH * arma_x;

    if (!grad.empty()) {

        grad.resize(n*2);

        for(int i=0;i<n;++i){
            auto p_scsc = sina_cosa_sinb_cosb.data()+i*4;

            //            int ind = i*3;
            //            grad[i*2] = a2(ind) * p_scsc[1] * p_scsc[3] + a2(ind+1) * p_scsc[1] * p_scsc[2] - a2(ind+2) * p_scsc[0];
            //            grad[i*2+1] = -a2(ind) * p_scsc[0] * p_scsc[2] + a2(ind+1) * p_scsc[0] * p_scsc[3];

            grad[i*2] = a2(i) * p_scsc[1] * p_scsc[3] + a2(i+n) * p_scsc[1] * p_scsc[2] - a2(i+n*2) * p_scsc[0];
            grad[i*2+1] = -a2(i) * p_scsc[0] * p_scsc[2] + a2(i+n) * p_scsc[0] * p_scsc[3];

        }
    }

    double re = arma::dot( arma_x, a2 );
    countopt++;

    acc_time+=(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9);

    //cout<<countopt++<<' '<<re<<endl;
    return re;

}

int RBF_Core::Opt_Hermite_PredictNormal_UnitNormal(){

    sol.solveval.resize(npt * 2);

    for(int i=0;i<npt;++i){
        double *veccc = initnormals.data()+i*3;
        {
            //MyUtility::normalize(veccc);
            sol.solveval[i*2] = atan2(sqrt(veccc[0]*veccc[0]+veccc[1]*veccc[1]),veccc[2] );
            sol.solveval[i*2 + 1] = atan2( veccc[1], veccc[0]   );
        }

    }
    //cout<<"smallvec: "<<smallvec<<endl;

    if(1){
        vector<double>upper(npt*2);
        vector<double>lower(npt*2);
        for(int i=0;i<npt;++i){
            upper[i*2] = 2 * my_PI;
            upper[i*2 + 1] = 2 * my_PI;

            lower[i*2] = -2 * my_PI;
            lower[i*2 + 1] = -2 * my_PI;
        }

        countopt = 0;
        acc_time = 0;

        //LocalIterativeSolver(sol,kk==0?normals:newnormals,300,1e-7);
        Solver::nloptwrapper(lower,upper,optfunc_Hermite,this,1e-7,3000,sol);
        cout<<"number of call: "<<countopt<<" t: "<<acc_time<<" ave: "<<acc_time/countopt<<endl;
        callfunc_time = acc_time;
        solve_time = sol.time;
        //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

    }
    newnormals.resize(npt*3);
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    for(int i=0;i<npt;++i){

        double a = sol.solveval[i*2], b = sol.solveval[i*2+1];
        newnormals[i*3]   = y(npt+i) = sin(a) * cos(b);
        newnormals[i*3+1] = y(npt+i+npt) = sin(a) * sin(b);
        newnormals[i*3+2] = y(npt+i+npt*2) = cos(a);
        MyUtility::normalize(newnormals.data()+i*3);
    }

    Set_RBFCoef(y);

    //sol.energy = arma::dot(a,M*a);
    cout<<"Opt_Hermite_PredictNormal_UnitNormal"<<endl;
    return 1;
}

void RBF_Core::Set_RBFCoef(arma::vec &y){
    cout<<"Set_RBFCoef"<<endl;
    if(curMethod==HandCraft){
        cout<<"HandCraft, not RBF"<<endl;
        return;
    }
    if(!isnewformula){
        b = bprey * y;
        a = Minv * (y - N*b);
    }else{

        if(User_Lamnbda>0)y.subvec(0,npt-1) = -User_Lamnbda*dI*K01*y.subvec(npt,npt*4-1);

        a = Minv*y;
        b = Ninv.t()*y;

    }

}

int RBF_Core::Lamnbda_Search_GlobalEigen(){

    vector<double>lamnbda_list({0, 0.001, 0.01, 0.1, 1});
    //vector<double>lamnbda_list({  0.5,0.6,0.7,0.8,0.9,1,1.1,1.5,2,3});
    //lamnbda_list.clear();
    //for(double i=1.5;i<2.5;i+=0.1)lamnbda_list.push_back(i);
    //vector<double>lamnbda_list({0});
    vector<double>initen_list(lamnbda_list.size());
    vector<double>finalen_list(lamnbda_list.size());
    vector<vector<double>>init_normallist;
    vector<vector<double>>opt_normallist;

    lamnbda_list_sa = lamnbda_list;
    for(int i=0;i<lamnbda_list.size();++i){

        Set_HermiteApprox_Lamnda(lamnbda_list[i]);

        if(curMethod==Hermite_UnitNormal){
            Solve_Hermite_PredictNormal_UnitNorm();
        }

        //Solve_Hermite_PredictNormal_UnitNorm();
        OptNormal(1);

        initen_list[i] = sol.init_energy;
        finalen_list[i] = sol.energy;

        init_normallist.emplace_back(initnormals);
        opt_normallist.emplace_back(newnormals);
    }

    lamnbdaGlobal_Be.emplace_back(initen_list);
    lamnbdaGlobal_Ed.emplace_back(finalen_list);

    cout<<std::setprecision(8);
    for(int i=0;i<initen_list.size();++i){
        cout<<lamnbda_list[i]<<": "<<initen_list[i]<<" -> "<<finalen_list[i]<<endl;
    }

    int minind = min_element(finalen_list.begin(),finalen_list.end()) - finalen_list.begin();
    cout<<"min energy: "<<endl;
    cout<<lamnbda_list[minind]<<": "<<initen_list[minind]<<" -> "<<finalen_list[minind]<<endl;

    initnormals = init_normallist[minind];
    SetInitnormal_Uninorm();
    newnormals = opt_normallist[minind];
	return 1;
}

void RBF_Core::Print_LamnbdaSearchTest(string fname){

    cout<<setprecision(7);
    cout<<"Print_LamnbdaSearchTest"<<endl;
    for(int i=0;i<lamnbda_list_sa.size();++i)cout<<lamnbda_list_sa[i]<<' ';cout<<endl;
    cout<<lamnbdaGlobal_Be.size()<<endl;
    for(int i=0;i<lamnbdaGlobal_Be.size();++i){
        for(int j=0;j<lamnbdaGlobal_Be[i].size();++j){
            cout<<lamnbdaGlobal_Be[i][j]<<"\t"<<lamnbdaGlobal_Ed[i][j]<<"\t";
        }
        cout<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
    }

    ofstream fout(fname);
    fout<<setprecision(7);
    if(!fout.fail()){
        for(int i=0;i<lamnbda_list_sa.size();++i)fout<<lamnbda_list_sa[i]<<' ';fout<<endl;
        fout<<lamnbdaGlobal_Be.size()<<endl;
        for(int i=0;i<lamnbdaGlobal_Be.size();++i){
            for(int j=0;j<lamnbdaGlobal_Be[i].size();++j){
                fout<<lamnbdaGlobal_Be[i][j]<<"\t"<<lamnbdaGlobal_Ed[i][j]<<"\t";
            }
            fout<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
        }
    }
    fout.close();

}


// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// 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 ETH Zurich, Wyss Zurich, Zurich Eye 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 ETH Zurich, Wyss Zurich, Zurich Eye 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.

#pragma once

#include <Eigen/LU>

#include <ze/common/logging.hpp>
#include <ze/common/macros.hpp>
#include <ze/common/matrix.hpp>
#include <ze/common/types.hpp>

namespace ze {

enum class ImuIntrinsicType
{
  Calibrated,
  ScaleMisalignment,
  ScaleMisalignmentGSensitivity,
  ScaleMisalignmentSizeEffect
};

//! Base Class for Intrinsic Models for both Accels and Gyros
class ImuIntrinsicModel
{
public:
  ZE_POINTER_TYPEDEFS(ImuIntrinsicModel);

  //! The inertial measurement models potentially depend on angular as well as
  //! linear quantities. Depending on whether a gyroscope or an accelerometer
  //! is modelled, the roles might be reversed. Accordingly, the 'primary'
  //! refers to the domain of the measurement that is being modelled (angular
  //! or linear) and the secondary on a measurement of the other domain
  //! respectively.

  typedef VectorX primary_measurement_t;
  typedef VectorX secondary_measurement_t;

  explicit ImuIntrinsicModel(ImuIntrinsicType type);
  ImuIntrinsicModel(ImuIntrinsicType type, real_t delay, real_t range);

  static constexpr real_t UndefinedRange = -1.;

  inline ImuIntrinsicType type() const { return type_; }
  std::string typeAsString() const;

  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
                          const Eigen::Ref<const secondary_measurement_t>& secondary) const = 0;

  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
                            const Eigen::Ref<const secondary_measurement_t>& secondary) const = 0;

  // getters
  inline real_t delay() const { return delay_; }
  inline real_t range() const { return range_; }

private:
  ImuIntrinsicType type_;
  real_t delay_;
  real_t range_;
};

//------------------------------------------------------------------------------
// Calibrated
class ImuIntrinsicModelCalibrated: public ImuIntrinsicModel
{
public:
  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelCalibrated);
  static constexpr ImuIntrinsicType Type = ImuIntrinsicType::Calibrated;

  using ImuIntrinsicModel::primary_measurement_t;
  using ImuIntrinsicModel::secondary_measurement_t;

  ImuIntrinsicModelCalibrated();
  ImuIntrinsicModelCalibrated(real_t delay, real_t range);

  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;
};

//------------------------------------------------------------------------------
// Scale Misalignment
class ImuIntrinsicModelScaleMisalignment : public ImuIntrinsicModel
{
public:
  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignment);
  static constexpr ImuIntrinsicType Type = ImuIntrinsicType::ScaleMisalignment;

  using ImuIntrinsicModel::primary_measurement_t;
  using ImuIntrinsicModel::secondary_measurement_t;

  //! delay, range, bias, scale misalignment matrix
  ImuIntrinsicModelScaleMisalignment(real_t delay, real_t range,
                                     const Vector3& b, const Matrix3& M);

  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  // getters
  inline const Vector3& b() const { return b_; }
  inline const Matrix3& M() const { return M_; }

private:
  Vector3 b_;
  Matrix3 M_;
  Matrix3 M_inverse_;
};

//------------------------------------------------------------------------------
// Scale Misalignment g-Sensitivity
class ImuIntrinsicModelScaleMisalignmentGSensitivity : public ImuIntrinsicModel
{
public:
  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignmentGSensitivity);

  static constexpr ImuIntrinsicType Type =
      ImuIntrinsicType::ScaleMisalignmentGSensitivity;

  using ImuIntrinsicModel::primary_measurement_t;
  using ImuIntrinsicModel::secondary_measurement_t;

  //! This model applies exclusively to gyroscopes.
  //! delay, range, bias, scale misalignment matrix, g-sensitivity matrix
  ImuIntrinsicModelScaleMisalignmentGSensitivity(real_t delay,
                                                 real_t range,
                                                 const Vector3& b,
                                                 const Matrix3& M,
                                                 const Matrix3& Ma);

  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  // getters
  inline const Vector3& b() const { return b_; }
  inline const Matrix3& M() const { return M_; }
  inline const Matrix3& Ma() const { return Ma_; }

private:
  Vector3 b_;
  Matrix3 M_;
  Matrix3 M_inverse_;
  Matrix3 Ma_;
};

//------------------------------------------------------------------------------
// Scale MisalignmentSize Effect
class ImuIntrinsicModelScaleMisalignmentSizeEffect : public ImuIntrinsicModel
{
public:
  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignmentSizeEffect);
  static constexpr ImuIntrinsicType Type =
      ImuIntrinsicType::ScaleMisalignmentSizeEffect;

  using ImuIntrinsicModel::primary_measurement_t;
  using ImuIntrinsicModel::secondary_measurement_t;

  //! This model applies exclusively to accelerometers.
  //! delay, range, bias, scale misalignment matrix, accel. column position vectors
  ImuIntrinsicModelScaleMisalignmentSizeEffect(real_t delay,
                                               real_t range,
                                               const Vector3& b,
                                               const Matrix3& M,
                                               const Matrix3& R);

  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;

  // getters
  inline const Vector3& b() const { return b_; }
  inline const Matrix3& M() const { return M_; }
  inline const Matrix3& R() const { return R_; }

private:
  Vector3 b_;
  Matrix3 M_;
  Matrix3 M_inverse_;
  Matrix3 R_;
};

} // namespace ze


// Copyright (c) 2020-2021 Franz Alt
// This code is licensed under MIT license (see LICENSE.txt for details).

#include <libcvpg/imageproc/scripting/algorithms/binary_threshold.hpp>

#include <chrono>
#include <functional>

#include <boost/asynchronous/continuation_task.hpp>

#include <libcvpg/core/exception.hpp>
#include <libcvpg/core/histogram.hpp>
#include <libcvpg/core/image.hpp>
#include <libcvpg/imageproc/algorithms/otsu_threshold.hpp>
#include <libcvpg/imageproc/algorithms/tiling.hpp>
#include <libcvpg/imageproc/algorithms/tiling/histogram.hpp>
#include <libcvpg/imageproc/algorithms/tiling/threshold.hpp>
#include <libcvpg/imageproc/algorithms/tiling/functors/histogram.hpp>
#include <libcvpg/imageproc/scripting/item.hpp>
#include <libcvpg/imageproc/scripting/processing_context.hpp>
#include <libcvpg/imageproc/scripting/detail/compiler.hpp>
#include <libcvpg/imageproc/scripting/detail/handler.hpp>
#include <libcvpg/imageproc/scripting/detail/parser.hpp>

namespace detail {

struct merge_histograms_task : public boost::asynchronous::continuation_task<std::shared_ptr<cvpg::histogram<std::size_t> > >
{
    merge_histograms_task(std::shared_ptr<cvpg::histogram<std::size_t> > a, std::shared_ptr<cvpg::histogram<std::size_t> > b)
        : boost::asynchronous::continuation_task<std::shared_ptr<cvpg::histogram<std::size_t> > >("algorithms::binary_threshold_task::merge_histograms")
        , m_a(a)
        , m_b(b)
    {}

    void operator()()
    {
        // TODO optimize this
        this_task_result().set_value(std::make_shared<cvpg::histogram<std::size_t> >(*m_a + *m_b));
    }

private:
    std::shared_ptr<cvpg::histogram<std::size_t> > m_a;
    std::shared_ptr<cvpg::histogram<std::size_t> > m_b;
};

boost::asynchronous::detail::callback_continuation<std::shared_ptr<cvpg::histogram<std::size_t> > >
merge_histograms(std::shared_ptr<cvpg::histogram<std::size_t> > a, std::shared_ptr<cvpg::histogram<std::size_t> > b)
{
    return boost::asynchronous::top_level_callback_continuation<std::shared_ptr<cvpg::histogram<std::size_t> > >(
               merge_histograms_task(a, b)
           );
}

struct binary_threshold_task :  public boost::asynchronous::continuation_task<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >
{
    binary_threshold_task(std::shared_ptr<cvpg::imageproc::scripting::processing_context> context, std::uint32_t result_id, cvpg::imageproc::scripting::detail::parser::item item)
        : boost::asynchronous::continuation_task<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >("algorithms::binary_threshold_task")
        , m_context(context)
        , m_result_id(result_id)
        , m_item(std::move(item))
    {}

    void operator()()
    {
        try
        {
            auto id = std::any_cast<std::uint32_t>(m_item.arguments.at(0).value());
            auto mode_str = std::any_cast<std::string>(m_item.arguments.at(1).value());

            auto input = m_context->load(id);
            auto parameters = m_context->parameters();

            std::uint32_t cutoff_x = 512;
            std::uint32_t cutoff_y = 512;

            {
                auto it = parameters.find("cutoff_x");

                if (it != parameters.end())
                {
                    cutoff_x = std::any_cast<std::uint32_t>(it->second);
                }
            }

            {
                auto it = parameters.find("cutoff_y");

                if (it != parameters.end())
                {
                    cutoff_y = std::any_cast<std::uint32_t>(it->second);
                }
            }

            if (input.type() == cvpg::imageproc::scripting::item::types::grayscale_8_bit_image)
            {
                auto image = std::any_cast<cvpg::image_gray_8bit>(input.value());

                const auto width = image.width();
                const auto height = image.height();

                auto start = std::chrono::system_clock::now();

                auto tf = cvpg::imageproc::algorithms::tiling_functors::histogram<cvpg::image_gray_8bit, cvpg::histogram<std::size_t> >({{ image }});
                tf.parameters.image_width = width;
                tf.parameters.image_height = height;
                tf.parameters.cutoff_x = cutoff_x;
                tf.parameters.cutoff_y = cutoff_y;

                tf.tile_algorithm_task = [](std::shared_ptr<cvpg::image_gray_8bit> src1, std::shared_ptr<cvpg::image_gray_8bit> /*src2*/, std::shared_ptr<cvpg::histogram<std::size_t> > dst, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, cvpg::imageproc::algorithms::tiling_parameters parameters)
                {
                    std::vector<std::size_t> h(256);
                    cvpg::imageproc::algorithms::histogram_gray_8bit(src1->data(0).get(), &h, from_x, to_x, from_y, to_y, std::move(parameters));

                    *dst = cvpg::histogram<std::size_t>(std::move(h));
                };

                tf.horizontal_merge_task = [](std::shared_ptr<cvpg::histogram<std::size_t> > dst1, std::shared_ptr<cvpg::histogram<std::size_t> > dst2, std::size_t /*from_x*/, std::size_t /*to_x*/, std::size_t /*from_y*/, std::size_t /*to_y*/, cvpg::imageproc::algorithms::tiling_parameters /*parameters*/)
                {
                    return merge_histograms(dst1, dst2);
                };

                tf.vertical_merge_task = [](std::shared_ptr<cvpg::histogram<std::size_t> > dst1, std::shared_ptr<cvpg::histogram<std::size_t> > dst2, std::size_t /*from_x*/, std::size_t /*to_x*/, std::size_t /*from_y*/, std::size_t /*to_y*/, cvpg::imageproc::algorithms::tiling_parameters /*parameters*/)
                {
                    return merge_histograms(dst1, dst2);
                };

                boost::asynchronous::create_callback_continuation(
                    [result = this->this_task_result(), context = m_context, result_id = m_result_id, image = std::move(image), mode_str = std::move(mode_str), cutoff_x, cutoff_y, start = std::move(start)](auto cont_res) mutable
                    {
                        auto histogram = std::move(std::get<0>(cont_res).get());

                        const std::size_t threshold = cvpg::imageproc::algorithms::otsu_threshold(histogram);

                        auto tf = cvpg::imageproc::algorithms::tiling_functors::image<cvpg::image_gray_8bit>({{ std::move(image) }});
                        tf.parameters.image_width = image.width();
                        tf.parameters.image_height = image.height();
                        tf.parameters.cutoff_x = cutoff_x;
                        tf.parameters.cutoff_y = cutoff_y;
                        tf.parameters.signed_integer_numbers.push_back(threshold);

                        tf.tile_algorithm_task = [mode_str = std::move(mode_str)](std::shared_ptr<cvpg::image_gray_8bit> src1, std::shared_ptr<cvpg::image_gray_8bit> /*src2*/, std::shared_ptr<cvpg::image_gray_8bit> dst, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, cvpg::imageproc::algorithms::tiling_parameters parameters) mutable
                        {
                            if (mode_str == "normal")
                            {
                                cvpg::imageproc::algorithms::threshold_gray_8bit(src1->data(0).get(), dst->data(0).get(), from_x, to_x, from_y, to_y, std::move(parameters));
                            }
                            else if (mode_str == "inverse")
                            {
                                cvpg::imageproc::algorithms::threshold_inverse_gray_8bit(src1->data(0).get(), dst->data(0).get(), from_x, to_x, from_y, to_y, std::move(parameters));
                            }
                        };

                        boost::asynchronous::create_callback_continuation(
                            [result = std::move(result), context = std::move(context), result_id, start = std::move(start)](auto cont_res) mutable
                            {
                                auto stop = std::chrono::system_clock::now();

                                try
                                {
                                    context->store(result_id, std::move(std::get<0>(cont_res).get()), std::chrono::duration_cast<std::chrono::microseconds>(stop - start));

                                    result.set_value(context);
                                }
                                catch (...)
                                {
                                    result.set_exception(std::current_exception());
                                }
                            },
                            cvpg::imageproc::algorithms::tiling(std::move(tf))
                        );
                    },
                    cvpg::imageproc::algorithms::tiling(std::move(tf))
                );
            }
            else if (input.type() == cvpg::imageproc::scripting::item::types::rgb_8_bit_image)
            {
                auto image_rgb = std::any_cast<cvpg::image_rgb_8bit>(input.value());

                const auto image_width = image_rgb.width();
                const auto image_height = image_rgb.height();
                const auto image_padding = image_rgb.padding();

                // create separate images for each channel
                auto image_red = cvpg::image_gray_8bit(image_width, image_height, image_padding, { image_rgb.data(0) });
                auto image_green = cvpg::image_gray_8bit(image_width, image_height, image_padding, { image_rgb.data(1) });
                auto image_blue = cvpg::image_gray_8bit(image_width, image_height, image_padding, { image_rgb.data(2) });

                auto start = std::chrono::system_clock::now();

                auto create_channel_histogram =
                    [cutoff_x, cutoff_y](auto image)
                    {
                        auto tf = cvpg::imageproc::algorithms::tiling_functors::histogram<cvpg::image_gray_8bit, cvpg::histogram<std::size_t> >({{ image }});
                        tf.parameters.image_width = image.width();
                        tf.parameters.image_height = image.height();
                        tf.parameters.cutoff_x = cutoff_x;
                        tf.parameters.cutoff_y = cutoff_y;

                        tf.tile_algorithm_task = [](std::shared_ptr<cvpg::image_gray_8bit> src1, std::shared_ptr<cvpg::image_gray_8bit> /*src2*/, std::shared_ptr<cvpg::histogram<std::size_t> > dst, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, cvpg::imageproc::algorithms::tiling_parameters parameters)
                        {
                            std::vector<std::size_t> h(256);
                            cvpg::imageproc::algorithms::histogram_gray_8bit(src1->data(0).get(), &h, from_x, to_x, from_y, to_y, std::move(parameters));

                            *dst = cvpg::histogram<std::size_t>(std::move(h));
                        };

                        tf.horizontal_merge_task = [](std::shared_ptr<cvpg::histogram<std::size_t> > dst1, std::shared_ptr<cvpg::histogram<std::size_t> > dst2, std::size_t /*from_x*/, std::size_t /*to_x*/, std::size_t /*from_y*/, std::size_t /*to_y*/, cvpg::imageproc::algorithms::tiling_parameters /*parameters*/)
                        {
                            return merge_histograms(dst1, dst2);
                        };

                        tf.vertical_merge_task = [](std::shared_ptr<cvpg::histogram<std::size_t> > dst1, std::shared_ptr<cvpg::histogram<std::size_t> > dst2, std::size_t /*from_x*/, std::size_t /*to_x*/, std::size_t /*from_y*/, std::size_t /*to_y*/, cvpg::imageproc::algorithms::tiling_parameters /*parameters*/)
                        {
                            return merge_histograms(dst1, dst2);
                        };

                        return tf;
                    };

                std::vector<boost::asynchronous::detail::callback_continuation<cvpg::histogram<std::size_t> > > tiling_functors;
                tiling_functors.reserve(3);
                tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_histogram(image_red)));
                tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_histogram(image_green)));
                tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_histogram(image_blue)));

                boost::asynchronous::create_callback_continuation(
                    [result = this->this_task_result()
                    ,context = m_context
                    ,result_id = m_result_id
                    ,image_width
                    ,image_height
                    ,image_padding
                    ,image_red = std::move(image_red)
                    ,image_green = std::move(image_green)
                    ,image_blue = std::move(image_blue)
                    ,mode_str = std::move(mode_str)
                    ,cutoff_x
                    ,cutoff_y
                    ,start = std::move(start)](auto cont_res) mutable
                    {
                        auto stop = std::chrono::system_clock::now();

                        try
                        {
                            const std::size_t threshold_red = cvpg::imageproc::algorithms::otsu_threshold(std::move(cont_res[0].get()));
                            const std::size_t threshold_green = cvpg::imageproc::algorithms::otsu_threshold(std::move(cont_res[1].get()));
                            const std::size_t threshold_blue = cvpg::imageproc::algorithms::otsu_threshold(std::move(cont_res[2].get()));

                            auto create_channel_threshold_image =
                                [cutoff_x, cutoff_y, mode_str](const auto & image, std::size_t threshold)
                                {
                                    auto tf = cvpg::imageproc::algorithms::tiling_functors::image<cvpg::image_gray_8bit>({{ std::move(image) }});
                                    tf.parameters.image_width = image.width();
                                    tf.parameters.image_height = image.height();
                                    tf.parameters.cutoff_x = cutoff_x;
                                    tf.parameters.cutoff_y = cutoff_y;
                                    tf.parameters.signed_integer_numbers.push_back(threshold);

                                    tf.tile_algorithm_task = [mode_str = std::move(mode_str)](std::shared_ptr<cvpg::image_gray_8bit> src1, std::shared_ptr<cvpg::image_gray_8bit> /*src2*/, std::shared_ptr<cvpg::image_gray_8bit> dst, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, cvpg::imageproc::algorithms::tiling_parameters parameters) mutable
                                    {
                                        if (mode_str == "normal")
                                        {
                                            cvpg::imageproc::algorithms::threshold_gray_8bit(src1->data(0).get(), dst->data(0).get(), from_x, to_x, from_y, to_y, std::move(parameters));
                                        }
                                        else if (mode_str == "inverse")
                                        {
                                            cvpg::imageproc::algorithms::threshold_inverse_gray_8bit(src1->data(0).get(), dst->data(0).get(), from_x, to_x, from_y, to_y, std::move(parameters));
                                        }
                                    };

                                    return tf;
                                };

                            std::vector<boost::asynchronous::detail::callback_continuation<cvpg::image_gray_8bit> > tiling_functors;
                            tiling_functors.reserve(3);
                            tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_threshold_image(image_red, threshold_red)));
                            tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_threshold_image(image_green, threshold_green)));
                            tiling_functors.emplace_back(cvpg::imageproc::algorithms::tiling(create_channel_threshold_image(image_blue, threshold_blue)));

                            boost::asynchronous::create_callback_continuation(
                                [result = std::move(result), context = std::move(context), image_width, image_height, image_padding, result_id, start = std::move(start)](auto cont_res) mutable
                                {
                                    auto stop = std::chrono::system_clock::now();

                                    try
                                    {
                                        context->store(result_id,
                                                       cvpg::image_rgb_8bit(
                                                           image_width,
                                                           image_height,
                                                           image_padding,
                                                           {
                                                               cont_res[0].get().data(0),
                                                               cont_res[1].get().data(0),
                                                               cont_res[2].get().data(0)
                                                           }
                                                       ),
                                                       std::chrono::duration_cast<std::chrono::microseconds>(stop - start));

                                        result.set_value(context);
                                    }
                                    catch (...)
                                    {
                                        result.set_exception(std::current_exception());
                                    }
                                },
                                std::move(tiling_functors)
                            );
                        }
                        catch (...)
                        {
                            result.set_exception(std::current_exception());
                        }
                    },
                    std::move(tiling_functors)
                );
            }
            else
            {
                // TODO error handling
            }
        }
        catch (...)
        {
            this->this_task_result().set_exception(std::current_exception());
        }
    }

private:
    std::shared_ptr<cvpg::imageproc::scripting::processing_context> m_context;

    std::uint32_t m_result_id;

    cvpg::imageproc::scripting::detail::parser::item m_item;
};

auto binary_threshold(std::shared_ptr<cvpg::imageproc::scripting::processing_context> context, std::uint32_t result_id, cvpg::imageproc::scripting::detail::parser::item item)
{
    return boost::asynchronous::top_level_callback_continuation<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >(
               binary_threshold_task(context, result_id, std::move(item))
           );
}

} // namespace detail

namespace cvpg::imageproc::scripting::algorithms {

std::string binary_threshold::name() const
{
    return "binary_threshold";
}

std::string binary_threshold::category() const
{
    return "filters/segmentation";
}

std::vector<scripting::item::types> binary_threshold::result() const
{
    return
    {
        scripting::item::types::grayscale_8_bit_image,
        scripting::item::types::rgb_8_bit_image
    };
}

parameter_set binary_threshold::parameters() const
{
    using namespace std::string_literals;

    return parameter_set
           ({
               parameter("image", "input image", "", { scripting::item::types::grayscale_8_bit_image, scripting::item::types::rgb_8_bit_image }),
               parameter("mode", "conversion mode", "", scripting::item::types::characters, { "normal"s, "inverse"s })
           });
}

void binary_threshold::on_parse(std::shared_ptr<detail::parser> parser) const
{
    std::function<std::uint32_t(std::uint32_t, std::string mode)> fct =
        [parser, parameters = this->parameters()](std::uint32_t image_id, std::string mode)
        {
            // find image
            if (!parser)
            {
                throw cvpg::invalid_parameter_exception("invalid parser");
            }

            auto image = parser->find_item(image_id);

            if (image.arguments.empty())
            {
                throw cvpg::invalid_parameter_exception("invalid input ID");
            }

            auto input_type = image.arguments.front().type();

            // check parameters
            if (!(input_type == scripting::item::types::grayscale_8_bit_image || input_type == scripting::item::types::rgb_8_bit_image))
            {
                throw cvpg::invalid_parameter_exception("invalid input");
            }

            if (!parameters.is_valid("mode", mode))
            {
                throw cvpg::invalid_parameter_exception("invalid conversion mode");
            }

            std::uint32_t result_id = 0;

            switch (input_type)
            {
                case scripting::item::types::grayscale_8_bit_image:
                {
                    detail::parser::item result_item
                    {
                        "binary_threshold",
                        {
                            scripting::item(scripting::item::types::grayscale_8_bit_image, image_id),
                            scripting::item(scripting::item::types::characters, mode)
                        }
                    };

                    result_id = parser->register_item(std::move(result_item));

                    break;
                }

                case scripting::item::types::rgb_8_bit_image:
                {
                    detail::parser::item result_item
                    {
                        "binary_threshold",
                        {
                            scripting::item(scripting::item::types::rgb_8_bit_image, image_id),
                            scripting::item(scripting::item::types::characters, mode)
                        }
                    };

                    result_id = parser->register_item(std::move(result_item));

                    break;
                }

                default:
                {
                    // to make the compiler happy ; other input types are not allowed and should be handled above
                    break;
                }
            }

            if (result_id != 0)
            {
                parser->register_link(image_id, result_id);
            }

            return result_id;
        };

    parser->register_specification(name(), std::move(fct));
}

void binary_threshold::on_compile(std::uint32_t item_id, std::shared_ptr<detail::compiler> compiler) const
{
    auto handler =
        detail::handler(
            [result_id = item_id, item = compiler->get_item(item_id)](std::shared_ptr<processing_context> context)
            {
                return ::detail::binary_threshold(context, result_id, std::move(item));
            });

    compiler->register_handler(item_id, name(), std::move(handler));
}

} // namespace cvpg::imageproc::scripting::algorithms


#include "rtree_vectormap_conversion.h"

#include "../types.h"

#include <cslibs_vectormaps/maps/rtree_vector_map.h>
#include <cslibs_vectormaps/maps/segment_rtree_vector_map.h>

#include <boost/version.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/distance.hpp>

#include <tuple>
#include <string>
#include <algorithm>
#include <iostream>

using namespace cslibs_vectormaps;

RtreeVectormapConversion::RtreeVectormapConversion(const RtreeVectormapConversionParameter& parameters) :
    parameters_(parameters)
{
}

bool RtreeVectormapConversion::operator()(const std::vector<polygon_t>& rooms, const std::vector<segment_t>& segments, point_t min, point_t max)
{
    segments_ = segments;
    if (parameters_.type == "R-tree of rooms") {
        // checkbox is ignored here, we always discard outliers when indexing rooms
        index_rooms(rooms);
        if (!index_segments(segments)
        || !drop_outliers()
        || !saveRooms(min, max))
            return false;
    } else if (parameters_.type == "R-tree of segments") {
        if (parameters_.discard_segments) {
            index_rooms(rooms);
            if (!index_segments(segments)
            || !drop_outliers()
            || !saveSegments(min, max))
                return false;
        } else {
            if (!saveSegments(min, max))
                return false;
        }
    } else {
        std::cerr << "[RtreeVectorMapConversion]: Unknown map type '" << parameters_.type << "'!\n";
        return false;
    }
    return true;
}

void RtreeVectormapConversion::index_rooms(const std::vector<polygon_t>& rooms)
{
    namespace bg = boost::geometry;
    namespace bgi = bg::index;

    rooms_ = rooms;

    // We bulk-insert into R-tree, which uses more efficient packing algorithm.
    // All values have to be passed to the constructor at once.
    std::vector<std::tuple<box_t, box_t, std::size_t>> values(rooms.size());
    for (std::size_t i = 0, size = rooms.size(); i < size; i++) {
        box_t envelope = bg::return_envelope<box_t>(rooms[i]);
        box_t envelope_with_leeway = envelope;
        point_t& min = envelope_with_leeway.min_corner();
        min.x(min.x() - parameters_.leeway);
        min.y(min.y() - parameters_.leeway);
        point_t& max = envelope_with_leeway.max_corner();
        max.x(max.x() + parameters_.leeway);
        max.y(max.y() + parameters_.leeway);
        values[i] = std::make_tuple(envelope_with_leeway, envelope, i);
    }

    rtree_.~rtree();
    new (&rtree_) decltype(rtree_)(values);
}

bool RtreeVectormapConversion::index_segments(const std::vector<segment_t>& segments)
{
#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for R-tree vectormap export.\n";
    return false;
#else
    namespace bg = boost::geometry;
    namespace bgi = bg::index;

    segment_indices_.clear();
    segment_indices_.resize(rooms_.size());
    std::size_t i = 0;
    for (const segment_t& segment : segments) {
        for (auto it = rtree_.qbegin(bgi::intersects(segment)), end = rtree_.qend(); it != end; ++it) {
            std::tuple<box_t, box_t, std::size_t> result = *it;
            segment_indices_[std::get<2>(result)].push_back(i);
        }
        i++;
    }
    return true;
#endif
}

// when dropping outliers, we allow for some leeway, so that we are not prone to rounding errors etc.
bool RtreeVectormapConversion::drop_outliers()
{
#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for R-tree vectormap export.\n";
    return false;
#else
    namespace bg = boost::geometry;
    namespace bgi = bg::index;

    // In order to drop non-referenced segments entirely, we first build
    // std::vectors of actual segments for each room and also build a single
    // std::set (to delete duplicates), transform this set into a single
    // std::vector, create a std::map that maps all the segments in this
    // std::vector to their indices and finally create a new std::vector per
    // room that contains only indices into the new big std::vector.

    struct segment_compare {
        // somewhere in the STL, there must be a shortcut for this...
        bool operator()(const segment_t& l, const segment_t& r) const
        {
            return l.first.x() < r.first.x()
                   || l.first.x() == r.first.x()
                      && (l.first.y() < r.first.y()
                          || l.first.y() == r.first.y()
                             && (l.second.x() < r.second.x()
                                 || l.second.x() == r.second.x()
                                    && l.second.y() < r.second.y()));
        }
    };

    std::size_t original_size = segments_.size();
    std::vector<std::vector<segment_t>> rooms_segments(rtree_.size());
    std::set<segment_t, segment_compare> segment_set;

    std::size_t iroom = 0;
#if BOOST_VERSION >= 105900
    for (const std::tuple<box_t, box_t, std::size_t>& room : rtree_) {
#else
    auto dummy_pred = [](const std::tuple<box_t, box_t, std::size_t>&) { return true; };
    for (auto it = rtree_.qbegin(bgi::satisfies(dummy_pred)), end = rtree_.qend(); it != end; ++it) {
        const std::tuple<box_t, box_t, std::size_t>& val = *it;
#endif
        std::vector<segment_t>& room_segments = rooms_segments[iroom++];
        for (std::size_t segment_index : segment_indices_[std::get<2>(val)]) {
            if (bg::distance(segments_[segment_index], rooms_[std::get<2>(val)]) <= parameters_.leeway) {
                room_segments.push_back(segments_[segment_index]);
                segment_set.insert(segments_[segment_index]);
            }
        }
    }

    segments_.assign(segment_set.begin(), segment_set.end());
    std::map<segment_t, std::size_t, segment_compare> segment_index_map;
    for (std::size_t i = 0, s = segments_.size(); i < s; i++) {
        segment_index_map[segments_[i]] = i;
    }

    for (std::size_t i = 0, s = segment_indices_.size(); i < s; i++) {
        std::vector<std::size_t> room_segment_indices(rooms_segments[i].size());
        for (std::size_t j = 0, t = rooms_segments[i].size(); j < t; j++)
            room_segment_indices[j] = segment_index_map[rooms_segments[i][j]];
        segment_indices_[i].swap(room_segment_indices);
    }

    std::cout << "Only " << segments_.size() << " out of " << original_size << " segments were exported as part of the selected rooms.\n";
    return true;
#endif
}

bool RtreeVectormapConversion::saveRooms(point_t min, point_t max)
{
    VectorMap::BoundingBox bounding(min, max);
    RtreeVectorMap map(bounding, true);
    map.insert(segments_, rooms_, segment_indices_);

    return save(map);
}

bool RtreeVectormapConversion::saveSegments(point_t min, point_t max)
{
    VectorMap::BoundingBox bounding(min, max);
    SegmentRtreeVectorMap map(bounding, true);
    map.insert(segments_);

    return save(map);
}

bool RtreeVectormapConversion::save(const VectorMap& map) const
{
    auto ends_with = [](const std::string& value, const std::string& ending) {
        return ending.size() <= value.size()
            && std::equal(ending.rbegin(), ending.rend(), value.rbegin());
    };

    const bool compress = ends_with(parameters_.path, std::string(".gzip"));
    return map.save(parameters_.path, compress);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_STATE_HPP
#define CRYPTO3_HASH_STATE_HPP

#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/framework/features.hpp>

#include <boost/crypto3/detail/type_traits.hpp>

#include <boost/crypto3/hash/accumulators/hash.hpp>

namespace boost {
    namespace crypto3 {
        template<typename Hash, typename = typename std::enable_if<detail::is_hash<Hash>::value>::type>
        using accumulator_set =
            boost::accumulators::accumulator_set<static_digest<Hash::digest_bits>,
                                                 boost::accumulators::features<accumulators::tag::hash<Hash>>,
                                                 std::size_t>;
    }    // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_STREAM_PREPROCESSOR_HPP


#include <random>
#include <set>
#include <vector>
#include <NTL/GF2E.h>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/access.hpp>
#include <chrono>
#include <shared_mutex>
#include <unordered_map>
#include<common.h>
#include<std>

#ifndef __Topology_Element_HPP__
#define __Topology_Element_HPP__

namespace lagrangian {

  /* c++11 pseudorandom number generator*/
  extern std::mt19937_64 rng

  /**
   *
   *  @class TopologyElement
   *  @brief Single context NTL instance of an element within a field.
   *
   *  @todo Implement Contextual-Switch for instance element-wise classification.
   */

  class YosiDB {
  public:
    struct TXRecord {
      std::string hash;
      std::string trunk;
      std::string branch;
      std::chrono::system_clock::time_point timestamp;

      TXRecord() = default;
    };

    nonstd::optional<TXRecord> find(const std::string& hash);

    void put(const TXRecord& tx);
    void removeAgedTxs(uint32_t ageInSeconds);

    std::unordered_map<std::string, TXRecord> getTXsMap() const;

    static YosiDB& instance();

  private:
    mutable std::shared_mutex mutex;
    std::unordered_map<std::string, TXRecord> _txs;

    YosiDB() = default;
    ~YosiDB() = default;

    YosiDB(const YosiDB&) = delete;
    YosiDB& operator=(const YosiDB&) = delete;
    YosiDB(YosiDB&&) = delete;
    YosiDB& operator =(YosiDB&&) = delete;

  };

  class TopologyElement {
  public:
    /**
     * @brief Default constructor
     */
    TopologyElement() :element_(){};

    /**                                                                            @brief conversion from NTL element
     */
    explicit TopologyElement (const ::NTL::GF2E& e) : element_(NTLoFFF(e)){};

    explicit operator const ::NTL::GF2E()const{ return element_};

    TopologyElement(const TopologyElement& src) : element_(src.element_){};

    /**
     * inverse
     */
    inline TopologyElement inverse()const{
      TopologyElement res;
      FFF::Element dummy = element_;
      FFF::Element::c_inv(dummy, res.element_);
      return res;

    }

    inline static TopologyElement sqr(const TopologyElement& x) {
      return x*x;
    }

    /**operators*/
    friend TopologyElement operator+(const TopologyElement&, const TopologyElement&);
    friend TopologyElement& operator+= (TopologyElement&, const TopologyElement&);
    friend TopologyElement operator-(const TopologyElement&, const TopologyElement&);
    friend TopologyElement operator/(const TopologyElement&, const TopologyElement&);
    friend TopologyElement operator*(const TopologyElement&, const TopologyElement&);

}

}


//  negate.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef BOOST_RATIO_MPL_NEGATE_HPP
#define BOOST_RATIO_MPL_NEGATE_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/mpl/negate.hpp>

namespace boost {
namespace mpl {

template<>
struct negate_impl< rational_c_tag >
{
    template< typename R > struct apply
        : ratio_negate<R>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_NEGATE_HPP


/*
 * Copyright (c) 2012, 2013 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include "utils.hpp"

#include <algorithm>
#include <iterator>
#include <random>

#include <boost/filesystem.hpp>
#include <boost/utility/string_ref.hpp>

#include <qi/os.hpp>
#include <qi/path.hpp>

namespace {

  static auto randomSource = [] {
    std::random_device trueRand;
    std::seed_seq seed{ trueRand(), trueRand(), trueRand(), trueRand()
                      , trueRand(), trueRand(), trueRand(), trueRand()
                      };
    return std::default_random_engine{ seed };
  }();

  // Returns a random number inside `[min, max]` (`min` and `max` are included).
  //
  // Precondition: min <= max
  //
  // N is one of {short, int, long, long long,
  //  unsigned short, unsigned int, unsigned long, unsigned long long}
  template<typename N>
  N randomNumber(N min, N max)
  {
    std::uniform_int_distribution<N> dice(min, max);
    return dice(randomSource);
  }

  // Returns a random character inside `[0-9a-zA-Z]`.
  char randomAlphaNum()
  {
    static const boost::string_ref alphanums
      = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";

    const auto randomIdx = randomNumber<int>(0, alphanums.size() - 1);
    return alphanums[randomIdx];
  }

}

std::string randomstr(std::string::size_type sz)
{
  std::string s;
  s.reserve(sz);
  generate_n(std::back_inserter(s), sz, randomAlphaNum);
  return s;
}

std::wstring wrandomstr(std::wstring::size_type sz)
{
  std::wstring s;
  s.reserve(sz);
  generate_n(std::back_inserter(s), sz, randomAlphaNum);
  return s;
}

std::string fsconcat(const std::vector<std::string>& parts)
{
  boost::filesystem::path p;
  for (const auto& part : parts)
  {
    if (!part.empty())
    {
      p.append(part, qi::unicodeFacet());
    }
  }
  return p.make_preferred().string(qi::unicodeFacet());
}