% 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'} ;
cType = 7 ; % smithsimc

parm.cType = cType ;
parm.simCost = 0 ;
parm.dispErrors = 1 ;
parm.cNum = 1 ; % For controller.m. Option not used in run.
parm.dFilter = 1 ;
parm.alph = 0.1 ;
parm.simTime = 50 ;
parm.distOpt = 1 ;

fprintf('This is SMITH:SIMC plot production')
tic ;
for ctrlr = 1:2
  modelStart = 1 ;
  if ctrlr == 1            % PID for only second order processes
    modelEnd = 9 ;
  else
    modelEnd = 14 ;
  end
  % for modelId = modelStart:modelEnd
  for modelId = 14:modelEnd
  % for modelId = 10:10
    %% Extracting data from PO:PID optimization
    dat = load([mDir,'/results/po/pid/chriss/',...
         num2str(modelName{modelId}),'.mat']) ;
    iae_y = dat.res.IAEweights(1) ;
    iae_d = dat.res.IAEweights(2) ;
        fprintf('\nIAE weights loaded.\n')
        fprintf('iae_y = %0.2f\n', iae_y)
        fprintf('iae_d = %0.2f\n', iae_d)
    res.iaeCtrl = dat.res.iaeCtrl ;   % Saving controller tuning
    res.iae0    = [iae_y iae_d] ;           % Saving iae weights
    res.iaeFlags= dat.res.iaeFlags ;           % Saving optFlags
    parm.Jweights   = [0.5 0.5] ;        % Cost function weights
    parm.IAEweights = [iae_y ; iae_d] ;            % IAE weights

    parm.modelId = modelId ;
    G = model(modelId) ;
    parm.G = G ;
    if modelId < 9 && ctrlr == 2
      G = halfrule(G) ;
    end
    dtmax = 0.9 ;
    L_max = dtmax*G.theta ;
    if modelId > 9
      tau_start = 0.05*L_max ;
    else
      tau_start = 0.1*L_max ;
    end
    tau_end   = 5*L_max ;
    i = 1 ;
    parm.ctrlr = ctrlr ;
    tau_span = tau_start:(tau_end-tau_start)/100:tau_end ;
    Ms = zeros(length(tau_span),1) ; % Ms for J(Ms)
    J = Ms ;                       % J for J(Ms)
    Gm = Ms ;
    Pm = Ms ;
    wcg = Ms ;
    wcp = Ms ;
    thetaMax = Ms ;
    fprintf('Model = %s.\n', titleName{modelId})
    fprintf('Ctrlr = %s. \n', ctrlrs{ctrlr})
    fprintf('J(Ms) in progress: ')
    for tau_c = tau_span
      parm.tau_c = tau_c ;
      C = controller(parm) ;
      J(i) = distCostFun(C,G,parm) ;
      Ms(i) = distMs(C,G) ;
      [Gm(i) Pm(i) wcg(i) wcp(i)] = margin(C.tf*G.tf) ;
      if (Gm(i) > 0 && Pm(i) > 0)
        thetaMax(i) = Pm(i)*pi/180/wcg(i) ;
      else
        thetaMax(i) = 0 ;
      end
      i = i + 1 ;
      fprintf('*') 
    end
    J(J>100) = NaN ;          % Removing sensless large numbers
    thetaMax(thetaMax>100) = 100 ; 
    MsJStr = struct ;
    MsJStr = setfield(MsJStr,'tau_span',tau_span) ;
    MsJStr = setfield(MsJStr,'J',J) ;
    MsJStr = setfield(MsJStr,'Ms',Ms) ;
    MsJStr = setfield(MsJStr,'Gm', Gm) ;
    MsJStr = setfield(MsJStr,'Pm', Pm) ;
    MsJStr = setfield(MsJStr,'wcg', wcg) ;
    MsJStr = setfield(MsJStr,'wcp', wcp) ;
    MsJStr = setfield(MsJStr,'thetaMax', thetaMax) ;
    fprintf('\t Finished. \n')
    tau_refV = [0.5 1 1.5 2] ;
    tau_ref = tau_refV*L_max ;
    Jref = zeros(1,length(tau_ref)) ;
    MsRef = Jref ;
    Gm  = zeros(1,length(tau_ref)) ;
    Pm  = Gm ; 
    wcg = Gm ;
    wcp = Gm ;
    thetaMax = Gm ;
    i = 1 ;
    fprintf('Reference points in progress: ')
    for tau_c = tau_ref
      parm.tau_c = tau_c ;
      C = controller(parm) ;
      Jref(i) = distCostFun(C,G,parm) ;
      MsRef(i) = distMs(C,G) ;
      [Gm(i) Pm(i) wcg(i) wcp(i)] = margin(C.tf*G.tf) ;
      if (Gm(i) > 0 && Pm(i) > 0)
        thetaMax(i) = Pm(i)*pi/180/wcg(i) ;
      else
        thetaMax(i) = 0 ;
      end
      i = i + 1;
      fprintf('*')
    end
    ref = struct ;
    ref = setfield(ref,'tau_ref',tau_ref) ;
    ref = setfield(ref,'MsRef',MsRef) ;
    ref = setfield(ref,'Jref',Jref) ;
    ref = setfield(ref,'GmRef', Gm) ;
    ref = setfield(ref,'PmRef', Pm) ;
    ref = setfield(ref,'wcgRef', wcg) ;
    ref = setfield(ref,'wcpRef', wcp) ;
    ref = setfield(ref,'thetaMaxRef', thetaMax) ;
    fprintf('\t Finished. \n')
    %
    fprintf('Disturbance performance in progress. \n')
    d_span  = [-0.9:0.01:0.9] ;
    Ms  = zeros(length(d_span),length(tau_ref)) ;% Ms values
    Mt  = zeros(length(d_span),length(tau_ref)) ;% Mt values
    J   = zeros(length(d_span),length(tau_ref)) ;% Cost init
    Gm  = zeros(length(d_span),length(tau_ref)) ;
    Pm  = Gm ; 
    wcg = Gm ;
    wcp = Gm ;
    thetaMax = Gm ;
    j = 1 ;
    for tau_c = tau_ref
      fprintf('  tau_c = %.2f*theta: ', tau_refV(j))
      i = 1 ;
      parm.tau_c = tau_c ;
      C = controller(parm) ;
      for d = d_span 
        Gdist = distModel(G,d) ;
        L = C.tf*Gdist.tf ;
        [Gm(i,j),Pm(i,j),wcg(i,j),wcp(i,j)] = margin(L) ;
        if (Gm(i,j)>0 && Pm(i,j)>0)
          thetaMax(i,j) = Pm(i,j)*pi/180/wcg(i,j) ;
          J(i,j) = distCostFun(C, Gdist, parm) ;
          Ms(i,j)   = distMs(C, Gdist) ;
          Mt(i,j)   = distMt(C, Gdist) ;
          fprintf('*')
        else
          thetaMax(i,j) = 0 ;
          J(i,j) = NaN;
          Ms(i,j) = NaN ;
          Mt(i,j) = NaN ;
          fprintf('|') ;
        end
        i = i + 1 ;
      end % d in d_span
      fprintf('\n')
      j = j + 1 ;
    end % tau_c in tau_span
    
    J(J>1000) = NaN ;
    dist = struct ;
    dist = setfield(dist, 'd_span', d_span) ;
    dist = setfield(dist, 'J', J) ;
    dist = setfield(dist, 'Ms', Ms) ;
    dist = setfield(dist, 'Mt', Mt) ;
    dist = setfield(dist, 'Gm', Gm) ;
    dist = setfield(dist, 'Pm', Pm) ;
    dist = setfield(dist, 'wcg', wcg) ;
    dist = setfield(dist, 'wcp', wcp) ;
    dist = setfield(dist, 'thetaMax', thetaMax) ;


    caption = strcat('Smith:simc:',ctrlrs{ctrlr},...
                     ':',modelName{modelId}) ;
    res = setfield(res,'theta_span',tau_refV) ;
    res = setfield(res,'Reference',ref) ;
    res = setfield(res,'JMs', MsJStr) ;
    res = setfield(res,'thetaMax',dist) ;
    % Do some printing and plotting
    fprintf('Completed making J(Ms). Plotting: \n')
    figure((ctrlr-1)*length(modelName)+modelId) ;
    clf
    hold on
    box on
    plot(res.JMs.Ms,res.JMs.J)
    plot(MsRef,Jref,'ok')
    hold off 
    xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    ylabel('Cost, $J$', 'interpreter', 'latex')
    title(titleName{modelId})
    legend(caption,'Location','EastOutside')
    % Saving
    sPath = strcat('/results/smith/simc/', ...
            num2str(ctrlrs{ctrlr}), '/', ...
            num2str(modelName{modelId})) ;
    save([mDir, sPath,'.mat'], 'res')
    saveas(gcf,[mDir, sPath,'.fig'])
    loc = strcat(mDir, sPath, '.tex');
    matlab2tikz(loc,'showInfo', false) ;
    fprintf('Finished plotting J(Ms)! Saved in %s \n\n', sPath)
    %
    figure((ctrlr+1)*length(modelName)+modelId) ;
    clf
    legendCell = {'tau_c = 0.5*theta', 'tau_c = theta', ...
                  'tau_c = 1.5*theta', 'tau_c = 2*theta'} ;
    plot(dist.d_span,dist.J)
    xlabel('Time-delay, $\theta$', 'interpreter', 'latex')
    ylabel('Cost, $J$', 'interpreter', 'latex')
    legend(legendCell,'Location','EastOutside')
    saveas(gcf,[mDir, sPath,'Jd.fig'])
    loc = strcat(mDir, sPath, 'Jd.tex');
    matlab2tikz(loc,'showInfo', false) ;
    %
    figure((ctrlr+3)*length(modelName)+modelId)
    clf
    plot(dist.Ms, dist.thetaMax)
    xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    ylabel('Max additional time-delay, $\thetamax$', 'interpreter', 'latex')
    legend(legendCell,'Location','EastOutside')
    saveas(gcf,[mDir, sPath,'distThetaMs.fig'])
    loc = strcat(mDir, sPath, 'distThetaMs.tex');
    matlab2tikz(loc,'showInfo', false) ;
    %
    figure((ctrlr+5)*length(modelName)+modelId)
    clf
    plot(MsJStr.Ms, MsJStr.thetaMax)
    xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    ylabel('Max additional time-delay, $\thetamax$', 'interpreter', 'latex')
    legend(legendCell,'Location','EastOutside')
    saveas(gcf,[mDir, sPath,'ThetaMs.fig'])
    loc = strcat(mDir, sPath, 'ThetaMs.tex');
    matlab2tikz(loc,'showInfo', false) ;
  end % ctrlr
end % modelId
fprintf('Finished after %.2f seconds. \n', toc) ;

