Corrected algorithm 5 and 6