Semi-working A* implementation