/*  -*- MaTX -*-
 *
 *  【名前】
 *      unwrap() - 位相角の修正
 *
 *  【形式】
 *      Y = unwrap(X)
 *         Array Y;
 *         Array X;
 *
 *      Y = unwrap(X,cutoff)
 *         Array Y;
 *         Array X;
 *         Real cutoff;
 *
 *  【機能説明】
 *      Y = unwrap(X) は，絶対値の変化が PI より大きいとき，位相を連続化
 *      するため，+- 2 PI の整数倍加えて行列 X の成分を修正する。X の
 *      成分は，左から右，上から下へ，参照される。Y = unwrap(X, cutoff)
 *      は，絶対値の位相の変化が cuffoff より大きいとき，修正をする。
 *      cutoff は度で指定する。
 *
 *      unwrap() は便利であるが，データが少ない場合やデータが急激に変化
 *      する場合は正しい結果が得られないことがある。
 * 
 *  【例題】
 *
 *      j = (0,1);
 *      s = Polynomial("s");
 *      w = logspace(-2.0, 3.0);
 *      G = 1/(s^3 + 2*s^2 + 3*s + 4);
 *      data = eval(G, j*w);
 *      p = unwrap(Im(log(data)));
 *      mgplot_semilogx(1,w,p);
 *
 *  【関連項目】
 *      unwrap_row(), unwrap_col()
 *
 *   ------------------------------------------------------------------------
 *
 *  【名前】
 *      unwrap_col() - 列毎の位相角の修正
 *
 *  【形式】
 *      Y = unwrap_col(X)
 *         Array Y;
 *         Array X;
 *
 *      Y = unwrap_col(X,cutoff)
 *         Array Y;
 *         Array X;
 *         Real cutoff;
 *
 *  【機能説明】
 *      Y = unwrap_col(X) は，絶対値の変化が PI より大きいとき，位相を連
 *      続化するため，+- 2 PI の整数倍加えてベクトル X の成分を修正する。
 *      Y = unwrap_col(X, cutoff)は，絶対値の位相の変化が cuffoff より大
 *      きいとき，修正をする。cutoff は度で指定する。
 *
 *      X が行列のとき，列毎に位相角を修正する。Y が X と同じ大きさになる。
 *
 *      unwrap_col() は便利であるが，データが少ない場合やデータが急激に変化
 *      する場合は正しい結果が得られないことがある。
 * 
 *  【例題】
 *
 *      j = (0,1);
 *      s = Polynomial("s");
 *      w = logspace(-2.0, 3.0);
 *      G = [1/(s^3 + 2*s^2 + 3*s + 4), 1/(s+1)];
 *      data = eval(G, j*w');
 *      p = unwrap_col(Im(log(data)));
 *      mgplot_semilogx(1,w,p');
 *
 *  【関連項目】
 *      unwrap(), unwrap_row()
 *
 *   ------------------------------------------------------------------------
 *
 *  【名前】
 *      unwrap_row() - 行毎の位相角の修正
 *
 *  【形式】
 *      Y = unwrap_row(X)
 *         Array Y;
 *         Array X;
 *
 *      Y = unwrap_row(X,cutoff)
 *         Array Y;
 *         Array X;
 *         Real cutoff;
 *
 *  【機能説明】
 *      Y = unwrap_row(X) は，絶対値の変化が PI より大きいとき，位相を連
 *      続化するため，+- 2 PI の整数倍加えてベクトル X の成分を修正する。
 *      Y = unwrap_row(X, cutoff)は，絶対値の位相の変化が cuffoff より大
 *      きいとき，修正をする。cutoff は度で指定する。
 *
 *      X が行列のとき，行毎に位相角を修正する。Y が X と同じ大きさになる。
 *
 *      unwrap_row() は便利であるが，データが少ない場合やデータが急激に変化
 *      する場合は正しい結果が得られないことがある。
 * 
 *  【例題】
 *
 *      j = (0,1);
 *      s = Polynomial("s");
 *      w = logspace(-2.0, 3.0);
 *      G = [[1/(s^3 + 2*s^2 + 3*s + 4)][1/(s+1)]];
 *      data = eval(G, j*w);
 *      p = unwrap_row(Im(log(data)));
 *      mgplot_semilogx(1,w,p);
 *
 *  【関連項目】
 *      unwrap and unwrap_row
 */


Func Array unwrap(ph, tol, ...)
	Array ph;
	Real tol;
{
	Array x;
	Integer m, n;

	error(nargchk(1, 2, nargs, "unwrap"));

	m = Rows(ph);
	n = Cols(ph);
	
	if (nargs == 1) {
		if (m == 1 || n == 1) {
			return unwrap_col(ph);
		} else {
			x = unwrap_col(reshape(ph, m*n, 1));
			return reshape(x, m, n);
		}
	} else {
		if (m == 1 || n == 1) {
			return unwrap_col(ph, tol);
		} else {
			x = unwrap_col(reshape(ph, m*n, 1), tol);
			return reshape(x, m, n);
		}
	}
}

Func Array unwrap_row(ph, tol, ...)
	Array ph;
	Real tol;
{
	error(nargchk(1, 2, nargs, "unwrap_row"));
	
	if (nargs == 1) {
		return trans(unwrap_col(trans(ph)));
	} else {
		return trans(unwrap_col(trans(ph), tol));

	}
}

Func Array unwrap_col(ph_, tol, ...)
	Array ph_;
	Real tol;
{
	Integer i,j,k,m,mo,no,n;
	Array ph,ph2,p,pd;
	Index idx;

	error(nargchk(1, 2, nargs, "unwrap_col"));

	if (nargs == 1) {
		tol = PI*170./180.;    // 許容誤差
	}

	ph = ph_;

	{m,n} = size(ph);
	mo = m;
	no = n;
	if (mo < no) {
		ph = trans(ph);
		{m,n} = size(ph);
	}

	ph2 = ph;

	for (j = 1; j <= n; j++) {
		p = ph(:,j);
		pd = diff(p);
		idx = find(abs(pd) > tol);
		for (i = 1; i <= length(idx); i++) {
			k = idx(i);
			p(k+1:m) = p(k+1:m) .- 2*PI*sgn(pd(k));
		}
		ph2(:,j) = p;
	}

	if (mo < no) {
		ph2 = trans(ph2);
	}
	return ph2;
}