Initial solution
add test, simplify code