Initial A* implementation