Refactors the AStar code to not be retarded.  Works just like it used to.
Fixes a small derp.
Fixes the heap fix.
Merge pull request #7360 from SkyMarshal/astar-fix

Fix/Cleanup for A* implementation