Some code optimization and removed duplication
Added more tests, looks like everything is up and running