Implemented matrix logic

Also fixed code errors and other things.