Added tests, fixed code