Algorithm A* for units