function [XU_new,E] = trace_conformance_optctrl__version03(T,Y,X0,U0,f,ed_max,em_max,varargin)
    %T: points of time
    %Y: measured outputs
    %x0: initial guess for first state
    %U0: initial guess for control sequence
    %f: system function dx = f(t,x,ed)
    %h: measurement function y=h(t,x,em)
    %ed_max: magnitude of disturbances
    %em_max: magnitude of measurement errors
    
    N = size(Y,2);%=size(Y,2) number of datapoints
    n = size(X0,1)+size(U0,1);%number of state/ctrl variables
    d = numel(ed_max);%number of disturbance variables
    n_diff = size(X0,1);
    r = round(n_diff/d);%this has to be the relative degree of f wrt e
    %m = numel(em_max);%number of measurement error variables = number of measured output variables
    % = size(Y,1);%

    if size(ed_max,1)==1
        ed_max = ed_max';
    end
    if size(em_max,1)==1
        em_max = em_max';
    end
    ed_max_relaxed = ed_max*1e4;
    ce = 1./ed_max;

    %parameters
    tol = 1e-4;%1e-4
    max_it = 5;%5
    min_it = 0;

    fig = [];
    hdls = [];
    
    do_plot = 0;
    for i = 1:numel(varargin)
        if ischar(varargin{i}) 
            if(strcmp(varargin{i},'do_plot'))
                do_plot = varargin{i+1};
            else
                if(strcmp(varargin{i},'max_it'))
                    max_it = varargin{i+1};
                else
                    if(strcmp(varargin{i},'cost'))
                        ce = varargin{i+1};
                        if size(ce,1)==1
                            ce = ce';
                        end
                    else
                        if(strcmp(varargin{i},'ed_max_relaxed'))
                            ed_max_relaxed = varargin{i+1};
                            if size(ed_max_relaxed,1)==1
                                ed_max_relaxed = ed_max_relaxed';
                            end
                        else
                            if(strcmp(varargin{i},'min_it'))
                                min_it = varargin{i+1};
                            end
                        end
                    end
                end
            end
        end
    end
    
    

    %% initializing the search variable:[...;xi;ui1;...;uir;emi;xj;...], Assume all errors as 0
    XU_new = [X0;U0];
    XU_old = inf(size(XU_new));
    it = 0;
    int_step = simulate2(T,XU_new,f,n,n_diff,d);
    
    
    %% iterate quadprog until convergence
    while (max(max(abs(XU_new-XU_old)))>tol || it<min_it) && it<max_it
        XU_old = XU_new;
        it = it + 1;
        ub = reshape(Y,N*n,1)-reshape(XU_old,N*n,1) + repmat(em_max,N,1);
        lb = reshape(Y,N*n,1)-reshape(XU_old,N*n,1) - repmat(em_max,N,1);
        
        if(it<min_it)%use relaxed constraints to find an initial solution
            [Wq,Wl,A,b,G,h] = cost_and_constraints2(T,Y,XU_old,int_step,f,ed_max_relaxed,ce,n_diff,r);
        else
            [Wq,Wl,A,b,G,h] = cost_and_constraints2(T,Y,XU_old,int_step,f,ed_max,ce,n_diff,r);
        end
        E = h;
        disp('maximum error-1: max(E)=');
        max(abs(reshape(E,r*d,N-1)),[],2)

        dXU = quadprog(Wq,Wl,A,b,[],[],lb,ub);
        XU_new = XU_old + reshape(dXU,n,N);
        if do_plot
            [hdls,fig] = plot_results(hdls,fig,T,Y,XU_new,E,N,n,d,r,varargin{:});
        end
        
        %compute error input, compute quadratic cost term with meaningful constant offset
        E = G*dXU+h;
        Ce = diag(repmat(ce,r*(N-1),1));
        J_new = E'*Ce*E;
        disp(['iteration ' num2str(it) ': J=' num2str(J_new)]);
        disp('maximum error-2: max(E)=');
        max(abs(reshape(E,r*d,N-1)),[],2)

        %simulate again
        int_step = simulate2(T,XU_new,f,n,n_diff,d);

    end

[Wq,Wl,A,b,G,h] = cost_and_constraints2(T,Y,XU_old,int_step,f,ed_max,ce,n_diff,r);
E = h;
disp('maximum error-1: max(E)=');
max(abs(reshape(E,r*d,N-1)),[],2)

end


function [Wq,Wl,A,b,G,h] = cost_and_constraints2(T,Y,XU_old,int_old,f,ed_max,ce,n_diff,r)
    N = size(Y,2);
    n = size(XU_old,1);
    d = size(ed_max,1);
    p = n-n_diff;
    assert(d*r>=n_diff);%errors must be flat inputs (Mie must be invertible)
    %convert from dXU to e: e = G*dXU+h
    G = zeros(r*d*(N-1),N*n);
    h = zeros(r*d*(N-1),1);
    for i = 1:N-1  % control constraints and cost
        %% compute X to e conversion
        dfdx = numerical_jacobi(@(x)f(T(i),x,XU_old(n_diff+(1:p),i),zeros(d,1)),XU_old(1:n_diff,i),1e-6);
        dfdu = numerical_jacobi(@(u)f(T(i),XU_old(1:n_diff,i),u,zeros(d,1)),XU_old(n_diff+(1:p),i),1e-6);
        dfde = numerical_jacobi(@(e)f(T(i),XU_old(1:n_diff,i),XU_old(n_diff+(1:p),i),e),zeros(d,1),1e-6);
        Adtmp = expm([dfdx,dfdu,dfde;zeros(p+d,n+d)]*(T(i+1)-T(i))/r);
        Ad = Adtmp(1:n_diff,1:n_diff);
        Bd = Adtmp(1:n_diff,n_diff+(1:p));
        Ed = Adtmp(1:n_diff,n+(1:d));
        Adr = eye(n_diff);
        Miu = zeros(n_diff,p);
        Mie = zeros(n_diff,d*r);
        for j = 1:r
            Miu = Miu + Adr*Bd;
            Mie(:,(r-j)*d+(1:d)) = Adr*Ed;
            Adr = Ad*Adr;
        end
%         invMie = inv(Mie);
        Miu = -Mie\Miu;
        Mix = -Mie\Adr;
        % chose e1..er in order to fix old integration error
        old_error = XU_old(1:n_diff,i+1)-XU_old(1:n_diff,i)-int_old(1:n_diff,i);
        offseti = Mie\old_error;
        % => [ei1,...,eir] = invMie * dxj + Mix * dxi + Miu * dui + offseti
        block_ei = (i-1)*r*d + (1:r*d);
        block_xj = i*n+(1:n_diff);
        block_xi = (i-1)*n+(1:n_diff);
        block_ui = (i-1)*n+((n_diff+1):n);
        G(block_ei,block_xj) = Mie\eye(size(Mie,1));
        G(block_ei,block_xi) = Mix;
        G(block_ei,block_ui) = Miu;
        h(block_ei,1) = offseti;
    end
    %% The cost function
    % J = E'*Ce*E = (G*dXU+h)'*Ce*(G*dXU+h)
    %   = dXU'* G'*Ce*G  *dXU + 2*h'*Ce*G *dXU + h'*Ce*h
    Ce = diag(repmat(ce,r*(N-1),1));
    Wq = G'*Ce*G;
    Wl = (h'*Ce*G)';
    
    %% The maximum disturbance constraints
    % E<e_max,E-e_max<0,E>-e_max,-E<e_max,-E-e_max<0
    % E = G*dXU+h
    if any(ed_max~=inf)
        E_max = repmat(ed_max,r*(N-1),1);
        A = [G;-G];
        b = -[h-E_max;-h-E_max];
    else
        A = [];
        b = [];
    end
end


function dX = simulate2(T,XU,f,n,n_diff,d)
    N = size(XU,2);
    dX = zeros(n_diff,N-1);
    e0 = zeros(d,1);
    for i = 1:N-1
        j = i + 1;
        dX(:,i) = rk4_(f,T(i),T(j),XU(1:n_diff,i),XU((n_diff+1):n,i),e0) - XU(1:n_diff,i);
    end
end

% classical Runge-Kutte integration
function xj = rk4_(f,ti,tj,xi,ui,ei)
    dt = tj-ti;
    k1 = f(ti,xi,ui,ei);
    k2 = f(ti + dt/2,xi + dt/2*k1,ui,ei);
    k3 = f(ti + dt/2,xi + dt/2*k2,ui,ei);
    k4 = f(ti + dt,xi + dt*k3,ui,ei);
    xj = xi + dt*(k1+2*k2+2*k3+k4)/6;
end





function [hdls,fig] = plot_results(hdls,fig,T,Y,XU,E,N,n,d,r,varargin)
    y_scale = num2cell(ones(1,n));
    y_descriptor = {};for i=1:n+d;y_descriptor{i}=['y_' num2str(i)];end
    
    if(n+d>4)
        rows = ceil((n+d)/2);
        cols = 2;
    else
        rows = n+d;
        cols = 1;
    end

    for i = 1:numel(varargin)
        if ischar(varargin{i}) 
            if(strcmp(varargin{i},'fig'))
                fig = varargin{i+1};
            else
                if(strcmp(varargin{i},'y_scale'))
                    y_scale = varargin{i+1};
                else
                    if(strcmp(varargin{i},'y_descriptor'))
                        y_descriptor = varargin{i+1};
                    else
                    end
                end
            end
        end
    end
    
    if isempty(fig)
        fig = figure(1);
    else
        figure(fig);
    end
    if isempty(hdls)
        create = 1;
    else
        create = 0;
    end
    for i = 1:n
        subplot(rows,cols,i);hold all;box on;grid on;
        if create
            hdls(i) = plot(T,(XU(i,:)-Y(i,:)).*y_scale{i});
        else
            set(hdls(i),'XData',T,'YData',(XU(i,:)-Y(i,:)).*y_scale{i});
        end
        ylabel(y_descriptor{i});
        axis tight;
    end
    E_ = reshape(E,r*d,N-1);
    for i = 1:d
        subplot(rows,cols,i+n);hold all;box on;grid on;
        if create
            hdls(i+n) = plot(E(i:d:end));
        else
            set(hdls(i+n),'YData',E(i:d:end));
        end
        axis tight;
    end
    drawnow;
end