function [deltax,epshat]=sofmAdaptStep(fun,y,x,CovVec,CovVal,deltax0, epshat0)
%Written by Jan Christoph Krueger, Hamburg University of Technology
%Implementation of a step size procedure for the principal sensitivity second-order
%fourth-moment method. Theoretical details are given in

%OUTPUT
% deltax: proposed step size deltax
% epshat: proposed step size epshat
%INPUT
% fun: objective function such that [f, dfdy, dfdx]=fun(y,x);
% y: Current design variables
% x: Current random variables
%CovVec: Matrix including the eigenvectors of the Covariance
%matrix
%CovVal: Column-Vector of eigenvalues of the Covariance matrix
%deltax0: Initial guess for Step Size deltax
%epsHat0: initial guess for normalized step size of the second order principal
%sensitivity directions

    interval = 2;
    %% optimize Step size step1
    %Optimize step 1 such that approximation of the hessian matrix is most
    %accurate
    %Evaluate error at small step size step 1
    err1 = SOFM_PS_err1(@(y,x) fun(y,x), y,x,CovVec,deltax0*10^(interval));

    %Evaluate error at small step size step 1
    err2 = SOFM_PS_err1(@(y,x) fun(y,x), y,x,CovVec,deltax0*10^(-interval));
    deltax = (err2 - err1 + interval + 3*log10(deltax0)) / 3;
    deltax = 10^deltax;

    %% Optimize step size step 2
    %Method: Compare second order and first order approximation using
    %output erStep1

    %Evaluate error at small step size step 2
    err1 = SOFM_PS_err2(@(y,x) fun(y,x), y,x,CovVec,CovVal,deltax,epshat0*10^(interval));

    %Evaluate error at small step size step 2
    err2 = SOFM_PS_err2(@(y,x) fun(y,x), y,x,CovVec,CovVal,deltax,epshat0*10^(-interval));
    epshat = (err2 - err1 + 2*log10(epshat0)) / 2;
    epshat = 10^epshat;
end

function erxx=SOFM_PS_err1(fun,y,x,CovVec,step1)
    %Compute the error of the hessian matrix at a given step size
    lx=size(CovVec,2);
    dfxx=zeros(lx,lx);
    erxx=0;

    dfxdata=zeros(lx,2*lx);
    %Step 1: Function evaluations at perturbed points
    dist=[-step1*CovVec,step1*CovVec,-2*step1*CovVec,2*step1*CovVec];
    for i=1:4*lx
        [~,~,dfx1]=fun(y,x+dist(:,i));
        dfxdata(:,i)=CovVec'*dfx1;
    end
    %Step 2: process data to compute the error of the hessian
    for i=1:lx
        dfxP=dfxdata(:,lx+i);
        dfxN=dfxdata(:,i);
        dfxNN=dfxdata(:,2*lx+i);
        dfxPP=dfxdata(:,3*lx+i);
        %Central differences with error order 2
        dfxx(:,i)=(dfxP-dfxN)/(2*step1);
        %Central differences with error order 4
        dfxx1=(8*dfxP-8*dfxN+dfxNN-dfxPP)/(12*step1);
        erxx=erxx+norm(dfxx1-dfxx(:,i))/norm(dfxx1); %Estimated error of hessian matrix
    end
    erxx=log10(erxx/lx);
end

function erStep2=SOFM_PS_err2(fun,y,x,CovVec,CovVal,step1,epshat)
%Compute the error of the SO-variance gradient caused by the step size
%epshat
    lx=size(CovVec,2); ly=length(y);
    dfxx=zeros(lx,lx); dfyx=zeros(ly,lx);
    dfydata=zeros(ly,2*lx); dfxdata=zeros(lx,2*lx);

    %Step 1: Function evaluation at perturbed design
    dist=[-step1*CovVec,step1*CovVec];
    for i=1:2*lx
        [~,dfydata(:,i),dfx1]=fun(y,x+dist(:,i));
        dfxdata(:,i)=CovVec'*dfx1;
    end
    %Step 2: Process function values
    for i=1:lx
        dfy1=dfydata(:,lx+i); dfx1=dfxdata(:,lx+i);
        dfy2=dfydata(:,i); dfx2=dfxdata(:,i);
        dfxx(:,i)=(dfx1-dfx2)/(2*step1); dfyx(:,i)=(dfy1-dfy2)/(2*step1); %Da Daten sowieso vorhanden nutze zentrale Differenzen
    end

    %Step 3: Compute and scale second order principal sensitivity directions
    deltax=CovVal.*dfxx.*CovVal';
    epsi=epshat./sqrt(sum(deltax.^2));
    deltax=deltax.*epsi;
    
    %Step 4: Function evaluations at perturbed designs
    dfyx2=zeros(ly,lx);
    dist=[-step1*CovVec+CovVec*deltax,step1*CovVec+CovVec*deltax];
    for i=1:2*lx
        [~,dfydata(:,i)]=fun(y,x+dist(:,i));
    end
    %Step 5: Process function values to compute the gradient of the
    %variance using the method with convergence order 1
    for i=1:lx
        dfy2=dfydata(:,lx+i); dfy3=dfydata(:,i);
        dfyx2(:,i)=(dfy2-dfy3)/(2*step1); 
    end
    dsigSOFM=sum((dfyx2-dfyx)./epsi,2);

    %Step 5: Process and compute function values to compute the gradient of the
    %variance using the method with convergence order 2
    dist=[-step1*CovVec-CovVec*deltax,step1*CovVec-CovVec*deltax];
    dfyx3=zeros(ly,lx);
    for i=1:2*lx
        [~,dfydata(:,i)]=fun(y,x+dist(:,i));
    end
    for i=1:lx
        dfy2=dfydata(:,lx+i); dfy3=dfydata(:,i);
        dfyx3(:,i)=(dfy2-dfy3)/(2*step1);
    end
    dsigSOFM2=sum((dfyx2-dfyx3)./(2*epsi),2);

    %Compute error by comparison of first order and second order
    %approximation
    erStep2=norm(dsigSOFM2-dsigSOFM,1)./norm(dsigSOFM2,1);
    erStep2=log10(erStep2);
end