Hw2

Update code for hw2 (refactor, add some new functions, add tests)