Initial working code for solve matrix