[Test] Fix bug and add test