Updated algorithm. Added more tests.