Code cleanup, added unit tests