Clean up implementation, add tests