Minor changes to matrixstack and matrix, almost done initial draft