Fixed bugs, added test