Rearrange functions
Add some tests