Implement A Star algorithm