/*
 *  The core class (implementation) for roadmap building
 *
 *  Created on: Jan 30, 2015
 *  Author: Jingjin Yu
 */

#include "roadmap.h"
#include "helper_functions.h"

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

#include <CGAL/Qt/Converter.h>
#include <CGAL/Boolean_set_operations_2.h>

#include <QGraphicsSimpleTextItem>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/lookup_edge.hpp>

/*------------------------------------------------------------------------------------------
			see resources/lattice-indexing.pptx for the processing logic
--------------------------------------------------------------------------------------------*/

static const QColor BASIC_QCOLORS8[] = {Qt::red, Qt::blue, Qt::green, Qt::magenta, Qt::cyan, Qt::gray, Qt::black, Qt::yellow};
static int colorCounter = 0;

void Roadmap::buildRoadmap(Polygon2_list* pObsList, Polygon_2 *pBoundingRect, double radius){
	// Populate some internal variables for use across calls
	m_radius = radius;
	m_edgeLength = radius/0.43;
	m_obstaclePolyList = *pObsList;
	m_pBoundingRect = pBoundingRect;
	if(m_pVisibilityGraph != 0){
		delete m_pVisibilityGraph;
		m_pVisibilityGraph = 0;
	}
	if(m_pEnvironment != 0){
		delete m_pEnvironment;
		m_pEnvironment = 0;
	}

	// Some basic setup
	ICPoint_2 bottomLeft = K_ICK_converter((*m_pBoundingRect)[0]);
	ICPoint_2 topRight = K_ICK_converter((*m_pBoundingRect)[2]);

	bottomLeftX = bottomLeft.x();
	bottomLeftY = bottomLeft.y();
	width = topRight.x() - bottomLeft.x();
	height = topRight.y() - bottomLeft.y();;
	sqrt3 = sqrt(3.0);

	// Compute number of columns and rows
	n_w = (int)(ceil(width/(m_edgeLength*3/2))) + 3;
	n_h = (int)(ceil(height/(m_edgeLength*sqrt3))) + 3;

	// The lattice start x, y
	xs = bottomLeftX - (3/2)*m_edgeLength*1.35;
	ys = bottomLeftY - sqrt3*m_edgeLength*1.4;

	// Clean up from previous build
	m_pointList.clear();
	m_vidPointMap.clear();
	m_pointVidMap.clear();
	m_graph.clear();
	m_finalGraph.clear();
	m_vidFGMap.clear();
	m_vidGFMap.clear();
	m_edgeToBeRemovedSet.clear();
	m_vertexToBeRemovedSet.clear();
	m_boundaryBoundingCycle.clear();
	m_connectingPathMap.clear();
	for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
		delete (*git);
	}
	m_obsBoundingCycleVec.clear();

	/*
	// Build the roadmap, first obtain a lattice that cover the outer boundary
	buildHexgaonLattice();

	// Remove extra edges, at the same time, find smallest cycles enclosing the obstacles
	removeExcessEdges();

	// Preserve connectivity
	checkAndFixConnectivity();
*/
}

void Roadmap::removeExcessEdges(){
	// We do this in several steps. First, we go through each polygon obstacle boundary
	// and delete all edges of the lattice that intersect with these boundaries. Then,
	// we find the connected components of the remaining lattice graph. For each component
	// we only need to test one vertex to know whether it belongs to the configuration space
	// or not. We keep all components that belong to the configuration space

	// =====================================================================================
	// Compute the set of edges that falls on obstacle boundaries, at the same time also
	// compute the smallest cycle in the full lattice that encloses the obstacle. First do
	// it for the bounding polygon
	getIntersectingEdges(*m_pBoundingRect, m_boundaryBoundingCycle, true);

	// Then for all obstacles
	for(Polygon2_list::iterator obsit = m_obstaclePolyList.begin(); obsit != m_obstaclePolyList.end(); obsit++){
		Graph* pg = new Graph();
		m_obsBoundingCycleVec.push_back(pg);
		getIntersectingEdges(*obsit, *pg);
	}

	// =====================================================================================
	// Remove edges that do not belong to the graph
	Graph g;
	std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
	for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
		int fv = (*eit).first;
		int sv = (*eit).second;
		if(!edgeInSet(fv, sv, m_edgeToBeRemovedSet)){
			g.addEdge(fv, sv);
		}
	}
	m_graph = g;

	// =====================================================================================
	// Go through all vertices and find all vertices and edges inside the configuration
	// space. To do so, iterate over all lattices points (the full lattice minus the edges that
	// crosses obstacle boundaries) and for each point, if it has not been checked, see whether
	// the point is inside the configuration space. Then we do a BFS from the point and visit
	// all points/edges connected to the point. We add the edges to our final graph if and only
	// if the starting vertex is in the c-space. This way, we need to do c-space membership
	// check geometrically only very limited number of times, usually around the number of
	// obstacles in the c-space.

	g.clear();
	std::set<int> visitedVertices;
	for(std::map<int, Point_2>::iterator vit = m_vidPointMap.begin(); vit != m_vidPointMap.end(); vit++){
		int vid = vit->first;
		if(visitedVertices.find(vid) == visitedVertices.end()){
			visitedVertices.insert(vid);
			// Test whether the vertex is inside the configuration space
			bool inCSpace = isPointInCSpace(vit->second);

			// Do BFS
			std::list<int> tempQueue;
			tempQueue.push_back(vid);
			while(tempQueue.size() > 0){
				int current = tempQueue.front();
				tempQueue.pop_front();

				std::set<int> neighborSet = m_graph.getNeighborSet(current);

				for(std::set<int>::iterator vit = neighborSet.begin(); vit != neighborSet.end(); vit++){
					// Retrieve first and second vertices
					int et = *vit;
					if(visitedVertices.find(et) == visitedVertices.end()){
						visitedVertices.insert(et);
						tempQueue.push_back(et);
					}
					// Add edge as needed
					if(inCSpace) g.addEdge(current, et);
				}
			}
		}
	}
	m_graph.clear();
	m_graph = g;

	// Remove isolated vertices
	for(std::set<int>::iterator vit = m_vertexToBeRemovedSet.begin(); vit != m_vertexToBeRemovedSet.end(); vit++){
		// std::cout << "Removing vertex with id: " << *vit << std::endl;
		m_graph.removeVertex(*vit);
	}

	// Remove single degree edges
	std::set<int> vSetCopy = m_graph.getVertexSet();
	for(std::set<int>::iterator vit = vSetCopy.begin(); vit != vSetCopy.end(); vit++){
		if(m_graph.hasVertex(*vit)){
			if(m_graph.getNeighborSet(*vit).size() == 0){
				m_graph.removeVertex(*vit);
				// std::cout << "Removing vertex with id: " << *vit << std::endl;
			}
		}
	}

	buildFinalGraph();
}

void Roadmap::buildFinalGraph(){
	// Construct m_finalGraph
	std::set<std::pair<int, int>>& eSet = m_graph.getEdgeSet();
	int vIDCount = 0;
	for(std::set<std::pair<int, int> >::iterator eit = eSet.begin(); eit != eSet.end(); eit++){
		int fv = eit->first;
		int sv = eit->second;

		// Check whether fv was added
		if(m_vidGFMap.find(fv) == m_vidGFMap.end()){
			m_vidGFMap[fv] = vIDCount;
			m_vidFGMap[vIDCount] = fv;
			fv = vIDCount++;
		}
		else{
			fv = m_vidGFMap[fv];
		}

		// Check whether sv was added
		if(m_vidGFMap.find(sv) == m_vidGFMap.end()){
			m_vidGFMap[sv] = vIDCount;
			m_vidFGMap[vIDCount] =sv;
			sv = vIDCount++;
		}
		else{
			sv = m_vidGFMap[sv];
		}
		m_finalGraph.addEdge(fv, sv);
	}
}

void Roadmap::drawBoundingCycle(QGraphicsScene& scene){
	// Draw the cycles
	QPen regularPen = QPen(QColor(0, 255, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	QPen illegalPen = QPen(QColor(255, 0, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
		Graph& g = *(*git);
		std::set<std::pair<int, int> > edgeSet = g.getEdgeSet();
		for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
			int v1 = (*eit).first;
			int v2 = (*eit).second;
			ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
			ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
			if(m_graph.hasEdge(v1, v2)){
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), regularPen);
			}
			else{
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), illegalPen);
			}
		}
	}
}

bool Roadmap::isPointInCSpace(Point_2 &p){
	// Check whether the point is inside the bounding rect
	if(m_pBoundingRect->bounded_side(p) == CGAL::ON_UNBOUNDED_SIDE){
		return false;
	}

    for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){
		Polygon_2 &tp = *(pli);
		if(tp.bounded_side(p) == CGAL::ON_BOUNDED_SIDE){
			return false;
		}
	}
	return true;
}

void Roadmap::drawVertexIds(QGraphicsScene& scene){
	// Draw the id text of the vretex
	QFont font;
	QPainterPath path;
	font.setPointSizeF(m_radius/1.5);
	font.setBold(false);
	for(std::set<int>::iterator vit = m_graph.getVertexSet().begin(); vit != m_graph.getVertexSet().end(); vit ++){
		ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]);
		QGraphicsSimpleTextItem *ti = scene.addSimpleText(QString::number(m_vidGFMap[*vit]), font);
		ti->setPos(p.x() + m_radius/2, p.y() - m_radius/2);
		ti->setPen(QPen(QColor(Qt::green), 0.03*m_radius, Qt::SolidLine, Qt::RoundCap,Qt::RoundJoin));
		ti->setZValue(2);
	}
}

void Roadmap::buildVisibilityGraph(){
	// To build the visibility graph, we use the package VisiLibity, which is not precise arithematic,
	// but good for our purpose since we do not use visibility graph as part of our main logic.

	// Only build once per roadmap
	if(m_pVisibilityGraph != 0) return;

	// Vector to hold all polygon for contructing VisiLibity object
	vector<Polygon> polyVec;

	// We assume that the infated obstacles do not intersect each other but may intersect the boundary.
	// Therefore, we test such intersection and obtain an updated boundary (the inside)
	ECPolygon_2 boundary = convertToExactPolygon(*m_pBoundingRect);
	if(boundary.is_clockwise_oriented()) boundary.reverse_orientation();
	for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){
		ECPolygon_2 ecPoly = convertToExactPolygon(*pli);
		if(boundaryInterset(boundary, ecPoly)){
			vector<ECPolygon_with_holes_2> outVec;

			// Remove the obstacle "from" the boundary polygon
			CGAL::difference(boundary, ecPoly, back_inserter(outVec));

			// Update boundary
			boundary = outVec[0].outer_boundary();
		}
		else{
			// Add all non intersecting obstacles
			Polygon tempPoly;
			converFromCGALtoVisilibity(ecPoly, tempPoly, false);
			polyVec.push_back(tempPoly);
		}
	}

	// Insert the boundary
	Polygon boundaryPoly;
	converFromCGALtoVisilibity(boundary, boundaryPoly, true);
	polyVec.insert(polyVec.begin(), boundaryPoly);

	// Build environment and visibility graph
	m_pEnvironment = new Environment(polyVec);
	m_pVisibilityGraph = new Visibility_Graph(*m_pEnvironment, m_epsilon);
}

double Roadmap::computeShortestPath(Point_2& p1, Point_2& p2, std::list<Point_2> & path){
	ICPoint_2 p1x = K_ICK_converter(p1);
	ICPoint_2 p2x = K_ICK_converter(p2);
	return computeShortestPath(p1x.x(), p1x.y(), p2x.x(), p2x.y(), path);
}

double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, std::list<Point_2> & path){
	Polyline pl = m_pEnvironment->shortest_path(Point(x1, y1), Point(x2, y2), *m_pVisibilityGraph, m_epsilon);
	for(int i = 0; i < pl.size(); i++){
		path.push_back(Point_2(pl[i].x(), pl[i].y()));
	}
	return getPathLength(path);
}

double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, vector<pair<double, double> >& path){
	// Compute shortest path
	std::list<Point_2> shortestPath;
	double length = computeShortestPath(x1, y1, x2, y2, shortestPath);

	// Convert the path
	for(std::list<Point_2>::iterator vi = shortestPath.begin(); vi != shortestPath.end(); vi++){
		ICPoint_2 p = K_ICK_converter(*vi);
		path.push_back(pair<double, double>(p.x(), p.y()));
	}
	return length;
}

void Roadmap::addToScene(QGraphicsScene& scene, bool drawEdge, QPen edgePen, bool drawVertex, QPen vertexPen){
	// Paint the edges
	if(drawEdge){
		std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
		for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
			int v1 = (*eit).first;
			int v2 = (*eit).second;
			ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
			ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
			scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen);
			/*}*/

			if(m_boundaryBoundingCycle.hasEdge(v1, v2)){
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::blue, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
			}
			else{
				for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
					if((*git)->hasEdge(v1, v2)){
						scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::yellow, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
					}
				}
			}
		}
	}

	// Paint vertices if needed and obtain the fill area
	if(!drawVertex){
		for(std::list<Point_2>::iterator it = m_pointList.begin(); it != m_pointList.end(); it ++){
			ICPoint_2 p = K_ICK_converter(*it);
			scene.addEllipse(p.x() - 0.025, p.y() - 0.025, 0.05, 0.05, vertexPen);
		}
	}

	for(Path_Map::iterator pathIt = m_connectingPathMap.begin(); pathIt != m_connectingPathMap.end(); pathIt ++){
		std::list<Point_2>& shortestPath = pathIt->second;
		std::list<Point_2>::iterator vit = shortestPath.begin();
		if(vit != shortestPath.end()){
			ICPoint_2 p = K_ICK_converter(*vit);
			vit++;
			for(; vit != shortestPath.end(); vit++){
				ICPoint_2 p2 = K_ICK_converter(*vit);
				scene.addLine(p.x(), p.y(), p2.x(), p2.y(), QPen(Qt::magenta, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
				p = p2;
			}
		}
	}

	// std::cout << shortestPath.size() << std::endl;

}

void Roadmap::getIntersectingEdges(Polygon_2 & poly, Graph& boundingCycle, bool outerBoundary){
	// Detecting all edges of the lattice graph that intersects the boundary of poly

	// =====================================================================================
	// Get a vertex of the poly and find the hexgaon of the lattice that contains the vetex.
	// For each col and row combo, there are three possible hexgaons the point may fall into

	// Locate the hexgon (relative to some arbitrary base choice)
	ICPoint_2 p0 = K_ICK_converter(poly.vertex(0));
	std::pair<int, int> crp = locateHexagonForPoint(p0);
	int col = crp.first;
	int row = crp.second;

	// Compute the current hexagon
	Point_2 p[6];
	Segment_2 e[6];
	Polygon_2 hex;
	int pIndex[6];
	populateHexagon(col, row, p, e, hex, pIndex);

	// =====================================================================================
	// With a point of the obstacle and a hexgon containing the point, we iteratively check
	// intersections between the segments of the obstacle and the hexagon. We assume that
	// the obstacle would have at least three vertices. Note that a obstacle may be smaller than
	// a single hexagon.

	std::set<std::pair<int, int> > tempEdgeToRemoveSet;
	int numVertices = poly.size();		//
	int currentVertexIndex = 0;
	while(currentVertexIndex < numVertices){
		// Get the current segment of polygon to be checked
		Point_2 nextVertex = poly[currentVertexIndex==numVertices-1?0:currentVertexIndex+1];

		// Check whether nextVertex is outside of the current hexagon. If we are still in the
		// same hexagon, then move to the next obstacle vertex
		if(hex.bounded_side(nextVertex) == CGAL::ON_BOUNDED_SIDE) {
			currentVertexIndex ++;
			continue;
		}

		// If we are here, then we jumped outside of a lattice hexagon. Figure out which edge is
		// being intersected. We keep doing this until we cover the entire obstacle edge
		Point_2 currentVertex = poly[currentVertexIndex];
		Segment_2 obsEdge(currentVertex, nextVertex);

		std::pair<int, int> lastPair(-10, -10);
		while(true){
			int edgeIndex = 0;
			while(edgeIndex < 6){
				if(intersection(e[edgeIndex], obsEdge)){
					std::pair<int, int> edgePair(pIndex[edgeIndex], pIndex[edgeIndex==5?0:edgeIndex+1]);
					std::pair<int, int> edgePairReverse(pIndex[edgeIndex==5?0:edgeIndex+1], pIndex[edgeIndex]);
					if(lastPair != edgePair && lastPair != edgePairReverse){
						lastPair = edgePair;
						break;
					}
				}
				edgeIndex ++;
			};

			if(edgeIndex < 6){
				// We hit an intersection, mark the edge as to be removed
				tempEdgeToRemoveSet.insert(lastPair);
				m_edgeToBeRemovedSet.insert(lastPair);

				// Figure out the next hexgon to be checked
				if(col%2 == 0){
					switch(edgeIndex){
						case 0: col--; break;
						case 1: row++; break;
						case 2: col++; break;
						case 3: col++; row--; break;
						case 4: row--; break;
						case 5: col--; row--; break;
						default: break;
					}
				}
				else{
					switch(edgeIndex){
						case 0: col--; row++; break;
						case 1: row++; break;
						case 2: col++; row++; break;
						case 3: col++; break;
						case 4: row--; break;
						case 5: col--; break;
						default: break;
					}
				}
				populateHexagon(col, row, p, e, hex, pIndex);
			}
			else{
				// No more intersections, we are dong with the current obstacle edge
				break;
			}
		}
		currentVertexIndex++;
	}

	// =====================================================================================
	// We now have all the edges of the lattice that lie on the polygonal obstacle boundary.
	// Next, we locate the bounding cycle in the full lattice graph surrounding the obstacle
	if(tempEdgeToRemoveSet.size() > 0){
		// Iterate through edges to be removed and find one with an end vertex that
		// has an associated edge in the configuraiton space
		int sIndex, t1Index, t2Index;
		for(std::set<std::pair<int,int> >::iterator eit = tempEdgeToRemoveSet.begin();
		eit != tempEdgeToRemoveSet.end(); eit++){
			std::pair<int, int> edge = *eit;
			// Try one vertex
			sIndex = edge.first;
			if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1)
			{
				t1Index = edge.second;
				break;
			}

			// Try the other vertex
			sIndex = edge.second;
			if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1)
			{
				t1Index = edge.first;
				break;
			}

		}

		// Now edge (sIndex, t1Index) crosses the boundary, and (sIndex, t2Index) it outside the boundary
		// We simply locate a hexagon containing these two edges to extend the cycle
		Point_2 p1 = m_vidPointMap[sIndex];
		Point_2 p2 = m_vidPointMap[t1Index];
		Point_2 p3 = m_vidPointMap[t2Index];
		ICPoint_2 midPoint = K_ICK_converter(Point_2((p2.x() + p3.x())/2, (p2.y() + p3.y())/2));
		crp = locateHexagonForPoint(midPoint);
		populateHexagon(crp.first, crp.second, p, e, hex, pIndex);

		int endIndex = sIndex;		// When we see this index again, we are done

		// Add valid edge to cycle
		addEdgeIfNotThere(sIndex, t2Index, boundingCycle);

		// With the first hexagon, we can keep going along the boundary
		while(true){
			// Get next candidate index
			int nextIndex = getNextNodeInSequence(sIndex, t2Index, pIndex);

			p1 = m_vidPointMap[sIndex];
			p2 = m_vidPointMap[nextIndex];
			p3 = m_vidPointMap[t2Index];

			// Check whether the edge (t2Index, nextIndex) crosses boundary
			if(edgeInSet(t2Index, nextIndex, tempEdgeToRemoveSet)){
				// In this case, we move to the next hexagon
				crp = getBorderHexagon(crp.first, crp.second, nextIndex, t2Index, pIndex);
				populateHexagon(crp.first, crp.second, p, e, hex, pIndex);
				sIndex = nextIndex;
				// t2Index = sIndex;
			}
			else{
				// Edge is valid, update indices
				sIndex = t2Index;
				t2Index = nextIndex;

				// Add valid edge to cycle
				addEdgeIfNotThere(sIndex, t2Index, boundingCycle);
			}

			if(endIndex == t2Index) break;
		}
	}

	// =====================================================================================
	// Process bounding cycle to remove single degree vertices
	std::set<int> vSet = boundingCycle.getVertexSet();
	std::set<int> vSingleSet;
	// Collect single degree vertices
	for(std::set<int>::iterator vit = vSet.begin(); vit != vSet.end(); vit++){
		if(boundingCycle.getNeighborSet(*vit).size() == 1){
			vSingleSet.insert(*vit);
			// Need to check the neighbor as well
			int prevNbr = *vit;
			int nbr = *(boundingCycle.getNeighborSet(*vit).begin());
			while(boundingCycle.getNeighborSet(nbr).size()==2){
				vSingleSet.insert(nbr);
				std::set<int> nbrSet = boundingCycle.getNeighborSet(nbr);
				nbrSet.erase(prevNbr);

				prevNbr = nbr;
				nbr = *(nbrSet.begin());
			}
		}
	}
	// Delete them
	for(std::set<int>::iterator vit = vSingleSet.begin(); vit != vSingleSet.end(); vit++){
		boundingCycle.removeVertex(*vit);

		// It seems good to remove these single degree vertices
		if(!outerBoundary)m_vertexToBeRemovedSet.insert(*vit);
	}
}

std::pair<int, int> Roadmap::getBorderHexagon(int col, int row, int i1, int i2, int pIndex[]){
	// First locate the index of i1
	int i1Index = 0;
	for(int i = 0; i < 6; i ++){
		if(pIndex[i] == i1){
			i1Index = i;
			break;
		}
	}

	// Figure out the edge index
	int edgeIndex = 0;
	int nextIndex = (i1Index == 5? 0 : i1Index + 1);
	if(pIndex[nextIndex] == i2){
		// Going clockwise
		edgeIndex = i1Index;
	}
	else{
		// Going counterclockwise
		edgeIndex = (i1Index + 6 - 1)%6;
	}

	// Locate the next hexagon
	if(col%2 == 0){
		switch(edgeIndex){
			case 0: col--; break;
			case 1: row++; break;
			case 2: col++; break;
			case 3: col++; row--; break;
			case 4: row--; break;
			case 5: col--; row--; break;
			default: break;
		}
	}
	else{
		switch(edgeIndex){
			case 0: col--; row++; break;
			case 1: row++; break;
			case 2: col++; row++; break;
			case 3: col++; break;
			case 4: row--; break;
			case 5: col--; break;
			default: break;
		}
	}
	return std::pair<int, int>(col, row);
}

bool Roadmap::edgeInSet(int v1, int v2, std::set<std::pair<int, int> >& edgeSet){
	std::pair<int, int> edge(v1, v2);
	std::pair<int, int> rEdge(v2, v1);
	return (edgeSet.find(edge) != edgeSet.end() || edgeSet.find(rEdge) != edgeSet.end());
}

void Roadmap::addEdgeIfNotThere(int v1, int v2, Graph & graph){
	if(!graph.hasEdge(v1, v2)){
		graph.addEdge(v1, v2);
	}
}

int Roadmap::getNextNodeInSequence(int i1, int i2, int pIndex[]){
	// First locate the index of i1
	int i1Index = 0;
	for(int i = 0; i < 6; i ++){
		if(pIndex[i] == i1){
			i1Index = i;
			break;
		}
	}
	int nextIndex = (i1Index == 5? 0 : i1Index + 1);
	if(pIndex[nextIndex] == i2){
		// Going clockwise
		return pIndex[(i1Index + 2)%6];
	}
	else{
		// Going counterclockwise
		return pIndex[(6 + i1Index - 2)%6];
	}
}

std::pair<int, int> Roadmap::locateHexagonForPoint(ICPoint_2 &p0){
	// Compute the rectangular (col, row)
	int col = (int)(floor((p0.x() - xs)/(m_edgeLength*1.5)));
	int row = (int)(floor((p0.y() - ys)/(m_edgeLength*sqrt3)));

	// Shift everything to the "origin"
	double dx = p0.x() - (xs + m_edgeLength*1.5*col);
	double dy = p0.y() - (ys + m_edgeLength*sqrt3*row);

	// For each col/row combo, which correspond to a rectangular region, there can be
	// three hexagons corrsponding to a point in that rectangle
	if(col%2 == 0){
		if(dx <= 0.5*m_edgeLength && dy <= sqrt3*m_edgeLength  - sqrt3*dx && dy >= sqrt3*dx){
			// We need to move left
			col --;
			// row ++;
		}
		else if(dy >= m_edgeLength*0.5*sqrt3){
			// Off by one row
			row ++;
		}
		else{
			// Already in the right place, do nothing
		}
	}
	else{
		if(dy >= sqrt3*0.5*m_edgeLength + sqrt3*dx){
			col--;
			row++;
		}
		else if(dy <= sqrt3*0.5*m_edgeLength - sqrt3*dx){
			col--;
		}
		else{
			// Already in the right place, do nothing
		}
	}
	return std::pair<int, int>(col, row);
}

int Roadmap::pointBelongToEdgeOutideObstacle(Polygon_2 & poly, int pIndex,
	std::set<std::pair<int,int> > &tempEdgeToRemoveSet, bool outerBoundary){
	// Grab the point and check whether it is in the c-space
	if(pointOutsideObstacle(poly, pIndex, outerBoundary)){
		// The point is not "in" the obstacle, get edges and check that they are not
		// intersecting with the boundary. Because pIndex is outside obstacle, as long
		// as one edge from pIndex is not intersecting boundary, the edge must be outside
		// the boundary
		std::set<int>& nbrSet = m_graph.getNeighborSet(pIndex);
		for(std::set<int>::iterator nit = nbrSet.begin(); nit != nbrSet.end(); nit++){
			int et = *nit;
			if(!edgeInSet(pIndex, et, tempEdgeToRemoveSet)){
				return et;
			}
		}
	}
	return -1;
}

bool Roadmap::pointOutsideObstacle(Polygon_2 & poly, int pIndex, bool outerBoundary){
	Point_2 p = m_vidPointMap[pIndex];
	int boundedStatus = poly.bounded_side(p);
	if((boundedStatus == CGAL::ON_BOUNDED_SIDE && outerBoundary) ||
		(boundedStatus == CGAL::ON_UNBOUNDED_SIDE && !outerBoundary)){
		return true;
	}
	return false;
}

void Roadmap::populateHexagon(int col, int row, Point_2* p, Segment_2* e, Polygon_2 &hex, int* pIndex){
	hex.clear();
	if(col%2 == 0){
		pIndex[0] = (col + row*n_w)*2;
		pIndex[1] = (col + row*n_w)*2 + 1;
		pIndex[2] = (col + 1 + row*n_w)*2 + 1;
		pIndex[3] = (col + 1 + row*n_w)*2;
		pIndex[4] = (col + 1 + (row-1)*n_w)*2 + 1;
		pIndex[5] = (col + (row-1)*n_w)*2 + 1;
	}
	else{
		pIndex[0] = (col + row*n_w)*2 + 1;
		pIndex[1] = (col + (row+1)*n_w)*2;
		pIndex[2] = (col + 1 + (row+1)*n_w)*2;
		pIndex[3] = (col + 1 + row*n_w)*2 + 1;
		pIndex[4] = (col + 1 + row*n_w)*2;
		pIndex[5] = (col + row*n_w)*2;
	}
	// Then we get all vertices
	for(int i = 0; i < 6; i ++){
		p[i] = m_vidPointMap[pIndex[i]];
	}

	// Then the edges and the hexgaon
	for(int i = 0; i < 6; i ++){
		e[i] = Segment_2(p[i], p[i == 5? 0 : i+1]);
		hex.push_back(p[i]);
	}
}

void Roadmap::buildHexgaonLattice(){

	// Start the lattice at bottomLeftX, bottomLeftY.
	// Assume that there are n_w and n_h hexgons inside the rectangle, then we should have
	// (1/2)*sideLength + n_w*sideLength*3/2 <= width and width < (1/2)*sideLength + (n_w + 1)*sideLength*3/2
	// (sqrt(3)/2)*sideLength + n_h*(sqrt(3)/2)*sideLength <= height and height < (sqrt(3)/2)*sideLength + (n_h + 1)*(sqrt(3)/2)*sideLength
	// From these we can compute n_w and n_h. We add a few to make sure that we cover everything.

	// Compute lattice nodes
	for(int i = 0; i < n_w; i ++){
		for(int j = 0; j < n_h; j ++){
			// Build one vertical "wave" of vertices
			Point_2 v0(xs + i*m_edgeLength*1.5 + (i%2 == 0? 0 : m_edgeLength*0.5),
				ys + j*m_edgeLength*sqrt3);
			Point_2 v1(xs + m_edgeLength/2 + i*m_edgeLength*1.5 + (i%2 == 0? 0 : - m_edgeLength*0.5),
				ys + sqrt3*m_edgeLength/2 + j*m_edgeLength*sqrt3);

			m_vidPointMap[(i + j*n_w)*2] = v0;
			m_pointVidMap[v0] = (i + j*n_w)*2;
			m_vidPointMap[(i + j*n_w)*2 + 1] = v1;
			m_pointVidMap[v1] = (i + j*n_w)*2 + 1;

			m_pointList.push_back(v0);
			m_pointList.push_back(v1);
		}
	}

	// Build adjacency, for each vertex, check whether its three neighbors are present
	for(int id = 0; id < n_w*n_h*2; id ++){
		// Compute w, h
		int w = (id/2)%n_w;
		int h = (id/2)/n_w;
		bool odd = (id%2==1);

		// If odd is true, check the vertex above, which has index (w, h + 1, 0)
		if(odd){
			if(h + 1 < n_h){
				m_graph.addEdge(id, (w + (h + 1)*n_w)*2);
			}
		}
		else{
			m_graph.addEdge(id, (w + (h)*n_w)*2 + 1);
		}
		// If odd is true and w is even, check (w + 1, h, 1)
		if(odd && w%2 == 0){
			if(w + 1 < n_w){
				m_graph.addEdge(id, (w + 1 + h*n_w)*2 + 1);
			}
		}

		// If odd is false and w is odd, check (w + 1, h, 0)
		if(odd == false && w%2 == 1){
			if(w + 1 < n_w){
				m_graph.addEdge(id, (w + 1 + h*n_w)*2);
			}
		}
	}
}

void Roadmap::drawHexagonLattice(QGraphicsScene& scene, bool drawVetex){
	QPen edgePen = QPen(Qt::gray, 0.25, Qt::DashLine, Qt::RoundCap, Qt::RoundJoin);
	QPen vertexPen = QPen(Qt::blue, 0.4, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
	std::set<int> vSet;
	for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
		int v1 = (*eit).first;
		int v2 = (*eit).second;
		vSet.insert(v1); vSet.insert(v2);
		ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
		ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
		scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen);
	}

	// Paint vertices if needed and obtain the fill area
	if(drawVetex){
		for(std::set<int>::iterator vit = vSet.begin(); vit != vSet.end(); vit ++){
			ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]);
			scene.addEllipse(p.x() - 0.25, p.y() - 0.25, 0.5, 0.5, vertexPen);
		}
	}
}

void Roadmap::snapToGraph(vector<pair<double, double> >&coords, vector<int>&snapped){
	set<int> usedVertices;
	// For each member in coords, locate the hexagon it belongs and then locate the closest
	// graph vertex to the said coordinate.
	for(int r = 0; r < coords.size(); r ++){
		double x = coords[r].first;
		double y = coords[r].second;
		Point_2 p0 = Point_2(x, y);

		ICPoint_2 p0x = K_ICK_converter(p0);
		std::pair<int, int> crp = locateHexagonForPoint(p0x);
		int col = crp.first;
		int row = crp.second;

		// Compute the current hexagon
		Point_2 p[6];
		Segment_2 e[6];
		Polygon_2 hex;
		int pIndex[6];
		populateHexagon(col, row, p, e, hex, pIndex);

		// Find the closest vertex that is not occupied and inside the configuration space
		int bestV = -1;
		double minDist = -1;
		for(int vi = 0; vi < 6; vi ++){
			// Only work with vertices inside the configuration space and not used
			if(m_graph.hasVertex(pIndex[vi])
			   && usedVertices.find(pIndex[vi]) == usedVertices.end()){
				if(minDist < 0){
					minDist = getDistance(p0, p[vi]);
					bestV = pIndex[vi];
				}
				else{
					double dist = getDistance(p0, p[vi]);
					if(minDist > dist){
						minDist = dist;
						bestV = pIndex[vi];
					}
				}
			}
		}

		// Snap!
		if(bestV != -1){
#ifdef _DEBUG
			cout << "Snapping point (" << x << ", " << y << ") to vertex " << m_vidGFMap[bestV] << endl;
#endif
			snapped.push_back(m_vidGFMap[bestV]);
			usedVertices.insert(bestV);
		}
		else{
			throw "ERROR: Cannot find suitable vertex for snapping";
		}
	}

}

void Roadmap::createRandomStartGoalPairs(int numRobots, double spacing,  vector<pair<double, double> >& starts,
	vector<pair<double, double> >& goals){

	createRandomCoords(numRobots, spacing, starts);
	createRandomCoords(numRobots, spacing, goals);
}

void Roadmap::createRandomCoords(int numRobots, double spacing, vector<pair<double, double> >& coords){
	// Uniform sample from the free space and pick 5 start/goal pairs
	CGAL::Bbox_2 bbox = m_pBoundingRect->bbox();
	while(true){
		long trialCount = 0;
		for(int i = 0; i < numRobots; i ++){
			while(trialCount < numRobots*30000){
				trialCount ++;
#ifdef _DEBUG
				if(trialCount %1000 == 0) {cout << trialCount << endl;}
#endif
				double x = (rand()%10000)/10000.*(bbox.xmax() - bbox.xmin())  + bbox.xmin();
				double y = (rand()%10000)/10000.*(bbox.ymax() - bbox.ymin())  + bbox.ymin();
#ifdef _DEBUG
				cout << "Randomly created point with x=" << x <<", y=" << y;
#endif
				Point_2 p(x, y);
				// Check that the point is in free configuration space
				if(isPointInCSpace(p)){
					bool good = true;
					// Check that the point has good distance from existing points
					for(int vi = 0; vi < coords.size(); vi ++){
						if(getDistance(p, coords[vi].first, coords[vi].second) < (spacing > 2 ? spacing : 2)*m_radius){
							good = false;
						}
					}

					if(good){
						// No problem! We have a good set
						coords.push_back(pair<double, double>(x, y));
#ifdef _DEBUG
						cout << " - added to set for robot " << i << endl;
#endif
						break;
					}
					else{
#ifdef _DEBUG
						cout << endl;
#endif
					}

				}
				else{
#ifdef _DEBUG
					cout << endl;
#endif
				}
			}
			if(trialCount >= numRobots*30000){
				// cout << coords.size() << endl;
				coords.clear();
				break;
			}
		}
		if(coords.size() == numRobots)return;
	}
}

bool Roadmap::solveProblem(vector<pair<int, int> >& sgVec, map<int, vector<int> >& paths,
	string& fileFolder, string& fileNameExtra){
	// Solve the problem by calling external java solver

	// =====================================================================================
	// First write the problem
	string pfString = fileFolder + "\\p" + fileNameExtra + ".txt";
	string sfString = fileFolder + "\\s" + fileNameExtra + ".txt";
	ofstream ps(pfString.c_str(), ios::out);

	// Write the the number of robots
	ps << m_finalGraph.getVertexSet().size() << endl;

	// Write out all the edges
	set<pair<int, int> >& edgeSet = m_finalGraph.getEdgeSet();
	for(set<pair<int, int> >::iterator ei = edgeSet.begin(); ei != edgeSet.end(); ei ++){
		ps << ei->first << ":" << ei->second << " ";
	}
	ps << endl;

	// Write out the starts and then the goals
	for(int r = 0; r < sgVec.size(); r ++){
		ps << sgVec[r].first << " ";
	}
	ps << endl;
	for(int r = 0; r < sgVec.size(); r ++){
		ps << sgVec[r].second << " ";
	}
	ps << endl;
	ps.close();

	// =====================================================================================
	// Make system call
	string callStr = "java -cp gurobi.jar;mp.jar projects.multipath.general.algorithms.Main";
	if(fileNameExtra.length() > 0) {
		callStr.append(" ").append(pfString).append(" ").append(sfString);
	}
	int i = system(callStr.c_str());

	// =====================================================================================
	// Read in the solution, there should be sgVec.size() robots
	ifstream ss(sfString.c_str(), ios::in);
	if(ss.good()){
		for(int r = 0; r < sgVec.size(); r++){
			string line;
			getline(ss, line);
			QString s(line.c_str());
			QStringList qsl = s.split(" ");
			paths[r] = vector<int>();
			for(int t = 0; t < qsl.size(); t ++){
				paths[r].push_back(qsl[t].toInt());
			}
			paths[r].pop_back();
		}
		ss.close();
	}
	else{
		ss.close();
		return false;
	}

	// Solve the problem locally
	// ILPSolver solver(&m_finalGraph);
	// solver.solve(sgVec, paths, -1);

	improvePaths(paths);
	return true;
}

void Roadmap::improvePaths(map<int, vector<int> >& paths){
	// Remove obvious ossilation from the paths
	for(int t = 1; t < paths[0].size() - 1; t ++){
		for(int r = 0; r < paths.size(); r ++){
			// Do we have single step ossilation?
			if(paths[r][t - 1] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){
				// Check whether any other robots goes to paths[r][t - 1] at t
				bool conflict = false;
				for(int ori = 0; ori < paths.size(); ori ++){
					if(ori == r) continue;
					if(paths[ori][t] == paths[r][t - 1]){
						conflict = true;
					}
				}

				// If not, let the robot stay
				if(!conflict){
					paths[r][t] = paths[r][t - 1];
				}
			}
			// Do a two step look ahead as well
			else if((t < paths[0].size() - 2) && paths[r][t - 1] == paths[r][t + 2] &&
				paths[r][t] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){
				// Check whether any other robots goes to paths[r][t - 1] at t
				bool conflict = false;
				for(int ori = 0; ori < paths.size(); ori ++){
					if(ori == r) continue;
					if(paths[ori][t] == paths[r][t - 1] || paths[ori][t + 1] == paths[r][t - 1]){
						conflict = true;
					}
				}

				// If not, let the robot stay
				if(!conflict){
					paths[r][t+1] = paths[r][t] = paths[r][t - 1];
				}
			}
		}
	}
}

pair<double, double> Roadmap::getVertexLocationFromID(int vid){
	// Get vid in the original graph
	vid = m_vidFGMap[vid];

	// Locate the vid
	Point_2& p = m_vidPointMap[vid];
	ICPoint_2 pp = K_ICK_converter(p);

	return pair<double, double>(pp.x(), pp.y());
}


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library 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
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_INTERSECTION_HPP
#define UTILITIES_GEOMETRY_INTERSECTION_HPP

#include "../UtilitiesAPI.hpp"

#include "Point3d.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  /** IntersectionResult contains detailed information about an intersection. */
  class UTILITIES_API IntersectionResult {
  public:
    IntersectionResult(const std::vector<Point3d>& polygon1,
                       const std::vector<Point3d>& polygon2,
                       const std::vector< std::vector<Point3d> >& newPolygons1,
                       const std::vector< std::vector<Point3d> >& newPolygons2);

    // vertices of first polygon after intersection
    std::vector<Point3d> polygon1() const;

    // vertices of second polygon after intersection
    std::vector<Point3d> polygon2() const;

    // new polygons generated from the first surface
    std::vector< std::vector<Point3d> > newPolygons1() const;

    // new polygons generated from the second surface
    std::vector< std::vector<Point3d> > newPolygons2() const;

  private:
    std::vector<Point3d> m_polygon1;
    std::vector<Point3d> m_polygon2;
    std::vector< std::vector<Point3d> > m_newPolygons1;
    std::vector< std::vector<Point3d> > m_newPolygons2;
  };

  /// removes spikes from a polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<Point3d> removeSpikes(const std::vector<Point3d>& polygon, double tol);

  /// returns true if point is inside polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API bool pointInPolygon(const Point3d& point, const std::vector<Point3d>& polygon, double tol);

  /// compute the union of two overlapping polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API boost::optional<std::vector<Point3d> > join(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// compute the union of many polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > joinAll(const std::vector<std::vector<Point3d> >& polygons, double tol);

  /// intersect two polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API boost::optional<IntersectionResult> intersect(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// subtract all holes from polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > subtract(const std::vector<Point3d>& polygon, const std::vector<std::vector<Point3d> >& holes, double tol);

  /// returns true polygon intersects iteself, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// returns false if polygon has less than three vertices
  UTILITIES_API bool selfIntersects(const std::vector<Point3d>& polygon, double tol);

  /// returns true if polygon1 intersects polygon2, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// returns false if either polygon has less than three vertices
  UTILITIES_API bool intersects(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// returns true if geometry1 is completely within polygon2, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// geometry1 can be a point or a polygon
  /// currently only tests that all points of geometry1 are within polygon2, better support when upgrade to boost 1.57
  UTILITIES_API bool within(const Point3d& point1, const std::vector<Point3d>& polygon2, double tol);
  UTILITIES_API bool within(const std::vector<Point3d>& geometry1, const std::vector<Point3d>& polygon2, double tol);

} // openstudio

#endif //UTILITIES_GEOMETRY_INTERSECTION_HPP


#include <idmlib/translation/ibm_model1.h>
#include <boost/unordered_map.hpp>
#include <iostream>
using namespace idmlib::tl;

IbmModel1::IbmModel1(uint32_t iteration)
:iteration_(iteration)
{
}

boost::unordered_map<std::pair<uint32_t, uint32_t>, double>* IbmModel1::Train(uint32_t source_num, const std::vector<SentencePair>& corpus)
{
    std::cout<<"IBM MODEL 1 starting.."<<std::endl;
    ///result gives P(source | target)
    boost::unordered_map<TermPair, double>* result = new boost::unordered_map<TermPair, double>();
    uint32_t iteration = 0;
    while(true)
    {
        ++iteration;
        if(iteration>iteration_) break;
        std::cout<<"IBM MODEL 1 iteration "<<iteration<<std::endl;
        boost::unordered_map<TermPair, double> count;
        boost::unordered_map<Term, double> total;
        for(std::size_t i=0;i<corpus.size();i++)
        {
            boost::unordered_map<Term, double> total_s;
            const Sentence& source = corpus[i].first;
            const Sentence& target = corpus[i].second;
            for(std::size_t i=0;i<source.size();i++)
            {
                Term source_term = source[i];
                boost::unordered_map<Term, double>::iterator total_s_it = total_s.insert(std::make_pair(source_term, 0.0) ).first;
                for(std::size_t j=0;j<target.size();j++)
                {
                    TermPair term_pair(source_term, target[j]);

                    boost::unordered_map<TermPair, double>::iterator it = result->find(term_pair);
                    if( it == result->end())
                    {
                        it = result->insert(std::make_pair(term_pair, 1.0/source_num)).first;
                    }
                    total_s_it->second += it->second;
                }
            }

            for(std::size_t i=0;i<source.size();i++)
            {
                Term source_term = source[i];
                for(std::size_t j=0;j<target.size();j++)
                {
                    Term target_term = target[j];
                    TermPair term_pair(source_term, target_term);

                    boost::unordered_map<TermPair, double>::iterator count_it = count.find(term_pair);
                    if(count_it == count.end())
                    {
                        count_it = count.insert(std::make_pair( term_pair, 0.0) ).first;
                    }

                    boost::unordered_map<Term, double>::iterator total_it = total.find(target_term);
                    if(total_it == total.end())
                    {
                        total_it = total.insert(std::make_pair( target_term, 0.0) ).first;
                    }

                    boost::unordered_map<TermPair, double>::iterator it = result->find(term_pair);
                    double t = it->second;

                    boost::unordered_map<Term, double>::iterator total_s_it = total_s.find(source_term);
                    double add = t/ (total_s_it->second);
                    count_it->second += add;
                    total_it->second += add;
                }
            }
        }

        boost::unordered_map<TermPair, double>::iterator it = result->begin();
        while(it!=result->end())
        {
            TermPair term_pair = it->first;
            boost::unordered_map<TermPair, double>::iterator count_it = count.find(term_pair);
            boost::unordered_map<Term, double>::iterator total_it = total.find(term_pair.second);
            it->second = count_it->second / total_it->second;
            ++it;
        }
    }
    std::cout<<"IBM MODEL 1 finished."<<std::endl;
    return result;
}


#ifndef STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP
#define STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP

#include <stan/math/torsten/dsolve/pmx_odeint_system.hpp>
#include <stan/math/torsten/mpi/precomputed_gradients.hpp>
#include <boost/numeric/odeint.hpp>
#include <type_traits>

namespace torsten {
namespace dsolve {

/**
 * @c boost::odeint ODE integrator.
 */
  template<typename scheme_t>
  struct PMXOdeintIntegrator {
    const double rtol_;
    const double atol_;
    const int64_t max_num_steps_;

    template<typename Ode, bool GenVar>
    struct SolObserver {
      const Ode& ode_;
      const size_t n;
      const size_t m;
      std::vector<std::vector<typename Ode::scalar_t>> y;
      int step_counter_;

      SolObserver(const Ode& ode) :
        ode_(ode), n(ode.N_), m(ode.M_),
        y(ode.ts_.size(), std::vector<typename Ode::scalar_t>(ode.N_, 0.0)),
        step_counter_(0)
      {}

      /*
       * use observer to convert y value and gradient to var
       * results, if necessary.
       */
      inline void operator()(const std::vector<double>& curr_result, double t) {
        if(t > ode_.t0_) {
          observer_impl(y[step_counter_], curr_result, ode_.ts_, ode_.y0_, ode_.theta_, step_counter_);
          step_counter_++;
        }
      }

    private:
      /*
       * All data, return data
       */
      inline void observer_impl(std::vector<double>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<double>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        std::copy(y.begin(), y.end(), y_res.begin());
      }

      /*
       * When only @c ts is @c var, we don't solve
       * sensitivity ODE since the sensitivity is simply the RHS.
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                const std::vector<double>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        int n = y0.size();
        std::vector<double> g(n * (1 + ts.size()), 0.0);
        std::copy(y.begin(), y.end(), g.begin());
        std::vector<double> dy_dt(n);
        ode_.dbl_rhs_impl(y, dy_dt, ts[i].val());
        std::copy(dy_dt.begin(), dy_dt.end(), g.begin() + n + i * n);
        y_res = torsten::precomputed_gradients(g, ts);
      }

      /*
       * Only @c theta is @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<double>& y0,
                                const std::vector<stan::math::var>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, theta);
      }

      /*
       * only @c y0 is @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<stan::math::var>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, y0);
      }

      /*
       * @c y0 and @c theta are @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<stan::math::var>& y0,
                                const std::vector<stan::math::var>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, ode_.vars());
      }

      /*
       * @c theta and/or &c y0 are @c var, together with @c ts.
       */
      template<typename T1, typename T2>
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                const std::vector<T1>& y0,
                                const std::vector<T2>& theta,
                                int i) const {
        int ns = ode_.ns;
        int n = y0.size();
        std::vector<double> g(n * (1 + ns + ts.size()), 0.0);
        std::copy(y.begin(), y.end(), g.begin());
        std::vector<double> dy_dt(n), y_dbl(y.begin(), y.begin() + n);
        ode_.dbl_rhs_impl(y_dbl, dy_dt, ts[i].val());
        std::copy(dy_dt.begin(), dy_dt.end(), g.begin() + n + ns * n + i * n);
        y_res = torsten::precomputed_gradients(g, ode_.vars());
      }
    };

    template<typename Ode>
    struct SolObserver<Ode, false> {
      const Ode& ode_;
      Eigen::MatrixXd y;
      int step_counter_;

      SolObserver(const Ode& ode) :
        ode_(ode),
        y(Eigen::MatrixXd::Zero(ode_.size_ + ode_.N_*(Ode::is_var_ts ? ode_.ts_.size() : 0), ode_.ts_.size())),
        step_counter_(0)
      {}

      inline void operator()(const std::vector<double>& curr_result, double t) {
        if(t > ode_.t0_) {
          observer_impl(y, curr_result, ode_.ts_, step_counter_);
          step_counter_++;
        }
      }

    private:
      /*
       * @@c ts is data
       */
      inline void observer_impl(Eigen::MatrixXd& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                int i) const {
        using Eigen::VectorXd;
        y_res.col(i) = VectorXd::Map(y.data(), ode_.size_);
      }

      /*
       * @@c ts is @c var
       */
      inline void observer_impl(Eigen::MatrixXd& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                int i) const {
        using Eigen::VectorXd;
        for (size_t j = 0; j < ode_.size_; ++j) y_res(j, i) = y[j];
        int n = ode_.N_;
        std::vector<double> dy_dt(n);
        std::vector<double> y_tmp(y.begin(), y.begin() + n);
        ode_.dbl_rhs_impl(y_tmp, dy_dt, ts[i].val());
        for (size_t j = 0; j < n; ++j) y_res(ode_.size_ + i * n + j, i) = dy_dt[j];
      }
    };

  public:
    /**
     * constructor
     * @param[in] rtol relative tolerance
     * @param[in] atol absolute tolerance
     * @param[in] max_num_steps max nb. of times steps
     */
    PMXOdeintIntegrator(const double rtol, const double atol,
                        const int64_t max_num_steps)
      : rtol_(rtol), atol_(atol), max_num_steps_(max_num_steps) {
      using stan::math::invalid_argument;
      if (rtol_ <= 0)
        invalid_argument("cvodes_integrator", "relative tolerance,", rtol_, "",
                         ", must be greater than 0");
      if (rtol_ > 1.0E-3)
        invalid_argument("cvodes_integrator", "relative tolerance,", rtol_, "",
                         ", must be less than 1.0E-3");
      if (atol_ <= 0)
        invalid_argument("cvodes_integrator", "absolute tolerance,", atol_, "",
                         ", must be greater than 0");
      if (max_num_steps_ <= 0)
        invalid_argument("cvodes_integrator", "max_num_steps,",
                         max_num_steps_, "",
                         ", must be greater than 0");
    }

    template <typename Ode, bool GenVar = true>
    auto integrate(Ode& ode) {
      std::vector<double> ts_vec(ode.ts_.size() + 1);
      ts_vec[0] = ode.t0_;
      for (size_t i = 0; i < ode.ts_.size(); ++i) {
        ts_vec[i + 1] = stan::math::value_of(ode.ts_[i]);
      }

      SolObserver<Ode, GenVar> observer(ode);

      const double init_dt = 0.1;
      integrate_times(make_dense_output(atol_, rtol_, scheme_t()),
                      boost::ref(ode), ode.y0_fwd_system,
                      ts_vec.begin(), ts_vec.end(),
                      init_dt, boost::ref(observer),
                      boost::numeric::odeint::max_step_checker(max_num_steps_));
      return observer.y;
    }
  };

}
}
#endif


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * k:结果最多只包含k个
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("BoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
	.Attr("classes_wise:bool")
	.Attr("k:int")
    .Input("bottom_box: T")
    .Input("classes: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsOp: public OpKernel {
	public:
		explicit BoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const int    data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const int    loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat[i];
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise && (bottom_classes_flat[j] != iclass)) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold) continue;
						keep_mask[j] = false;
					}
				}
			}

			const auto _out_size = count(keep_mask.begin(),keep_mask.end(),true);
			const auto out_size = k_>0?std::min<int>(k_,_out_size):_out_size;
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
				if(j>=out_size)
					break;
			}
		}
	private:
		float threshold    = 0.2;
		bool  classes_wise = true;
		int   k_           = -1;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsOp<CPUDevice, float>);
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * group:[Z,2]分组信息，分别为一个组里标签的开始与结束编号，不在分组信息的的默认为一个组
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("GroupBoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
    .Input("bottom_box: T")
    .Input("classes: int32")
    .Input("group: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class GroupBoxesNmsOp: public OpKernel {
	public:
		explicit GroupBoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			const Tensor &_group              = context->input(2);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();
			auto          group               = _group.template tensor<int,2>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));
			OP_REQUIRES(context, _group.dims() == 2, errors::InvalidArgument("group data must be 2-dimensional"));

			const auto   data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat[i];
                    const auto igroup = get_group(group,iclass);
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(igroup != get_group(group,bottom_classes_flat[j])) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold) continue;
						keep_mask[j] = false;
					}
				}
			}

			const auto out_size = count(keep_mask.begin(),keep_mask.end(),true);
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
			}
		}
        template<typename TM>
        int get_group(const TM& group_data,int label)
        {
            for(auto i=0; i<group_data.dimension(0); ++i) {
                if((label>=group_data(i,0)) && (label<=group_data(i,1)))
                    return i;
            }
            return -1;
        }
	private:
		float threshold    = 0.2;
};
REGISTER_KERNEL_BUILDER(Name("GroupBoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), GroupBoxesNmsOp<CPUDevice, float>);
/*
 * 数据不需要排序
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * confidence:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("BoxesSoftNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
	.Attr("classes_wise:bool")
	.Attr("delta:float")
    .Input("bottom_box: T")
    .Input("classes: int32")
    .Input("confidence:float")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesSoftNmsOp: public OpKernel {
    public:
        struct InterData
        {
            int index;
            float score;
            bool operator<(const InterData& v)const {
                return score<v.score;
            }
        };
        explicit BoxesSoftNmsOp(OpKernelConstruction* context) : OpKernel(context) {
            OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
            OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
            OP_REQUIRES_OK(context, context->GetAttr("delta", &delta));
        }

        void Compute(OpKernelContext* context) override
        {
            TIME_THIS();
            const Tensor &bottom_box          = context->input(0);
            const Tensor &bottom_classes      = context->input(1);
            const Tensor &confidence          = context->input(2);
            auto          bottom_box_flat     = bottom_box.flat<T>();
            auto          bottom_classes_flat = bottom_classes.flat<int32>();
            auto          confidence_flat     = confidence.flat<float>();

            OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
            OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));
            OP_REQUIRES(context, confidence.dims() == 1, errors::InvalidArgument("confidence data must be 1-dimensional"));

            const auto   data_nr           = bottom_box.dim_size(0);
            vector<InterData> set_D(data_nr,InterData({0,0.0f}));
            vector<InterData> set_B;
            const auto   loop_end          = data_nr-1;

            for(auto i=0; i<data_nr; ++i) {
                set_D[i].index = i;
                set_D[i].score = confidence_flat.data()[i];
            }
            set_B.reserve(data_nr);

            for(auto i=0; i<data_nr; ++i) {
                auto it = max_element(set_D.begin(),set_D.end());
                if(it->score<threshold)
                    break;
                auto M = *it;
                set_D.erase(it);
                set_B.push_back(M);
                const auto index = M.index;
                const auto iclass = bottom_classes_flat(index);
                for(auto& data:set_D) {
                    const auto j = data.index;
                    if(classes_wise && (bottom_classes_flat(j) != iclass)) continue;
                    const auto iou = bboxes_jaccard(bottom_box_flat.data()+index*4,bottom_box_flat.data()+j*4);
                    if(iou>1e-2)
                        data.score *= exp(-iou*iou/delta);
                }
            }
            sort(set_B.begin(),set_B.end(),[](const InterData& lhv, const InterData& rhv){ return lhv.index<rhv.index;});
            const auto out_size = set_B.size();
            int dims_2d[2] = {int(out_size),4};
            int dims_1d[1] = {int(out_size)};
            TensorShape  outshape0;
            TensorShape  outshape1;
            Tensor      *output_box         = NULL;
            Tensor      *output_classes     = NULL;
            Tensor      *output_index = NULL;

            TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
            TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

            OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
            OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
            OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

            auto obox     = output_box->template flat<T>();
            auto oclasses = output_classes->template flat<int32>();
            auto oindex   = output_index->template flat<int32>();

            for(auto j=0; j<out_size; ++j) {
                const auto i = set_B[j].index;
                auto box = bottom_box_flat.data()+i*4;
                std::copy(box,box+4,obox.data()+4*j);
                oclasses(j) = bottom_classes_flat(i);
                oindex(j) = i;
            }
        }
    private:
        float threshold    = 0.2;
        float delta        = 2.0;
        bool  classes_wise = true;
};
REGISTER_KERNEL_BUILDER(Name("BoxesSoftNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesSoftNmsOp<CPUDevice, float>);
/*
 * 与BoxesNms的主要区别为BoxesNmsNr使用输出人数来进行处理
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_indices:[X]
 * 输出时的相对位置不能改变
 * 程序会自动改变threshold的方式来使输出box的数量为k个
 */
REGISTER_OP("BoxesNmsNr")
    .Attr("T: {float, double,int32}")
	.Attr("classes_wise:bool")
	.Attr("k:int")
	.Attr("max_loop:int")
    .Input("bottom_box: T")
    .Input("classes:int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			int k = 0;
			c->GetAttr("k",&k);
			c->set_output(0, c->Matrix(k, 4));
			c->set_output(1, c->Vector(k));
			c->set_output(2, c->Vector(k));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsNrOp: public OpKernel {
	public:
		explicit BoxesNmsNrOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise_));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("max_loop", &max_loop_));
            if(k_ <= 0) k_ = 8;
            if(max_loop_ <= 0) max_loop_ = 4;
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>();
			auto          bottom_classes_flat = bottom_classes.flat<int32>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr               = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			vector<bool> old_keep_mask(data_nr,true);
			const auto   loop_end              = data_nr-1;
			int          old_nr                = 0;

            auto loop_fn = [&](float threshold) {
                if(old_nr>k_)
				    std::swap(old_keep_mask,keep_mask);
				fill(keep_mask.begin(),keep_mask.end(),true);
                auto keep_nr = keep_mask.size();
				for(auto i=0; i<loop_end; ++i) {
					if(!keep_mask.at(i)) continue;
					const auto iclass = bottom_classes_flat(i);
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise_ && (bottom_classes_flat(j) != iclass)) continue;
						if(!keep_mask.at(j)) continue;
						if(bboxes_jaccard(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4) < threshold) continue;
						keep_mask[j] = false;
						--keep_nr;
					}
				}
				//cout<<"keep nr:"<<keep_nr<<", threshold "<<threshold<<endl;
			   //keep_nr = count(keep_mask.begin(),keep_mask.end(),true);
               return keep_nr;
            };

            auto threshold_low   = 0.0;
            auto threshold_hight = 1.0;

            for(auto i=0; i<max_loop_; ++i) {
                auto threshold = (threshold_low+threshold_hight)/2.0;
                auto nr = loop_fn(threshold);
                old_nr = nr;
                if(nr == k_) break;
                if(nr>k_)
                    threshold_hight = threshold;
                else
                    threshold_low = threshold;
            }

			auto out_size = count(keep_mask.begin(),keep_mask.end(),true);

            if(k_>data_nr) k_ = data_nr;

            if(out_size<k_) {
                auto delta = k_-out_size;
                for(auto it=keep_mask.begin(); it!=keep_mask.end(); ++it)
                    if((*it) == false) {
                        (*it) = true;
                        --delta;
                        if(0 == delta) break;
                    }
                out_size = count(keep_mask.begin(),keep_mask.end(),true);
            } else if(out_size>k_) {
                auto nr = out_size-k_;
                for(auto it = keep_mask.rbegin(); it!=keep_mask.rend(); ++it) {
                    if((*it) == true) {
                        *it = false;
                        --nr;
                        if(0 == nr) break;
                    }
                }
            }
            out_size = k_;

			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();
			int  j        = 0;
			int  i        = 0;

			for(i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat.data()+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat(i);
				oindex(j) = i;
				++j;
				if(j>=out_size) break;
			}
            if(j<out_size) {
				cout<<"out size = "<<out_size<<", in size = "<<data_nr<<", j= "<<j<<std::endl;
                auto i = data_nr-1;
                for(;j<out_size; ++j) {
                    auto box = bottom_box_flat.data()+i*4;
                    std::copy(box,box+4,obox.data()+4*j);
                    oclasses(j) = bottom_classes_flat(i);
                    oindex(j) = i;
                }
            }
		}
	private:
		bool  classes_wise_ = true;
		int   k_            = 0;
		int   max_loop_     = 4;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNmsNr").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsNrOp<CPUDevice, float>);
/*
 * 与BoxesNmsNr的主要区别, 使用输入的theshold进行处理，选靠前的nr个boxes, 如果NMS后没有足够的boxes部分被删除的boxes会重新加入进来
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * threshold: 第一次处理时使用的阀值，与普通NMS中阀值的用法一致
 * fast_mode: True, 表示结果中box较少时，会简单的从前到后添加已经标记为删除的box, 否则会使用一种启发式的方式添加（规则类似于soft-nms)
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_indices:[X]
 * 输出时的相对位置不能改变
 * 程序会自动改变threshold的方式来使输出box的数量为k个
 */
REGISTER_OP("BoxesNmsNr2")
    .Attr("T: {float, double,int32}")
	.Attr("classes_wise:bool")
	.Attr("k:int")
	.Attr("threshold:float")
	.Attr("fast_mode:bool")
	.Attr("allow_less_output:bool=False")
    .Input("bottom_box: T")
    .Input("classes:int32")
    .Input("confidence:T")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			int k = 0;
            bool allow_less_output = false;
			c->GetAttr("k",&k);
			c->GetAttr("allow_less_output",&allow_less_output);
            if(allow_less_output) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
            } else {
			c->set_output(0, c->Matrix(k, 4));
			c->set_output(1, c->Vector(k));
			c->set_output(2, c->Vector(k));
            }
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsNr2Op: public OpKernel {
    private:
        struct InterData
        {
            InterData(int i,float s):index(i),score(s){}
            int index;
            float score;
            bool operator<(const InterData& v)const {
                return score<v.score;
            }
        };
	public:
		explicit BoxesNmsNr2Op(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise_));
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold_));
			OP_REQUIRES_OK(context, context->GetAttr("fast_mode", &fast_mode_));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("allow_less_output", &allow_less_output_));
            if(k_ <= 0) k_ = 1;
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("NmsNr2");
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			const Tensor &confidence          = context->input(2);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();
			auto          confidence_flat     = confidence.flat<T>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr               = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end              = data_nr-1;

            if((k_>data_nr) && (!allow_less_output_)) {
                cout<<"ERROR NMSNR2 input size is less than require ("<<data_nr<<" vs "<<k_<<")."<<endl;
            }

            if(k_<data_nr){
				for(auto i=0; i<loop_end; ++i) {
					if(!keep_mask.at(i)) continue;
					const auto iclass = bottom_classes_flat[i];
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise_ && (bottom_classes_flat[j] != iclass)) continue;
						if(!keep_mask[j]) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold_) continue;
						keep_mask[j] = false;
					}
				}
            };
			auto out_size = count(keep_mask.begin(),keep_mask.end(),true);

            if(out_size<k_) {
                if(k_<data_nr) {
                    /*
                     * 处理返回的box过少的情况
                     */
                    if(fast_mode_) {
                        /*
                         * 快速模式，从前往后加入已经删除的box
                         */
                        auto nr = k_-out_size;
                        for(auto it = keep_mask.begin(); it!=keep_mask.end(); ++it) {
                            if((*it) == false) {
                                *it = true;
                                --nr;
                                if(0 == nr) break;
                            }
                        }
                    } else {
                        /*
                         * 使用启发的方式添加box
                         */
                        auto delta = k_-out_size;
                        vector<InterData> datas;
                        datas.reserve((data_nr-out_size)/2);

                        for(auto it=keep_mask.begin(); it!=keep_mask.end(); ++it)
                            if((*it) == false) {
                                auto index = std::distance(keep_mask.begin(),it);
                                auto score = confidence_flat.data()[index];
                                for(auto kt=keep_mask.begin(); kt != it; ++kt) {
                                    if((*kt) == false)
                                        continue;
                                    auto index0 = std::distance(keep_mask.begin(),kt);
                                    auto iou = bboxes_jaccard(bottom_box_flat+index*4,bottom_box_flat+index0*4);
                                    score *= (1.0-iou);
                                }
                                datas.emplace_back(int(index),score);
                            }
                        for(auto x=0; x<delta; ++x) {
                            auto jt = max_element(datas.begin(),datas.end());
                            const auto index = jt->index;
                            keep_mask[jt->index] = true;
                            datas.erase(jt);
                            for(auto jt=datas.begin(); jt!=datas.end(); ++jt) {
                                auto index0 = jt->index;
                                auto iou = bboxes_jaccard(bottom_box_flat+index*4,bottom_box_flat+index0*4);
                                jt->score *= (1.0-iou);
                            }
                        }
                    }
                    out_size = k_;
                } else if(allow_less_output_)  {
                    out_size = data_nr;
                }
            } else if(out_size>k_) {
              /*
               * 如果返回的box过多，简单的从后往前删除多余的box
               */
                auto nr = out_size-k_;
                for(auto it = keep_mask.rbegin(); it!=keep_mask.rend(); ++it) {
                    if((*it) == true) {
                        *it = false;
                        --nr;
                        if(0 == nr) break;
                    }
                }
                out_size = k_;
            }

			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();
			int  j        = 0;
			int  i        = 0;

            obox.setZero();
            oindex.setZero();
            oclasses.setZero();

			for(i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
				if(j>=out_size) break;
			}
            /*
            if(j<out_size) {
				cout<<"out size = "<<out_size<<", in size = "<<data_nr<<", j= "<<j<<std::endl;
                auto i = data_nr-1;
                for(;j<out_size; ++j) {
                    auto box = bottom_box_flat+i*4;
                    std::copy(box,box+4,obox.data()+4*j);
                    oclasses(j) = bottom_classes_flat[i];
                    oindex(j) = i;
                }
            }
            */
		}
	private:
		bool  classes_wise_      = true;
		float threshold_         = 0.0;
		int   k_                 = 0;
		bool  fast_mode_         = false;
		bool  allow_less_output_ = false;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNmsNr2").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsNr2Op<CPUDevice, float>);
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * 用于处理目标不会重叠的情况
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("NoOverlapBoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold0:float")
	.Attr("threshold1:float")
	.Attr("classes_wise:bool")
    .Input("bottom_box: T")
    .Input("classes: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class NoOverlapBoxesNmsOp: public OpKernel {
	public:
		explicit NoOverlapBoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold0", &threshold0));
			OP_REQUIRES_OK(context, context->GetAttr("threshold1", &threshold1));
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>();
			auto          bottom_classes_flat = bottom_classes.flat<int32>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat(i);
                    for(auto j=i+1; j<data_nr; ++j) {
                        if(classes_wise && (bottom_classes_flat(j) != iclass)) continue;
                        if((bboxes_jaccard(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4) < threshold0)
                                && (bboxes_jaccard_of_box0(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4)<threshold1)
                                && (bboxes_jaccard_of_box0(bottom_box_flat.data()+j*4,bottom_box_flat.data()+i*4)<threshold1)
                          ) continue;
                        keep_mask[j] = false;
                    }
				}
			}

			const auto out_size = count(keep_mask.begin(),keep_mask.end(),true);
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat.data()+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat(i);
				oindex(j) = i;
				++j;
			}
		}
	private:
		float threshold0    = 0.2;
		float threshold1    = 0.8;
		bool  classes_wise = true;
};
REGISTER_KERNEL_BUILDER(Name("NoOverlapBoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), NoOverlapBoxesNmsOp<CPUDevice, float>);


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP

#include <iterator>
#include <utility>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost {

  template<typename RandomGenerator, typename Graph>
  class erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    erdos_renyi_iterator() : gen(0), n(0), edges(0), allow_self_loops(false) {}
    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         double prob = 0.0, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(edges_size_type(prob * n * n)),
        allow_self_loops(allow_self_loops)
    {
      if (is_undirected) edges = edges / 2;
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    erdos_renyi_iterator& operator++()
    {
      --edges;
      next();
      return *this;
    }

    erdos_renyi_iterator operator++(int)
    {
      erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const erdos_renyi_iterator& other) const
    { return edges == other.edges; }

    bool operator!=(const erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      uniform_int<vertices_size_type> rand_vertex(0, n-1);
      current.first = rand_vertex(*gen);
      do {
        current.second = rand_vertex(*gen);
      } while (current.first == current.second && !allow_self_loops);
    }

    RandomGenerator* gen;
    vertices_size_type n;
    edges_size_type edges;
    bool allow_self_loops;
    value_type current;
  };

} // end namespace boost

#endif // BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP


#include "stdafx.h"
#include <sstream>
#include <fstream>
#include <ostream>
#include <filesystem>
#include "PersistenceBarcodes.h"
#include "../TopoUtils_D64/StringUtils.h"

#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/regex.hpp>

namespace NPersistenceUtils {
    namespace fs = std::experimental::filesystem;

    template <typename Dtype>
    CPersistenceBarcodes<Dtype>::~CPersistenceBarcodes() {
    };

    template <typename Dtype>
    CPersistenceBarcodes<Dtype>::CPersistenceBarcodes(const std::wstring filename,
        const SHoleParam<Dtype> rprms, const SKerParam<Dtype> kprms) {
        auto dim = rprms.dim;
        auto valInfty = rprms.valInf;
        auto skipInfty = rprms.skipInf;
        auto threshold = rprms.threshold;

        auto single_tau = kprms.single_tau;
        auto max_tau    = kprms.max_tau;
		auto interval = kprms.interval;

        //std::cout << "Filename = " << NStringUtil::_w2s(filename) << std::endl;
        std::wifstream read_op;
        read_op.open(NStringUtil::_w2s(filename));
        if (!read_op.good()) {
            return;
        }
        else {
            while (read_op.good()) {
                std::wstring line;
                std::getline(read_op, line);

                if (line.size()) {
                    std::wstringstream s;
                    std::wstring wbirth, wdeath, wtau;
                    s << line;
                    s >> wbirth;
                    s >> wdeath;
                    s >> wtau;

                    Dtype birth, death, tau;
                    if (typeid(Dtype) == typeid(double)) {
                        birth = _wtof(wbirth.c_str());
                        death = _wtof(wdeath.c_str());
                        tau   = _wtof(wtau.c_str());
                    }
                    else {
                        birth = (float) _wtof(wbirth.c_str());
                        death = (float) _wtof(wdeath.c_str());
                        tau   = (float) _wtof(wtau.c_str());
                    }

                    // read with specified tau
                    if (max_tau > 0 && tau > max_tau)
                        break;
                    if (single_tau >= 0 && single_tau != tau)
                        continue;
					if (interval >= 2.0 && tau > 1.0 && ((int)(tau) % (int)(interval) != 0))
						continue;
                    if (skipInfty && death == std::numeric_limits<Dtype>::infinity()) {
                        continue;
                    }
                    if (death == std::numeric_limits<Dtype>::infinity()) {
                        death = std::max(birth, valInfty);
                    }
                    if (abs(death - birth) <= threshold) continue;
                    PushBar(birth, death, tau);
                }
                else {
                    break;
                }
            }
        }
        read_op.close();
        //std::cout << "Number of holes " << m_barcodes.size() << std::endl;
        m_dim = static_cast<size_t>(dim);
        m_filesrc = filename;
    }

    template <typename Dtype>
    CPersistenceBarcodes<Dtype>::CPersistenceBarcodes(const std::wstring filename,
        const SHoleParam<Dtype> rprms) {
        const SKerParam<Dtype> kprms(-1.0, 0.0, 1.0);
        CPersistenceBarcodes<Dtype>(filename, rprms, kprms);
    }

    template <typename Dtype>
    std::vector<Dtype> CPersistenceBarcodes<Dtype>::births() {
        std::vector<Dtype> birth_vecs;
        for (auto bar : m_barcodes) birth_vecs.push_back(bar->birth);
        return birth_vecs;
    }

    template <typename Dtype>
    std::vector<Dtype> CPersistenceBarcodes<Dtype>::deaths() {
        std::vector<Dtype> death_vecs;
        for (auto bar : m_barcodes) death_vecs.push_back(bar->death);
        return death_vecs;
    }

    template<typename Dtype>
    std::wstring CPersistenceBarcodes<Dtype>::ToFile(const std::wstring outpath, const std::wstring fileInitial)
    {
        namespace io = boost::iostreams;

        std::wstring outfile = outpath + L"/barcode_" + fileInitial + L"_dim_" + std::to_wstring(dim()) + L".txt";
        io::stream_buffer<io::file_sink> buf(NStringUtil::_w2s(outfile));
        std::ostream out(&buf);
        for (auto bar : m_barcodes) {
            out << bar->birth << " " << bar->death << std::endl;
        }
        return outfile;
    }

    // Get the median of an unordered set of numbers of arbitrary
    // type without modifying the underlying dataset.
    template <typename It>
    auto GetMedian(It begin, It end)
    {
        using T = typename std::iterator_traits<It>::value_type;
        std::vector<T> data(begin, end);
        std::nth_element(data.begin(), data.begin() + data.size() / 2, data.end());
        return data[data.size() / 2];
    }

    template<typename Dtype>
    Dtype CPersistenceBarcodes<Dtype>::GetOptimalTimeHole()
    {
        std::vector<Dtype> dis_diffs;
        Dtype tHole = Dtype(0.0);
        auto n = m_barcodes.size();
        for (size_t i = 0; i < n; ++i) {
            auto bar1 = m_barcodes[i];
            for (size_t j = i + 1; j < n; ++j) {
                auto bar2 = m_barcodes[j];
                auto xdiff = bar1->birth - bar2->birth;
                auto ydiff = bar1->death - bar2->death;
                dis_diffs.push_back(xdiff * xdiff + ydiff * ydiff);
            }
        }
        if (dis_diffs.empty() == false) {
            tHole = GetMedian(dis_diffs.begin(), dis_diffs.end());
        }
        return tHole;
    }

    template PersistenceUtils_D64_API struct SHoleParam<float>;
    template PersistenceUtils_D64_API struct SHoleParam<double>;

    template PersistenceUtils_D64_API struct SKerParam<float>;
    template PersistenceUtils_D64_API struct SKerParam<double>;

    template PersistenceUtils_D64_API class CPersistenceBarcodes<float>;
    template PersistenceUtils_D64_API class CPersistenceBarcodes<double>;
}

namespace NPersistenceUtils {
    PersistenceUtils_D64_API bool dummy_func()
    {
        return true;
    }

    template <typename Dtype>
    bool MakeDiagramVecFromBarcodesFile(TypeBarcodesPtrVec<Dtype> &diagram_vec,
        const std::wstring barlist_filename, const SHoleParam<Dtype> rprms, const SKerParam<Dtype> kprms) {
        // Read parameters from file
        std::wifstream read_op;
        read_op.open(barlist_filename);
        if (!read_op.good()) {
            return false;
        }
        else {
            while (read_op.good()) {
                std::wstring line;
                std::getline(read_op, line);

                if (!line.empty()) {
                    std::wstring bar_path = L"";
                    fs::path bpath(line);
                    if (fs::is_regular_file(bpath) == false) {
                        fs::path barlist_path(barlist_filename);
                        bpath = barlist_path.remove_filename() / bpath;
                        if (fs::is_regular_file(bpath)) bar_path = bpath.c_str();
                        else continue;
                    }
                    else {
                        bar_path = line;
                    }
                    //std::cout << "Line = " << NStringUtil::_w2s(line) << std::endl;
                    //std::cout << "barlist = " << NStringUtil::_w2s(barlist_filename) << std::endl;
                    //std::cout << "Barpath = " << NStringUtil::_w2s(bar_path) << std::endl;
                    CPersistenceBarcodesPtr<Dtype> bar_ptr(new CPersistenceBarcodes<Dtype>(bar_path, rprms, kprms));
                    diagram_vec.push_back(bar_ptr);
                }
            }
        }
        read_op.close();
        return true;
    }

    template <typename Dtype>
    PersistenceUtils_D64_API bool MakeDiagramVec(TypeBarcodesPtrVec<Dtype>& diagram_vec,
        const std::wstring barcodes_path,
        const SHoleParam<Dtype> rprms, const SKerParam<Dtype> kprms) {
        fs::path input_path(barcodes_path);

        if (fs::is_directory(input_path)) {
            // find barcode files in folder
            fs::directory_iterator it(input_path), eod;
            BOOST_FOREACH(fs::path const &p, std::make_pair(it, eod)) {
                if (fs::is_regular_file(p)) {
                    CPersistenceBarcodesPtr<Dtype> bar_ptr(new CPersistenceBarcodes<Dtype>(p.c_str(), rprms));
                    if (bar_ptr->IsEmpty() == false) {
                        diagram_vec.push_back(bar_ptr);
                    }
                }
            }
        }
        else if (fs::is_regular_file(input_path)) {
            MakeDiagramVecFromBarcodesFile(diagram_vec, barcodes_path, rprms, kprms);
        }
        else {
            return false;
        }
        return true;
    }
}