% Compute weights for 4th order height function method
clear; clc

syms x z xik zik dx dz

% Order of expansion
M=4;
N=4;

% Taylor series incluing upto max(M,N) order terms
h=sym(zeros(M+1,N+1));
for n=0:N
    for m=0:M
        h(m+1,n+1)=(x-xik)^m*(z-zik)^n/(factorial(m)*factorial(n));
    end
end

% Allocate Matrices
Z=sym(zeros((M+1)*(N+1),(M+1)*(N+1)));
B=sym(eye  ((M+1)*(N+1),(M+1)*(N+1)));

% Loop over cells in stencil
c=0;
for kp=-2:2
    for ip=-2:2
        c=c+1; % Column number
        fprintf('%i/25 \n',c)
        % Integrate h over this cell to form height
        H=int(int(h,z,zik+(kp-1/2)*dz,zik+(kp+1/2)*dz) ...
            ,x,xik+(ip-1/2)*dx,xik+(ip+1/2)*dx)/(dx*dz);
        % Get coefficents from F
        Z(:,c)=H(:);
    end
end

% Solve for A's (in a vector)
Av=Z\B;

%%   Weights for needed derivatives    %
% ------------------------------------ % 
c=0;
for n=0:N
    for m=0:M
        c=c+1;
        if     m==1 && n==0;  hx =reshape(Av(:,c),5,5); % h_x
        elseif m==0 && n==1;  hz =reshape(Av(:,c),5,5); % h_z
        elseif m==2 && n==0;  hxx=reshape(Av(:,c),5,5); % h_xx
        elseif m==0 && n==2;  hzz=reshape(Av(:,c),5,5); % h_zz
        elseif m==1 && n==1;  hxz=reshape(Av(:,c),5,5); % h_xz
        end
    end
end

%% Test accuracy
figure(1); clf(1)
syms x z
% Function
f=x^5+z^5+2*x+3*z+cos(x)*cos(2*z);
% Location to compute derivative
xo=1;
zo=1;

% Compute integral of f for construction of heights
syms z1 z2 x1 x2
intf=matlabFunction(int(int(f,z,z1,z2),x,x1,x2),'Vars',[x1,x2,z1,z2]);

% Loop over derivatives to test
c=0;
for n=0:N % d/dy^n
    for m=0:M % d/dx^m
        c=c+1;
        % Get correct A's for this derivative
        A=reshape(Av(:,c),5,5);
        
        % Grid size to compute heights on
        dxs=[1,1/2,1/4,1/8,1/16,1/32,1/64];
        error=zeros(1,length(dxs));
        for ndx=1:length(dxs);
            dx=dxs(ndx);
            dz=dxs(ndx);
            exact=subs(diff(diff(f,z,n),x,m),[x,z],[xo,zo]);
            % Compute heights
            ht=zeros(5,5);
            for j=-2:2
                for i=-2:2
                    ht(i+3,j+3)=intf(xo+(i-1/2)*dx,xo+(i+1/2)*dx,zo+(j-1/2)*dz,zo+(j+1/2)*dz)/(dx*dz);
                end
            end
            
            % Perform sum(A*heights) (Evaluates A with this dx/dy)
            computed=sum(sum(eval(A).*ht));
            
            % Analysis
            error(ndx)=abs(exact-computed);
            %fprintf('Exact=%5.5f, Computed=%5.5f, Error=%5.5e\n',exact,computed,error(ndx))
        end
        subplot(N+1,M+1,c)
        loglog(dxs,error,'-o')
        hold on
        loglog(dxs,dxs.^2)
        title(['d/dx^',num2str(m),' d/dy^',num2str(n)])
        drawnow
    end
end

%% Convert to Fortran notation (Removing 
dhs={'hx','hz','hxx','hzz','hxz'};
dx=1;
dz=1;
for n=1:length(dhs)
    dh=eval(dhs{n});
    for kp=-2:2
        for ip=-2:2
            strs{ip+3}=eval(dh(ip+3,kp+3));
        end
        fprintf('data dhd%s(:,%+i) / %+5.15e_WP, %+5.15e_WP, %+5.15e_WP, %+5.15e_WP, %+5.15e_WP / \n',dhs{n}(2:end),kp,strs{:})
    end
end
        


    

