Simplify code, add initial tests