Added test code and fixed some bugs