%% Optimization problem 
% v.BareBone-Lund
% written by: Chriss Grimholt 2015
function res = opt_problem(problem)

x0      = problem.x0;
G       = problem.G;

S_fun   = problem.S;
K_fun   = problem.K;
KS_fun  = problem.KS;
GS_fun  = problem.GS;
dK_fun  = problem.dK;

de_di_fun = problem.de_di;
t         = problem.time_gridding;
w         = problem.freqency_gridding;
ms_eqcon  = problem.ms_eqcon;
mt_eqcon  = problem.mt_eqcon;

opt = problem.opt;

[res.tuning, res.cost, res.exitflag, res.output, res.lambda] = fmincon(...
    @(x) obj_fun_grad(x), ... %cost function
    x0, ... % initial guess
    [], ...
    [], ...
    [], ...
    [], ...
    [], ... % lower bounds
    [], ... % upper nounds
    @(x) nonlin_con(x), ... % constraints
    opt);

% calculating usefull numbers
K = K_fun(res.tuning);
S = S_fun(res.tuning);

[e_do] = step(S, t);
iae_do = trapz(t, abs(e_do)); % iae_tuning 

res.iae = [iae_do, res.cost];
res.ms  = max(abs(squeeze(freqresp(S_fun(res.tuning),w))));
res.mt  = max(abs(1-squeeze(freqresp(S_fun(res.tuning),w))));
res.mks = max(abs(squeeze(freqresp(KS_fun(res.tuning),w))));
res.margins = allmargin(G*K);
res.K = K;



    function [J, g_J] = obj_fun_grad(x)
        
        GS = GS_fun(x);
        e_di = step(GS, t);
        J = trapz(t, abs(e_di)); % iae
        
        % if requested, calculate cost function gradients
        if nargout > 1; 
            dGSs = step(de_di_fun(x), t);  % step GS
            g_J = trapz(t, bsxfun(@times, sign(e_di), dGSs)); %dIAE
        end
    end

    function [c, ceq, g_c, g_ceq] = nonlin_con(x)
        
        ceq=[];
        nx = length(x);
        
        % Find corresponding frequency responce
        Sw = squeeze(freqresp(S_fun(x),w));
        Sm = abs(Sw);
        
        % Mt constraint is commented out
        %Tw = 1 - Sw;  %T_fun(m,x,jw);
        %Tm = abs(Tw);
        
        cs = Sm - ms_eqcon;
        %ct = Tm - mt_eqcon;
        
        %c=[cs,ct];
        c = cs;
        
        % if requested, calculate the gradients
        if nargout > 2;
            
            % Empty matrix for the gradients
            g_cs = zeros(nx,length(Sm));
            %g_ct = zeros(nx,length(Tm));
            g_ceq = [];
            
            
            % Finding the complex value of G at grid frequencies
            Gw = squeeze(freqresp(G, w));
            dKw = squeeze(freqresp(dK_fun(x), w)).'; %Non-complex transpose

            for i = 1:nx
                % The constraint sensitivities
                g_cs(i,:) = -Sm.*real(Sw.*Gw.*dKw(:,i));
            %    g_ct(i,:) = Tm.*real((Sw).^2./Tw.*Gw.*dKw(:,i));
            end
            
            %g_c = [g_cs, g_ct];
            g_c = g_cs;
        end
    end
end
