Re-added correct colission handling and removed some debug info
Renamed all "Distance" to "Heuristic"