/***************************************************************************
                          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();

}


#ifndef ODESOLVER_HPP_INCLUDED
#define ODESOLVER_HPP_INCLUDED

#include <petscts.h>
#include <string>
#include <cmath>
#include <vector>
#include <algorithm>
#include <assert.h>
#include <boost/circular_buffer.hpp>
#include "integratorContextEx.hpp"
#include "genFuncs.hpp"

using namespace std;
/*
 * Provides a set of algorithms to solve a system of ODEs of
 * the form y' = f(t,y) using EXPLICIT time stepping.
 *
 * SOLVER TYPE      ALGORITHM
 *  FEuler        forward Euler
 *  RK32          explicit Runge-Kutta (2,3)
 *  RK43          explicit Runge-Kutta (3,4)
 *
 * To obtain solutions at user-specified times, use FEuler and call setStepSize
 * in the routine f(t,y).
 *
 * y is represented as an array of one or more Vecs (PETSc data type).
 *
 * At minimum, the user must specify:
 *     QUANTITY               FUNCTION
 *  max number of steps      constructor
 *  solver type              constructor
 *  initial conditions       setInitialConds     Note: this array will be modified during integration
 *  initial step size        constructor
 *  step size alg            constructor (this is only used by the adaptive time-stepping algorithm)
 *  f(t,y)                   object passed to integrate must have member function d_dt(PetscScalar, PetscScalar*,PetscScalar*)
 *  final time               constructor and setTimeRange
 *  timeMonitor              object passed to integrate must have member function timeMonitor
 *
 *
 * Optional fields that can also be specified:
 *     QUANTITY               FUNCTION
 *  tolerance                setTolerance
 *  maximum step size        setStepSize
 *  minimum step size        setTimeStepBounds
 *  initial step size        setTimeStepBounds
 *
 * Once the odeSolver context is set, call integrate() to perform
 * the integration.
 *
 * y(t = final_time) is stored in the initial conditions array. Summary output
 * information is provided by viewSolver. Users can obtain information at
 * each time step within a user-defined monitor function.
 *
 */

class OdeSolver
{
public:

  PetscReal          _initT,_finalT,_currT,_deltaT;
  PetscReal          _newDeltaT; // stores future deltaT for access by outside classes, primarily for checkpointing
  PetscInt           _maxNumSteps,_stepCount;
  map<string,Vec>    _var,_dvar; // integration variable and rate
  vector<string>     _errInds; // which keys of _var to use for error control
  vector<double>     _scale; // scale factor for entries in _errInds
  double             _runTime;
  string             _controlType;
  string             _normType;

  // for PID error control
  boost::circular_buffer<double> _errA;
  map<string,Vec> _y2,_y3,_y4;

  OdeSolver(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  virtual ~OdeSolver() {};

  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setInitialStepCount(const PetscReal stepCount);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setToleranceType(const string normType); // type of norm used for error control

  virtual PetscErrorCode setTolerance(const PetscReal tol) = 0;
  virtual PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT) = 0;
  virtual PetscErrorCode setInitialConds(map<string,Vec>& var){return 1;};
  virtual PetscErrorCode setErrInds(vector<string>& errInds) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) = 0;
  virtual PetscErrorCode view() = 0;
  virtual PetscErrorCode integrate(IntegratorContextEx *obj) = 0;
};

// FEuler is a derived class from OdeSolver
class FEuler : public OdeSolver
{
public:
  FEuler(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~FEuler();
  PetscErrorCode view();

  PetscErrorCode setTolerance(const PetscReal tol){return 0;};
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT){ return 0;};
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds) {return 0;};
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) {return 0;};
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

// Runge-kutta time-stepping, 3rd-order
// Based on algorithm from Hairer et al.
class RK32 : public OdeSolver
{
public:

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _totTol; // total tolerance, might be atol, or rtol, or a combination of both
  PetscReal   _kappa,_ord; // safety factor in step size determinance, order of accuracy of method
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;

  PetscReal   _totErr;

  map<string,Vec> _k1,_f1,_k2,_f2,_y2,_y3;

  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();

  // constructor and destructor
  RK32(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK32();

  // member functions of this class
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

// Based on "ARK4(3)6L[2]SA-ERK" algorithm from Kennedy and Carpenter (2003):
// "Additive Runge-Kutta schemes for convection-diffusion-reaction equations"
// Runge-Kutta time-stepping, 4th order
// Note: Has matching IMEX equivalent
class RK43 : public OdeSolver
{
public:

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _atol,_rtol;
  PetscReal   _totTol;
  PetscReal   _kappa,_ord;
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;
  PetscReal   _totErr;

  map<string,Vec> _k1,_k2,_k3,_k4,_k5,_k6,_y3,_y4;
  map<string,Vec> _f1,_f2,_f3,_f4,_f5,_f6;

  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();

  // constructor and destructor
  RK43(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK43();

  // various member functions
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

#endif


/* -*- c++ -*- */
/*
 * Copyright 2014 Communications Engineering Lab, KIT.
 *
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <boost/circular_buffer.hpp>
#include "estimator_rcs_impl.h"
#include <numeric>

namespace gr {
  namespace radar {

    estimator_rcs::sptr
    estimator_rcs::make(int num_mean, float center_freq, float antenna_gain_tx, float antenna_gain_rx, float usrp_gain_rx, float power_tx, float corr_factor, float exponent)
    {
      return gnuradio::get_initial_sptr
        (new estimator_rcs_impl(num_mean, center_freq, antenna_gain_tx, antenna_gain_rx, usrp_gain_rx, power_tx, corr_factor, exponent));
    }

    /*
     * The private constructor
     */
    estimator_rcs_impl::estimator_rcs_impl(int num_mean, float center_freq, float antenna_gain_tx, float antenna_gain_rx, float usrp_gain_rx, float power_tx, float corr_factor, float exponent)
      : gr::block("estimator_rcs",
              gr::io_signature::make(0,0,0),
              gr::io_signature::make(0,0,0))
    {
        d_num_mean = num_mean;
        d_center_freq = center_freq;
        d_antenna_gain_tx = antenna_gain_tx;
        d_antenna_gain_rx = antenna_gain_rx;
        d_usrp_gain_rx = usrp_gain_rx;
        d_power_tx = power_tx; // needs to be calibrated for every usage
        d_corr_factor = corr_factor;
        d_exponent = exponent;

        d_rcs_vals.resize(d_num_mean);

        // Register input message port
        d_port_id_in = pmt::mp("Msg in");
        message_port_register_in(d_port_id_in);
        set_msg_handler(d_port_id_in, boost::bind(&estimator_rcs_impl::handle_msg, this, _1));

        // Register output message port
        d_port_id_out = pmt::mp("Msg out");
        message_port_register_out(d_port_id_out);

        d_loop_counter = 0;

        // constant factors in radar equation
        d_antenna_gain_abs_rx = pow(10, d_antenna_gain_rx/10);
        d_antenna_gain_abs_tx = pow(10, d_antenna_gain_tx/10);
        d_lambda = c_light/d_center_freq;
        d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));

    }

    /*
     * Our virtual destructor.
     */
    estimator_rcs_impl::~estimator_rcs_impl()
    {
    }

    void
    estimator_rcs_impl::set_num_mean(int val){
      d_num_mean = val;
      d_rcs_vals.clear();
      d_rcs_vals.resize(d_num_mean);
      d_loop_counter = 0;
    }

    void
    estimator_rcs_impl::set_center_freq(float val){
      d_center_freq = val;
      d_lambda = c_light/d_center_freq;
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_antenna_gain_tx(float val){
      d_antenna_gain_tx = val;
      d_antenna_gain_abs_tx = pow(10, d_antenna_gain_tx/10);
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_antenna_gain_rx(float val){
      d_antenna_gain_rx = val;
      d_antenna_gain_abs_rx = pow(10, d_antenna_gain_rx/10);
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_usrp_gain_rx(float val){
      d_usrp_gain_rx = val;
    }

    void
    estimator_rcs_impl::set_power_tx(float val){
      d_power_tx = val;
    }

    void
    estimator_rcs_impl::set_corr_factor(float val){
      d_corr_factor = val;
    }

    float
    estimator_rcs_impl::calculate_vector_mean(boost::circular_buffer<float>* rcs_vals) {
      float sum_of_elems = 0;
      for(int k=0; k<rcs_vals->size(); k++){
        sum_of_elems += (*rcs_vals)[k];
      }
      return sum_of_elems/rcs_vals->size();
    }

    float
    estimator_rcs_impl::calculate_rcs() {
      // catch errors
			if(d_range.size() == 0) throw std::runtime_error("range vector has size zero");
			if(d_power.size() == 0) throw std::runtime_error("power vector has size zero");

      // regard usrp gain and signal path
			float power_rx = pow(d_power[0], d_exponent) / d_power_tx / pow(10, d_usrp_gain_rx/10);

			float fak = d_fak * pow(d_range[0], 4);

      // debug output
      // std::cout << "PowerTx: " << d_power_tx << std::endl;
      // std::cout << "PowerRx: " << power_rx << std::endl;
      // std::cout << "Lambda: " << d_lambda << std::endl;
      // std::cout << "GainRx: " << d_antenna_gain_rx << std::endl;
      // std::cout << "GainTx: " << d_antenna_gain_tx << std::endl;
      // std::cout << "fak: " << fak << std::endl;

			return power_rx/d_power_tx * fak * d_corr_factor;
    }

    void
    estimator_rcs_impl::handle_msg(pmt::pmt_t msg)
    {
        // Read msg from peak detector
        d_msg_hold.clear();
        pmt::pmt_t msg_part;
        bool found_range = false;
        bool found_power = false;
        for(int k=0; k<pmt::length(msg); k++){
          msg_part = pmt::nth(k,msg);
          if(pmt::symbol_to_string(pmt::nth(0,msg_part))=="range"){
            d_prange = pmt::nth(1,msg_part);
            d_msg_hold.push_back(msg_part);
            found_range = true;
          }
          else if(pmt::symbol_to_string(pmt::nth(0,msg_part))=="power"){
            d_ppower = pmt::nth(1,msg_part);
            found_power = true;
          }
          else{
            d_msg_hold.push_back(msg_part);
          }
        }

        if(not(found_range&&found_power)) throw std::runtime_error("range or power identifier (symbol) not found");

        d_range = pmt::f32vector_elements(d_prange);
        d_power = pmt::f32vector_elements(d_ppower);
        d_rcs.clear();

        if(d_range.size()!=d_power.size()) throw std::runtime_error("range and power vectors do not have same size");

        // Calculate RCS
        float rcs_mean = 0.0;
        if(d_range.size() == 0 && d_power.size() == 0){
          //std::cout << "ERROR: No target detected for RCS calculation" << std::endl;
        }
        else {
          d_rcs_vals.push_back(calculate_rcs());
        }
        //std::cout << "RCS_TEMP: " << calculate_rcs() << std::endl;

        if(d_loop_counter+1 >= d_num_mean) {
            rcs_mean = calculate_vector_mean(&d_rcs_vals);
        }
        else {
            d_loop_counter++;
        }

        for(int k=0; k<d_range.size(); k++){
            d_rcs.push_back(rcs_mean);
        }

        // Push pmt to output msg port
        d_rcs_key = pmt::string_to_symbol("rcs"); // identifier velocity
        d_rcs_value = pmt::init_f32vector(d_rcs.size(), d_rcs); // vector to pmt
        d_rcs_pack = pmt::list2(d_rcs_key, d_rcs_value); // make list for velocity information

        d_value = pmt::list1(d_rcs_pack);
        for(int k=0; k<d_msg_hold.size(); k++){
            d_value = pmt::list_add(d_value, d_msg_hold[k]);
        }

        message_port_pub(d_port_id_out,d_value); // publish message
    }

  } /* namespace radar */
} /* namespace gr */


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/indexes/iborindex.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>

#include <boost/bind.hpp>

namespace QuantExt {

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<LinearGaussMarkovModel>& model,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->parametrization()),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<CrossAssetModel>& model, const Size ccy,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->irlgm1f(ccy)),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<IrLgm1fParametrization> irlgm1f,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(irlgm1f),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(c_);
}

void AnalyticLgmSwaptionEngine::calculate() const {

    QL_REQUIRE(arguments_.settlementType == Settlement::Physical, "cash-settled swaptions are not supported ...");

    Date reference = p_->termStructure()->referenceDate();

    Date expiry = arguments_.exercise->dates().back();

    if (expiry <= reference) {
        // swaption is expired, possibly generated swap is not
        // valued by this engine, so we set the npv to zero
        results_.value = 0.0;
        return;
    }

    VanillaSwap swap = *arguments_.swap;
    Option::Type type = arguments_.type == VanillaSwap::Payer ? Option::Call : Option::Put;
    Schedule fixedSchedule = swap.fixedSchedule();
    Schedule floatSchedule = swap.floatingSchedule();

    j1_ = std::lower_bound(fixedSchedule.dates().begin(), fixedSchedule.dates().end(), expiry) -
          fixedSchedule.dates().begin();
    k1_ = std::lower_bound(floatSchedule.dates().begin(), floatSchedule.dates().end(), expiry) -
          floatSchedule.dates().begin();

    // compute S_i, i.e. equivalent fixed rate spreads compensating for
    // a) a possibly non-zero float spread and
    // b) a spread between the ibor indices forwarding curve and the
    //     discounting curve
    // here, we do not work with a spread corrections directly, but
    // with this multiplied by the nominal and accrual basis,
    // so S_i is really an amount correction.

    S_.resize(arguments_.fixedCoupons.size() - j1_);
    for (Size i = 0; i < S_.size(); ++i) {
        S_[i] = 0.0;
    }
    S_m1 = 0.0;
    Size ratio = static_cast<Size>(
        static_cast<Real>(arguments_.floatingCoupons.size()) / static_cast<Real>(arguments_.fixedCoupons.size()) + 0.5);
    QL_REQUIRE(ratio >= 1, "floating leg's payment frequency must be equal or "
                           "higher than fixed leg's payment frequency in "
                           "analytic lgm swaption engine");

    Size k = k1_;
    boost::shared_ptr<IborIndex> flatIbor = swap.iborIndex()->clone(c_);
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        Real sum1 = 0.0, sum2 = 0.0;
        for (Size rr = 0; rr < ratio && k < arguments_.floatingCoupons.size(); ++rr, ++k) {
            Real amount = arguments_.floatingCoupons[k];
            Real lambda1 = 0.0, lambda2 = 1.0;
            if (floatSpreadMapping_ == proRata) {
                // we do not use the exact pay dates but the ratio to determine
                // the distance to the adjacent payment dates
                lambda2 = static_cast<Real>(rr + 1) / static_cast<Real>(ratio);
                lambda1 = 1.0 - lambda2;
            }
            if (amount != Null<Real>()) {
                Real flatAmount = flatIbor->fixing(arguments_.floatingFixingDates[k]) *
                                  arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                Real correction = (amount - flatAmount) * c_->discount(arguments_.floatingPayDates[k]);
                sum1 += lambda1 * correction;
                sum2 += lambda2 * correction;
            } else {
                // if no amount is given, we do not need a spread correction
                // due to different forward / discounting curves since then
                // no curve is attached to the swap's ibor index and so we
                // assume a one curve setup;
                // but we can still have a float spread that has to be converted
                // into a fixed leg's payment
                Real correction = arguments_.nominal * arguments_.floatingSpreads[k] *
                                  arguments_.floatingAccrualTimes[k] * c_->discount(arguments_.floatingPayDates[k]);
                sum1 += lambda1 * correction;
                sum2 += lambda2 * correction;
            }
        }
        if (j > j1_) {
            S_[j - j1_ - 1] += sum1 / c_->discount(arguments_.fixedPayDates[j - 1]);
        } else {
            S_m1 += sum1 / c_->discount(arguments_.floatingResetDates[k1_]);
        }
        S_[j - j1_] += sum2 / c_->discount(arguments_.fixedPayDates[j]);
    }

    Real w = type == Option::Call ? -1.0 : 1.0;

    // it is a requirement that H' does not change its sign,
    // with u = -1.0 we handle the case H' < 0
    Real u = p_->Hprime(0.0) > 0.0 ? 1.0 : -1.0;

    // do the actual pricing

    zetaex_ = p_->zeta(p_->termStructure()->timeFromReference(expiry));
    H0_ = p_->H(p_->termStructure()->timeFromReference(arguments_.floatingResetDates[k1_]));
    D0_ = c_->discount(arguments_.floatingResetDates[k1_]);
    Hj_.resize(arguments_.fixedCoupons.size() - j1_);
    Dj_.resize(arguments_.fixedCoupons.size() - j1_);
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        Hj_[j - j1_] = p_->H(p_->termStructure()->timeFromReference(arguments_.fixedPayDates[j]));
        Dj_[j - j1_] = c_->discount(arguments_.fixedPayDates[j - j1_]);
    }

    Brent b;
    Real yStar;
    try {
        yStar = b.solve(boost::bind(&AnalyticLgmSwaptionEngine::yStarHelper, this, _1), 1.0E-6, 0.0, 0.01);
    } catch (const std::exception& e) {
        QL_FAIL("AnalyticLgmSwaptionEngine, failed to compute yStar, " << e.what());
    }

    CumulativeNormalDistribution N;
    Real sqrt_zetaex = std::sqrt(zetaex_);
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += w * (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               N(u * w * (yStar + (Hj_[j - j1_] - H0_) * zetaex_) / sqrt_zetaex);
    }
    sum += -w * S_m1 * D0_ * N(u * w * yStar / sqrt_zetaex);
    sum += w * (arguments_.nominal * Dj_.back() * N(u * w * (yStar + (Hj_.back() - H0_) * zetaex_) / sqrt_zetaex) -
                arguments_.nominal * D0_ * N(u * w * yStar / sqrt_zetaex));
    results_.value = sum;

    results_.additionalResults["fixedAmountCorrectionSettlement"] = S_m1;
    results_.additionalResults["fixedAmountCorrections"] = S_;

} // calculate

Real AnalyticLgmSwaptionEngine::yStarHelper(const Real y) const {
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               std::exp(-(Hj_[j - j1_] - H0_) * y - 0.5 * (Hj_[j - j1_] - H0_) * (Hj_[j - j1_] - H0_) * zetaex_);
    }
    sum += -S_m1 * D0_;
    sum += Dj_.back() * arguments_.nominal *
           std::exp(-(Hj_.back() - H0_) * y - 0.5 * (Hj_.back() - H0_) * (Hj_.back() - H0_) * zetaex_);
    sum -= D0_ * arguments_.nominal;
    return sum;
}

} // namespace QuantExt


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to train a CNN based object detector using dlib's
    loss_mmod loss layer.  This loss layer implements the Max-Margin Object
    Detection loss as described in the paper:
        Max-Margin Object Detection by Davis E. King (http://arxiv.org/abs/1502.00046).
    This is the same loss used by the popular SVM+HOG object detector in dlib
    (see fhog_object_detector_ex.cpp) except here we replace the HOG features
    with a CNN and train the entire detector end-to-end.  This allows us to make
    much more powerful detectors.

    It would be a good idea to become familiar with dlib's DNN tooling before reading this
    example.  So you should read dnn_introduction_ex.cpp and dnn_introduction2_ex.cpp
    before reading this example program.  You should also read the introductory DNN+MMOD
    example dnn_mmod_ex.cpp as well before proceeding.

    This example is essentially a more complex version of dnn_mmod_ex.cpp.  In it we train
    a detector that finds the rear ends of motor vehicles.  I will also discuss some
    aspects of data preparation useful when training this kind of detector.

*/

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>

using namespace std;
using namespace dlib;

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;
template <typename SUBNET> using downsampler  = relu<bn_con<con5d<32, relu<bn_con<con5d<32, relu<bn_con<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<bn_con<con5<55,SUBNET>>>;
using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

// ----------------------------------------------------------------------------------------

int ignore_overlapped_boxes(
    std::vector<mmod_rect>& boxes,
    const test_box_overlap& overlaps
)
/*!
    ensures
        - Whenever two rectangles in boxes overlap, according to overlaps(), we set the
          smallest box to ignore.
        - returns the number of newly ignored boxes.
!*/
{
    int num_ignored = 0;
    for (size_t i = 0; i < boxes.size(); ++i)
    {
        if (boxes[i].ignore)
            continue;
        for (size_t j = i+1; j < boxes.size(); ++j)
        {
            if (boxes[j].ignore)
                continue;
            if (overlaps(boxes[i], boxes[j]))
            {
                ++num_ignored;
                if(boxes[i].rect.area() < boxes[j].rect.area())
                    boxes[i].ignore = true;
                else
                    boxes[j].ignore = true;
            }
        }
    }
    return num_ignored;
}

// ----------------------------------------------------------------------------------------

int main(int argc, char** argv) try
{
    if (argc != 2)
    {
        cout << "Give the path to a folder containing training.xml and testing.xml files." << endl;
        cout << "This example program is specifically designed to run on the dlib vehicle " << endl;
        cout << "detection dataset, which is available at this URL: " << endl;
        cout << "   http://dlib.net/files/data/dlib_rear_end_vehicles_v1.tar" << endl;
        cout << endl;
        cout << "So download that dataset, extract it somewhere, and then run this program" << endl;
        cout << "with the dlib_rear_end_vehicles folder as an argument.  E.g. if you extract" << endl;
        cout << "the dataset to the current folder then you should run this example program" << endl;
        cout << "by typing: " << endl;
        cout << "   ./dnn_mmod_train_find_cars_ex dlib_rear_end_vehicles" << endl;
        cout << endl;
        cout << "It takes about a day to finish if run on a high end GPU like a 1080ti." << endl;
        cout << endl;
        return 0;
    }
    const std::string data_directory = argv[1];

    std::vector<matrix<rgb_pixel>> images_train, images_test;
    std::vector<std::vector<mmod_rect>> boxes_train, boxes_test;
    load_image_dataset(images_train, boxes_train, data_directory+"/training.xml");
    load_image_dataset(images_test,  boxes_test,  data_directory+"/testing.xml");

    // When I was creating the dlib vehicle detection dataset I had to label all the cars
    // in each image.  MMOD requires all cars to be labeled, since any unlabeled part of an
    // image is implicitly assumed to be not a car, and the algorithm will use it as
    // negative training data.  So every car must be labeled, either with a normal
    // rectangle or an "ignore" rectangle that tells MMOD to simply ignore it (i.e. neither
    // treat it as a thing to detect nor as negative training data).
    //
    // In our present case, many images contain very tiny cars in the distance, ones that
    // are essentially just dark smudges.  It's not reasonable to expect the CNN
    // architecture we defined to detect such vehicles.  However, I erred on the side of
    // having more complete annotations when creating the dataset.  So when I labeled these
    // images I labeled many of these really difficult cases as vehicles to detect.
    //
    // So the first thing we are going to do is clean up our dataset a little bit.  In
    // particular, we are going to mark boxes smaller than 35*35 pixels as ignore since
    // only really small and blurry cars appear at those sizes.  We will also mark boxes
    // that are heavily overlapped by another box as ignore.  We do this because we want to
    // allow for stronger non-maximum suppression logic in the learned detector, since that
    // will help make it easier to learn a good detector.
    //
    // To explain this non-max suppression idea further it's important to understand how
    // the detector works.  Essentially, sliding window detectors scan all image locations
    // and ask "is there a care here?".  If there really is a car in a specific location in
    // an image then usually many slightly different sliding window locations will produce
    // high detection scores, indicating that there is a car at those locations.  If we
    // just stopped there then each car would produce multiple detections.  But that isn't
    // what we want.  We want each car to produce just one detection.  So it's common for
    // detectors to include "non-maximum suppression" logic which simply takes the
    // strongest detection and then deletes all detections "close to" the strongest.  This
    // is a simple post-processing step that can eliminate duplicate detections.  However,
    // we have to define what "close to" means.  We can do this by looking at your training
    // data and checking how close the closest target boxes are to each other, and then
    // picking a "close to" measure that doesn't suppress those target boxes but is
    // otherwise as tight as possible.  This is exactly what the mmod_options object does
    // by default.
    //
    // Importantly, this means that if your training dataset contains an image with two
    // target boxes that really overlap a whole lot, then the non-maximum suppression
    // "close to" measure will be configured to allow detections to really overlap a whole
    // lot.  On the other hand, if your dataset didn't contain any overlapped boxes at all,
    // then the non-max suppression logic would be configured to filter out any boxes that
    // overlapped at all, and thus would be performing a much stronger non-max suppression.
    //
    // Why does this matter?  Well, remember that we want to avoid duplicate detections.
    // If non-max suppression just kills everything in a really wide area around a car then
    // the CNN doesn't really need to learn anything about avoiding duplicate detections.
    // However, if non-max suppression only suppresses a tiny area around each detection
    // then the CNN will need to learn to output small detection scores for those areas of
    // the image not suppressed.  The smaller the non-max suppression region the more the
    // CNN has to learn and the more difficult the learning problem will become.  This is
    // why we remove highly overlapped objects from the training dataset.  That is, we do
    // it so the non-max suppression logic will be able to be reasonably effective.  Here
    // we are ensuring that any boxes that are entirely contained by another are
    // suppressed.  We also ensure that boxes with an intersection over union of 0.5 or
    // greater are suppressed.  This will improve the resulting detector since it will be
    // able to use more aggressive non-max suppression settings.

    int num_overlapped_ignored_test = 0;
    for (auto& v : boxes_test)
        num_overlapped_ignored_test += ignore_overlapped_boxes(v, test_box_overlap(0.50, 0.95));

    int num_overlapped_ignored = 0;
    int num_additional_ignored = 0;
    for (auto& v : boxes_train)
    {
        num_overlapped_ignored += ignore_overlapped_boxes(v, test_box_overlap(0.50, 0.95));
        for (auto& bb : v)
        {
            if (bb.rect.width() < 35 && bb.rect.height() < 35)
            {
                if (!bb.ignore)
                {
                    bb.ignore = true;
                    ++num_additional_ignored;
                }
            }

            // The dlib vehicle detection dataset doesn't contain any detections with
            // really extreme aspect ratios.  However, some datasets do, often because of
            // bad labeling.  So it's a good idea to check for that and either eliminate
            // those boxes or set them to ignore.  Although, this depends on your
            // application.
            //
            // For instance, if your dataset has boxes with an aspect ratio
            // of 10 then you should think about what that means for the network
            // architecture.  Does the receptive field even cover the entirety of the box
            // in those cases?  Do you care about these boxes?  Are they labeling errors?
            // I find that many people will download some dataset from the internet and
            // just take it as given.  They run it through some training algorithm and take
            // the dataset as unchallengeable truth.  But many datasets are full of
            // labeling errors.  There are also a lot of datasets that aren't full of
            // errors, but are annotated in a sloppy and inconsistent way.  Fixing those
            // errors and inconsistencies can often greatly improve models trained from
            // such data.  It's almost always worth the time to try and improve your
            // training dataset.
            //
            // In any case, my point is that there are other types of dataset cleaning you
            // could put here.  What exactly you need depends on your application.  But you
            // should carefully consider it and not take your dataset as a given.  The work
            // of creating a good detector is largely about creating a high quality
            // training dataset.
        }
    }

    // When modifying a dataset like this, it's a really good idea to print a log of how
    // many boxes you ignored.  It's easy to accidentally ignore a huge block of data, so
    // you should always look and see that things are doing what you expect.
    cout << "num_overlapped_ignored: "<< num_overlapped_ignored << endl;
    cout << "num_additional_ignored: "<< num_additional_ignored << endl;
    cout << "num_overlapped_ignored_test: "<< num_overlapped_ignored_test << endl;

    cout << "num training images: " << images_train.size() << endl;
    cout << "num testing images: " << images_test.size() << endl;

    // Our vehicle detection dataset has basically 3 different types of boxes.  Square
    // boxes, tall and skinny boxes (e.g. semi trucks), and short and wide boxes (e.g.
    // sedans).  Here we are telling the MMOD algorithm that a vehicle is recognizable as
    // long as the longest box side is at least 70 pixels long and the shortest box side is
    // at least 30 pixels long.  mmod_options will use these parameters to decide how large
    // each of the sliding windows needs to be so as to be able to detect all the vehicles.
    // Since our dataset has basically these 3 different aspect ratios, it will decide to
    // use 3 different sliding windows.  This means the final con layer in the network will
    // have 3 filters, one for each of these aspect ratios.
    //
    // Another thing to consider when setting the sliding window size is the "stride" of
    // your network.  The network we defined above downsamples the image by a factor of 8x
    // in the first few layers.  So when the sliding windows are scanning the image, they
    // are stepping over it with a stride of 8 pixels.  If you set the sliding window size
    // too small then the stride will become an issue.  For instance, if you set the
    // sliding window size to 4 pixels, then it means a 4x4 window will be moved by 8
    // pixels at a time when scanning. This is obviously a problem since 75% of the image
    // won't even be visited by the sliding window.  So you need to set the window size to
    // be big enough relative to the stride of your network.  In our case, the windows are
    // at least 30 pixels in length, so being moved by 8 pixel steps is fine.
    mmod_options options(boxes_train, 70, 30);

    // This setting is very important and dataset specific.  The vehicle detection dataset
    // contains boxes that are marked as "ignore", as we discussed above.  Some of them are
    // ignored because we set ignore to true in the above code.  However, the xml files
    // also contained a lot of ignore boxes.  Some of them are large boxes that encompass
    // large parts of an image and the intention is to have everything inside those boxes
    // be ignored.  Therefore, we need to tell the MMOD algorithm to do that, which we do
    // by setting options.overlaps_ignore appropriately.
    //
    // But first, we need to understand exactly what this option does.  The MMOD loss
    // is essentially counting the number of false alarms + missed detections produced by
    // the detector for each image.  During training, the code is running the detector on
    // each image in a mini-batch and looking at its output and counting the number of
    // mistakes.  The optimizer tries to find parameters settings that minimize the number
    // of detector mistakes.
    //
    // This overlaps_ignore option allows you to tell the loss that some outputs from the
    // detector should be totally ignored, as if they never happened.  In particular, if a
    // detection overlaps a box in the training data with ignore==true then that detection
    // is ignored.  This overlap is determined by calling
    // options.overlaps_ignore(the_detection, the_ignored_training_box).  If it returns
    // true then that detection is ignored.
    //
    // You should read the documentation for test_box_overlap, the class type for
    // overlaps_ignore for full details.  However, the gist is that the default behavior is
    // to only consider boxes as overlapping if their intersection over union is > 0.5.
    // However, the dlib vehicle detection dataset contains large boxes that are meant to
    // mask out large areas of an image.  So intersection over union isn't an appropriate
    // way to measure "overlaps with box" in this case.  We want any box that is contained
    // inside one of these big regions to be ignored, even if the detection box is really
    // small.  So we set overlaps_ignore to behave that way with this line.
    options.overlaps_ignore = test_box_overlap(0.5, 0.95);

    net_type net(options);

    // The final layer of the network must be a con layer that contains
    // options.detector_windows.size() filters.  This is because these final filters are
    // what perform the final "sliding window" detection in the network.  For the dlib
    // vehicle dataset, there will be 3 sliding window detectors, so we will be setting
    // num_filters to 3 here.
    net.subnet().layer_details().set_num_filters(options.detector_windows.size());

    dnn_trainer<net_type> trainer(net,sgd(0.0001,0.9));
    trainer.set_learning_rate(0.1);
    trainer.be_verbose();

    // While training, we are going to use early stopping.  That is, we will be checking
    // how good the detector is performing on our test data and when it stops getting
    // better on the test data we will drop the learning rate.  We will keep doing that
    // until the learning rate is less than 1e-4.   These two settings tell the trainer to
    // do that.  Essentially, we are setting the first argument to infinity, and only the
    // test iterations without progress threshold will matter.  In particular, it says that
    // once we observe 1000 testing mini-batches where the test loss clearly isn't
    // decreasing we will lower the learning rate.
    trainer.set_iterations_without_progress_threshold(50000);
    trainer.set_test_iterations_without_progress_threshold(1000);

    const string sync_filename = "mmod_cars_sync";
    trainer.set_synchronization_file(sync_filename, std::chrono::minutes(5));

    std::vector<matrix<rgb_pixel>> mini_batch_samples;
    std::vector<std::vector<mmod_rect>> mini_batch_labels;
    random_cropper cropper;
    cropper.set_seed(time(0));
    cropper.set_chip_dims(350, 350);
    // Usually you want to give the cropper whatever min sizes you passed to the
    // mmod_options constructor, or very slightly smaller sizes, which is what we do here.
    cropper.set_min_object_size(69,28);
    cropper.set_max_rotation_degrees(2);
    dlib::rand rnd;

    // Log the training parameters to the console
    cout << trainer << cropper << endl;

    int cnt = 1;
    // Run the trainer until the learning rate gets small.
    while(trainer.get_learning_rate() >= 1e-4)
    {
        // Every 30 mini-batches we do a testing mini-batch.
        if (cnt%30 != 0 || images_test.size() == 0)
        {
            cropper(87, images_train, boxes_train, mini_batch_samples, mini_batch_labels);
            // We can also randomly jitter the colors and that often helps a detector
            // generalize better to new images.
            for (auto&& img : mini_batch_samples)
                disturb_colors(img, rnd);

            // It's a good idea to, at least once, put code here that displays the images
            // and boxes the random cropper is generating.  You should look at them and
            // think about if the output makes sense for your problem.  Most of the time
            // it will be fine, but sometimes you will realize that the pattern of cropping
            // isn't really appropriate for your problem and you will need to make some
            // change to how the mini-batches are being generated.  Maybe you will tweak
            // some of the cropper's settings, or write your own entirely separate code to
            // create mini-batches.  But either way, if you don't look you will never know.
            // An easy way to do this is to create a dlib::image_window to display the
            // images and boxes.

            trainer.train_one_step(mini_batch_samples, mini_batch_labels);
        }
        else
        {
            cropper(87, images_test, boxes_test, mini_batch_samples, mini_batch_labels);
            // We can also randomly jitter the colors and that often helps a detector
            // generalize better to new images.
            for (auto&& img : mini_batch_samples)
                disturb_colors(img, rnd);

            trainer.test_one_step(mini_batch_samples, mini_batch_labels);
        }
        ++cnt;
    }
    // wait for training threads to stop
    trainer.get_net();
    cout << "done training" << endl;

    // Save the network to disk
    net.clean();
    serialize("mmod_rear_end_vehicle_detector.dat") << net;

    // It's a really good idea to print the training parameters.  This is because you will
    // invariably be running multiple rounds of training and should be logging the output
    // to a file.  This print statement will include many of the training parameters in
    // your log.
    cout << trainer << cropper << endl;

    cout << "\nsync_filename: " << sync_filename << endl;
    cout << "num training images: "<< images_train.size() << endl;
    cout << "training results: " << test_object_detection_function(net, images_train, boxes_train, test_box_overlap(), 0, options.overlaps_ignore);
    // Upsampling the data will allow the detector to find smaller cars.  Recall that
    // we configured it to use a sliding window nominally 70 pixels in size.  So upsampling
    // here will let it find things nominally 35 pixels in size.  Although we include a
    // limit of 1800*1800 here which means "don't upsample an image if it's already larger
    // than 1800*1800".  We do this so we don't run out of RAM, which is a concern because
    // some of the images in the dlib vehicle dataset are really high resolution.
    upsample_image_dataset<pyramid_down<2>>(images_train, boxes_train, 1800*1800);
    cout << "training upsampled results: " << test_object_detection_function(net, images_train, boxes_train, test_box_overlap(), 0, options.overlaps_ignore);

    cout << "num testing images: "<< images_test.size() << endl;
    cout << "testing results: " << test_object_detection_function(net, images_test, boxes_test, test_box_overlap(), 0, options.overlaps_ignore);
    upsample_image_dataset<pyramid_down<2>>(images_test, boxes_test, 1800*1800);
    cout << "testing upsampled results: " << test_object_detection_function(net, images_test, boxes_test, test_box_overlap(), 0, options.overlaps_ignore);

    /*
        This program takes many hours to execute on a high end GPU.  It took about a day to
        train on a NVIDIA 1080ti.  The resulting model file is available at
            http://dlib.net/files/mmod_rear_end_vehicle_detector.dat.bz2
        It should be noted that this file on dlib.net has a dlib::shape_predictor appended
        onto the end of it (see dnn_mmod_find_cars_ex.cpp for an example of its use).  This
        explains why the model file on dlib.net is larger than the
        mmod_rear_end_vehicle_detector.dat output by this program.

        You can see some videos of this vehicle detector running on YouTube:
            https://www.youtube.com/watch?v=4B3bzmxMAZU
            https://www.youtube.com/watch?v=bP2SUo5vSlc

        Also, the training and testing accuracies were:
            num training images: 2217
            training results: 0.990738 0.736431 0.736073
            training upsampled results: 0.986837 0.937694 0.936912
            num testing images: 135
            testing results: 0.988827 0.471372 0.470806
            testing upsampled results: 0.987879 0.651132 0.650399
    */

    return 0;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


//          Copyright Carl Philipp Reh 2009 - 2021.
// 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 FCPPT_MAKE_LITERAL_BOOST_UNITS_HPP_INCLUDED
#define FCPPT_MAKE_LITERAL_BOOST_UNITS_HPP_INCLUDED

#include <fcppt/check_literal_conversion.hpp>
#include <fcppt/make_literal_fwd.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/dimensionless_type.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/unit.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
/**
\brief Literal specialization for boost.units

\ingroup fcpptboost
*/
template <typename Unit, typename Type>
struct make_literal<boost::units::quantity<Unit, Type>>
{
  using decorated_type = boost::units::quantity<
      boost::units::unit<boost::units::dimensionless_type, typename Unit::system_type>,
      Type>;

  template <typename Fundamental>
  static decorated_type get(Fundamental const _fundamental)
  {
    FCPPT_CHECK_LITERAL_CONVERSION(Type, Fundamental);

    return decorated_type::from_value(static_cast<Type>(_fundamental));
  }
};

}

#endif