/* -*- MaTX -*-
 *
 *  NAME
 *      margin() - Gain margin, phase margin, and crossover frequencies
 *
 *  SYNOPSIS
 *      {gm,pm,wgc,wpc} = margin(Mg,Ph,w)
 *          Real gm,pm,wgc,wpc;
 *          Array Mg,Ph,w;
 *
 *  DESCRIPTION
 *       Given the frequencies w and the corresponding magnitude Mg 
 *       (absolute value) and phase Ph (in degree)of a system, 
 *       margin(Mg,Ph,W) returns gain margin gm (absolute value), phase 
 *       margin pm (in degree), and crossover frequencies wgc and wpc.
 *
 *  EXAMPLE
 *       w = logspace(-1.0,2.0);
 *       {Mg,Ph} = bode(A,B,C,D,1,w);
 *       {gm,pm,wgc,wpc} = margin(Mg,Ph,w);
 *
 *  SEE ALSO
 *      gmargin and pmargin
 */

Func List margin(Mg, Ph, w)
	Array Mg, Ph, w;
{
	Integer i,j;
	Real gm,pm,wgc,wpc,wi,wj;
	Index idx;

	// Gain margin:
	if (Ph(1) > -180.0) {
		idx = find(Ph < -180.0);
	} else {
		idx = find(Ph > -180.0);
	}
	if (length(idx) == 0 || idx(1) == 1) {
		warning("margin(): Phase does not cross -180 [deg]\n");
		wpc = 0.0;
		gm = 1.0E300;  // Big number
	} else {
		j = idx(1);
		i = j - 1;
		wi = w(i);
		wj = w(j);
		// linear approximation
		wpc = wi - (180+Ph(i))/(Ph(j)-Ph(i))*(wj-wi);
		gm = 1.0 / (Mg(i) + (Mg(j)-Mg(i))/(wj-wi)*(wpc-wi));
	}

	// Phase margin:
	if (Mg(1) > 1.0) {
		idx = find(Mg < 1.0);
	} else {
		idx = find(Mg > 1.0);
	}
	if (length(idx) == 0 || idx(1) == 1) {
		warning("margin(): Gain does not cross 0 [dB]\n");
		wgc = 0.0;
		pm = 1.0E300; // Big number
		return {gm,pm,wgc,wpc};
	}
	j = idx(1);
	i = j - 1;
	wi = w(i);
	wj = w(j);
	// linear approximation
	wgc = wi + (1 - Mg(i))/(Mg(j)-Mg(i))*(wj-wi);
	pm = Ph(i) + (Ph(j)-Ph(i))/(wj-wi)*(wgc-wi) + 180;

	return {gm,pm,wgc,wpc};
}