%% Header
% Purpose:  Calculation and graphical representation of pareto 
%           optimal PID controller tuning.
% Author:   Axel Lodemel Holene
% E-mail:   axel.holene@gmail.com
% Date:     February 2013
% About:    ctrl = [Kc I D]

% parm.Ms_s = 2; ctrl = ctrl; Jfun = @(ctrl)costFun(ctrl,parm) ; cFun = @(ctrl)conFun(ctrl,parm) ; [ctrl J optFlag] = fmincon(Jfun,ctrl,[],[],[], [],lb,[],cFun,opt)
%% Initialising
clear all
% close all
% clc
% profile on


restoredefaultpath 
mDir = fullfile(pwd) ;
shared = fullfile(mDir,'commonFiles') ;     addpath(shared)
m2tkz  = fullfile(mDir,'matlab2tikz/src') ; addpath(m2tkz) 
varclr = fullfile(mDir,'varycolor') ;       addpath(varclr)

runTime = cputime ;               % Start total computation time

%% Simulation, process and controller  parameters
modelName = {'case1', 'case2', 'case3', 'case4', 'case5',    ...
             'case6', 'case7', 'case8', 'case9', 'case10',   ...
             'case11', 'case12', 'case13','case14'} ;
cTypes  = {'po', 'simc', 'smith', 'smithDist', 'thetaMax'} ;
ctrlrs  = {'pid', 'pi', 'p'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;
cType   = 2 ;                                % Optimisation type
ctrlr   = 2 ;                          % Controller (PID, PI, P)
cNum    = 4 ;                                 % Parameterisation
dFilter = 1 ;                               % PID with D-filter?
distOpt = 3 ;                        % PO or Smith to smithDist?
nyqOpt  = 4 ;                        % Which controller to load?
alph = 0.01 ;                           % Derivative filter size
runParallel = 0 ;                         % Parallel processing?
simCost = 1 ;                 % CostFun: step(0) or Simulink(1)?
runGradientfree = 0 ;              % Gradient free optimisation?
dispWarnings = 0 ;                  % Display possible warnings?
dispErrors   = 0 ;             % Display (some) possible errors?
runTomlab = 0 ;                            % Run Tomlab toolbox?
simTime = 50 ;                         % Step/simulink sim. time

startMod = 11 ;
endMod   = 14 ;

if runParallel
  try
    matlabpool
  catch
  end
end


sNum    = 2 ;                                    % Solver option
solvers = {'active-set', 'sqp', 'fminsearch'} ;        % Solvers
sFlags  = {5,2,1} ;             % Solver flag limits for success
solver  = solvers{sNum} ;                      % Choosing solver
sFlag   = sFlags{sNum}  ;                  % Choosing flag limit
MsRes   = 0.025 ;                        % Ms PO plot resolution
MsStart = 1.25 ;
MsEnd   = 3 ;
dRes    = 0.01 ;                   % Smith error plot resolution
dMin    = -0.9 ;
dMax    = abs(dMin) ;

if cType == 3                 % For Smith predictors, max Ms = 2
  MsEnd = 1.99 ;
  MsRes = 0.01 ;
end

if ~dispWarnings
  warning('off','Control:combination:SingularRightDivision') ;
  warning('off','Simulink:blocks:TDelayBufferTooSmall') ;
  warning('off','MATLAB:singularMatrix') ;
  warning('off', 'Control:analysis:ZeroApproximateDelay1') ;
  warning('off', 'Control:transformation:StateSpaceScaling') ;
  warning('off', 'Control:transformation:C2dApproximate') ;
  warning('off', 'Simulink:blocks:TDelayBufferTooSmall') ;
  warning('off', 'Control:analysis:PoleApproximateDelay1') ;
end
if runTomlab
  P = genpath('/Applications/tomlab') ;
  addpath(P) ;
  global parm                           % need global parameters
end

% Generate parameter struct
sNum_s  = sNum ;
parm.cType = cType ;
parm.ctrlr = ctrlr ;
parm.cNum  = cNum  ;
parm.alph  = alph ;
parm.RP    = runParallel ;
parm.simTime = simTime ;
parm.dFilter = dFilter ;
parm.simCost = simCost ;
parm.dispErrors = dispErrors ;
parm.distOpt = distOpt ;

for modelId = startMod:endMod ;                       % Model ID
% for modelId = [1 4 11]
startTime = cputime ;
G = model(modelId) ;
parm.G = G ;
parm.modelId = modelId ;

%% Start diary
diary([mDir, '/diary/', num2str(cTypes{cType}), ...
      '/', num2str(ctrlrs{ctrlr}),'/', ...
      '/', num2str(cParm{cNum}),'/', ...
      num2str(modelName{modelId}),'.txt'])

%% Parameters for finding IAE weights
Ms_s = 1.59 ;                                      % Ms setpoint
init = initValues(parm) ;                % Initial values matrix
lb = zeros(size(init,2),1) ;                % Lower tuning bound
parm.Ms_s = Ms_s ;

% Optimizer options
opt = optimoptions('fmincon','algorithm',solver, ...
                   'UseParallel','always','Display','off', ...
                   'TolCon',1e-4,'PlotFcns',{@optimplotx, ...
                   @optimplotfval, @optimplotfunccount}, ...
                   'InitBarrierParam', 1e-5) ; 
sOpt = optimset('TolFun',1e-3,'TolX',1e-4,...
                'Display','off', 'MaxIter', 50, ...
                'PlotFcns',...
                {@optimplotx, @optimplotfval, ...
                @optimplotfunccount}) ;

  fprintf('%s %s CONTROLLER \n',  upper(cTypes{cType}),...
                                  upper(ctrlrs{ctrlr})) ;
  fprintf('%s\n', datestr(clock, 'dd.mm.yy HH:MM'))
  fprintf('Main Directory: %s\n', mDir)
  fprintf('Case: %g\n',modelId)
  fprintf('Target %s\n', upper(cTypes{cType})) ;
  fprintf('Structure: %s\n', upper(ctrlrs{ctrlr})) ;
  fprintf('Parametrization: %s\n', cParm{cNum})
  fprintf('Parallel processing: ')
  if runParallel == 1
      fprintf('yes \n')
  else
      fprintf('no \n')
  end
  fprintf('Start model: %d \n', startMod)
  fprintf('End model  : %d \n', endMod)
  
if ctrlr == 1 && cType == 1  && cNum ~= 4 
                          % strcmpi(cTypes{cType}, 'po')
  fprintf('\nFinding Optimal IAE Weights \n')
  fprintf('Solver: %s\n', solvers{sNum})
  fprintf('------------------------------------------------')
  fprintf('------------------------------------------------\n')
  if ctrlr == 1
      fprintf('J\tKc\tI\tD\tMs_s\tMs\tflag\tCPU time\n')
  else
      fprintf('J\tKc\tI\tMs\tflag\tCPU time\n')
  end
  fprintf('------------------------------------------------')
  fprintf('------------------------------------------------\n')
  %% Optimizing for IAE0_y
  parm.IAEweights = [1 1]' ;     % Setting IAE costFun weighting
  parm.Jweights   = [1 0]  ;           % Setting costFun weights
  ctrl = init(1,:) ;
  Jfun = @(ctrl)costFun(ctrl, parm) ;
  cFun = @(ctrl)conFun(ctrl, parm) ;
  sTime = 0 ;
  try
      tic ;
      [ctrl iae_y optFlag] = fmincon(Jfun, ctrl, [], [], [],...
                                  [], lb, [], cFun, opt) ;
      sTime = toc ;
      % if min(ctrl) < 1e-3
      %     name = {'Kc' 'I' 'D'} ;
      %     fprintf('%s failed. %s approx zero.\n', ...
      %             solvers{sNum}, ...
      %             name{find(ctrl==min(ctrl))})
      %     optFlag = 0 ;
      %     ctrl = init(1,:) ;         % Resetting initial values
      % end
  catch
      optFlag = 0 ;
  end
  if optFlag < sFlags{sNum}
    Jfun  = @(ID)noGrad([ctrl(1) ID], parm) ; % fminsearch help function
    tic ;
    [ctrl(2:end) iae_y optFlag] = ...
                         fminsearch(Jfun, ctrl(2:end), sOpt) ;
    sTime = sTime + toc ;        % Ending CPU time measurement
    MsFun = @(Kc) parm.Ms_s - Ms([Kc ctrl(2:end)], parm) ;
    ctrl(1) = fzero(MsFun, ctrl(1)) ;             % Finding Kc
    Jfun = @(ctrl)costFun(ctrl, parm) ;
    cFun = @(ctrl)conFun(ctrl, parm) ;
    try
      tic ;
      [ctrl iae_y optFlag] = fmincon(Jfun, ctrl, [], [], [], ...
                                [], lb, [], cFun, opt) ;
      sTime = sTime + toc ;
    catch
      fprintf('%s failed. Fminsearch results yields.\n', ...
              upper(solver))
    end
  end
  Msc  = Ms(ctrl, parm) ;            % Calc. Ms for confirmation
  Ciaey = ctrl ;
  yOpt  = optFlag ;
  if ctrlr == 1
    fprintf('%0.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%3.2f\n',...
            iae_y,ctrl,Ms_s,Msc,optFlag,sTime)
  else
    fprintf('%0.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%3.2f\n',...
          iae_y,ctrl,Ms_s,Msc,optFlag,sTime)
  end

  % Optimizing for IAE0_d
  ctrl          = init(2,:) ;            % IAE0_d starting point
  parm.Jweights = [0 1] ;
  Jfun = @(ctrl)costFun(ctrl, parm) ;
  cFun = @(ctrl)conFun(ctrl, parm) ;
   try
    tic ;
    [ctrl iae_d optFlag] = fmincon(Jfun, ctrl, [], [], [],...
                                [], lb, [], cFun, opt, Prob) ;
    sTime = toc ;
  catch
    optFlag = 0 ;
  end
  if optFlag < sFlags{sNum}
    Jfun  = @(ID)noGrad([ctrl(1) ID], parm) ;    % help function
    tic ;
    [ctrl(2:end) iae_d optFlag] = ...
                           fminsearch(Jfun, ctrl(2:end), sOpt) ;
    sTime = sTime + toc ;          % Ending CPU time measurement
    MsFun = @(Kc) parm.Ms_s - Ms([Kc ctrl(2:end)], parm) ;
    ctrl(1) = fzero(MsFun, ctrl(1)) ;               % Finding Kc
    Jfun = @(ctrl)costFun(ctrl, parm) ;
    cFun = @(ctrl)conFun(ctrl, parm) ;
    try
      tic ;
      [ctrl iae_d optFlag] = fmincon(Jfun, ctrl, [], ...
                          [], [],[], lb, [], cFun, opt) ;
      sTime = sTime + toc ;
    catch
      fprintf('%s failed. Fminsearch results yields.\n',...
              upper(solver))
    end
  end
  Msc  = Ms(ctrl, parm) ;  % Calc. Ms for confirmation
  Ciaed = ctrl ;
  dOpt  = optFlag ;
  if ctrlr == 1
    fprintf('%0.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%3.2f\n',...
        iae_d,ctrl,Ms_s,Msc,optFlag,sTime)
  else
    fprintf('%0.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%3.2f\n',...
        iae_d,ctrl,Ms_s,Msc,optFlag,sTime)
  end
  res.iaeCtrl = [Ciaey; Ciaed] ;      % Saving controller tuning
  res.iae0    = [iae_y iae_d] ;             % Saving iae weights
  res.iaeFlags= [yOpt dOpt] ;                  % Saving optFlags
else
  %% Extracting data fra 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
end
parm.Jweights   = [0.5 0.5] ;            % Cost function weights
parm.IAEweights = [iae_y iae_d]' ;                 % IAE weights

pFlag = true ;
%%%%%%%%%%%%%%%%%%%%%%%%%%
if cType == 1 || cType == 3 
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generating pareto curve
i = 1 ;                                      % Iteration counter
ctrl    = init(3,:) ;  % Setting initial values for pareto curve
Ms_span = MsStart:MsRes:MsEnd ;                  % Values for Ms
Msc     = zeros(length(Ms_span),1) ;      % Calculated Ms values 
Mtc     = zeros(length(Ms_span),1) ;           % Init. Mt vector
sTime   = zeros(length(Ms_span),1) ;    % Init. time consumption
Jtune   = zeros(length(Ms_span),1) ;         % Init. cost vector
flags   = zeros(length(Ms_span),1) ;   % Init. exit flags vector
IAEtune = zeros(length(Ms_span),2) ;  % IAE values per iteration
minTune = zeros(length(Ms_span),length(ctrl)) ;    % Opt. tuning
initial = zeros(length(Ms_span),length(ctrl)) ;   % Init. values
ctrlRoot = {} ;                               % Controller zeros
  fprintf('\n')
  fprintf('Generating the Pareto Optimal Curve\n')
  fprintf('------------------------------------------------')
  fprintf('------------------------------------------------\n')
  fprintf('Solver: %s\n', solvers{sNum})
  fprintf('Number of iterations: %d\n', length(Ms_span))
  if ctrlr == 1
    fprintf('J\tKc\tI\tD\tMs_s\tMs\tflag\titer.\t')
  elseif ctrlr == 2
    fprintf('J\tKc\tI\tMs_s\tMs\tflag\titer.\t')
  else 
    fprintf('J\tKc\tMs_s\tMs\tflag\titer.\t')
  end
  fprintf('time[s] \t timeleft[min] \n')
  fprintf('------------------------------------------------')
  fprintf('------------------------------------------------\n')

exitFlag = true ;
pFlag = true ;
while exitFlag 
for Ms_s = Ms_span
  parm.Ms_s = Ms_s ;                % Updating Ms setpoint value
  Jfun      = @(ctrl)costFun(ctrl,parm) ;        % Updating cost
  cFun      = @(ctrl)conFun(ctrl,parm);   % Updating constraints
  initial(i,:) = ctrl ; % Saving starting point for optimization
  runOpt = true ;
  if ctrlr == 3 % Just for P-control
    sNum = 5 ;
    % try
    %     tic ;
    %     [ctrl J optFlag] = fmincon(Jfun,ctrl,[],[],[],...
    %                                [],lb,[],cFun,opt) ;
    %                     % Optimizing from last optimal point
    %     sTime(i) = toc ;       % Ending CPU time measurement
    %     if optFlag < 1
    %         sNum = 5 ;
    %     end
    % catch
    %     sNum = 5
    % end
    if sNum == 5
        tic ;
        MsFun = @(ctrl) parm.Ms_s - Ms(ctrl,parm) ;
        opt = optimset('Display','off',...
                       'UseParallel','Always') ;
        [ctrl dmy optFlag] = fzero(MsFun, ctrl, opt) ;
        J = costFun(ctrl, parm) ;
        % [ctrl J optFlag] = fmincon(Jfun,ctrl,[],[],[],...
        %                            [],lb,[],cFun,opt); 
        sTime(i) = sTime(i) + toc ;          % Ending CPU time
    end % if sNum
  else
    if runGradientfree
      runOpt = false ;            % No standard opt. routine
      Kc = ctrl(1); ID = ctrl(2:end);
      Jfun = @(ID)noGrad([Kc ID], parm) ;
      try
          tic ;
          [ID J optFlag] = ...
                          fminsearch(Jfun, ID, sOpt) ;
          sTime(i) = toc ;
      catch
          pFlag = false ;
      end % try
      if pFlag
          fprintf('Success! (%.2f s)\n', sTime(i))
          MsFun = @(Kc) Ms_s - Ms([Kc ID], parm) ;
          tic
          Kc = fzero(MsFun, Kc) ;
          sTime(i) = sTime(i) + toc ;
          ctrl = [Kc ID] ;
      else
          % Running standard opt routine.  Keep  the  faith!
          pFlag = true ;
          runOpt = true ; 
      end % pFlag
    end % runGradientfree
    while runOpt
      while sNum < 3
        try
          if runTomlab
            tic ;
            Prob = ProbDef ;
            Prob.Warning = 0;    % Turning off warnings. 
            % Prob.Solver.Tomlab = 'npsol' ;
            [ctrl J optFlag] = fmincon(@Jfunc, ...
                ctrl,[],[], [],[],lb,[],@conFunc,...
                opt,Prob) ;
            sTime(i) = toc ;
          else % if not Tomlab
            % ctrl = simc(G.theta,parm) ; % setting simc as initial point
            tic ;
            [ctrl J optFlag] = fmincon(Jfun,ctrl,[],[],[],...
                                       [],lb,[],cFun,opt); 
                         % Optimizing from last optimal point
            sTime(i) = toc ;    % Ending CPU time measurement
          end % tomLab
          if optFlag < sFlags{sNum}
            fprintf('%s failed. Error: %s \n', ...
                    solvers{sNum}, lasterr) 
            sNum = sNum + 1 ;
          elseif min(ctrl) < 1e-3 && modelId < 10
            name = {'Kc' 'I' 'D'} ;
            fprintf('%s failed. ', solvers{sNum})
            fprintf('Error: %s approx zero.\n', ...
                    name{find(ctrl==min(ctrl))})
            % Hot start from previous working values
            if i == 1; ctrl = init(3,:) ;
            else; ctrl = minTune(i-1,:); end;
            sNum = sNum + 1 ;
          else % this is done anyway
            tic ;
            [ctrl J optFlag] = fmincon(Jfun,ctrl,...
                               [],[],[],[],lb,[],...
                               cFun,opt); 
                          % Optimizing from last optimal point
            sTime(i) = sTime(i) + toc ;
            if optFlag < sFlags{sNum}
              fprintf('%s failed. ', ...
                      solvers{sNum}) 
              sNum = sNum + 1 ;
            else
              runOpt = false ;     % Opt success. Ending while
              break
            end
          end % optFlag < sFlags
        catch
          fprintf('%s failed. Error: %s',solvers{sNum},...
                                                  lasterr)
          sNum = sNum + 1 ;
        end
      end % while
        if sNum == 3
          fprintf('Using %s... ', solvers{sNum})
          Kc = ctrl(1); ID = ctrl(2:end);
          Jfun = @(ID)noGrad([Kc ID], parm) ;
          try
              tic ;
              [ID J optFlag] = ...
                              fminsearch(Jfun, ID, sOpt) ;
              extrat = toc ;
              sTime(i) = sTime(i) + extrat ;
          catch
              pFlag = false ;
              fprintf('fminsearch failed.\nGiving up.\n')
              fprintf('Error message:\n'); disp(lasterr)
              runOpt = false ;
              exitFlag = false ;
              break
          end % try
          if pFlag
            fprintf('Success! (%.2f s)\n', extrat)
            sTime(i) = sTime(i) + extrat ;
          else
            pFlag = true ;
          end % if pFlag
          MsFun = @(Kc) Ms_s - Ms([Kc ID], parm) ;
          Kc = fzero(MsFun, Kc) ;
          ctrl = [Kc ID] ;
          Jfun = @(ctrl)costFun(ctrl, parm) ;   % Updating cost
          cFun = @(ctrl)conFun(ctrl, parm);    % Updating cons. 
          try
            fprintf('Second run sqp start... ')
            tic ;
            [ctrl J optFlag] = fmincon(Jfun, ctrl, [], ...
                                        [], [], [], lb,...
                                        [], cFun, opt) ;
            extrat = toc ;
          catch
            pFlag = false ;
            fprintf('Second run sqp failed\n')
            ctrl = [Kc ID];
          end
          if pFlag
            tic ;
            [ctrl J optFlag] = fmincon(Jfun, ctrl, [], ...
                                        [], [], [], lb,...
                                        [], cFun, opt) ;
            extrat = extrat + toc ;
            fprintf('Success! (%.2f s)\n', extrat)
            sTime(i) = sTime(i) + extrat ;
          else
            pFlag = true ;
          end % if pFlag
          C = controller(ctrl, parm) ;
          MsCalc = Ms(ctrl, parm) ;
          if (MsCalc > Ms_s + 1e-4)||(MsCalc < Ms_s - 1e-4)
          fprintf('Ms values differ at control calculation.\n')
            fprintf('Ms_s = %0.5f, Ms = %.5f\n', Ms_s, MsCalc)
          end
          runOpt = false ; % Opt success. Ending while.
        end % if sNum == 3
      end % while runOpt
    end % if ctrlr == 3
  if exitFlag == false; break; end;
  C            = controller(ctrl, parm) ;           % Controller
  minTune(i,:) = ctrl ;              % Storing controller tuning
  Msc(i)       = Ms(ctrl, parm) ;         % Calculating Ms value
  Mtc(i)       = Mt(ctrl, parm) ;       % Corresponding Mt-value
  ctrlRoot{i}  = zero(C.tf) ;            % Find controller zeros
  IAEtune(i,:) = iae(ctrl, parm) ;          % Saving IAE weights
  Jtune(i,:)   = J    ;            % Storing cost function value
  flags(i,:)   = optFlag ;                   % Storing exit flag
  timeleft     = sum(sTime)/i*(length(Ms_span)-i)/60 ; 
                % Estimation of time left for computations [min]
  if ctrlr == 1
    fprintf('%.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%d\t%0.2f\t\t%0.2f',...
    J,ctrl,Ms_s,Msc(i), optFlag, ...
    length(Ms_span)-i,sTime(i),timeleft)
  elseif ctrlr == 2
    fprintf('%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%d\t%0.2f\t\t%0.2f',...
    J,ctrl,Ms_s,Msc(i), optFlag, ...
    length(Ms_span)-i,sTime(i),timeleft)
  else
    fprintf('%.2f\t%.2f\t%.3f\t%.3f\t%d\t%d\t%0.2f\t\t%0.2f',...
    J,ctrl,Ms_s,Msc(i),optFlag, ...
    length(Ms_span)-i,sTime(i),timeleft)
  end
  fprintf('\n')
  i = i + 1 ;                           % Incrementing counter
  sNum = sNum_s ;
end % Ending for-loop
exitFlag = false ;
end % Ending while-loop

calcTime = cputime - startTime ; % End of total computation time


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif cType == 2      % SIMC
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  if (ctrlr == 2 && modelId ~= 10)
    tau_start = 1/8*G.theta ;
    tau_end = 5*G.theta ;
    G = halfrule(G) ;
    ctrl = [0 0] ;
  elseif (ctrlr == 2 && modelId == 10)
    tau_start = -1/8*G.theta ;
    tau_end = 4*G.theta ;
    ctrl = [0 0] ;
  else
    tau_start = -1/4*G.theta ;
    tau_end = 4*G.theta ;
    ctrl = [0 0 0] ;
  end
  tau_res = (tau_end - tau_start) / 80 ;    % tau_c resolution

  % Generating optimal controllers
  i = 1 ;                                  % Iteration counter
  tau_span = tau_start:tau_res:tau_end ;

  Msc     = zeros(length(tau_span),1) ;        % Values for Ms
  Mtc     = zeros(length(tau_span),1) ;      % Init. Mt vector
  sTime   = zeros(length(tau_span),1) ;% Init time consumption
  Jtune   = zeros(length(tau_span),1) ;     % Init cost vector
  flags   = zeros(length(tau_span),1) ;% Init exit flag vector
  IAEtune = zeros(length(tau_span),2) ;      % Init IAE vector
  minTune = zeros(length(tau_span),size(ctrl,2)) ;   % Tunings
  ctrlRoot = {} ;              % Roots of produced controllers

  fprintf('Case: %g\n',modelId)
  fprintf('Controller: SIMC %s\n', ctrlrs{ctrlr})
  fprintf('-----------------------------------------------\n')
  fprintf('Number of iterations: %d\n', length(tau_span))
  if ctrlr == 1 && modelId < 10
      fprintf('J\tKc\tI\tD\tMs\ttau_c\n')
  else
      fprintf('J\tKc\tI\tMs\ttau_c\n')
  end
  fprintf('-----------------------------------------------\n')
  
  for tau_c = tau_span
    parm.tau_c   = tau_c ;
    C            = controller(parm) ;
    try
        ctrl = [C.Kc C.tau_i C.tau_d] ;
    catch
        ctrl = [C.Kc C.tau_i] ;
    end
    J        = costFun(ctrl, parm) ;
    Msc(i)   = Ms(ctrl, parm) ;
    Mtc(i)   = Mt(ctrl, parm) ;
    minTune(i,:) = ctrl ;            % Storing controller tuning
    ctrlRoot{i}  = zero(C.tf) ;          % Find controller zeros
    IAEtune(i,:) = iae(ctrl, parm) ;        % Saving IAE weights
    Jtune(i)     = J ;             % Storing cost function value
    if ctrlr == 1 && modelId < 10
      fprintf('%.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\n',...
              J,ctrl,Msc(i), tau_c)
    else
      fprintf('%.2f\t%.2f\t%.2f\t%.3f\t%.3f\n',...
              J,ctrl,Msc(i), tau_c)
    end
    i = i + 1 ;                           % Incrementing counter
  end
  % Tau_c reference points
  tau_cRef = [G.theta/2, G.theta, 1.5*G.theta, 2*G.theta] ;
  MscRef = zeros(1,length(tau_cRef)) ;
  MtcRef = MscRef ;
  JcRef  = MscRef ;
  RefTune = zeros(length(tau_cRef),4-ctrlr) ;
  iaeRef  = zeros(length(tau_cRef),2) ;
  i = 1 ;
  for tau_c = tau_cRef
    parm.tau_c = tau_c ;
    C = controller(parm) ;
    try
        ctrl = [C.Kc C.tau_i C.tau_d] ;
    catch
        ctrl = [C.Kc C.tau_i] ;
    end
    RefTune(i,:) = ctrl ;
    JcRef(i)  = costFun(ctrl, parm) ;
    MscRef(i) = Ms(ctrl,parm) ;
    MtcRef(i) = Mt(ctrl,parm) ;
    iaeRef(i,:) = iae(ctrl, parm) ;
    i = i + 1 ;
  end
  Ms_span = Msc ; % No Ms_span existing for SIMC. Generating one
  calcTime = cputime - startTime ;      % End of total calc time

elseif cType == 4
  tic;
  fprintf('\n')
  fprintf('SMITH DISTURBANCE SENSITIVITY\n')
  fprintf('-------------------------------')
  fprintf('-----------------------------------------------\n')
  if ctrlr == 1
    fprintf('Kc\tI\tD\tMs\tMs_s\td\tJ\n')
  elseif ctrlr == 2
    fprintf('Kc\tI\tMs\tMs_s\td\tJ\n')
  elseif ctrlr == 3
    fprintf('Kc\tMs\tMs_s\td\tJ\n')
  end
  fprintf('-------------------------------')
  fprintf('-----------------------------------------------\n')
  d_span  = dMin:dRes:dMax ;
  Ms_span = [1.3 1.6 1.7 1.9] ;
  % Ms_span = [1.3 1.4 1.5 1.6 1.65 1.7 1.75 1.8 1.85 1.9]
  Msc     = zeros(length(d_span),length(Ms_span)) ;  % Ms values
  Mtc     = zeros(length(d_span),length(Ms_span)) ;  % Mt values
  sTime   = zeros(length(d_span),length(Ms_span)) ;  % Calc.time
  Jtune   = zeros(length(d_span),length(Ms_span)) ;  % Cost init
  Gm  = zeros(length(d_span),length(Ms_span)) ;
  Pm  = Gm ; 
  wcg = Gm ;
  wcp = Gm ;
  thetaMax = Gm ;
  L   = {} ;
  poles = {} ;
  j = 1 ;                                    % Iteration counter
  for Ms_s = Ms_span 
    i = 1 ;                                  % Iteration counter
    parm.Ms_s = Ms_s ;
    fprintf('Ms_s = %.2f: \n', Ms_s)
    if ctrlr == 3 % P controller only
      load([mDir, '/results/',             ...
              num2str(cTypes{distOpt}),'/',...
              num2str(ctrlrs{ctrlr}), '/', ...
              num2str(modelName{modelId}),'.mat'], 'res')
    else
      load([mDir, '/results/',             ...
              num2str(cTypes{distOpt}),'/',...
              num2str(ctrlrs{ctrlr}), '/', ...
              num2str(cParm{cNum}),   '/', ...
              num2str(modelName{modelId}),'.mat'], 'res')
    end % ctrlr
    this = getfield(res,strcat(upper(cTypes{distOpt}),...
                    upper(ctrlrs{ctrlr}))) ;
    ctrl = this.minTune(this.Ms_span==Ms_s,:) ;
    C = controller(ctrl, parm) ;
    for d = d_span
      Gdist = distModel(G,d) ;              % Disturbing process
      L = C.tf*Gdist.tf ;
      [mag phase w] = bode(L,{0.001,500}) ;
      [Gm(i,j),Pm(i,j),wcg(i,j),wcp(i,j)]=margin(mag,phase,w) ;
      if (Gm(i,j) > 0 && Pm(i,j) > 0)   % Unstable CL behaviour?
        thetaMax(i,j) = Pm(i,j)*pi/180/wcg(i,j) ;
        jf = distCostFun(C, Gdist, parm) ;
        if jf > 100
          Jtune(i,j) = 100;
        else
          Jtune(i,j) = jf ;
        end
        ms = distMs(C,Gdist) ;
        if ms > 100
          Msc(i,j) = 100 ;
        else
          Msc(i,j)   = ms ;
        end
        Mtc(i,j)   = distMt(C,Gdist) ;
        fprintf('*')
      else % unstable
        thetaMax(i,j) = 0 ;
        Jtune(i,j) = NaN ;
        Msc(i,j)   = NaN ;
        Mtc(i,j)   = NaN ;
        fprintf('|')
      end % if
      i = i + 1 ;
    end % for d in d_span
    fprintf('\n')
    j = j + 1 ;
  fprintf('------------------------------------------------\n')
  end % for Ms_s in Ms_span
  calcTime = toc ;
  ColorSet = varycolor(length(Ms_span)) ;
else
  fprintf('This is it!\n')
  fprintf('(something went wrong in the outer "if"-loop)\n')
end

%% Ploting routines
h = figure(modelId) ;          % Figure number from model number
clf
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$'} ;

if cType == 4
  set(gca, 'ColorOrder', ColorSet);
  plot(d_span,Jtune) ;
  xlabel('Process delay perturbation, $\dtheta$', ...
         'interpreter','latex')
  ylabel('Performance, $J$', ...
         'interpreter','latex')
  legendCell = cellstr(num2str(Ms_span', 'Ms = %.2f')) ;
  legend(legendCell, 'Location','NorthEast')
  title(titleName{modelId},'interpreter','latex')
  axis([-1 1 0 50])
  h2 = figure(modelId+numel(modelName)) ;
  set(gca,'ColorOrder',ColorSet);
  plot(d_span,Msc)
  legend(legendCell, 'Location','NorthEast')
  xlabel('Process delay perturbation, $\dtheta$', ...
         'interpreter','latex')
  ylabel('Robustness, $\Ms$', ...
         'interpreter','latex')
  title(titleName{modelId},'interpreter','latex')
  axis([-1 1 0 50])
else
  plot(Msc,Jtune) ;
  xlabel('Robustness, $\Ms$', ...
         'interpreter','latex')
  ylabel('Performance, $J$', ...
         'interpreter','latex')
  title(titleName{modelId},'interpreter','latex')
end

%% Storing results fprintf('%s\n', datestr(clock, 'dd.mm.yy HH:MM'))
res.case = num2str(modelName{modelId});
res.lastUpdate = datestr(clock, 'dd.mm.yy HH:MM') ;
res.G          = G ;
res.cTypes     = cTypes{cType} ;
res.ctrlr      = ctrlrs{ctrlr} ;
res.cParm      = cParm{cNum} ;
res.solver     = solver ;
res.IAEweights = parm.IAEweights ;

sub = struct ;
if cType == 4
  sub = setfield(sub, 'd_span', d_span') ;
  sub = setfield(sub, 'Ms_span', Ms_span') ;
  sub = setfield(sub, 'Msc', Msc) ;
  sub = setfield(sub, 'Mtc', Mtc) ;
  sub = setfield(sub, 'J', Jtune) ;
  sub = setfield(sub, 'calcTime', calcTime) ;
  if ctrlr == 3
    loc = strcat('results/tex/smithDist',       ...
          num2str(cTypes{distOpt}), '/',...
          num2str(ctrlrs{ctrlr}), '/', ...
          num2str(modelName{modelId}),'.tex') ;
  else
    loc = strcat('results/tex/smithDist',...
          num2str(cTypes{distOpt}), '/',  ...
          num2str(ctrlrs{ctrlr}), '/',   ...
          num2str(cParm{cNum}),   '/',   ...
          num2str(modelName{modelId}),'.tex') ;
  end % if ctrlr == 3
else
  sub = setfield(sub, 'Ms_span', Ms_span') ;
  sub = setfield(sub, 'Msc', Msc) ;
  sub = setfield(sub, 'Mt', Mtc) ;
  sub = setfield(sub, 'minTune', minTune) ;
  sub = setfield(sub, 'J', Jtune) ;
  sub = setfield(sub, 'flags', flags) ;
  sub = setfield(sub, 'calcTime', calcTime) ;
  sub = setfield(sub, 'IAEtune', IAEtune) ;
  sub = setfield(sub, 'roots', ctrlRoot) ;
  try
    myRef = struct ;
    myRef = setfield(myRef,'tau_cRef', tau_cRef) ;
    myRef = setfield(myRef,'MscRef', MscRef) ;
    myRef = setfield(myRef,'MtcRef', MtcRef) ;
    myRef = setfield(myRef,'JcRef', JcRef) ;
    myRef = setfield(myRef,'RefTune', RefTune) ;
    myRef = setfield(myRef,'iaeRef', iaeRef) ;
    sub = setfield(sub,'Reference',myRef);
  catch
  end
  if cType == 1 || cType == 3
    sub = setfield(sub, 'initial', initial) ;
  end
end

field = strcat(upper(cTypes{cType}),upper(ctrlrs{ctrlr})) ;
res = setfield(res,field,sub) ;

% Saving struct and plot
if cType==2||(cType==3&&ctrlr==3)||(cType==4&&ctrlr==3)|| ...
  (cType==5&&ctrlr==3)
  sPath = strcat( num2str(cTypes{cType}), '/', ...
                  num2str(ctrlrs{ctrlr}), '/', ...
                  num2str(modelName{modelId})) ;
  save([mDir, '/results/',sPath,'.mat'], 'res')
  saveas(h,[mDir, '/results/figures/',sPath,'.fig'])
  loc = strcat('results/tex/',sPath,'.tex') ;
  matlab2tikz(loc, 'width',  '\textwidth',  ...
                   'height', '\textheight', ...
                   'showInfo', false) ;
elseif cType == 5
  sPath = strcat( num2str(cTypes{cType}),  '/', ...
                  num2str(cTypes{distOpt}),'/', ...
                  num2str(ctrlrs{ctrlr}),  '/', ...
                  num2str(cParm{cNum}),    '/', ...
                  num2str(modelName{modelId})) ;
  save([mDir, '/results/',sPath,'.mat'], 'res')
  loc = strcat('results/tex/',sPath,'.tex') ;
  saveas(h,[mDir, '/results/figures/',sPath,'.fig']) ;
  matlab2tikz(loc, 'showInfo', false) ;
elseif cType == 4
  sPath = strcat( num2str(cTypes{cType}),  '/', ...
                  num2str(cTypes{distOpt}),'/', ...
                  num2str(ctrlrs{ctrlr}),  '/', ...
                  num2str(cParm{cNum}),    '/', ...
                  num2str(modelName{modelId})) ;
  save([mDir, '/results/',sPath,'.mat'], 'res')
  loc = strcat('results/',sPath,'.tex') ;
  set(0,'currentfigure',h) ;
  saveas(h,[mDir, '/results/',sPath,'.fig']) ;
  matlab2tikz(loc, 'width',  '\textwidth',  ...
                   'height', '\textheight', ...
                   'showInfo', false) ;
  set(0,'currentfigure',h2) ;
  saveas(h2,[mDir, '/results/',sPath,'_d_Ms.fig']) ;
  loc = strcat('results/',sPath,'_d_Ms.tex') ;
  matlab2tikz(loc,'width', '\textwidth',  ...
                  'height', '\textheight',...
                  'showInfo', false) ;
else
  sPath = strcat( num2str(cTypes{cType}), '/', ...
                  num2str(ctrlrs{ctrlr}), '/', ...
                  num2str(cParm{cNum}),   '/', ...
                  num2str(modelName{modelId})) ;
  save([mDir, '/results/',sPath,'.mat'], 'res')
  saveas(h,[mDir, '/results/figures/',sPath,'.fig']) ;
  loc = strcat('results/tex/',sPath,'.tex') ;
  matlab2tikz(loc, 'width',  '\textwidth',  ...
                   'height', '\textheight', ...
                   'showInfo', false) ;
  try
      i = find(Ms_span==1.59);
      eOut = [minTune(i,:), Jtune(i),IAEtune(i,:)] ;
      eOut = round(eOut*100)/100 ;          % Rounding numbers
      dlmwrite(sPath,eOut)
  catch
      fprintf('CSV file not saved. %s \n', lasterr)
  end
end

fprintf('------------------------------------------------\n')
fprintf('Saved in: \t%s\n', strcat(sPath,'.mat')) ;
fprintf('------------------------------------------------\n')

fprintf('Calculation finished after %0.2f minutes', ...
         calcTime/60)
fprintf(' (%0.2f seconds). \n', calcTime)

end % Ending modelId for-loop

runTime = cputime - runTime;
fprintf('Total computation time: %2.2f minutes', runTime/60)
fprintf(' (%2.2f seconds). \n', runTime)
fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n\n')

try
  matlabpool close
catch
end
diary off
% profile viewer
restoredefaultpath 
