Fixed #22 and added test