Fix bugged implementation and add test to show it works