Extracts matrix solving function :gift:

Using unit tests! :kiss:

Ps: Needed to modify c -> cpp