Adding collision cache checking for better time performance
Cost to go heuristics added