Fixed bugs and add some tests