% Purpose:  Graphical representation of max time delay
% Author:   Axel Lodemel Holene
% E-mail:   axel.holene@gmail.com
% Date:     Mai 2013
% About:    

%% Initialising
clear all
close all
clc

% Adding commonFiles to search path
mDir   = fileparts(pwd) ;
shared = fullfile(mDir,'commonFiles') ;     addpath(shared)
m2tkz  = fullfile(mDir,'matlab2tikz/src') ; addpath(m2tkz)
varclr = fullfile(mDir,'varycolor') ;       addpath(varclr)

titleName = {'Case 1, $\G_{1}(s)=\gOne$',  ...
            'Case 2, $\G_{2}(s)=\gTwo$',  ...
            'Case 3, $\G_{3}(s)=\gThree$',...
            'Case 4, $\G_{4}(s)=\gFour$', ...
            'Case 5, $\G_{5}(s)=\gFive$', ...
            'Case 6, $\G_{6}(s)=\gSix$',  ...
            'Case 7, $\G_{7}(s)=\gSev$',  ...
            'Case 8, $\G_{8}(s)=\gEight$',...
            'Case 9, $\G_{9}(s)=\gNine$', ...
            'Case 10, $\G_{10}(s)=\gTen$', ...
            'Case 11, $\G_{11}(s)=\gElv$', ...
            'Case 12, $\G_{12}(s)=\gTwlv$',...
            'Case 13, $\G_{13}(s)=\gTh$', ...
            'Case 14, $\G_{14}(s)=\gFth$'} ;
modelName = {'case1', 'case2', 'case3', 'case4', 'case5',  ...
             'case6', 'case7', 'case8', 'case9', 'case10', ...
             'case11', 'case12', 'case13','case14'} ;

cTypes  = {'po', 'simc', 'smith'} ;
ctrlrs  = {'pid', 'pi'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;

tic ;
for Ms_s = [1.3 1.6 1.7 1.9] %
for modelId = [10 14] % 14:14
  i = 1 ;                                              % Counter
  J = zeros(200,24) ;
  d = J ;
  captions = {};
  for cType = [1 3]
    for ctrlr = 1:2
      for cNum = 4
        innerLoop = true ;
        lPath = strcat(mDir,'/results/smithDist/',...
            cTypes{cType},'/',...
            ctrlrs{ctrlr},'/',cParm{cNum},'/', ...
            modelName{modelId},'.mat') ;
        fprintf('Attempting to load %s\n',lPath)
        try
          load(lPath,'res') ;
        catch
          fprintf('Load failed. \n')
          fprintf('Error: %s\n', lasterr)
          innerLoop = false ;
        end % try
        while innerLoop
          try
            this = getfield(res, ...
                   strcat('SMITHDIST',upper(ctrlrs{ctrlr}))) ;
          catch
            fprintf('Failed at InnerLoop.\n')
            innerLoop = false ;
          end % try
          captions{i} = strcat(upper(cTypes{cType}),...
                        ':', upper(ctrlrs{ctrlr}),':',...
                        cParm{cNum}) ;
          idx = find(this.Ms_span == Ms_s) ;
          n = length(this.d_span) ;
          d(1:n,i) = this.d_span ;
          j = this.J(:,idx) ;
          j(j>100) = -1 ;
          J(1:n,i) = j ;
          innerLoop = false ;
        end % innerLoop
        i = i + 1 ;
      end % cNum
    end % ctrlr
  end % cType
  % Find Smith:rules:plots
  cType = 3 ;
  smithRules = {'simc','brules'} ;
  tau_cRef = [0.5 1 1.5 2] ; % Note: Ms for case 1 spans through
  for rule = [1] % 1.26-1.46, while Ms for PO solutions are 1.6
    for ctrlr = 1:2
      for tau_c = 1:2                  % Tau_c = 0.5theta, theta
        innerLoop = true ;
        lPath = strcat(mDir,'/results/',...
                cTypes{cType},    '/',...
                smithRules{rule}, '/', ...
                ctrlrs{ctrlr},    '/', ...
                modelName{modelId},'.mat') ;
        fprintf('Attempting to load %s\n',lPath)
        try
          load(lPath,'res') ;
        catch
          fprintf('Load failed. \n')
          fprintf('Error: %s\n', lasterr)
          innerLoop = false ;
        end % try
        while innerLoop
          try
            this = getfield(res, 'thetaMax') ;
          catch
            fprintf('Failed at getting field thetaMax. \n')
            innerLoop = false ;
          end % try
          captions{i} = strcat(cTypes{cType},':', ...
                        smithRules{rule},':',ctrlrs{ctrlr}, ...
                        ':tau_c:',num2str(tau_cRef(tau_c))) ;
          n = length(this.d_span) ;
          d(1:n,i) = this.d_span ;
          J(1:n,i) = this.J(:,tau_c) ;
          innerLoop = false ;
        end % innerLoop
        i = i + 1 ;
      end % tau_c
    end % ctrlr
  end % rule

  % Set final captions and plotting colors
  captions = captions(~cellfun('isempty',captions)) ;
  ColorSet = varycolor(length(captions)) ;
  % Removing zeros
  J(:,all(~any(J),1)) = []; % and columns
  d(:,all(~any(d),1)) = []; % and columns
  J(J==0) = NaN ;
  J(J==-1) = NaN ;

  % Plotting routine
  figure(modelId)
  clf 
  set(gca, 'ColorOrder', ColorSet) ;
  % hold on
  plot(d,J) ;
  % for n = 1:b
  %   if idx(n)
  %     plot(d(:,n),J(:,n), 'o')
  %   end
  % end
  xlabel('Time-delay, $\thetamax$', 'interpreter', 'latex')
  ylabel('Cost, $J$', 'interpreter', 'latex') 
  legend(captions, 'Location', 'EastOutside')
  title(titleName{modelId},'interpreter','latex')

  sPath = strcat(mDir,'/results/merged/distCost/', ...
          num2str(10*Ms_s),'/', modelName{modelId}) ;
  save([sPath,'.mat'])
  saveas(gcf,[sPath,'.fig'])
  loc = strcat(sPath,'.tex');
  matlab2tikz(loc,'showInfo', false) ;
    
  fprintf('\t Finished!\n\n')
end % modelId
end % Ms

fprintf('Finished after %.2f seconds.\n', toc)








