Some refactoring, add bench function and fix some minor mistakes