Fix implementation and add tests