Add test and fix code