Freewheeling/native/route_search.cpp

#include <emscripten/bind.h>
#include <emscripten/val.h>

#include <iostream>
#include <fstream>
#include <arpa/inet.h>
#include <map>
#include <set>

// Constants
const float GRAVITY_ACCELERATION = 9.81;


// Data structures
struct Connection {
	int connected_point_number;
	float distance;
	uint8_t speed_limit;
	bool motorway;
	bool tunnel;
	bool againstOneWay;
};

struct RoadNode{
	float position_x;
	float position_y;
	float position_z;
	
	Connection connection_one;
	Connection connection_two;
	std::vector<Connection> *extra_connections;
};

struct RoadNodeSet {
	uint32_t numberOfNodes;
	RoadNode* roadNodes;
};

struct SearchNodeInfo {
	float distanceFromPrevious;
	float currentSpeed;
};

struct SearchResult {
	uint32_t startingNode;
	std::map<uint32_t, uint32_t> previous;
	std::map<uint32_t, SearchNodeInfo> reachableNodes;
};

struct JSNodeInfo {
	uint32_t nodeId;
	float positionX;
	float positionY;
	float positionZ;
	float distanceFromStart;
	float currentSpeed;
	float requiredSpeed;
};

struct JSSearchResult {
	std::vector<JSNodeInfo> endPoints;
};

struct ListNode {
	int id;
	float current_speed;
	
	ListNode* next = NULL;
};

struct PolygonCoordinate {
	float x;
	float y;
};

struct AreaSearchEntry {
	uint32_t nodeId;
	float positionX;
	float positionY;
	float longestRoute;
};

struct AreaSearchResult {
	uint32_t remainingNodes;
	std::vector<AreaSearchEntry> searchedNodes;
};

struct CurrentAreaSearch {
	float minimumSpeed;
	float maximumSpeed;
	float maximumSpeedLimit;
	float dragCoefficient;
	bool allowMotorways;
	bool allowTunnels;
	bool allowAgainstOneway;

	std::vector<uint32_t> startNodes;
	size_t startNodeCounter = 0;
	std::vector<AreaSearchEntry> currentAreaSearchResults;
	std::set<uint32_t> utilizedNodes;
};

// Data
RoadNodeSet set;
SearchResult lastSearchResult;
std::set<uint32_t> excludedNodes;

CurrentAreaSearch currentAreaSearch;

// Data loading functions
float getFloatFromBuffer(char* buffer) {
	// First, cast the buffer to an int, in order to reverse bytes from network order to host order
	uint32_t correctByteOrder = ntohl(*reinterpret_cast<uint32_t*>(buffer));
	return *((float*) &correctByteOrder);
}

void addLinkToRoadNode(RoadNode* roadNodes, int node_from, int node_to, uint8_t speed_limit, bool motorway, bool tunnel, bool againstOneWay) {
	float from_x = roadNodes[node_from].position_x;
	float from_y = roadNodes[node_from].position_y;
	float to_x = roadNodes[node_to].position_x;
	float to_y = roadNodes[node_to].position_y;

	float distance = sqrt(pow(to_x - from_x, 2) + pow(to_y - from_y, 2));

	Connection connection;
	connection.connected_point_number = node_to;
	connection.distance = distance;
	connection.speed_limit = speed_limit;
	connection.motorway = motorway;
	connection.tunnel = tunnel;
	connection.againstOneWay = againstOneWay;
	
	if (roadNodes[node_from].connection_one.connected_point_number == -1) {
		roadNodes[node_from].connection_one = connection;
	} else if (roadNodes[node_from].connection_two.connected_point_number == -1) {
		roadNodes[node_from].connection_two = connection;
	} else {
		if (roadNodes[node_from].extra_connections == NULL) {
			roadNodes[node_from].extra_connections = new std::vector<Connection>();
		}
		
		roadNodes[node_from].extra_connections->push_back(connection);
	}
}

void loadData(std::string filePath) {
	set = RoadNodeSet();
	
	std::ifstream source(filePath.c_str(), std::ios_base::binary);
	char *buffer = new char[4];
	source.read(buffer, 4);
	uint32_t number_of_entries = ntohl(*reinterpret_cast<uint32_t*>(buffer));
	std::cout << "Reading " << number_of_entries << " road nodes" << std::endl;
	
	// Create the memory space for all these road nodes
	set.numberOfNodes = number_of_entries;
	set.roadNodes = new RoadNode[number_of_entries];
	for (size_t i = 0; i < number_of_entries; i++) {
		// Each node in the file is of the type float x, float y, short z
		source.read(buffer, 4);
		// First, cast this to an int, reverse the byte order, and then cast to float
		float position_x = getFloatFromBuffer(buffer);
		source.read(buffer, 4);
		float position_y = getFloatFromBuffer(buffer);
		source.read(buffer, 2);
		int position_z_int = ntohs(*reinterpret_cast<uint16_t*>(buffer));
		float position_z = (position_z_int - 30000) / 10.0;
		
		set.roadNodes[i].position_x = position_x;
		set.roadNodes[i].position_y = position_y;
		set.roadNodes[i].position_z = position_z;
		
		set.roadNodes[i].connection_one.connected_point_number = -1;
		set.roadNodes[i].connection_two.connected_point_number = -1;
		
		set.roadNodes[i].extra_connections = NULL;
	}
	
	source.read(buffer, 4);
	uint32_t number_of_links = ntohl(*reinterpret_cast<uint32_t*>(buffer));
	std::cout << "Reading " << number_of_links << " links" << std::endl;
	
	// Read all the links between nodes
	int connection_vectors = 0;
	for (size_t i = 0; i < number_of_links; i++) {
		source.read(buffer, 4);
		uint32_t from_point = ntohl(*reinterpret_cast<uint32_t*>(buffer));
		source.read(buffer, 4);
		uint32_t to_point = ntohl(*reinterpret_cast<uint32_t*>(buffer));
		
		source.read(buffer, 1);
		uint8_t flags_byte = *reinterpret_cast<uint8_t*>(buffer);
		
		uint8_t speed_limit = (flags_byte >> 4) * 10;
		
		bool motorway = (flags_byte & 0x01) > 0;
		bool tunnel = (flags_byte & 0x02) > 0;
		bool passable_same_direction = (flags_byte & 0x04) > 0;
		bool passable_opposite_direction = (flags_byte & 0x08) > 0;

		addLinkToRoadNode(set.roadNodes, from_point, to_point, speed_limit, motorway, tunnel, !passable_same_direction);
		addLinkToRoadNode(set.roadNodes, to_point, from_point, speed_limit, motorway, tunnel, !passable_opposite_direction);
	}
	
	delete[] buffer;
}

// Search functions
JSNodeInfo findClosestNode(float position_x, float position_y) {
	float closestDistance = 1e99;
	uint32_t closestNode = 0;

	for (size_t i = 0; i < set.numberOfNodes; i++) {
		RoadNode node = set.roadNodes[i];
		float node_x = node.position_x;
		float node_y = node.position_y;

		float distance = sqrt(pow(position_x - node_x, 2) + pow(position_y - node_y, 2));
		if (distance < closestDistance) {
			closestDistance = distance;
			closestNode = i;
		}
	}

	JSNodeInfo result;
	result.nodeId = closestNode;
	result.positionX = set.roadNodes[closestNode].position_x;
	result.positionY = set.roadNodes[closestNode].position_y;
	result.positionZ = set.roadNodes[closestNode].position_z;

	return result;
}

float calculate_speed(float starting_speed, float horizontal_distance, float height_difference, float minimum_speed, float maximum_speed, float drag_coefficient) {
	float slope_tan = height_difference / horizontal_distance;
	float final_speed = -1;
	
	// If the slope is flat, that is one calculation
	if (fabs(slope_tan) < 0.0001) {
		float time_to_finish = (exp(horizontal_distance * drag_coefficient) - 1) / (starting_speed * drag_coefficient);
		final_speed = starting_speed / (starting_speed * drag_coefficient * time_to_finish + 1);
	} else {		
		// Otherwise, we need to find some parameters
		float slope = atan(slope_tan);
		float slope_sin = sin(slope);
		float full_distance = horizontal_distance * slope_tan / slope_sin;
		float acceleration = -GRAVITY_ACCELERATION * slope_sin;
		float terminal_velocity = sqrt(fabs(acceleration) / drag_coefficient);
		
		// Uphill
		if (slope > 0) {
			float time_to_peak = atan(starting_speed / terminal_velocity) / (drag_coefficient * terminal_velocity);
			// If the discriminant is greater than 1, the slope is so steep that we cannot reach the end with our starting speed
			float discriminant = cos(drag_coefficient * terminal_velocity * time_to_peak) * exp(full_distance * drag_coefficient);
			if (discriminant > 1.f) {
				return -1;
			}
			
			float time_to_reach_end = time_to_peak - acos(discriminant) / (drag_coefficient * terminal_velocity);
			final_speed = terminal_velocity * tan(drag_coefficient * terminal_velocity * (time_to_peak - time_to_reach_end));
		} else {
			// Downhill
			// If the starting speed is very close to the terminal velocity, we'll just stay at terminal velocity
			if (fabs(starting_speed - terminal_velocity) < 0.001) {
				final_speed = terminal_velocity;
			} else if (starting_speed < terminal_velocity) {
				float k1 = terminal_velocity * log((terminal_velocity + starting_speed) / (terminal_velocity - starting_speed)) * 0.5;
				float k2 = -log(cosh(k1 / terminal_velocity)) / drag_coefficient;
				float time_spent = acosh(exp(drag_coefficient * (full_distance - k2))) / (drag_coefficient * terminal_velocity) - k1 / (drag_coefficient * pow(terminal_velocity, 2));
				final_speed = terminal_velocity * tanh(drag_coefficient * terminal_velocity * time_spent + k1 / terminal_velocity);
			} else if (starting_speed > terminal_velocity) {
				float k1 = log((starting_speed - terminal_velocity) / (starting_speed + terminal_velocity)) * terminal_velocity / 2;
				float k2 = -log(-sinh(k1 / terminal_velocity)) / drag_coefficient;
				float time_spent = k1 / (drag_coefficient * pow(terminal_velocity, 2)) - asinh(-exp(drag_coefficient * (full_distance - k2))) / (drag_coefficient * terminal_velocity);
				final_speed = -terminal_velocity / tanh(k1 / terminal_velocity - drag_coefficient * terminal_velocity * time_spent);
			}
		}
	}
	
	if (final_speed < minimum_speed) {
		return -1;
	} else {
		return std::fmin(final_speed, maximum_speed);
	}
}

void getNeighbourConnections(RoadNode node, Connection* targetArray, int &numberOfConnections) {
	numberOfConnections = 0;
	if (node.connection_one.connected_point_number != -1) {
		*(targetArray + numberOfConnections) = node.connection_one;
		numberOfConnections++;
	}
	if (node.connection_two.connected_point_number != -1) {
		*(targetArray + numberOfConnections) = node.connection_two;
		numberOfConnections++;
	}
	if (node.extra_connections != NULL) {
		for (auto it = node.extra_connections->begin(); it != node.extra_connections->end(); it++) {
			*(targetArray + numberOfConnections) = *it;
			numberOfConnections++;
		}
	}
}

SearchResult findAllPathsFromPoint(int startingNode, float minimum_speed, float maximum_speed, int maximumSpeedLimit, float drag_coefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway) {
	SearchResult result;
	result.startingNode = startingNode;

	RoadNode firstNode = set.roadNodes[startingNode];
	SearchNodeInfo firstNodeInfo;
	firstNodeInfo.distanceFromPrevious = 0;
	firstNodeInfo.currentSpeed = minimum_speed;
	result.reachableNodes[startingNode] = firstNodeInfo;

	ListNode *nextNode = new ListNode;
	ListNode *lastNode = nextNode;
	
	nextNode->id = startingNode;
	nextNode->current_speed = minimum_speed;
	
	while (nextNode != NULL) {
		ListNode *currentNode = nextNode;
		nextNode = currentNode->next;
		
		int currentId = currentNode->id;
		float currentSpeed = currentNode->current_speed;

		RoadNode bestNode = set.roadNodes[currentId];
		
		if (lastNode == currentNode) {
			lastNode = NULL;
		}
		delete currentNode;
		
		Connection neighbours[10];
		int neighbourCounter = 0;
		getNeighbourConnections(bestNode, neighbours, neighbourCounter);
		
		for (int i = 0; i < neighbourCounter; i++) {
			Connection neighbour = neighbours[i];
			// First, if neighbour is in excluded area, skip it
			if (excludedNodes.find(neighbour.connected_point_number) != excludedNodes.end()) {
				continue;
			}

			if (neighbour.speed_limit > maximumSpeedLimit) {
				continue;
			}

			if (neighbour.motorway && !allowMotorways) {
				continue;
			}

			if (neighbour.tunnel && !allowTunnels) {
				continue;
			}

			if (neighbour.againstOneWay && ! allowAgainstOneway) {
				continue;
			}
			
			RoadNode neighbourNode = set.roadNodes[neighbour.connected_point_number];
			float heightDifference = neighbourNode.position_z - bestNode.position_z;
			float resultingSpeed = calculate_speed(currentSpeed, neighbour.distance, heightDifference, minimum_speed, maximum_speed, drag_coefficient);
			
			if (resultingSpeed < 0) {
				continue;
			}
			
			// Check if this node is already in the reachable nodes map
			auto resultIterator = result.reachableNodes.find(neighbour.connected_point_number);
			if (resultIterator != result.reachableNodes.end() && resultingSpeed <= resultIterator->second.currentSpeed) {
				continue;
			}
			
			SearchNodeInfo reachableNodeInfo;
			reachableNodeInfo.currentSpeed = resultingSpeed;
			reachableNodeInfo.distanceFromPrevious = neighbour.distance;
			result.reachableNodes[neighbour.connected_point_number] = reachableNodeInfo;
			
			result.previous[neighbour.connected_point_number] = currentId;
			
			ListNode *neighbourListNode = new ListNode;
			neighbourListNode->id = neighbour.connected_point_number;
			neighbourListNode->current_speed = reachableNodeInfo.currentSpeed;
			
			if (nextNode == NULL || resultingSpeed < nextNode->current_speed) {
				neighbourListNode->next = nextNode;
				nextNode = neighbourListNode;
			} else {
				ListNode* previousSearchNode = nextNode;
				ListNode* currentSearchNode = nextNode->next;
				
				while(currentSearchNode != NULL && currentSearchNode->current_speed > resultingSpeed) {
					previousSearchNode = currentSearchNode;
					currentSearchNode = currentSearchNode->next;
				}
				
				previousSearchNode->next = neighbourListNode;
				neighbourListNode->next = currentSearchNode;
			}
		}
	}
	
	return result;
}

JSSearchResult findAllPathsFromPointJS(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway) {
	lastSearchResult = findAllPathsFromPoint(startingNode, minimumSpeed, maximumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway);

	float start_x = set.roadNodes[startingNode].position_x;
	float start_y = set.roadNodes[startingNode].position_y;

		// Find all end points and sort them by distance
	std::set<uint32_t> notEndPoints;
	for (auto it = lastSearchResult.previous.begin(); it != lastSearchResult.previous.end(); it++) {
		notEndPoints.insert(it->second);
	}

	std::vector<JSNodeInfo> farthestEndpoints;
	for (auto it = lastSearchResult.reachableNodes.begin(); it != lastSearchResult.reachableNodes.end(); it++) {
		if (notEndPoints.find(it->first) == notEndPoints.end()) {
			JSNodeInfo entry;
			entry.nodeId = it->first;
			entry.positionX = set.roadNodes[entry.nodeId].position_x;
			entry.positionY = set.roadNodes[entry.nodeId].position_y;
			entry.distanceFromStart = sqrt(pow(entry.positionX - start_x, 2) + pow(entry.positionY - start_y, 2));

			farthestEndpoints.push_back(entry);
		}
	}

	std::sort(farthestEndpoints.begin(), farthestEndpoints.end(), [](JSNodeInfo first, JSNodeInfo second){return second.distanceFromStart < first.distanceFromStart;});

	// Discard all points that are too close to each other
	float minimumDistance = 300;

	std::vector<JSNodeInfo> filteredEndpoints;
	for (size_t i = 0; i < farthestEndpoints.size(); i++) {
		float closestNode = 1e99;
		JSNodeInfo currentNode = farthestEndpoints.at(i);
		for (size_t j = 0; j < filteredEndpoints.size(); j++) {
			JSNodeInfo otherNode = filteredEndpoints.at(j);
			float distance = sqrt(pow(otherNode.positionX - currentNode.positionX, 2) + pow(otherNode.positionY - currentNode.positionY, 2));
			if (distance < closestNode) {
				closestNode = distance;
				if (distance <= minimumDistance) {
					break;
				}
			}
		}

		if (closestNode > minimumDistance) {
			filteredEndpoints.push_back(currentNode);
		}
	}
	
	JSSearchResult searchResult;
	searchResult.endPoints = filteredEndpoints;
	return searchResult;
}

float calculateRequiredSpeed(float endSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
	// The bike will never reach faster speeds than terminal velocity in free fall
	float maximumSpeed = sqrt(GRAVITY_ACCELERATION / dragCoefficient);

	// First, check if we'll reach the required speed no matter what
	if (calculate_speed(0.00001, horizontalDistance, heightDifference, 0.0, maximumSpeed, dragCoefficient) >= endSpeed) {
		return 0.0;
	}

	// Divide and conquer
	float numerator = 0;
	float denominator = 0.5;
	float foundEndSpeed = -100.0;
	int loopCounter = 0;

	float closestFoundSpeed = 1e99;
	float bestNumerator = 1;
	float bestDenominator = 1;
	do {
		numerator *= 2;
		denominator *= 2;
		if (foundEndSpeed > endSpeed) {
			numerator -= 1;
		} else {
			numerator += 1;
		}
		foundEndSpeed = calculate_speed(maximumSpeed * numerator / denominator, horizontalDistance, heightDifference, 0.01, 100.0, dragCoefficient);
		loopCounter++;

		if (fabs(foundEndSpeed - endSpeed) < closestFoundSpeed) {
			closestFoundSpeed = foundEndSpeed;
			bestNumerator = numerator;
			bestDenominator = denominator;
		}
	} while (fabs(foundEndSpeed - endSpeed) > 0.013 && loopCounter < 32);

	float requiredSpeed = maximumSpeed * bestNumerator / bestDenominator;

	return requiredSpeed;
}

std::vector<JSNodeInfo> getPathJS(uint32_t startingNode, uint32_t endNode, float minimumSpeed, float dragCoefficient) {
	std::vector<JSNodeInfo> path;

	if (lastSearchResult.startingNode != startingNode) {
		return path;
	}

	if (lastSearchResult.reachableNodes.find(endNode) == lastSearchResult.reachableNodes.end()) {
		return path;
	}

	std::vector<uint32_t> nodes;
	uint32_t currentNode = endNode;
	nodes.push_back(currentNode);
	while (lastSearchResult.previous.find(currentNode) != lastSearchResult.previous.end()) {
		currentNode = lastSearchResult.previous.find(currentNode)->second;
		nodes.push_back(currentNode);
	}

	float currentDistance = 0;

	for (auto it = nodes.rbegin(); it != nodes.rend(); it++) {
		RoadNode roadNode = set.roadNodes[*it];
		JSNodeInfo nodeInfo;
		nodeInfo.nodeId = *it;
		nodeInfo.positionX = roadNode.position_x;
		nodeInfo.positionY = roadNode.position_y;
		nodeInfo.positionZ = roadNode.position_z;
		
		SearchNodeInfo searchNodeInfo = lastSearchResult.reachableNodes.find(*it)->second;
		nodeInfo.currentSpeed = searchNodeInfo.currentSpeed;
		currentDistance += searchNodeInfo.distanceFromPrevious;
		nodeInfo.distanceFromStart = currentDistance;

		path.push_back(nodeInfo);
	}

	float currentRequiredSpeed = -1.0;
	for (auto it = path.rbegin(); it != path.rend(); it++) {
		if (currentRequiredSpeed <= -1.0) {
			it->requiredSpeed = minimumSpeed;
			currentRequiredSpeed = minimumSpeed;
		} else {
			uint32_t currentNodeId = it->nodeId;
			RoadNode currentNode = set.roadNodes[currentNodeId];
			uint32_t nextNodeId = (it - 1)->nodeId;
			RoadNode nextNode = set.roadNodes[nextNodeId];

			float heightDifference = nextNode.position_z - currentNode.position_z;

			Connection neighbours[10];
			int numberOfNeighbours = 0;
			getNeighbourConnections(currentNode, neighbours, numberOfNeighbours);
			for (int i = 0; i < numberOfNeighbours; i++) {
				Connection neighbour = neighbours[i];
				if (neighbour.connected_point_number == nextNodeId) {
					float horizontalDistance = neighbour.distance;
					currentRequiredSpeed = fmax(calculateRequiredSpeed(currentRequiredSpeed, horizontalDistance, heightDifference, dragCoefficient), minimumSpeed);
					it->requiredSpeed = currentRequiredSpeed;
					break;
				}
			}
		}
	}

	return path;
}

bool isInsideRing(float pointX, float pointY, std::vector<PolygonCoordinate> ring) {
	// Draw a ray from the point directly towards the right, see if it crosses any line in the ring
	int crossings = 0;
	for (int i = 1; i < ring.size() + 1; i++) {
		int currentPoint = i % ring.size();
		int lastPoint = i - 1;

		float lastY = ring.at(lastPoint).y;
		float currentY = ring.at(currentPoint).y;

		if (pointY > fmax(lastY, currentY) || pointY < fmin(lastY, currentY)) {
			continue;
		}

		float lastX = ring.at(lastPoint).x;
		float currentX = ring.at(currentPoint).x;

		if (pointX > fmax(lastX, currentX)) {
			continue;
		}

		if (pointX < fmin(lastX, currentX)) {
			crossings += 1;
			continue;
		}

		// If we don't go cleanly through the bounding box, we have to be a bit more smart about this
		// We don't want to divide by zero. If the line is completely horizontal, we have a freak line which we will ignore
		if (fabs(lastY - currentY) < 0.00001) {
			continue;
		}

		float slope = (currentX - lastX) / (currentY - lastY);
		float xAtPointY = lastX + (pointY - lastY) * slope;

		if (xAtPointY >= pointX) {
			crossings += 1;
		}
	}

	return crossings % 2 == 1;
}

void getNodesWithinPolygons(std::vector<std::vector<std::vector<PolygonCoordinate>>> polygons, std::set<uint32_t> &resultSet) {
	// Add new ones
	for (auto polygon : polygons) {
		// Get a bounding box for each polygon
		float maxX = -1e99;
		float minX = 1e99;
		float maxY = -1e99;
		float minY = 1e99;

		for (auto ring : polygon) {
			for (auto coordinate : ring) {
				minX = fmin(minX, coordinate.x);
				minY = fmin(minY, coordinate.y);
				maxX = fmax(maxX, coordinate.x);
				maxY = fmax(maxY, coordinate.y);
			}
		}

		// Go through all nodes
		for (size_t nodeId = 0; nodeId < set.numberOfNodes; nodeId++) {
			RoadNode node = set.roadNodes[nodeId];
			// If the node is outside the bounding box, just move on
			if (node.position_x < minX || node.position_x > maxX || node.position_y < minY || node.position_y > maxY) {
				continue;
			}
			// Otherwise, count how many times a ray straight east from the point crosses the polygon's rings
			int crossings = 0;
			for (auto ring : polygon) {
				if (isInsideRing(node.position_x, node.position_y, ring)) {
					crossings += 1;
				}
			}

			if (crossings % 2 == 1) {
				resultSet.insert(nodeId);
			}
		}
	}
}

void excludeNodesWithinPolygons(std::vector<std::vector<std::vector<PolygonCoordinate>>> polygons) {
	// Clear any old excluded nodes
	excludedNodes.clear();
	getNodesWithinPolygons(polygons, excludedNodes);
}

std::vector<uint32_t> findPossibleStartNodes(float minimumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, std::vector<std::vector<std::vector<PolygonCoordinate>>> searchArea) {
	std::set<uint32_t> allNodesWithinSearchArea;
	getNodesWithinPolygons(searchArea, allNodesWithinSearchArea);

	float minimumSlope = asin(dragCoefficient * minimumSpeed * minimumSpeed / GRAVITY_ACCELERATION);
	float minimumSlopeTan = tan(minimumSlope);

	std::vector<uint32_t> possibleStartNodes;
	for (uint32_t nodeId : allNodesWithinSearchArea) {
		if (excludedNodes.find(nodeId) != excludedNodes.end()) {
			continue;
		}

		bool hasWayOut = false;
		bool hasWayIn = false;

		RoadNode node = set.roadNodes[nodeId];
		Connection neighbours[10];
		int numberOfNeighbours = 0;
		getNeighbourConnections(node, neighbours, numberOfNeighbours);

		for (int i = 0; i < numberOfNeighbours; i++) {
			Connection connection = neighbours[i];
			if (excludedNodes.find(connection.connected_point_number) != excludedNodes.end()) {
				continue;
			}
			if (connection.motorway && !allowMotorways) {
				continue;
			}
			if (connection.tunnel && !allowTunnels) {
				continue;
			}
			if (connection.againstOneWay && !allowAgainstOneway) {
				continue;
			}

			RoadNode neighbourNode = set.roadNodes[connection.connected_point_number];
			float slopeTan = (neighbourNode.position_z - node.position_z) / connection.distance;
			if (slopeTan > minimumSlopeTan) {
				hasWayIn = true;
				break;
			} else if (slopeTan < -minimumSlopeTan) {
				hasWayOut = true;
			}
		}

		if (hasWayOut && !hasWayIn) {
			// Insertion sort by height
			possibleStartNodes.insert(
				std::upper_bound(
					possibleStartNodes.begin(),
					possibleStartNodes.end(),
					nodeId,
					[](uint32_t nodeOne, uint32_t nodeTwo){return set.roadNodes[nodeOne].position_z > set.roadNodes[nodeTwo].position_z;}
				),
				nodeId
			);
		}
	}

	return possibleStartNodes;
}

AreaSearchResult startAreaSearch(float minimumSpeed, float maximumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, std::vector<std::vector<std::vector<PolygonCoordinate>>> searchArea) {
	currentAreaSearch = CurrentAreaSearch();
	currentAreaSearch.startNodes = findPossibleStartNodes(minimumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway, searchArea);
	currentAreaSearch.minimumSpeed = minimumSpeed;
	currentAreaSearch.maximumSpeed = maximumSpeed;
	currentAreaSearch.maximumSpeedLimit = maximumSpeedLimit;
	currentAreaSearch.dragCoefficient = dragCoefficient;
	currentAreaSearch.allowMotorways = allowMotorways;
	currentAreaSearch.allowTunnels = allowTunnels;
	currentAreaSearch.allowAgainstOneway = allowAgainstOneway;

	AreaSearchResult result;
	result.remainingNodes = currentAreaSearch.startNodes.size();
	return result;
}

AreaSearchResult continueAreaSearch() {
	uint32_t currentStartNode = currentAreaSearch.startNodes.at(currentAreaSearch.startNodeCounter);
	SearchResult result = findAllPathsFromPoint(
		currentStartNode,
		currentAreaSearch.minimumSpeed,
		currentAreaSearch.maximumSpeed,
		currentAreaSearch.maximumSpeedLimit,
		currentAreaSearch.dragCoefficient,
		currentAreaSearch.allowMotorways,
		currentAreaSearch.allowTunnels,
		currentAreaSearch.allowAgainstOneway
	);

	// Remove all nodes we have reached from here as possible future start nodes
	auto startNodeIterator = currentAreaSearch.startNodes.begin() + currentAreaSearch.startNodeCounter + 1;
	while (startNodeIterator != currentAreaSearch.startNodes.end()) {
		uint32_t startNodeId = *startNodeIterator;
		if (result.reachableNodes.find(startNodeId) != result.reachableNodes.end()) {
			startNodeIterator = currentAreaSearch.startNodes.erase(startNodeIterator);
		} else {
			startNodeIterator++;
		}
	}

	// Find the farthest reachable point from our current start point
	std::set<uint32_t> notEndPoints;
	for (auto it = result.previous.begin(); it != result.previous.end(); it++) {
		notEndPoints.insert(it->second);
	}

	RoadNode startNode = set.roadNodes[currentStartNode];
	float farthestDistance = 0;
	uint32_t farthestNode = 0;
	for (auto it = result.reachableNodes.begin(); it != result.reachableNodes.end(); it++) {
		if (notEndPoints.find(it->first) == notEndPoints.end()) {
			RoadNode endNode = set.roadNodes[it->first];
			float distance = sqrt(pow(endNode.position_x - startNode.position_x, 2) + pow( endNode.position_y - startNode.position_y, 2));
			if (distance > farthestDistance) {
				farthestDistance = distance;
				farthestNode = it->first;
			}
		}
	}

	std::set<uint32_t> path;
	path.insert(farthestNode);
	uint32_t currentNode = farthestNode;
	while (result.previous.find(currentNode) != result.previous.end()) {
		currentNode = result.previous.find(currentNode)->second;
		path.insert(currentNode);
	}

	// Check if our path overlaps too much with already travelled paths
	std::vector<uint32_t> overlap;
	std::set_intersection(path.begin(), path.end(), currentAreaSearch.utilizedNodes.begin(), currentAreaSearch.utilizedNodes.end(), std::back_inserter(overlap));
	if (overlap.size() < 0.5 * path.size()) {
		AreaSearchEntry searchEntry;
		searchEntry.nodeId = currentStartNode;
		searchEntry.positionX = startNode.position_x;
		searchEntry.positionY = startNode.position_y;
		searchEntry.longestRoute = farthestDistance;

		currentAreaSearch.currentAreaSearchResults.insert(
			std::upper_bound(
				currentAreaSearch.currentAreaSearchResults.begin(),
				currentAreaSearch.currentAreaSearchResults.end(),
				searchEntry,
				[](AreaSearchEntry one, AreaSearchEntry two){return one.longestRoute > two.longestRoute;}
			),
			searchEntry
		);

		for (uint32_t pathNode : path) {
			currentAreaSearch.utilizedNodes.insert(pathNode);
		}
	}
	
	currentAreaSearch.startNodeCounter++;

	AreaSearchResult searchResult;
	searchResult.remainingNodes = currentAreaSearch.startNodes.size() - currentAreaSearch.startNodeCounter;
	searchResult.searchedNodes = currentAreaSearch.currentAreaSearchResults;

	return searchResult;
}

EMSCRIPTEN_BINDINGS(my_module) {
	emscripten::class_<JSNodeInfo>("NodeInfo")
		.constructor<>()
		.property("nodeId", &JSNodeInfo::nodeId)
		.property("positionX", &JSNodeInfo::positionX)
		.property("positionY", &JSNodeInfo::positionY)
		.property("positionZ", &JSNodeInfo::positionZ)
		.property("currentSpeed", &JSNodeInfo::currentSpeed)
		.property("requiredSpeed", &JSNodeInfo::requiredSpeed)
		.property("distanceFromStart", &JSNodeInfo::distanceFromStart);

	emscripten::class_<JSSearchResult>("SearchResult")
		.constructor<>()
		.property("endPoints", &JSSearchResult::endPoints);

	emscripten::class_<PolygonCoordinate>("PolygonCoordinate")
		.constructor<>()
		.property("x", &PolygonCoordinate::x)
		.property("y", &PolygonCoordinate::y);

	emscripten::class_<AreaSearchEntry>("AreaSearchEntry")
		.constructor<>()
		.property("nodeId", &AreaSearchEntry::nodeId)
		.property("positionX", &AreaSearchEntry::positionX)
		.property("positionY", &AreaSearchEntry::positionY)
		.property("longestRoute", &AreaSearchEntry::longestRoute);

	emscripten::class_<AreaSearchResult>("AreaSearchResult")
		.constructor<>()
		.property("remainingNodes", &AreaSearchResult::remainingNodes)
		.property("searchedNodes", &AreaSearchResult::searchedNodes);

	emscripten::register_vector<JSNodeInfo>("NodeInfoArray");
	emscripten::register_vector<PolygonCoordinate>("Ring");
	emscripten::register_vector<std::vector<PolygonCoordinate>>("Polygon");
	emscripten::register_vector<std::vector<std::vector<PolygonCoordinate>>>("MultiPolygon");
	emscripten::register_vector<AreaSearchEntry>("AreaSearchEntries");
	
	emscripten::function("loadData", &loadData);
	emscripten::function("findClosestNode", &findClosestNode);
	emscripten::function("findAllPathsFromPoint", &findAllPathsFromPointJS);
	emscripten::function("getPath", &getPathJS);
	emscripten::function("excludeNodesWithinPolygons", &excludeNodesWithinPolygons);
	emscripten::function("startAreaSearch", &startAreaSearch);
	emscripten::function("continueAreaSearch", &continueAreaSearch);
}