Small A* optimization and constant change. Also added comments.