Add some initial work and test of Matrix class