Fixed a few bugs, wrote tests