#ifdef __EMSCRIPTEN__
#include <emscripten/bind.h>
#include <emscripten/val.h>
#endif

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

// Constants
const float GRAVITY_ACCELERATION = 9.81;


// Data structures
struct Connection {
	int connectedPointNumber;
	float distance;
	float course;
	uint8_t speedLimit;
	bool motorway;
	bool tunnel;
	bool againstOneWay;
};

struct RoadNode{
	float positionX;
	float positionY;
	float positionZ;
	
	Connection connectionOne;
	Connection connectionTwo;
	std::vector<Connection> *extraConnections;
};

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 currentSpeed;
	float currentCourse;
	
	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;
	bool limitCornerSpeed;

	std::vector<uint32_t> startNodes;
	size_t startNodeCounter = 0;
	std::vector<AreaSearchEntry> currentAreaSearchResults;
	std::map<uint32_t, std::vector<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, uint32_t nodeFrom, uint32_t nodeTo, uint8_t speedLimit, bool motorway, bool tunnel, bool againstOneWay) {
	float fromX = roadNodes[nodeFrom].positionX;
	float fromY = roadNodes[nodeFrom].positionY;
	float toX = roadNodes[nodeTo].positionX;
	float toY = roadNodes[nodeTo].positionY;

	float distance = sqrt(pow(toX - fromX, 2) + pow(toY - fromY, 2));
	float course = atan2(toX - fromX, toY - fromY) * 180 / 3.14152965;

	Connection connection;
	connection.connectedPointNumber = nodeTo;
	connection.distance = distance;
	connection.course = course;
	connection.speedLimit = speedLimit;
	connection.motorway = motorway;
	connection.tunnel = tunnel;
	connection.againstOneWay = againstOneWay;
	
	if (roadNodes[nodeFrom].connectionOne.connectedPointNumber == -1) {
		roadNodes[nodeFrom].connectionOne = connection;
	} else if (roadNodes[nodeFrom].connectionTwo.connectedPointNumber == -1) {
		roadNodes[nodeFrom].connectionTwo = connection;
	} else {
		if (roadNodes[nodeFrom].extraConnections == NULL) {
			roadNodes[nodeFrom].extraConnections = new std::vector<Connection>();
		}
		
		roadNodes[nodeFrom].extraConnections->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 numberOfEntries = ntohl(*reinterpret_cast<uint32_t*>(buffer));
	std::cout << "Reading " << numberOfEntries << " road nodes" << std::endl;
	
	// Create the memory space for all these road nodes
	set.numberOfNodes = numberOfEntries;
	set.roadNodes = new RoadNode[numberOfEntries];
	for (size_t i = 0; i < numberOfEntries; 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 positionX = getFloatFromBuffer(buffer);
		source.read(buffer, 4);
		float positionY = getFloatFromBuffer(buffer);
		source.read(buffer, 2);
		int positionZInt = ntohs(*reinterpret_cast<uint16_t*>(buffer));
		float positionZ = (positionZInt - 30000) / 10.0;
		
		set.roadNodes[i].positionX = positionX;
		set.roadNodes[i].positionY = positionY;
		set.roadNodes[i].positionZ = positionZ;
		
		set.roadNodes[i].connectionOne.connectedPointNumber = -1;
		set.roadNodes[i].connectionTwo.connectedPointNumber = -1;
		
		set.roadNodes[i].extraConnections = NULL;
	}
	
	source.read(buffer, 4);
	uint32_t numberOfLinks = ntohl(*reinterpret_cast<uint32_t*>(buffer));
	std::cout << "Reading " << numberOfLinks << " links" << std::endl;
	
	// Read all the links between nodes
	for (size_t i = 0; i < numberOfLinks; i++) {
		source.read(buffer, 4);
		uint32_t fromPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
		source.read(buffer, 4);
		uint32_t toPoint = ntohl(*reinterpret_cast<uint32_t*>(buffer));
		
		source.read(buffer, 1);
		uint8_t flagsByte = *reinterpret_cast<uint8_t*>(buffer);
		
		uint8_t speedLimit = (flagsByte >> 4) * 10;
		
		bool motorway = (flagsByte & 0x01) > 0;
		bool tunnel = (flagsByte & 0x02) > 0;
		bool passableSameDirection = (flagsByte & 0x04) > 0;
		bool passableOppositeDirection = (flagsByte & 0x08) > 0;

		addLinkToRoadNode(set.roadNodes, fromPoint, toPoint, speedLimit, motorway, tunnel, !passableSameDirection);
		addLinkToRoadNode(set.roadNodes, toPoint, fromPoint, speedLimit, motorway, tunnel, !passableOppositeDirection);
	}
	
	delete[] buffer;
}

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

	for (size_t i = 0; i < set.numberOfNodes; i++) {
		RoadNode node = set.roadNodes[i];
		float nodeX = node.positionX;
		float nodeY = node.positionY;

		float distance = sqrt(pow(positionX - nodeX, 2) + pow(positionY - nodeY, 2));
		if (distance < closestDistance) {
			closestDistance = distance;
			closestNode = i;
		}
	}

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

	return result;
}

float calculateSpeed(float startingSpeed, float horizontalDistance, float heightDifference, float minimumSpeed, float maximumSpeed, float dragCoefficient) {
	float slopeTan = heightDifference / horizontalDistance;
	float finalSpeed = -1;
	
	// If the slope is flat, that is one calculation
	if (fabs(slopeTan) < 0.0001) {
		float timeToFinish = (exp(horizontalDistance * dragCoefficient) - 1) / (startingSpeed * dragCoefficient);
		finalSpeed = startingSpeed / (startingSpeed * dragCoefficient * timeToFinish + 1);
	} else {		
		// Otherwise, we need to find some parameters
		float slope = atan(slopeTan);
		float slopeSin = sin(slope);
		float fullDistance = horizontalDistance * slopeTan / slopeSin;
		float acceleration = -GRAVITY_ACCELERATION * slopeSin;
		float terminalVelocity = sqrt(fabs(acceleration) / dragCoefficient);
		
		// Uphill
		if (slope > 0) {
			float timeToPeak = atan(startingSpeed / terminalVelocity) / (dragCoefficient * terminalVelocity);
			// 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(dragCoefficient * terminalVelocity * timeToPeak) * exp(fullDistance * dragCoefficient);
			if (discriminant > 1.f) {
				return -1;
			}
			
			float timeToReachEnd = timeToPeak - acos(discriminant) / (dragCoefficient * terminalVelocity);
			finalSpeed = terminalVelocity * tan(dragCoefficient * terminalVelocity * (timeToPeak - timeToReachEnd));
		} else {
			// Downhill
			// If the starting speed is very close to the terminal velocity, we'll just stay at terminal velocity
			if (fabs(startingSpeed - terminalVelocity) < 0.001) {
				finalSpeed = terminalVelocity;
			} else if (startingSpeed < terminalVelocity) {
				float k1 = terminalVelocity * log((terminalVelocity + startingSpeed) / (terminalVelocity - startingSpeed)) * 0.5;
				float k2 = -log(cosh(k1 / terminalVelocity)) / dragCoefficient;
				float timeSpent = acosh(exp(dragCoefficient * (fullDistance - k2))) / (dragCoefficient * terminalVelocity) - k1 / (dragCoefficient * pow(terminalVelocity, 2));
				finalSpeed = terminalVelocity * tanh(dragCoefficient * terminalVelocity * timeSpent + k1 / terminalVelocity);
			} else if (startingSpeed > terminalVelocity) {
				float k1 = log((startingSpeed - terminalVelocity) / (startingSpeed + terminalVelocity)) * terminalVelocity / 2;
				float k2 = -log(-sinh(k1 / terminalVelocity)) / dragCoefficient;
				float timeSpent = k1 / (dragCoefficient * pow(terminalVelocity, 2)) - asinh(-exp(dragCoefficient * (fullDistance - k2))) / (dragCoefficient * terminalVelocity);
				finalSpeed = -terminalVelocity / tanh(k1 / terminalVelocity - dragCoefficient * terminalVelocity * timeSpent);
			}
		}
	}
	
	if (finalSpeed < minimumSpeed) {
		return -1;
	} else {
		return std::fmin(finalSpeed, maximumSpeed);
	}
}

float calculateRequiredSpeed(float endSpeed, float horizontalDistance, float heightDifference, float dragCoefficient) {
	float slopeTan = heightDifference / horizontalDistance;
	// If it's flat, the calculation is kinda simple
	if (fabs(slopeTan) < 0.0001) {
		return endSpeed * exp(horizontalDistance * dragCoefficient);
	}

	// If it's not flat, we're going to need the terminal velocity
	float slope = atan(slopeTan);
	float slopeSin = sin(slope);
	float fullDistance = horizontalDistance * slopeTan / slopeSin;
	float acceleration = -GRAVITY_ACCELERATION * slopeSin;
	float terminalVelocity = sqrt(fabs(acceleration) / dragCoefficient);

	// Uphill
	if (slope > 0) {
		float timeToPeak = acos(cos(atan(endSpeed / terminalVelocity))/exp(fullDistance * dragCoefficient)) / (terminalVelocity * dragCoefficient);
		return terminalVelocity * tan(terminalVelocity * dragCoefficient * timeToPeak);
	}

	// Downhill, end speed equal to terminal velocity
	if (fabs(endSpeed - terminalVelocity) < 0.001) {
		return terminalVelocity;
	}

	// Downhill, end speed higher than terminal velocity
	if (endSpeed > terminalVelocity) {
		float k1 = asinh(exp(-fullDistance * dragCoefficient) * sinh(atanh(-terminalVelocity / endSpeed)));
		return -terminalVelocity / tanh(k1);
	}

	// If we got here, we're going downhill with an end speed lower than terminal velocity
	float discriminant = cosh(atanh(endSpeed / terminalVelocity)) / exp(fullDistance * dragCoefficient);
	if (discriminant >= 1) {
		return terminalVelocity * tanh(acosh(discriminant));
	}

	// If we got here, we will gain a higher speed than required even when starting from zero.
	return 0;
}

void getNeighbourConnections(RoadNode node, Connection* targetArray, int &numberOfConnections) {
	numberOfConnections = 0;
	if (node.connectionOne.connectedPointNumber != -1) {
		*(targetArray + numberOfConnections) = node.connectionOne;
		numberOfConnections++;
	}
	if (node.connectionTwo.connectedPointNumber != -1) {
		*(targetArray + numberOfConnections) = node.connectionTwo;
		numberOfConnections++;
	}
	if (node.extraConnections != NULL) {
		for (auto it = node.extraConnections->begin(); it != node.extraConnections->end(); it++) {
			*(targetArray + numberOfConnections) = *it;
			numberOfConnections++;
		}
	}
}

SearchResult findAllPathsFromPoint(int startingNode, float minimumSpeed, float maximumSpeed, int maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed) {
	SearchResult result;
	result.startingNode = startingNode;

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

	ListNode *nextNode = new ListNode;
	
	nextNode->id = startingNode;
	nextNode->currentSpeed = minimumSpeed;
	nextNode->currentCourse = 0;
	
	while (nextNode != NULL) {
		ListNode *currentNode = nextNode;
		nextNode = currentNode->next;
		
		int currentId = currentNode->id;
		float currentSpeed = currentNode->currentSpeed;
		float currentCourse = currentNode->currentCourse;

		RoadNode bestNode = set.roadNodes[currentId];
		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.connectedPointNumber) != excludedNodes.end()) {
				continue;
			}

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

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

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

			if (neighbour.againstOneWay && ! allowAgainstOneway) {
				continue;
			}
			
			RoadNode neighbourNode = set.roadNodes[neighbour.connectedPointNumber];
			float heightDifference = neighbourNode.positionZ - bestNode.positionZ;
			float resultingSpeed = calculateSpeed(currentSpeed, neighbour.distance, heightDifference, minimumSpeed, maximumSpeed, dragCoefficient);
			
			if (resultingSpeed < 0) {
				continue;
			}

			// If we limit the speed on corners, do that here
			if (limitCornerSpeed) {
				float courseDifference = fabs(currentCourse - neighbour.course);
				if (courseDifference > 180.0) {
					courseDifference = 360 - courseDifference;
				}

				if (courseDifference > 95) {
					resultingSpeed = minimumSpeed;
				} else if (courseDifference > 45.0) {
					float maximumCornerSpeed = (95 - courseDifference) / 50.0 * (maximumSpeed - minimumSpeed) + minimumSpeed;
					resultingSpeed = fmin(resultingSpeed, maximumCornerSpeed);
				}
			}
			
			// Check if this node is already in the reachable nodes map
			auto resultIterator = result.reachableNodes.find(neighbour.connectedPointNumber);
			if (resultIterator != result.reachableNodes.end() && resultingSpeed <= resultIterator->second.currentSpeed) {
				continue;
			}
			
			SearchNodeInfo reachableNodeInfo;
			reachableNodeInfo.currentSpeed = resultingSpeed;
			reachableNodeInfo.distanceFromPrevious = neighbour.distance;
			result.reachableNodes[neighbour.connectedPointNumber] = reachableNodeInfo;
			
			result.previous[neighbour.connectedPointNumber] = currentId;
			
			ListNode *neighbourListNode = new ListNode;
			neighbourListNode->id = neighbour.connectedPointNumber;
			neighbourListNode->currentSpeed = reachableNodeInfo.currentSpeed;
			neighbourListNode->currentCourse = neighbour.course;
			
			if (nextNode == NULL || resultingSpeed < nextNode->currentSpeed) {
				neighbourListNode->next = nextNode;
				nextNode = neighbourListNode;
			} else {
				ListNode* previousSearchNode = nextNode;
				ListNode* currentSearchNode = nextNode->next;
				
				while(currentSearchNode != NULL && currentSearchNode->currentSpeed > 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, bool limitCornerSpeed) {
	lastSearchResult = findAllPathsFromPoint(startingNode, minimumSpeed, maximumSpeed, maximumSpeedLimit, dragCoefficient, allowMotorways, allowTunnels, allowAgainstOneway, limitCornerSpeed);

	float startX = set.roadNodes[startingNode].positionX;
	float startY = set.roadNodes[startingNode].positionY;

		// 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].positionX;
			entry.positionY = set.roadNodes[entry.nodeId].positionY;
			entry.distanceFromStart = sqrt(pow(entry.positionX - startX, 2) + pow(entry.positionY - startY, 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;
}

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.positionX;
		nodeInfo.positionY = roadNode.positionY;
		nodeInfo.positionZ = roadNode.positionZ;
		
		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.positionZ - currentNode.positionZ;

			Connection neighbours[10];
			int numberOfNeighbours = 0;
			getNeighbourConnections(currentNode, neighbours, numberOfNeighbours);
			for (int i = 0; i < numberOfNeighbours; i++) {
				Connection neighbour = neighbours[i];
				if (neighbour.connectedPointNumber == 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.positionX < minX || node.positionX > maxX || node.positionY < minY || node.positionY > 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.positionX, node.positionY, 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.connectedPointNumber) != excludedNodes.end()) {
				continue;
			}
			if (connection.motorway && !allowMotorways) {
				continue;
			}
			if (connection.tunnel && !allowTunnels) {
				continue;
			}
			if (connection.againstOneWay && !allowAgainstOneway) {
				continue;
			}

			RoadNode neighbourNode = set.roadNodes[connection.connectedPointNumber];
			float slopeTan = (neighbourNode.positionZ - node.positionZ) / 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].positionZ > set.roadNodes[nodeTwo].positionZ;}
				),
				nodeId
			);
		}
	}

	return possibleStartNodes;
}

AreaSearchResult startAreaSearch(float minimumSpeed, float maximumSpeed, float maximumSpeedLimit, float dragCoefficient, bool allowMotorways, bool allowTunnels, bool allowAgainstOneway, bool limitCornerSpeed, 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;
	currentAreaSearch.limitCornerSpeed = limitCornerSpeed;

	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,
		currentAreaSearch.limitCornerSpeed
	);

	// 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.positionX - startNode.positionX, 2) + pow( endNode.positionY - startNode.positionY, 2));
			if (distance > farthestDistance) {
				farthestDistance = distance;
				farthestNode = it->first;
			}
		}
	}

	std::map<uint32_t, std::vector<uint32_t>> path;
	path[farthestNode] = std::vector<uint32_t>();
	uint32_t currentNode = farthestNode;
	while (result.previous.find(currentNode) != result.previous.end()) {
		currentNode = result.previous.find(currentNode)->second;
		path[currentNode] = std::vector<uint32_t>();
	}

	// Check if our path overlaps too much with already travelled paths
	std::map<uint32_t, std::vector<uint32_t>> overlap;
	auto inserterIterator = overlap.begin();
	std::set_intersection(
		currentAreaSearch.utilizedNodes.begin(),
		currentAreaSearch.utilizedNodes.end(),
		path.begin(),
		path.end(),
		std::inserter(overlap, inserterIterator),
		currentAreaSearch.utilizedNodes.value_comp()
	);

	std::map<uint32_t, uint32_t> overlapCounts;
	for (auto it = overlap.begin(); it != overlap.end(); it++) {
		for (auto startingNodeIt = it->second.begin(); startingNodeIt != it->second.end(); startingNodeIt++) {
			uint32_t startingNode = *startingNodeIt;
			if (overlapCounts.find(startingNode) == overlapCounts.end()) {
				overlapCounts[startingNode] = 0;
			}
			overlapCounts[startingNode] = overlapCounts[startingNode] + 1;
		}
	}

	auto overlapIterator = overlapCounts.begin();
	while (overlapIterator != overlapCounts.end()) {
		uint32_t overlapCount = overlapIterator->second;
		if (overlapCount < path.size() * 0.5) {
			overlapIterator = overlapCounts.erase(overlapIterator);
			continue;
		}

		// In case we overlap more than 50% with another path, check if the current path is longer
		uint32_t otherPath = overlapIterator->first;
		bool isLongerThanOtherPath = true;
		for (auto otherPathIterator = currentAreaSearch.currentAreaSearchResults.begin(); otherPathIterator != currentAreaSearch.currentAreaSearchResults.end(); otherPathIterator++) {
			if (otherPathIterator->nodeId != otherPath) {
				continue;
			}

			if (farthestDistance > otherPathIterator->longestRoute) {
				currentAreaSearch.currentAreaSearchResults.erase(otherPathIterator);
				break;
			} else {
				isLongerThanOtherPath = false;
				break;
			}
		}

		if (isLongerThanOtherPath) {
			overlapIterator = overlapCounts.erase(overlapIterator);
			continue;
		}

		// If we got here, we found another path that shares at least 50% of or route points, that is longer than the current route
		break;
	}

	if (overlapCounts.size() == 0) {
		AreaSearchEntry searchEntry;
		searchEntry.nodeId = currentStartNode;
		searchEntry.positionX = startNode.positionX;
		searchEntry.positionY = startNode.positionY;
		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 (std::pair<uint32_t, std::vector<uint32_t>> pathNodeEntry : path) {
			uint32_t pathNode = pathNodeEntry.first;
			if (currentAreaSearch.utilizedNodes.find(pathNode) == currentAreaSearch.utilizedNodes.end()) {
				currentAreaSearch.utilizedNodes[pathNode] = std::vector<uint32_t>();
			}
			currentAreaSearch.utilizedNodes[pathNode].push_back(currentStartNode);
		}
	}
	
	currentAreaSearch.startNodeCounter++;

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

	return searchResult;
}

#ifdef __EMSCRIPTEN__
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);
}
#endif