/* -*- MaTX -*-
 *
 *  NAME
 *      pmargin() -  Phase margin and crossover frequency
 *
 *  SYNOPSIS
 *      {pm wcg} = pmargin(G)
 *         Real pm, wcg;
 *         Rational G;
 *
 *      {pm wcg} = pmargin(G,wmin)
 *         Real pm, wcg;
 *         Rational G;
 *         Real wmin;
 *
 *      {pm wcg} = pmargin(G,wmin,wmax)
 *         Real pm, wcg;
 *         Rational G;
 *         Real wmin;
 *         Real wmax;
 *
 *      {pm wcg} = pmargin(G,wmin,wmax,tol)
 *         Real pm, wcg;
 *         Rational G;
 *         Real wmin;
 *         Real wmax;
 *         Real tol;
 *
 *  DESCRIPTION
 *       Given a transfer function G, range of frequency [wmin:wmax],
 *       and tolerance of frequency tol, pmargin() returns phase 
 *       margin pm and the crossover frequency wcg.
 *
 *  SEE ALSO
 *      gmargin and margin
 */

Func List pmargin(G, wmin, wmax, tol, ...)
	Rational G;
	Real wmin, wmax, tol;
{
	Real wcg, pm;
	Array w, gain, phase;
	Index idx;

	error(nargchk(1, 4, nargs, "pmargin"));

	if (nargs == 1) {
		wmin = 0.001;
		wmax = 1000.0;
		tol = 1.0E-3;
	} else if (nargs == 2) {
		wmax = 1000.0;
		tol = 1.0E-3;
	} else if (nargs == 3) {
		tol = 1.0E-3;
	}

	for (;;) {
		w = logspace(log10(wmin), log10(wmax), 100);
		{gain, phase} = Bode_tf(G, w);
		gain = 20*log10(gain);
		phase = unwrap(phase);

		idx = find(gain < 0.0);
		if (length(idx) == 0) {
			warning("pmargin(): Gain does not cross 0 [dB]\n");
			return {0.0, 0.0};
		} else if (length(idx) == Cols(w)) {
			warning("pmargin(): Gain has already crossed 0 [dB]\n");
			return {0.0, 0.0};
		}

		wmin = w(idx(1)-1);
		wmax = w(idx(1));

		if (wmax - wmin < tol) {
			wcg = wmax;
			pm = 180 + phase(idx(1));
			break;
		}
	}

	return {pm, wcg};
}

Func List pmargin_roots(G, tol, ...)
	Rational G;
	Real tol;
{
	Complex j;
	Real wcg;
	Polynomial w, nc, dc, eq;
	CoArray rt;
	Matrix wcgs;

	error(nargchk(1, 2, nargs, "pmargin_roots"));
	
	if (nargs == 1) {
		tol = 1.0e-12;
	}

	j = (0,1);
	tol = 1.0E-12;

	w = Polynomial("s");
	nc = eval(Nu(G), j*w);
	dc = eval(De(G), j*w);
	eq = (Re(nc)^2 + Im(nc)^2) - (Re(dc)^2 + Im(dc)^2);
	rt = roots(eq);
	wcgs = Re(rt(find(Re(rt(find(abs(Im(rt)) < tol))) > 0.0)));
	
	if (length(wcgs) == 0) {  // no crossover frequency
		warning("pmargin_roots(): Gain does not cross 0 [dB].\n");
		wcg = -1.0;
	} else {
		wcg = wcgs(1);
	}
	
	if (wcg < 0.0) {
		return {0.0, 0.0};
	} else {
		return {180/PI*arg(eval(G, j*wcg)), wcg};
	}
}