Refactor and add tests

@eb-fsilva see if you like the code, added a lot of testes is 100% CC now, the code looks simpler IMO