Add test function and fix small bugs in add