Pathfind DONE!

Pathfind should be finally done and commented.  Some additional changes.