Adding working unit tests
Fix some bugs