Add code and test
Update readme