Add the implementation of the algorithm.

Now this class can be used. The algorithm work and pass all tests.