Finish first implementation, add unit tests