Fixed bugs in inverse matrix calculation
Made code more c++-y
Fixed bugs in inverse calculation
Made code more c++-y