Add SIMD for matrix code, fix some bugs