Reorganize and add test case