Fixed major bugs in pathfinding - works now as it should. Improvements
needed.