Pathfinding logic adjusted according to TorbenC's research.