Pathfinding algorithm

Still working on it.