added working version of algorithm 1