Main algorithm and tests