Fixed bugs and added test code