Adding unit test + fix a bug

This should be the final version. Might add more unit test.