Fix ctor, fix ident, add mat addition
No longer using 2d array