Add unit tests for some functions

I've began to write some unit test, will continue and try to coverage 100% code.