%% Header
% Purpose:	Cost function for Smith Disturbance calculations
% Author:   Axel Lodemel Holene (based on work done by Martin S.
%           Foss (2012)). 
% E-mail:	axel.holene@gmail.com
% Date:     14. February 2013
% About:    J = [Jw_y Jw_d]*[IAE_y IAE_d]./[IAEw_y IAEw_d]
%           ctrl =          struct of controller starting points
%                  Kc                            Controller gain
%                  tau_i                         Integral action
%                  tau_d                       Derivative action
%           Jweights   ([Jw_y Jw_d])       Cost function weights
%           IAEweights ([IAEw_y IAEw_d])           IAE weighting

function J = distCostFun(C, G, parm)
    try
        S = 1/(1+G.tf*C.tf) ;     % Ouput feedback loop tf(y->e)
        Sd = -G.tf/(1+G.tf*C.tf) ;% Input feedback loop tf(d->e)
    catch
        fprintf('This is costFun. Error in loop TFs.\n')
    end

    if parm.simCost == 0
        try
            [e t] = step([S; Sd], parm.simTime) ;
            iae0 = trapz(t, abs(e))' ;
        catch
            iae0 = [100; 100] ;% Penalize bad tuning from solver
            if parm.dispErrors
                fprintf('This is costFun catch. ')
                fprintf('Error in step function: %s\n', ...
                        lasterr) ;
            end
        end
    else
        try
            simOut = sim('simulink/iaeBlock', ...
                 'SrcWorkspace', 'current') ;
            iae_y = get(simOut,'iae_y') ;
            iae_d = get(simOut,'iae_d') ;
        catch
            iae_y = 100 ;      % Penalize bad tuning from solver
            iae_d = 100 ;
            if parm.dispErrors
                fprintf('This is distCostFun catch. ')
                fprintf('Error in simulink simulation: %s\n',...
                        lasterr) ;
            end
        end
        iae0 = [iae_y(end); iae_d(end)] ;
    end
    J = parm.Jweights*(iae0./parm.IAEweights) ;
end