Update algorithm and add more tests