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.
Not pretty either.