Add unit tests for algorithms

Fixes #1