ENH: Little bit faster, added test