Sorta debugged, at least it worked on some random 3x3 matrix