Add test code and fix bugs