Added some comments, Made Pathfinder a bit faster