Finished matrix class and added some unit tests