Rearrenged code, added unit tests