% Purpose:  Creating the process transfer function G (struct)
% Author:   Axel Lodemel Holene
% E-mail:   axel.holene@gmail.com
% Date:     April 2013
% About:    

%% Initialising
clear all
% close all
% clc

% Adding commonFiles to search path
mDir   = fileparts(pwd) ;
shared = fullfile(fileparts(pwd),'commonFiles'); addpath(shared)

cd(mDir)
warning('off','MATLAB:xlswrite:NoCOMServer');
warning('off','Control:combination:SingularRightDivision');
warning('off','Control:combination:SingularRightDivision') ;
warning('off','Simulink:blocks:TDelayBufferTooSmall') ;
warning('off','MATLAB:singularMatrix') ;
warning('off', 'Control:analysis:ZeroApproximateDelay1') ;
warning('off', 'Control:transformation:StateSpaceScaling') ;
warning('off', 'Control:transformation:C2dApproximate') ;
warning('off', 'Simulink:blocks:TDelayBufferTooSmall') ;
warning('off', 'Control:analysis:PoleApproximateDelay1') ;

% Find and/or print (save) values?
FIND = false ; 
PRINT = ~FIND ; 

runTime = cputime ;               % Start total computation time

%% Simulation, process and controller  parameters
modelName = {'case1', 'case2', 'case3', 'case4', 'case5', ...
             'case6', 'case7', 'case8', 'case9', 'case10', ...
             'case11', 'case12', 'case13','case14'} ;
cTypes  = {'po', 'simc', 'smith', 'smithDist'} ;
ctrlrs  = {'pid', 'pi'} ;
cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;
simTime = 50 ;
dFilter = 1 ;
alph = 0.01 ;
runParallel = 0 ;
simCost = 1 ;
dispErrors = 0 ;

sNum_s  = 2 ; 
sNum    = 2 ;                                    % Solver option
solvers = {'active-set', 'sqp', 'fminsearch'} ;   % Solvers
sFlags  = {5,2,1} ;             % Solver flag limits for success
solver  = solvers{sNum} ;                      % Choosing solver
sFlag   = sFlags{sNum}  ;                  % Choosing flag limit
MsRes   = 0.025 ;                        % Ms PO plot resolution
dRes    = 0.025 ;                  % Smith error plot resolution

parm.dispErrors = dispErrors ;
parm.distOpt = 1 ;
if runParallel
    opt=optimoptions('fmincon','algorithm',solver,'UseParallel', ...
                 'always','Display','off','TolCon',...
                 1e-4,'PlotFcns',{@optimplotx, ...
                 @optimplotfval, @optimplotfunccount}, ...
                 'InitBarrierParam', 1e-5) ; 
else
    opt=optimoptions('fmincon','algorithm',solver,...
                 'Display','off','TolCon',...
                 1e-4,'PlotFcns',{@optimplotx, ...
                 @optimplotfval, @optimplotfunccount}, ...
                 'InitBarrierParam', 1e-5) ; 
end
sOpt=optimset('TolFun',1e-3,'TolX',1e-4,...
            'Display','off','PlotFcns',...
            {@optimplotx, @optimplotfval, @optimplotfunccount});

if FIND
% runParallel = 0;
% parm.RP = runParallel ;
% if runParallel == 1
%     % Active matlabpool?
%     matlabpool size ;
%         if ans < 4
%             matlabpool
%         end
% end

fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n')
fprintf('ONE FIFTY NINE FIND VALUES\n')
fprintf('Case\t\tModel\tJ\tKc\tI\tD\tMs_s\tMs\tflag\tt(CPU)\n')
fprintf('------------------------------------------------')
fprintf('------------------------------------------------\n')

% cTypes  = {'po', 'simc', 'smith', 'smithDist'} ;
% cType   = 1 ;
% ctrlrs  = {'pid', 'pi'} ;
% ctrlr   = 1 ;
% cParm   = {'Chriss', 'Parallel', 'AltParallel', 'Cascade'} ;
% cNum    = 1 ;


modelStart = 14 ;          % Always calculate models 1 through 13
modelEnd = 14 ;
cNumVec = [1 4] ; 
for cType = [1 3] 
for ctrlr = [1 2]
    if cType == 3 && ctrlr == 1
        cNumVec = [4] ;                 % Only Smith:pid:cascade
    end
for cNum = cNumVec 
for modelId = modelStart:modelEnd
% for modelId = 12
    Ms_s = 1.59 ;
    G = model(modelId) ;
    parm.G = G ;
    parm.modelId = modelId;
    innerLoop = true ;
    pFlag     = true ;
    loadCorrect = false ;
    fprintf('%s\t%s\t', ...
            strcat( upper(cTypes{cType}), ':', ...
                    upper(ctrlrs{ctrlr}), ':', ...
                    cParm{cNum}),modelName{modelId})
    try
        dat = load([mDir, '/results/', ...
            num2str(cTypes{cType}), '/', ...
            num2str(ctrlrs{ctrlr}), '/', ...
            num2str(cParm{cNum}),   '/', ...
            num2str(modelName{modelId}),'.mat'],...
            'res') ;
    catch
        fprintf('Failed. \n')
        innerLoop = false ;
    end
    % if innerLoop
    this = getfield(dat.res, ...
        strcat(upper(cTypes{cType}),...
        upper(ctrlrs{ctrlr}))) ;
    ctrl = this.minTune(find(this.Ms_span==Ms_s),:) ;
    if numel(ctrl) == 0
        ctrl = this.minTune(find(this.Ms_span==1.6),:) ;
        if numel(ctrl) == 0
            ctrl = this.minTune(15,:) ;
        end
        loadCorrect = false ;
    else
        loadCorrect = true ;
    end
    lb = zeros(size(ctrl,2),1) ;            % Lower tuning bound

    try % Saving the original IAE reference weight
        parm.IAEweights = dat.res.iae0' ;
    catch
        parm.IAEweights = dat.res.IAEweights ;
    end
    parm.Jweights = [0.5 0.5] ;
    parm.cType = cType ;
    parm.ctrlr = ctrlr ;
    parm.cNum  = cNum  ;
    parm.simTime = simTime ;
    parm.dFilter = dFilter ;
    parm.alph    = alph ;
    parm.simCost = simCost ;
    parm.Ms_s = Ms_s ;              % Updating Ms setpoint value
    Jfun      = @(ctrl)costFun(ctrl,parm) ;      % Updating cost
    cFun      = @(ctrl)conFun(ctrl,parm); % Updating constraints
    runOpt = true ;
    if loadCorrect
        J = this.J(find(this.Ms_span==Ms_s)) ;
        optFlag = this.flags(this.Ms_span==Ms_s) ;
        sTime = 0.1 ;
    else
        tic ;
        [ctrl J optFlag] = fmincon(Jfun,ctrl,[],[],[],...
                                   [],lb,[],cFun,opt); 
                            % Optimizing from last optimal point
        sTime = toc ;              % Ending CPU time measurement
        optFlag = 0 ;
        if optFlag < sFlags{sNum}
            tic ;
            fprintf('%s failed. Fminsearching\n\t\t\t', ...
                    solvers{sNum}) 
            Kc = ctrl(1); ID = ctrl(2:end);
            Jfun = @(ID)noGrad([Kc ID], parm) ;
            [ID J optFlag] = fminsearch(Jfun, ID, sOpt) ;
            MsFun = @(Kc) parm.Ms_s - Ms([Kc ID], parm) ;
            Kc = fzero(MsFun, Kc) ;
            ctrl = [Kc ID] ;
            Jfun = @(ctrl)costFun(ctrl, parm) ;  % Updating cost
            cFun = @(ctrl)conFun(ctrl, parm);   % Updating cons. 
            [ctrl J optFlag] = fmincon(Jfun, ctrl, [], ...
                                        [], [], [], lb,...
                                        [], cFun, opt) ;
            sTime = sTime + toc ;
        end
    end
    C       = controller(ctrl, parm) ;              % Controller
    root    = zero(C.tf) ;                          % Find zeros
    minTune = ctrl ;                 % Storing controller tuning
    Msc     = Ms(ctrl, parm) ;            % Calculating Ms value
    Mtc     = Mt(ctrl, parm) ;          % Corresponding Mt-value
    % Finding the IAE trade-off at this point. 
    IAEtune = this.IAEtune(find(this.Ms_span==Ms_s),:) ;
    if numel(IAEtune)==0
        IAEtune = iae(ctrl, parm)'         % Saving IAE weights
    end
    if ctrlr == 1
     fprintf('%.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t%d\t%0.2f',...
        J,ctrl,parm.Ms_s,Msc, optFlag,sTime)
    elseif ctrlr == 2
       fprintf('%.2f\t%.2f\t%.2f\t%s\t%.3f\t%.3f\t%d\t%0.2f',...
        J,ctrl,'-',parm.Ms_s,Msc, optFlag,sTime)
    elseif ctrlr == 3
        fprintf('%.2f\t%.2f\t%s\t%s\t%.3f\t%.3f\t%d\t%0.2f',...
        J,ctrl,'-','-',parm.Ms_s,Msc, optFlag,sTime)
    end
    fprintf('\n')
    sNum = sNum_s ;

    %% Storing results
    res.case = num2str(modelName{modelId});
    res.G          = G ;
    res.cTypes     = cTypes{cType} ;
    res.ctrlr      = ctrlrs{ctrlr} ;
    res.cParm      = cParm{cNum} ;
    res.solver     = solver ;
    res.IAEweights = parm.IAEweights ;

    sub = struct ;
    sub = setfield(sub, 'Msc', Msc) ;
    sub = setfield(sub, 'Mt', Mtc) ;
    sub = setfield(sub, 'minTune', minTune) ;
    sub = setfield(sub, 'J', J) ;
    sub = setfield(sub, 'flags', optFlag) ;
    sub = setfield(sub, 'IAEweights', IAEtune) ;
    sub = setfield(sub, 'Roots', root) ;

    field = strcat(upper(cTypes{cType}),upper(ctrlrs{ctrlr})) ;
    res = setfield(res,field,sub) ;

    % Saving struct and plot
    save([mDir, '/results/oneFiftyNine/',...
            num2str(cTypes{cType}), '/', ...
            num2str(ctrlrs{ctrlr}), '/', ...
            num2str(cParm{cNum}),   '/', ...
            num2str(modelName{modelId}),'.mat'], 'res')
    % end                 % Inner Loop
end                                    % Ending modelId for-loop
end                                      % Ending ctrlr for-loop
end                                      % Ending cType for-loop
end                                       % Ending cNum for-loop

runTime = cputime - runTime;
fprintf('Total computation time: %2.2f minutes', runTime/60)
fprintf(' (%2.2f seconds). \n\n', runTime)
try
    matlabpool close
catch
end

elseif PRINT
    fprintf('-------------------------------------------------')
    fprintf('-----------------------------------------------\n')
    fprintf('ONE FIFTY NINE PRINT VALUES\n')
fprintf('Case\t\t\tModel\tJ\tKc\tI\tD\tMs\tIAE_dout\tIAE_din\n')
    fprintf('-------------------------------------------------')
    fprintf('-----------------------------------------------\n')
    for cType = [1 3] ;
    for ctrlr = [1 2] ;
    for cNum = [1 4]
    for modelId = 14:14
        try
            dat = load([mDir, '/results/oneFiftyNine/', ...
                num2str(cTypes{cType}), '/', ...
                num2str(ctrlrs{ctrlr}), '/', ...
                num2str(cParm{cNum}),   '/', ...
                num2str(modelName{modelId}),'.mat'],...
                'res') ;
            org = load([mDir,'/results/',...
                num2str(cTypes{cType}), '/', ...
                num2str(ctrlrs{ctrlr}), '/', ...
                num2str(cParm{cNum}),   '/', ...
                num2str(modelName{modelId}),'.mat'],...
                'res') ;
            field = strcat(upper(cTypes{cType}), ...
                    upper(ctrlrs{ctrlr})) ;
            this = getfield(dat.res,field) ;
            that = getfield(org,'res') ;
            fprintf('%s\t', ...
                    strcat( upper(cTypes{cType}), ':', ...
                            upper(ctrlrs{ctrlr}), ':', ...
                            cParm{cNum}))
            if (cType == 1 && (cNum == 1 || cNum == 4))
                fprintf('\t')
            end
            fprintf('%s\t',modelName{modelId})
            name = strcat( upper(cTypes{cType}), ':', ...
                            upper(ctrlrs{ctrlr}), ':', ...
                            cParm{cNum}) ;
            caseNum = modelName{modelId} ;
            if ctrlr == 1
       fprintf('%.2f\t%.2f\t%.2f\t%.2f\t%.3f\t%.3f\t\t%0.3f',...
                this.J,this.minTune,this.Msc, this.IAEweights)
                eOut = {'Case','Model','J','Kc','I','D','Ms',...
                    'IAE_dout', 'IAE_din'; 
                    name, caseNum, this.J, this.minTune(1), ...
                    this.minTune(2),this.minTune(3),this.Msc,...
                    this.IAEweights(1), this.IAEweights(2)} ;
            else
         fprintf('%.2f\t%.2f\t%.2f\t%s\t%.3f\t%.3f\t\t%0.3f',...
               this.J,this.minTune,'-',this.Msc,this.IAEweights)
                eOut = {'Case','Model','J','Kc','I','D','Ms',...
                    'IAE_dout', 'IAE_din'; 
                    name, caseNum, this.J, this.minTune(1), ...
                    this.minTune(2),'-',this.Msc,...
                    this.IAEweights(1), this.IAEweights(2)} ;
            end
            fprintf('\n')
            loc = strcat(mDir, '/results/oneFiftyNine/',...
                num2str(cTypes{cType}), '/', ...
                num2str(ctrlrs{ctrlr}), '/', ...
                num2str(cParm{cNum}),   '/', ...
                num2str(modelName{modelId}),'.csv');
            
        catch
            % fprintf('Failed. \n')
            % disp(lasterr)
            innerLoop = false ;
        end
        if (cType == 1 && ctrlr == 1 && cNum == 1)%PO:PID Chriss
            eOut = [that.iaeCtrl(1,:) that.iae0(1) 0 0 0 ;
                    that.iaeCtrl(2,:) that.iae0(2) 0 0 0 ;
                    this.minTune this.J ...
                    this.IAEweights this.Msc] ;
        else
            eOut = [this.minTune,this.J, ...
                    this.IAEweights,this.Msc] ;
        end
        dlmwrite(loc,eOut,'precision','%.2f')
    end % modelId
    end % ctrlr 
    end % cType
    end % cNum

else
    fprintf('No actions taken. Intentional? \n')
end
restoredefaultpath

