Fix test and algorithm