Fixed bug in mod_subtract()

Fixed a bug that was causing mod substraction to not compute correctly
yielding negative numbers.
Added equals function to matrix

We ended up not needing it but I think its cool anyway.