% 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 SECOND ORDER \n') ;
fprintf('%s\n', datestr(clock, 'dd.mm.yy HH:MM'))
fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n')

tic ;
for modelId = 1:9 %length(modelName)
    i = 1 ;                                            % Counter
    J = zeros(120,24) ;
    Ms = J ;
    Mt = J ;
    captions = {};
    % Load PO plots:
    cType = 1;
    for ctrlr = 1:2             % Skipping P-controllers for now
        if ctrlr == 2
            cNumVec = [4] ;                % Just cascade for PI
        else 
            cNumVec = [1 4] ;       % Chriss and cascade for PID
        end
        for cNum = cNumVec
            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}))) ;
                captions{i} = strcat(cTypes{cType},':', ...
                                ctrlrs{ctrlr},':', ...
                                cParm{cNum}) ;
                J(1:length(this.J),i) = this.J ;
                Ms(1:length(this.Msc),i) = this.Msc ;
                Mt(1:length(this.Mt),i) = this.Mt ;
                idx = find(this.Ms_span==1.6) ;
                tuneOut = [this.minTune(idx,:), ...
                           this.J(idx), ...
                           this.Msc(idx), ...
                           this.IAEtune(idx,:)] ;
                dlmPath = strcat(mDir,'/results/merged/so/', ...
                modelName{modelId},cTypes{cType},ctrlrs{ctrlr});
                % dlmwrite([dlmPath,'.csv'],tuneOut,...
                        % 'precision','%.2f') ;
                % The cascade tuning values are the last to be
                % found, so the Chriss values are overwritten if
                % they are found. 
                fprintf('Success! \n')
            end % success
            i = i + 1 ;
        end % cNum
    end % ctrlr
    % Loading simc
    simcRef = {} ;
    cType = 2 ;
    for ctrlr = 1:2 ;
        eOut = zeros(4, 9-ctrlr) ;
        parm.cNum = 2 ; parm.cType = cType; parm.ctrlr = ctrlr ;
        success = true ;
        lPath = strcat(mDir,'/results/', ...
                cTypes{cType}, '/', ...
                ctrlrs{ctrlr}, '/', ...
                modelName{modelId}) ;
        fprintf('Loading %s\n', lPath)
        try
            load([lPath,'.mat'],'res') ;
        catch
            fprintf('Failed: %s\n', lasterr)
            success = false ;
        end
        if success
            this = getfield(res, ...
                    strcat(upper(cTypes{cType}), ...
                    upper(ctrlrs{ctrlr}))) ;
            captions{i} = strcat(cTypes{cType},':', ...
                            ctrlrs{ctrlr}) ;
            J(1:length(this.J),i) = this.J ;
            Ms(1:length(this.Msc),i) = this.Msc ;
            Mt(1:length(this.Mt),i) = this.Mt ;
            simcRef{ctrlr} = this.Reference ;
            eOut = [this.Reference.tau_cRef'...
                    this.Reference.RefTune  ...
                    this.Reference.JcRef'   ...
                    this.Reference.MscRef'  ...
                    this.Reference.iaeRef] ;
            sPath = strcat(mDir,'/results/merged/so/', ...
                    modelName{modelId},ctrlrs{ctrlr}) ;
            % dlmwrite([sPath,'.csv'], eOut, 'precision', '%.2f');
            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)) = []; 
    J(J==0)     = NaN ;
    Ms(Ms == 0) = NaN ;
    Mt(Mt == 0) = NaN ;
    captions = captions(~cellfun('isempty',captions)) ;
    % ColorSet = varycolor(length(captions)) ;

% Plotting routine
    fprintf('Plotting')
%     % J(Ms) plot
    figure(modelId)
    clf
    try
        set(gca, 'ColorOrder', ColorSet);
    catch
    end
    try
        hold on; 
        box on
        plot(Ms,J,'LineWidth',1)
        legend(captions, 'Location','EastOutside')
        for n = 1:2
            a = simcRef{n} ;
            plot(a.MscRef,a.JcRef,'ok')
        end
        hold off
    catch
        plot(Ms, J, 'LineWidth', 1);
    end
    xlabel('Robustness, $\Ms$', ...
           'interpreter','latex')
    ylabel('Performance, $J$', ...
            'interpreter','latex')
    title(titleName{modelId}, ...
            'interpreter','latex')
    sPath = strcat(mDir,'/results/merged/so/', ...
                    modelName{modelId}) ;
    save([sPath,'.mat'])
    saveas(gcf,[sPath,'.fig']) ;
    loc = strcat(sPath,'.tex') ;
    matlab2tikz(loc,'showInfo', false) ;

    fprintf('.....')
    % Mt(Ms) plot
    figure(modelId+length(modelName))
    clf
    try
        set(gca, 'ColorOrder', ColorSet);
    catch
    end
    hold on
    box on
    plot(Ms, Mt, 'LineWidth', 1)
    plot([0 3],[0 3], '--k') ;
    hold off
    xlabel('Robustness, $\Ms$', 'interpreter', 'latex')
    ylabel('Complementary sensitivity, $\Mt$', 'interpreter', 'latex')
    title(titleName{modelId},'interpreter','latex')
    legend(captions, 'Location','EastOutside')

    sPath = strcat(mDir,'/results/merged/so/', ...
                    modelName{modelId}) ;
    save([sPath,'MsMt.mat'])
    saveas(gcf,[sPath,'MsMt.fig']) ;
    loc = strcat(sPath,'MsMt.tex') ;
    matlab2tikz(loc,'showInfo', false) ;
    fprintf('Finished!\n')
end % modelId
fprintf('Finished in %.2f seconds! \n', toc)
restoredefaultpath