ICP checked to be right,
merge algorithm 1 implemented