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

Func List gmargin(G, wmin, wmax, tol, ...)
	Rational G;
	Real wmin, wmax, tol;
{
	Real wcp, gm;
	Array w, gain, phase;
	Index idx;

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

	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(phase < -180.0);
		if (length(idx) == 0) {
			warning("gmargin(): Phase does not cross -180 [deg]\n");
			return {0.0, 0.0};
		} else if (length(idx) == Cols(w)) {
			warning("gmargin(): Phase has already crossed -180 [deg]\n");
			return {0.0, 0.0};
		}

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

		if (wmax - wmin < tol) {
			wcp = wmax;
			gm = - gain(idx(1));
			break;
		}
	}

	return {gm, wcp};
}