main algorithm code
added unit tests little bit