Add algorithm and related tests