% Purpose:  Calculation and graphical representation of SIMC 
%           tuning performance
% Author:   Axel Lodemel Holene
% E-mail:   axel.holene@gmail.com
% Date:     April 2013
% About:    ctrl = [Kc I D]

%% Initialising
clear all
close all
clc

% Adding commonFiles to search path
mDir   = fileparts(pwd) ;
shared = fullfile(mDir,'commonFiles') ;
m2tkz  = fullfile(mDir,'matlab2tikz/src') ;
varclr = fullfile(mDir,'varycolor') ;
addpath(shared)
addpath(m2tkz)
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', 'p'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;

fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n')
fprintf('PLOTTING TUNING \n') ;
fprintf('%s\n', datestr(clock, 'dd.mm.yy HH:MM'))
fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n')

tic
for modelId = 14:14%length(modelName)
    POtune = zeros(100,3,2) ;
    POms = zeros(100,2) ;
    POcaptions = {};
    % Load PO tuning:
    cType = 1;                                              % PO
    i = 1 ;                                            % Counter
    cNum = 4 ;                        % Just Cascade controllers
    for ctrlr = 1:2             % Skipping P-controllers for now
        success = true ;
        lPath = strcat(mDir,'/results/', ...
                cTypes{cType}, '/', ...
                ctrlrs{ctrlr}, '/', ...
                cParm{cNum},   '/', ...
                modelName{modelId}) ;
        fprintf('Attempting to load %s\n', lPath)
        try
            load([lPath,'.mat'],'res') ;
        catch
            fprintf('Loading failed. \n')
            fprintf('Error: %s\n', lasterr)
            success = false ;
        end
        if success
            this = getfield(res, ...
                        strcat(upper(cTypes{cType}), ...
                        upper(ctrlrs{ctrlr}))) ;
            POcaptions{i} = strcat(cTypes{cType},':', ...
                            ctrlrs{ctrlr},':', ...
                            cParm{cNum}) ;
            [m n] = size(this.minTune) ;
            POtune(1:m,1:n,ctrlr) = this.minTune ;
            POms(1:length(this.Msc),ctrlr) = this.Msc ;
            fprintf('Success! \n')
        end % success
        i = i + 1 ;
    end % ctrlr
    SMITHtune = zeros(100,3,2) ;
    SMITHms = zeros(100,2) ;
    SMITHcaptions = {};
    % Load Smith tuning:
    cType = 3;                                           % Smith
    i = 1 ;                                            % Counter
    cNum = 4 ;                        % Just Cascade controllers
    for ctrlr = 1:2             % Skipping P-controllers for now
        success = true ;
        lPath = strcat(mDir,'/results/', ...
                cTypes{cType}, '/', ...
                ctrlrs{ctrlr}, '/', ...
                cParm{cNum},   '/', ...
                modelName{modelId}) ;
        fprintf('Attempting to load %s\n', lPath)
        try
            load([lPath,'.mat'],'res') ;
        catch
            fprintf('Loading failed. \n')
            fprintf('Error: %s\n', lasterr)
            success = false ;
        end
        if success
            this = getfield(res, ...
                        strcat(upper(cTypes{cType}), ...
                        upper(ctrlrs{ctrlr}))) ;
            SMITHcaptions{i} = strcat(cTypes{cType},':', ...
                            ctrlrs{ctrlr},':', ...
                            cParm{cNum}) ;
            [m n] = size(this.minTune) ;
            SMITHtune(1:m,1:n,ctrlr) = this.minTune;
            SMITHms(1:length(this.Msc),ctrlr) = this.Msc ;
            fprintf('Success! \n')
        end % success
        i = i + 1 ;
    end % ctrlr

    % Removing zeros
    % J(:,all(~any(J),1)) = [];
    % Ms(:,all(~any(Ms),1)) = []; 
    % Mt(:,all(~any(Mt),1)) = []; 
    POtune(POtune==0) = NaN ;
    POms(POms == 0)   = NaN ;
    SMITHtune(SMITHtune == 0) = NaN ;
    SMITHms(SMITHms==0) = NaN ;
    POtune(POtune>100) = 100 ;
    POtune(POtune<0) = 0 ;
    SMITHtune(SMITHtune>100) = 100 ;
    SMITHtune(SMITHtune<0) = 0 ;

    % Plotting routine
    fprintf('Plotting')
    % Kc(Ms)
    figure(modelId)
    clf
    hold on; 
    box on
    plot(POms(:,1),POtune(:,:,1),'--') ;
    plot(SMITHms(:,1),SMITHtune(:,:,1)) ;
    % plot(POms,POtune(:,1,1),'b')
    % plot(POms,POtune(:,1,2),'b--')
    % plot(SMITHms,SMITHtune(:,1,1),'r')
    % plot(SMITHms,SMITHtune(:,1,2),'r--')
    hold off
    % legend([POcaptions SMITHcaptions],'Location','NorthEast')
    legend('po:pid', 'Smith:pid')
    xlabel('Robustness, $\Ms$','interpreter','latex')
    ylabel('Controller $\Kc,\I,\D$','interpreter','latex')
    title(titleName{modelId}, 'interpreter','latex')
    sPath = strcat(mDir,'/results/merged/tuning/', ...
                    modelName{modelId}) ;
    save([sPath,'.mat'])
    saveas(gcf,[sPath,'_pid.fig']) ;
    matlab2tikz([sPath,'_pid.tex'],'showInfo', false) ;

    fprintf('.....\n')
    % tau_i(Ms)
    figure(modelId+length(modelName))
    clf
    hold on
    box on
    plot(POms(:,2),POtune(:,1,2),'b--') ;
    plot(POms(:,2),POtune(:,2,2),'r--') ;
    plot(SMITHms(:,2),SMITHtune(:,1,2),'b') ;
    plot(SMITHms(:,2),SMITHtune(:,2,2),'r') ;
    % plot(POms,POtune(:,2,1),'b')
    % plot(POms,POtune(:,2,2),'b--')
    % plot(SMITHms,SMITHtune(:,2,1),'r')
    % plot(SMITHms,SMITHtune(:,2,2),'r--')
    hold off
    xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    ylabel('Controller $\Kc,\I,\D$','interpreter','latex')
    title(titleName{modelId},'interpreter','latex')
    % legend([POcaptions SMITHcaptions],'Location','NorthEast')
    legend('po:pi', 'Smith:pi')
    saveas(gcf,[sPath,'_pi.fig']) ;
    matlab2tikz([sPath,'_pi.tex'],'showInfo', false) ;

    % fprintf('.....')
    % % tau_d(Ms)
    % figure(modelId+2*length(modelName))
    % clf
    % hold on
    % box on
    % plot(POms,POtune(:,3,1),'b')
    % plot(SMITHms,SMITHtune(:,3,1),'r')
    % hold off
    % xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    % ylabel('Integral time, $\D$', 'interpreter', 'latex')
    % title(titleName{modelId},'interpreter','latex')
    % legend([POcaptions{1}, SMITHcaptions{1}],'Location','NorthEast')
    % saveas(gcf,[sPath,'_D.fig']) ;
    % matlab2tikz([sPath,'_D.tex'],'showInfo', false) ;
    fprintf('Finished!\n')
end % modelId
fprintf('Finished in %.2f seconds! \n', toc)
restoredefaultpath