Added tests and a bit of refactor

@opyate have a look please