Fixed some bugs and added testing code