Add test and bug fix

Please see the diff and commit into master if you feel good.

Thanks.