Fix and add unit test