% 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', 'Case 2', 'Case 3', 'Case 4',    ...
             'Case 5', 'Case 6', 'Case 7', 'Case 8',    ...
             'Case 9', 'Case 10', 'Case 11', 'Case 12', ...
             'Case 13'} ;
modelName = {'case1', 'case2', 'case3', 'case4', 'case5',  ...
             'case6', 'case7', 'case8', 'case9', 'case10', ...
             'case11', 'case12', 'case13'} ;

cTypes  = {'po', 'simc', 'smith'} ;
ctrlrs  = {'pid', 'pi'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;
cType = 8 ; % smith:bookrules

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:bookrules plot production')

for modelId = 1:13
  %% 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 ;
  if modelId < 10
    G = halfrule(model(modelId)) ;
  else
    G = model(modelId) ;
  end
  parm.G = G ;
  parm.cType = cType ;
  parm.ctrlr = 1 ;                             % Option not used
  parm.cNum = 1 ;                              % Option not used
  parm.dFilter = 1 ;                           % Option not used

  % Calculations
  F_span = [0:0.01:1] ;
  dMax = 0.9 ;
  parm.dMax = 0.9 ;         % Maximum time-delay modelling error
  i = 1 ;
  Ms = zeros(length(F_span),1) ;
  J = Ms ;
  fprintf('Model = %s.\n', titleName{modelId})
  fprintf('J(Ms) in progress: ')
  for F = F_span
    parm.F = F ;
    C = controller(parm) ;
    J(i) = distCostFun(C,G,parm) ;
    Ms(i) = distMs(C,G) ;
    i = i + 1 ;
    fprintf('*') 
  end
  MsJStr = struct ;
  MsJStr = setfield(MsJStr,'F_span',F_span) ;
  MsJStr = setfield(MsJStr,'J',J) ;
  MsJStr = setfield(MsJStr,'Ms',Ms) ;
  fprintf('\t Finished. \n')
  F_ref = [0 1/4 1/2 3/4 1] ;
  % F_ref = F_refV*(1.7*dMax*G.theta - G.tau1) + G.tau1 ;
  Jref = zeros(1,length(F_ref)) ;
  MsRef = Jref ;
  i = 1 ;
  fprintf('Reference points in progress: ')
  for F = F_ref
    parm.F = F ;
    C = controller(parm) ;
    Jref(i) = distCostFun(C,G,parm) ;
    MsRef(i) = distMs(C,G) ;
    i = i + 1;
    fprintf('*')
  end
  ref = struct ;
  ref = setfield(ref,'F_ref',F_ref) ;
  ref = setfield(ref,'MsRef',MsRef) ;
  ref = setfield(ref,'Jref',Jref) ;
  fprintf('\t Finished. \n')
  %
  fprintf('Disturbance performance in progress. \n')
  d_span  = [-dMax:0.01:dMax] ;
  Ms  = zeros(length(d_span),length(F_ref)) ;% Ms values
  Mt  = zeros(length(d_span),length(F_ref)) ;% Mt values
  J   = zeros(length(d_span),length(F_ref)) ;% Cost init
  Gm  = zeros(length(d_span),length(F_ref)) ;
  Pm  = Gm ; 
  wcg = Gm ;
  wcp = Gm ;
  thetaMax = Gm ;
  j = 1 ;
  for F = F_ref
    fprintf('  F = %.2f: ', F_ref(j))
    i = 1 ;
    parm.F = F ;
    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) ;
      thetaMax(i,j) = Pm(i,j)*pi/180/wcp(i,j) ;
      J(i,j) = distCostFun(C, Gdist, parm) ;
      Ms(i,j)   = distMs(C, Gdist) ;
      Mt(i,j)   = distMt(C, Gdist) ;
      i = i + 1 ;
      fprintf('*')
    end % d in d_span
    fprintf('\n')
    j = j + 1 ;
  end % F in F_span
  % do some printing and plotting
  
  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:brules:',modelName{modelId}) ;
  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(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','NorthEast')
  % Saving
  sPath = strcat('/results/smith/brules/', ...
          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(2*length(modelName)+modelId) ;
  clf
  legendCell = cellstr(num2str(F_ref', 'F = %.2f')) ;
  plot(dist.d_span,dist.J)
  xlabel('Time-delay, $\theta$', 'interpreter', 'latex')
  ylabel('Cost, $J$', 'interpreter', 'latex')
  legend(legendCell,'Location','NorthEast')
  saveas(gcf,[mDir, sPath,'Jd.fig'])
  loc = strcat(mDir, sPath, 'Jd.tex');
  matlab2tikz(loc,'showInfo', false) ;
  %
  figure(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','NorthEast')
  saveas(gcf,[mDir, sPath,'thetaMs.fig'])
  loc = strcat(mDir, sPath, 'thetaMs.tex');
  matlab2tikz(loc,'showInfo', false) ;
end % modelId 




