Fixed output matrx size, and some bugs