[Feat] implement A start algorithm