First working implementation, with tests