Solves assignment 2.
Fixes bad print of matrix in test function.