Add test and small code fix