finished A star task 1 subproblem 1