function  [mp, sp] = mtriangle(A)
%Find square submatrix A(mp, sp) that is approximately an upper triangle matrix

[m,n] = size(A);
%initialize the index sets
mp = []; 
sp = []; 
mpLeft = (1:m)'; 
spLeft = 1:n;
  
[mpBlock,spBlock] = mdiag(A); %find the maximal diagonal submatrix
%update the index sets
mp = [mp; mpLeft(mpBlock)]; 
sp = [sp spLeft(spBlock)];
%number of columns needed for the non-upper triangular part
colsNeeded = 0; 
  
while length(mp) < m 
	mpLeft = setdiff(mpLeft, mpLeft(mpBlock));
	spLeft = setdiff(spLeft, spLeft(spBlock)); 
	
	[mpBlock, spBlock] = mdiag(A(mpLeft,spLeft));

	if (size(mpBlock,1) ~= 0) 
		mp = [mp; mpLeft(mpBlock)]; 
		sp = [sp spLeft(spBlock)];
	else
		colsNeeded = m - length(mp);
		fprintf('The non-upper triangular part of S has size of %g.\n', colsNeeded);
		[sl,su,p,q] = lu(A(mpLeft,spLeft),1);
		tmp = 1:colsNeeded;
		mpBlock = p*tmp'; %row permutation
		[spR,spTmp] = rref(su); %basis of upper triangular matrix
		qvec = (1:size(q,2)) * q; %column permutation
		spBlock = qvec(spTmp); %column indices of the basis

		mp = [mp; mpLeft(mpBlock)]; 
		sp = [sp spLeft(spBlock)];
	end
end