Reworked based on Hannes' comments, added unit tests