Algorithm done, need test