Fixed some bugs
added testbench