A* search algorithm simple implementation