Add test and bugfixes