Finished algorythm.

All functions has passed the tests.