Unit tested+some bugs fixed