% 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 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 = {'case10', 'case11', 'case12', 'case13', 'case14'} ;
cTypes  = {'po', 'simc', 'smith'} ;
ctrlrs  = {'pid', 'pi', 'p'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;

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

tic ;
for modelId = 2:5 %length(modelName)
    i = 1 ;                                            % Counter
    J = zeros(120,24) ;
    Ms = J ;
    Mt = J ;
    captions = {};
    % Load PO plots:
    cType = 1;
    cNumVec = [1 4] ;      % Just Chriss and Cascade controllers
    for ctrlr = 1:2             % Skipping P-controllers for now
        if ctrlr == 2                      % Cascade only for PI
            cNumVec = [4] ;
        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/fo/', ...
                modelName{modelId},cTypes{cType},ctrlrs{ctrlr});
                % dlmwrite([dlmPath,'.csv'],tuneOut,...
                        'precision','%.2f') ;
                fprintf('Success! \n')
            end % success
            i = i + 1 ;
        end % cNum
    end % ctrlr
    % Loading simc
    simcRef = {} ;
    cType = 2 ;
    ctrlr = 2 ;
    parm.cNum = 2 ; parm.cType = cType; parm.ctrlr = ctrlr ;
    eOut = zeros(4, 9-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] ;
        fprintf('Success! \n')
    end
    i = i + 1 ;

    % 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')
    % ColorSet = varycolor(length(this.Ms_span)) ;
    % set(0,'DefaultAxesColorOrder',jet)
    % set(0,'DefaultColorMap',jet)
%     % J(Ms) plot
    figure(modelId)
    clf
    % set(gcf, 'ColorMap', ColorSet);
    try
        hold on
        box on 
        plot(Ms,J,'LineWidth',1)
        legend(captions, 'Location','EastOutside')
        a = simcRef{2} ;
        plot(a.MscRef,a.JcRef,'ok')
        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/fo/', ...
                    modelName{modelId}) ;
    save([sPath,'.mat'])
    saveas(gcf,[sPath,'.fig']) ;
    loc = strcat(sPath,'.tex') ;
    matlab2tikz(loc,'showInfo', false) ;
    % dlmwrite([sPath,'.csv'], eOut,'precision','%.2f')

    fprintf('.....')
    % Mt(Ms) plot
    MsCaptions=captions;
    figure(modelId+length(modelName))
    clf
    hold on
    box on
    plot(Ms, Mt, 'LineWidth', 1)
    plot([0 5],[0 5], '--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/fo/', ...
                    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