Some code cleanup and fixed a few problems.
Added some unit tests.