Fix various bugs. Use pointer to matrix instead.