Added test, cleaned code