Updated Matrix class with TODOs and ASSERTs

Some TODOs and ASSERTs (mostly SLOW_ASSERTs) are now used in the Matrix class. Still, the solution will unlikely compile, because we still have to use Utility library in the Matrix class.