A-star algorithm working given start and end