Adds tests and fix inverse matrix