Fixed a few bugs and added unit testing