Apply a little refactor in algorith and main test