Add unit tests. Simplify code