Gaussian elimination with partial pivoting working correctly