Moved some functions around, fixed bugs and wrote a few tests