Simplify code and add test