Theoretically have finished the pathfinding algorithm...

This will probably break like a bitch...