/* -*- MaTX -*-
 *
 *  NAME
 *      schord() - Schur decomposition with ordering
 *
 *  SYNOPSIS
 *      {Qo,To} = schord(Qi, Ti, idx)
 *          Matrix Qo, To;
 *          Matrix Qi, Ti;
 *          Matrix idx;
 *
 *  DESCRIPTION
 *      schord() returns an orthogonal matrix Q so that the eigenvalues of
 *      Ti are ordered according to the increasing values of the index idx. 
 *
 *      Ti is square upper-triangular matrix, and Qi is orthogonal matrix.
 *
 *  SEE ALSO
 *      schur
 */

Func List schord(Qi, Ti, idx_)
	Matrix Qi, Ti, idx_;
{
	Integer i,j,k,p,n,flag;
	Real ix;
	Matrix Qo,To,G,idx;

	if (Rows(Ti) != Cols(Ti)) {
		error("schord(): Ti is not square.\n");
	}
	n = Rows(Ti);

	idx = idx_;
	To = Ti;
	Qo = Qi;

	for (i = 1; i <= n-1; i++) {
		flag = 0;
		k = i;
		for (j = i+1; j <= n; j++) {
			if (idx(j) < idx(k)) {
				k = j;
				flag = 1;
			}
		}

		if (flag) {
			for (p = k; p >= i+1; p = p-1) {
				G = flipud(givens(To(p-1,p-1)-To(p,p), To(p-1,p)));
				To(:,p-1:p) = To(:,p-1:p)*G;
				To(p-1:p,:) = G# * To(p-1:p,:);
				Qo(:,p-1:p) = Qo(:,p-1:p)*G;

				ix = idx(p-1);
				idx(p-1) = idx(p);
				idx(p) = ix;
			}
		}
	}

	return {Qo, To};
}