Add backbone of Algorithm2
Make a bit refactoring