Cleaned up code and added some tests.

(Joshua and Shuotian)