function [mag,pha,G] = mimoBode(A,B,C,D,w,dt,figno,ax,leg,tol)
% [mag,pha,G] = mimoBode(A,B,C,D,w,dt,figno,ax,leg,tol)
% plots the magnitude and phase of the
% steady-state harmonic reponse of a MIMO linear dynamic system.
% where:
%  A,B,C,D are the standard state-space matrices of a dynamic system
%  w is a vector of frequencies		(default: w = logspace(-2,2,200)*2*pi
%  dt is the sample period              (default: dt = [] for continuous time)
%  ax is either x,y,n, or b to indidate wich ax should be log-scaled ...
%   x, y, neither, or both.  The default is both.  The y-axis for the phase plot
%  is always linearly-scaled.
%  mag and pha are the magnitude and phase of the frequency response fctn matrix
%  mag and pha have dimension ( length(w) x m x r )

%  Henri Gavin, Dept. Civil Engineering, Duke University, henri.gavin@duke.edu

% Krajnik, Eduard, 'A simple and reliable phase unwrapping algorithm,'
% http://www.mathnet.or.kr/mathnet/paper_file/Czech/Eduard/phase.ps

 if (nargin < 10) tol = 1e-16;	end		% default rcond level
 if (nargin <  9) leg = []; end
 if (nargin <  8) ax = 'n';	end		% default plot formatting
 if (nargin <  7) figno = 100;	end		% default plot formatting
 if (nargin <  6) dt = [];	end		% default to continuous time 
 if (nargin <  5) w = logspace(-2,2,200)*2*pi;	end	% default frequency axis

 [n,r,m] = abcddim(A,B,C,D);		% check for compatibile dimensions
 nw = length(w);

%warning off

 I  = sqrt(-1.0);
 In = eye(n);

% continuous time or discrete time ... 
 if (length(dt) == 0) sz = I*w; else sz = exp(I*w*dt); end

 G   = zeros(nw,m,r); % allocate memory for the frequency response function, g
 mag = NaN(nw,m,r);
 pha = NaN(nw,m,r);
		  
 for ii=1:nw  % compute the frequency response function, G
%  G(ii,:,:) = C *  ((w(ii)*I*In-A) \  B)  + D; % (sI-A) is ill-conditioned
   [u,s,v] = svd( sz(ii)*In - A );	            % SVD of (sI-A)
   idx = max( find( diag(s) > s(1,1)*tol ) );
   char_eq_inv = v(:,1:idx) * inv(s(1:idx,1:idx)) * u(:,1:idx)';
   G(ii,:,:) = C * char_eq_inv * B + D;
 end

 mag = abs(G);
 pha(1,:,:)    = -atan2(imag(G(1,:,:)),real(G(1,:,:)));	% -ve sign!
 pha1 = -atan2(imag(G(1,:,:)),real(G(1,:,:)));	% -ve sign!
%pha1 = reshape( ones(nw-1,1,1)*pha1 , [nw-1,m,r] );
 pha1 = repmat( pha1, [nw-1,1,1] );
 pha(2:nw,:,:) = pha1 - cumtrapz(angle(G(2:nw,:,:)./G(1:nw-1,:,:)));

 if length(dt) == 1          % remove out-of-Nyquest range values in DT systems
   w_out = find(w>pi/dt);   % frequencies outside of the Nyquist range
   mag(w_out) = NaN; 
   pha(w_out) = NaN; 
 end

 if (figno > 0)	 % PLOTS  =================================================== 

  figure(figno);
   clf
   for k=1:r
    subplot(2,r,k)
     if (ax == 'x')
       semilogx(w/2/pi, mag(:,:,k));		% plot the magnitude response
     elseif (ax == 'y')
       semilogy(w/2/pi, mag(:,:,k));		% plot the magnitude response
     elseif (ax == 'n')
       plot(w/2/pi, mag(:,:,k) );		% plot the magnitude response
     else
       loglog(w/2/pi, mag(:,:,k) );		% plot the magnitude response
     end
     if (nargin > 9), legend(leg); end

     axis([ min(w)/2/pi , max(w)/2/pi , min(min(min(mag))) , max(max(max(mag))) ]);
     if k == 1, ylabel('magnitude'); end
     grid on
 
    subplot(2,r,k+r)
     if (ax == 'n' || ax == 'y')
      plot(w/(2*pi), pha(:,:,k)*180/pi );	        % plot the phase
     else
      semilogx(w/(2*pi), pha(:,:,k)*180/pi );		% plot the phase
     end
%    if (nargin > 9), legend(leg); end
     pha_min = floor(min(min(min(pha*180/pi))/90))*90;
     pha_max =  ceil(max(max(max(pha*180/pi))/90))*90;
     set (gca, 'ytick', [pha_min : 90 : pha_max ])
     axis([ min(w)/2/pi max(w)/2/pi pha_min pha_max ]);
     xlabel('frequency (Hertz)')
     if k == 1, ylabel('phase (degrees)'); end
     grid on

   end
 end

% ------------------------------------------------------------------ MIMOBODE
