Add unit tests using boost test

Added UnitTests for a few classes, as well as fixing some minor bugs and cleaning up a little bit in the process.
The function needs a better name.