Remove useless prints
FIx some bugs and adding test code