Fixed implementation and added test case