Added code. TODO: test inverse matrix, add other features and comment
Comming after failing merging. Still getting used to GIT.