Add unit test and fix bug