Fix some bugs.
Clean.
Modify matrix reader macro.
Added initial test for matrix.

* Add/substract