%FRF: Plot Frequency Response Function
%
%   Usage:   frf(sys1,'color/linetype/marker',sys2,'color/linetype/marker',...
%            ...{fmin,fmax,magmin,magmax,phasemin,phasemax,wraponoff,type})
%
%   Inputs:  You can use custom colors, e.g. lightgrey, pink, etc. (see line 180)
%            example strings: 'b', 'lightgrey--*', 'orange', '-^'
% 
%            All elements in the options-struct are optional 
%            fmin/fmax in Hz
%            magmin/magmax in dB
%            phasemin/phasemax in degrees
%            wraponoff: [ 'on' | 'off' (default) ]
%            type: [ 'mag' | 'phase' | 'magphase' (default) ]
%
%   Outputs: Plots the frf in the current figure.
%            Note that the xticklabels might overlap the xlabel in the
%            figure. In an exported pdf file they will be correct.
%   
%   See also FRFMAG, FRFPHASE, SETPLOT   

%   Changes: 
%   2011083 - Initial version (Rob Hoogendijk)
%
%   Rob Hoogendijk (2011-08-3) 
%   ________________________________
%   Eindhoven University of Technology
%   Dept. of Mechanical Engineering
%   Control Systems Technology group
%   PO Box 513, WH -1.126
%   5600 MB Eindhoven, NL
%   T +31 (0)40 247 4227
%   F +31 (0)40 246 1418
%   E r.hoogendijk@tue.nl

function frf(varargin)  
  
clf;set(gcf,'color',[0.57 0.76 0.95]);linewidth=1.5;  
  
%% get inputs  
[sysarr options] = getinputs(varargin);       %read inputs and put in struct  
options          = getbodeoptions(options);   %get limits for mag, phase, freq and wrap settings  
sysarr           = getcustomcolors(sysarr);   %store custom color if defined  
  
%% analyze systems  
[sysarr nrows ncols] = getsyssizes(sysarr);           %get col size and row size determine number of subplots  
if(nrows>1||ncols>1),mimo=1;else,mimo=0;end;          %determine mimo or not  
[sysarr limits]      = getbodedata(sysarr,options);   %get plot data using original bode function and detect auto limits or use options   
  
%% determine plottype  
switch options.plottype, case 'magphase', plotmag=1;plotphase=1;  
                         case 'mag',      plotmag=1;plotphase=0;  
                         case 'phase',    plotmag=0;plotphase=1; end;  
  
%% plot the data  
% create handles for each subplot  
handles=[];  
for nn=1:nrows  
    for mm=1:ncols  
        if(plotmag && plotphase)  
            handles(2*nn-1,mm)=subplot(2*nrows,ncols,(2*(nn-1))*ncols+mm);  
            handles(2*nn,mm)=subplot(2*nrows,ncols,(2*(nn-1)+1)*ncols+mm);  
        else  
            handles(nn,mm)=subplot(nrows,ncols,(nn-1)*ncols+mm);   
        end  
              
    end  
end  
% plot the frf in each subplot for each system  
if(mimo)  
    % one x and y label for mimo systems  
    ax=axes('position',[0.1,0.1,1,1],'visible','off');  
    xlab=get(ax,'xlabel');       ylab=get(ax,'ylabel');  
    if(plotmag && plotphase),set(ylab,'string','Phase[deg.], Magnitude [dB]','visible','on');end  
    if(plotmag && ~plotphase),set(ylab,'string','Magnitude [dB]','visible','on');end  
    if(~plotmag && plotphase),set(ylab,'string','Phase[deg.]','visible','on');end      
    set(xlab,'string','Frequency [Hz]','visible','on');  
end  
for ii=1:length(sysarr)  
    sys=sysarr{ii};   
    for nn=1:sys.nrows  
        for mm=1:sys.ncols  
            %magnitude plots  
            if(plotmag)  
                axes(handles((plotmag+plotphase)*nn-plotphase,mm));                 
                h=semilogx(gca,sys.f,db(squeeze(sys.mag(nn,mm,:))),sys.sty,'linewidth',linewidth); hold on; grid on;              
                if(~isempty(sys.customcolor)),set(h,'color',sys.customcolor);end;     % set custom colors e.g. 'lightgrey'              
                if(mm==1 && ~mimo),ylabel('Magnitude [dB]');end                       % if first column and not mimo, add y label  
                if(mm~=1),set(gca,'YTickLabel',{});end                                % if not first column remove ticks  
                if(plotphase || nn~=nrows),set(gca,'XTickLabel',{});end;                           % remove xticks exept when no phase plot         
                axis([limits.fmin limits.fmax limits.magmin limits.magmax]);          % axis limits  
                if(mimo && nn==1),title(['In:' num2str(mm)],'fontsize',8);end       %i/o labels: inputs / outputs below  
                if(mimo && mm==ncols),text(limits.fmax,limits.magmin+0.5*(limits.magmax-limits.magmin),[' Out:' num2str(nn)],'fontsize',8);end;  
            end  
            %phase plots  
            if(plotphase)  
                axes(handles((plotmag+plotphase)*nn,mm));  
                h=semilogx(gca,sys.f,squeeze(sys.phase(nn,mm,:)),sys.sty,'linewidth',linewidth); hold on; grid on;  
                if(~isempty(sys.customcolor)),set(h,'color',sys.customcolor);end;        % set custom colors e.g. 'lightgrey'             
                if(mm==1 && ~mimo),ylabel('Phase [deg.]');end;                           %if first col and not mimo add label  
                if(mm~=1),set(gca,'YTickLabel',{});end                                   % if not first column remove ticks  
                if(nn==nrows && ~mimo),xlabel('Frequency [Hz]');end;                     % if last row and not mimo put x label  
                if(nn~=nrows),set(gca,'XTickLabel',{});end                               % if not last row remove ticks  
                set(gca,'ytick',[-36000:90:36000]);                                      % set ticks every 90 degrees             
                axis([limits.fmin limits.fmax limits.phasemin limits.phasemax]);         % axis limits  
                if(mimo && nn==1 && ~plotmag),title(['In:' num2str(mm)],'fontsize',8);end %i/o labels: inputs / outputs below  
                if(mimo && mm==ncols),text(limits.fmax,limits.phasemin+0.5*(limits.phasemax-limits.phasemin),[' Out:' num2str(nn)],'fontsize',8);end;                  
            end  
        end  
    end  
end  

  
%% plot settings  
%axis positions and i/o labels  
if exist('setplot','file')==2   
    setplot([], [], {[],1.0,[],1.1,1.1,[]});     
else  
    disp('Install setplot.m for optimal viewing of your frf.')  
end  
%linking axis  
for ii=1:size(handles,2)%link x axes  
    eval(['global link_fig' num2str(gcf) '_xaxis_' num2str(ii)]);  
    eval(['link_fig' num2str(gcf) '_xaxis_' num2str(ii) '=linkprop(handles(:,ii),''xlim'' );']);  
end  
for ii=1:size(handles,1)%link y axes  
    eval(['global link_fig' num2str(gcf) '_yaxis_' num2str(ii)]);  
    eval(['link_fig' num2str(gcf) '_yaxis_' num2str(ii) '=linkprop(handles(ii,:),''ylim'' );']);  
end  
  
end%function  
 
 
 
 
 
%% private functions 
%to process the inputs 
function [sysarr options]=getinputs(varargin) 
    varargin=varargin{1}; 
    sysarr=[]; 
    options=[]; 
    nsys=0; 
    for ii=1:length(varargin) 
        inputii=varargin{ii}; 
        %detect systems 
        if issystem(inputii) 
            nsys=nsys+1; 
            sysarr{nsys}.sys=inputii; 
            sysarr{nsys}.sty='-'; %default style is only a line 
        end 
        %detect formatting 
        if ischar(inputii) 
            sysarr{nsys}.sty=inputii; 
        end 
        %detect options 
        if iscell(inputii) 
            options=inputii; 
        end 
    end 
end%function 
 
%to get the options and set default options 
function options_out = getbodeoptions(options_in) 
    try options_out.fmin     = options_in{1};         catch options_out.fmin=[];             end; 
    try options_out.fmax     = options_in{2};         catch options_out.fmax=[];             end; 
    try options_out.magmin   = 10^(options_in{3}/20); catch options_out.magmin=[];           end; 
    try options_out.magmax   = 10^(options_in{4}/20); catch options_out.magmax=[];           end; 
    try options_out.phasemin = options_in{5};         catch options_out.phasemin=[];         end; 
    try options_out.phasemax = options_in{6};         catch options_out.phasemax=[];         end; 
    try options_out.wrapping = options_in{7};         catch options_out.wrapping='off';      end; 
    try options_out.plottype = options_in{8};         catch options_out.plottype='magphase'; end;    
end%function 
  
%to get custom color for sysarr struct and store in sysarr{ii}.customcolor 
function sysarr=getcustomcolors(sysarr) 
    custom={... 
        'lightgrey', [0.80 0.80 0.80]; 
        'midgrey',   [0.60 0.60 0.60]; 
        'darkgrey',  [0.40 0.40 0.40]; 
        'orange',    [1.00 0.50 0.00]; 
        'pink',      [1.00 0.50 0.60]; 
        'purple',    [0.80 0.00 1.00]; 
        'darkgreen', [0.00 0.70 0.00];     
        }; 
 
    for nn=1:length(sysarr) 
        sysarr{nn}.customcolor = []; 
        for ii = 1 : length(custom) 
            k = strfind(sysarr{nn}.sty,custom{ii,1}); 
            if ~isempty(k) 
                sysarr{nn}.customcolor = custom{ii,2}; 
                % Remove the found linestyle 
                sysarr{nn}.sty = strrep(sysarr{nn}.sty,custom{ii,1},''); 
            end 
        end%colorloop 
    end%systemsloop 
end%function 
 
%to get the size of each system and the max syssize 
function [sysarr nrows ncols] = getsyssizes(sysarr) 
    nrows=0;ncols=0; 
    for ii=1:length(sysarr) 
        sysarr{ii}.nrows=size(sysarr{ii}.sys,1); 
        sysarr{ii}.ncols=size(sysarr{ii}.sys,2);        
        nrows=max( nrows , sysarr{ii}.nrows ); 
        ncols=max( ncols , sysarr{ii}.ncols ); 
    end 
end%function 
 
%to get the bode data using the matlab BODE function 
function [sysarr limits] = getbodedata(sysarr, options) 
limits.fmin     = []; 
limits.fmax     = []; 
limits.magmin   = []; 
limits.magmax   = []; 
limits.phasemin = []; 
limits.phasemax = []; 
for ii=1:length(sysarr) 
    % generatebode diagram data with original 'bode' function 
    if (~isempty(options.fmin) && ~isempty(options.fmax))  %use freq range from options if given 
        [sysarr{ii}.mag,sysarr{ii}.phase,sysarr{ii}.w]=bode(sysarr{ii}.sys,{options.fmin*2*pi,options.fmax*2*pi}); 
        sysarr{ii}.f=sysarr{ii}.w/2/pi; 
    else 
        [sysarr{ii}.mag,sysarr{ii}.phase,sysarr{ii}.w]=bode(sysarr{ii}.sys); 
        sysarr{ii}.f=sysarr{ii}.w/2/pi; 
    end 
    % wrap phase depending on options 
    if strcmp(options.wrapping,'on'),sysarr{ii}.phase=wrap(sysarr{ii}.phase);end     
    % store min/max freqs/mag/phase or take value from options if provided 
    if(isempty(options.fmin)),    limits.fmin    =updatemin(limits.fmin,    sysarr{ii}.f);    else,limits.fmin    =options.fmin;    end; 
    if(isempty(options.fmax)),    limits.fmax    =updatemax(limits.fmax,    sysarr{ii}.f);    else,limits.fmax    =options.fmax;    end; 
    if(isempty(options.magmin)),  limits.magmin  =updatemin(limits.magmin,  sysarr{ii}.mag);  else,limits.magmin  =options.magmin;  end; 
    if(isempty(options.magmax)),  limits.magmax  =updatemax(limits.magmax,  sysarr{ii}.mag);  else,limits.magmax  =options.magmax;  end; 
    if(isempty(options.phasemin)),limits.phasemin=updatemin(limits.phasemin,sysarr{ii}.phase);else,limits.phasemin=options.phasemin;end; 
    if(isempty(options.phasemax)),limits.phasemax=updatemax(limits.phasemax,sysarr{ii}.phase);else,limits.phasemax=options.phasemax;end; 
end 
%transform magnitude limits to db 
if(limits.magmin==0),magmin=1E-16;end; 
if(limits.magmax==inf),magmax=1E16;end; 
limits.magmin=db(limits.magmin); 
limits.magmax=db(limits.magmax); 
%add whitespace to y limits 
ws=(limits.magmax-limits.magmin)*0.05; %mag 
if(isempty(options.magmin)),limits.magmin=limits.magmin-ws;end 
if(isempty(options.magmax)),limits.magmax=limits.magmax+ws;end 
ws=(limits.phasemax-limits.phasemin)*0.05; %phase 
if(isempty(options.phasemin)),limits.phasemin=limits.phasemin-ws;end 
if(isempty(options.phasemax)),limits.phasemax=limits.phasemax+ws;end 
end%function 
%wrap function 
function phase=wrap(phase) 
phase = phase-(round(phase/360)*360); 
end 
function [newmin] = updatemin(oldmin,arr) 
currmin=min(min(min(arr))); 
if(~isempty(oldmin)), newmin=min(oldmin,currmin); else, newmin=currmin; end 
end%subfunctionforlimitsmin 
function [newmax] = updatemax(oldmax,arr) 
currmax=max(max(max(arr))); 
if(~isempty(oldmax)), newmax=max(oldmax,currmax); else, newmax=currmax; end 
end%subfunctionforlimitsmax 
