Add implementation and tests
Write readme