% Purpose:  
% Author:   Axel Lodemel Holene
% E-mail:   axel.holene@gmail.com
% Date:     May 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', 'brules','simc'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;


tic;
for modelId = 14:14 %length(modelName)
  J = zeros(100,12) ;
  Ms = J ;
  Mt = J ;
  Gm = Ms ;
  Pm = Ms ;
  wcg = Ms ;
  wcp = Ms ;
  thetaMax = Ms ;
  captions = {};
  ctrlrVec = [1 2] ;
  cNum = 4 ;
  dFilter = 1 ;
  alph = 0.01 ;
  parm.dFilter = dFilter ;
  parm.alph = alph ;
  parm.distOpt = 1 ;
  i = 1 ;                                            % Counter
  for cType = [1 3]
    for ctrlr = ctrlrVec
      parm.modelId = modelId ;
      parm.cNum = cNum ;
      parm.cType = cType ;
      parm.ctrlr = ctrlr ;

      success = true ;
      lPath = strcat(mDir,'/results/', ...
              cTypes{cType}, '/', ...
              ctrlrs{ctrlr}, '/', ...
              cParm{cNum},   '/', ...
              modelName{modelId}) ;
      fprintf('Loading %s\n', lPath)
      try
          load([lPath,'.mat'],'res') ;
      catch
          fprintf('Failed: %s\n', lasterr)
          success = false ;
      end
      if success
        this = getfield(res, ...
                strcat(upper(cTypes{cType}), ...
                upper(ctrlrs{ctrlr}))) ;
        captions{i} = strcat(cTypes{cType},':', ...
                        ctrlrs{ctrlr}) ;
        G = res.G ;
        parm.G = G ;
        J(1:length(this.J),i) = this.J ;
        Ms(1:length(this.Msc),i) = this.Msc ;
        fprintf('Calculating:\n')
        for idx = 1:length(this.Msc)
          C = controller(this.minTune(idx,:),parm) ;
          % Making a shortcut via the 'bode' function to avoid
          % having 'margin' checking out to many frequencies
          [mag phase w] = bode(C.tf*G.tf, {0.1 20}) ;
          [Gm(idx,i) Pm(idx,i) wcg(idx,i) wcp(idx,i)] = ...
                                           margin(mag,phase,w) ;
          if (Gm(idx,i) > 0 && Pm(idx,i) > 0)
            thetaMax(idx,i) = Pm(idx,i)*pi/180/wcg(idx,i);
            if thetaMax(idx,i)>100
              fprintf('Super high theta = %.2f at (%d,%d)\n',...
                      thetaMax(idx,i),idx,i)
              thetaMax(idx,i) = 100 ;
            end % if thetaMax large
            fprintf('*')
          else % not stable
            thetaMax(idx,i) = 0 ;
            fprintf('|')
          end % if stable
        end % Ms_s = Ms
        fprintf('\nSuccess! \n')
      end % success
      i = i + 1 ;
    end % ctrlr
  end % cType
  Gm = Gm(:,1:find(Gm(1,:),1,'last')) ;
  Pm = Pm(:,1:find(Pm(1,:),1,'last')) ;
  J = J(:,1:find(J(1,:),1,'last')) ;
  Ms = Ms(:,1:find(Ms(1,:),1,'last')) ;
  thetaMax = thetaMax(:,1:find(thetaMax(1,:),1,'last')) ;
  % Gm(:,all(~any(Gm),1)) = []; % and columns
  % Pm(:,all(~any(Pm),1)) = []; % and columns
  % J(:,all(~any(J),1)) = []; % and columns
  % Ms(:,all(~any(Ms),1)) = []; % and columns
  % thetaMax(:,all(~any(thetaMax),1)) = []; % and columns
  % J(J==0) = NaN ;
  Ms(Ms==0) = NaN ;
  J(J>100) = NaN ;
  Ms(Ms>100) = NaN ;
  thetaMax(thetaMax>100) = 100 ;

  % thetaMax(thetaMax==0) = NaN ;
  captions = captions(~cellfun('isempty',captions)) ;
  ColorSet = varycolor(length(captions)) ;

  % Plotting routine
  fprintf('Plotting')
  figure(modelId)
  clf
  set(gca, 'ColorOrder', ColorSet);
  plot(Ms,J,'LineWidth',1)
  % axis([1.1 2.5 0.5 3])
  xlabel('Robustness, $\Ms$', ...
         'interpreter','latex','FontSize',12)
  ylabel('Performance, $J$', ...
          'interpreter','latex','FontSize',12)
  title(titleName{modelId}, ...
          'interpreter','latex','FontSize',12)
  legend(captions, 'Location','EastOutside')

  sPath = strcat(mDir,'/results/merged/smithCompareTheta/', ...
                  modelName{modelId}) ;
  save([sPath,'.mat'])
  saveas(gcf,[sPath,'.fig'])
  loc = strcat(sPath,'.tex');
  matlab2tikz(loc, 'width', '\figureheight', 'height', ...
              '\figurewidth', 'showInfo', false) ;

  % Ms-theta-plot
  figure(length(modelName)+modelId)
  clf
  set(gca, 'ColorOrder', ColorSet);
  plot(Ms,thetaMax,'LineWidth',1)
  % axis([1.1 2.5 0.5 3])
  xlabel('Robustness, $\Ms$', ...
         'interpreter','latex','FontSize',12)
  ylabel('Max additional time-delay, $\thetamax$', ...
          'interpreter','latex','FontSize',12)
  title(titleName{modelId}, ...
          'interpreter','latex','FontSize',12)
  legend(captions, 'Location','EastOutside')

  sPath = strcat(mDir,'/results/merged/smithCompareTheta/', ...
                  modelName{modelId}) ;
  save([sPath,'MsTheta.mat'])
  saveas(gcf,[sPath,'MsTheta.fig'])
  loc = strcat(sPath,'MsTheta.tex');
  matlab2tikz(loc, 'width', '\figureheight', 'height', ...
          '\figurewidth', 'showInfo', false) ;
  fprintf('\t Finished!\n\n')
end % modelId
fprintf('Finished after %.2f seconds!\n\n', toc)
restoredefaultpath


