Updated algorithm and testing