Add tests, refacgtor a little