Cleaned up GEMM routine, added comments