% Script for creating plots comparing tunings with the PO curve
% written by: Chriss Grimholt, 12 jan. 2012

modelId = 4;

modelName={0,1,8,'Inf'};

try
    load(['res',num2str(modelName{modelId}),'.mat'])
catch me
    ME = MException(me.identifier,'could not open file, check correct modelId!');
    throw(ME)
end 

minSp = res.minSp.iae; %loading iaeWeights;
minD  = res.minD.iae;
minWeights = [minSp;minD];

manWeights = [.5 .5]; %setting manWeights

gp = model(modelId); %gets the model

tcSpace = 0:0.01:5; %closed loop time constant search rang for simc

simcJ  = []; %setting up result matrices
simcMs = [];
i = 1; %setting iteration counter

for tc = tcSpace
    [simcTuning, simcGc] = simc(gp,tc); %finding the simc tuning
    jTemp = costFun(gp,simcGc,minWeights, manWeights); %finding the cost
    msTemp = ms(gp,simcGc); %finding simc ms value
    
    simcJ(i)  = jTemp; %storing results
    simcMs(i) = msTemp;
    i = i+1;
end

% finding refrence dots;
simcRefJ  = [];
simcRefMs = [];

for tc = [.5 1 1.5]
    [simcTuning, simcGc] = simc(gp,tc); %finding the simc tuning
    jTemp = costFun(gp,simcGc,minWeights, manWeights); %finding the cost
    msTemp = ms(gp,simcGc); %finding simc ms value
 
    simcRefJ  = [simcRefJ, jTemp];
    simcRefMs = [simcRefMs, msTemp];
end

 simcImpJ = [];
 simcImpMs = [];
 i=1;
for tc = tcSpace
    [simcTuning, simcGc] = simc2(gp,tc); %finding the simc tuning
    jTemp = costFun(gp,simcGc,minWeights, manWeights); %finding the cost
    msTemp = ms(gp,simcGc); %finding simc ms value
    
    simcImpJ(i)  = jTemp; %storing results
    simcImpMs(i) = msTemp;
    i = i+1;
end

% finding refrence dots;
simcImpRefJ  = [];
simcImpRefMs = [];

for tc = [.5 1 1.5]
    [simcTuning, simcGc] = simc2(gp,tc); %finding the simc tuning
    jTemp = costFun(gp,simcGc,minWeights, manWeights); %finding the cost
    msTemp = ms(gp,simcGc); %finding simc ms value
 
    simcImpRefJ  = [simcImpRefJ, jTemp];
    simcImpRefMs = [simcImpRefMs, msTemp];
end


%% Additional tunings
% ZN-PI
% I 
% znGc = controller(0.71,0.71/3.33);
% znJ = costFun(gp,znGc,minWeights, manWeights); %finding the cost
% znMs = ms(gp,znGc); %finding simc ms value
% 
% %Astrom/Schei
% asGc = controller(0.404,0.404/7);
% asJ = costFun(gp,asGc,minWeights, manWeights); %finding the cost
% asMs = ms(gp,asGc); %finding simc ms value 
% 
% %Tyreus-Leyben
% 
% tlGc = controller(0.49,0.49/7.32);
% tlJ = costFun(gp,tlGc,minWeights, manWeights); %finding the cost
% tlMs = ms(gp,tlGc); %finding simc ms value 
%%



figure(1)
clf
% 
% hold on
% y = [[0.8 0.8]; [2.4 2.4]-[0.8 0.8]]'; 
% ha = area([1.9 3], y);
% hold off
% 
% 
%  set(ha(1), 'FaceColor', 'none') % this makes the bottom area invisible
%  set(ha, 'LineStyle', 'none')
%  set(ha(2), 'FaceColor', [.8 .8 .8])
%jbfill([1.9 3],[0.8 0.8],[2.4 2.4])


[minCostFun, indexMinTuning] = min(res.minTuning.costFun);



%greayBox = shadedplot([1.9 3],[0.8 0.8],[2.4 2.4]);
%grayBox = annotation('rectangle',[.2 .2 .5 .5],'FaceColor',[0.5 0.5 0.5],'LineStyle','non')
hold on
h=plot(res.minTuning.ms,res.minTuning.costFun,...%);%,...
    simcMs,simcJ,...    
    simcImpMs,simcImpJ,'--');
    %znMs,znJ,'x',...
    %asMs,asJ,'d',...
    %tlMs,tlJ,'s',...
    %    );
    set(h,'LineWidth',1.5)
        set(h(3),'color',[.75 0 .75])
h = plot(simcRefMs,simcRefJ,'o',simcImpRefMs,simcImpRefJ,'o');
set(h(1),'color',[0 .5 0]);
set(h(2),'color',[.75 0 .75]);
hold off

axis([1.3 3 0.8 2.4])
textBox={'$\tau_c=0.5\theta$'
         '$\tau_c=\theta$'
         '$\tau_c=1.5\theta$'};
     
xpos = get(h(1),'Xdata');   
ypos = get(h(1),'Ydata');    

hold on
for i = 1:length(xpos);
[figx figy] = dsxy2figxy(gca, xpos(i), ypos(i)); 
if modelId == 1
    figy=figy+0.02;
end
box = annotation('textbox',[figx+.02, figy, .07, .05],'string',textBox{i},'interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
end
hold off

textString = {'$g=$~exp$(- s)$'
              '$g= \frac{1}{s+1} $~exp$(-  s)$'
              '$g= \frac{1}{8s+1} $~exp$(-  s)$'
              '$g= \frac{1}{s} $~exp$(- s)$'};

[figx figy] = dsxy2figxy(gca, res.minTuning.ms(indexMinTuning), [.8 2.4]);           
  
if modelId == 1          
curve1 = annotation('textbox',[0.70, 0.12, .07, .05],'string','pareto optimal','interpreter','latex','fontsize',14,'color',[0 0 1],'FitBoxToText','on','LineStyle','non');    
curve2 = annotation('textbox',[0.6, 0.58, .07, .05],'string','SIMC (original, $\tau_I=0$)','interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
%curve3 = annotation('textbox',[0.8, 0.6, .07, .05],'string','improved SIMC','interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
plotTitle = annotation('textbox',[0.65, 0.8, .07, .05],'string',textString{modelId},'interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
curve3 = annotation('textbox',[0.55, 0.3, .07, .05],'string','SIMC (improved, $\tau_I=\theta/3$)','interpreter','latex','fontsize',14,'color',[1 0 0],'FitBoxToText','on','LineStyle','non');    
%text1 = annotation('textbox',[figx+.05, .5, .07, .05],'string','uninteresting','interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
%line1 = annotation('line', [figx figx],figy,'LineWidth',1.5,'LineStyle','--');
%arrow1 = annotation('arrow', [figx figx+.3],[.5 .5],'LineWidth',1.5,'LineStyle','-');
elseif modelId == 2
curve1 = annotation('textbox',[0.15, 0.11, .07, .05],'string','pareto optimal','interpreter','latex','fontsize',14,'color',[0 0 1],'FitBoxToText','on','LineStyle','non');    
curve2 = annotation('textbox',[0.68, 0.27, .07, .05],'string','SIMC (original)','interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
plotTitle = annotation('textbox',[0.65, 0.8, .07, .05],'string',textString{modelId},'interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
curve3 = annotation('textbox',[0.17, 0.55, .07, .05],'string','SIMC (improved)','interpreter','latex','fontsize',14,'color',[1 0 0],'FitBoxToText','on','LineStyle','non');    
%plotTitle = annotation('textbox',[0.55, 0.8, .07, .05],'string',textString{modelId},'interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
%text1 = annotation('textbox',[figx+.02, .5, .07, .05],'string','uninteresting','interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
%line1 = annotation('line', [figx figx],figy,'LineWidth',1.5,'LineStyle','--');
%arrow1 = annotation('arrow', [figx figx+.25],[.5 .5],'LineWidth',1.5,'LineStyle','-');
elseif modelId == 3
curve1 = annotation('textbox',[0.15, 0.11, .07, .05],'string','pareto optimal','interpreter','latex','fontsize',14,'color',[0 0 1],'FitBoxToText','on','LineStyle','non');    
curve2 = annotation('textbox',[0.18, 0.75, .07, .05],'string','SIMC (original)','interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
plotTitle = annotation('textbox',[0.65, 0.8, .07, .05],'string',textString{modelId},'interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
curve3 = annotation('textbox',[0.2, 0.70, .07, .05],'string','SIMC (improved)','interpreter','latex','fontsize',14,'color',[1 0 0],'FitBoxToText','on','LineStyle','non');    
elseif modelId == 4
curve1 = annotation('textbox',[0.2, 0.2, .07, .05],'string','pareto optimal','interpreter','latex','fontsize',14,'color',[0 0 1],'FitBoxToText','on','LineStyle','non');    
curve2 = annotation('textbox',[0.23, 0.78, .07, .05],'string','SIMC ($c=4$)','interpreter','latex','fontsize',14,'color',[0 0.5 0],'FitBoxToText','on','LineStyle','non');    
plotTitle = annotation('textbox',[0.65, 0.8, .07, .05],'string',textString{modelId},'interpreter','latex','fontsize',14,'color','black','FitBoxToText','on','LineStyle','non');    
curve3 = annotation('textbox',[0.25, 0.73, .07, .05],'string','Modified SIMC ($c=2$)','interpreter','latex','fontsize',14,'color',[.75 0 .75],'FitBoxToText','on','LineStyle','non');    
end


set(h,'LineWidth',1.5)

xlab=xlabel('Robustness, $M_s$');%,'interpreter','latex');
ylab=ylabel('Performance, $J(c)$');%,'interpreter','latex');


set(xlab,'interpreter','latex','fontsize',14)
set(ylab,'interpreter','latex','fontsize',14)

set(gca, 'Layer', 'top','Box','on')




%saveas(h(1) , ['simcImpRes',num2str(modelName{modelId}),'.eps'],'psc2')