Working code tested on invertable matrices