diff --git a/src/MultilevelAStarEx.cpp b/src/MultilevelAStarEx.cpp
index 85632e0..698bcc5 100644
--- a/src/MultilevelAStarEx.cpp
+++ b/src/MultilevelAStarEx.cpp
@@ -263,12 +263,16 @@ Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, b
 
 	auto process = [this, &open, &to2, &closest](Node *current, int x, int y, int distance) {
 		Node *node = &NODES(x, y);
+		
 		if (node->state(_pass) == Node::UNUSED)
 		{
 			node->open(_pass, current, current->distanceFromStart + distance, to2);
 			open.push_back(node);
 		}
-		// TODO: check if this should only be performed on open nodes or all nodes
+		else if (node->state(_pass) == Node::CLOSED)
+		{
+			return;
+		}
 		else if (current->distanceFromStart + distance < node->distanceFromStart)
 		{
 			node->parent = current;
@@ -299,6 +303,10 @@ Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, b
 			{
 				current = n;
 			}
+			else if ((n->total_cost() == current->total_cost()) && (n->distanceToEnd < current->distanceToEnd))
+			{
+				current = n;
+			}
 		}
 
 		if (current->distanceToEnd == 0)