Actually making matrices work + rustifying stuff