n_x0  = 31;
n_y0  = 31;
x0    = linspace(-3, 3, n_x0);
y0    = linspace(-3, 3, n_y0);
sig   = 1;
res   = 11;

%% integrate
f_exact=zeros(n_x0, n_y0);
f_bicubic=zeros(n_x0, n_y0);
f_trapz =zeros(n_x0, n_y0);

fx_exact=zeros(n_x0, n_y0);
fy_exact=zeros(n_x0, n_y0);
fx_bicubic=zeros(n_x0, n_y0);
fy_bicubic=zeros(n_x0, n_y0);


phi   = @(r) exp(-r.^2/sig^2);
dphi  = @(r) (-2*r/sig^2).*exp(-r.^2/sig^2);
d2phi = @(r) (4*r.^2/sig^4).*exp(-r.^2/sig^2) - 2/sig^2*exp(-r.^2/sig^2);

for i = 1 : n_x0
    for j = 1 : n_y0
        x_lim = [-0.5 0.5] - x0(i);
        y_lim = [-0.5 0.5] - y0(j);

        %% integrate exact
        f_exact(i,j)= 1/4*pi*sig^2 ...
                * (erf(x_lim(2)/sig) - erf(x_lim(1)/sig)) ...
                * (erf(y_lim(2)/sig) - erf(y_lim(1)/sig));
        fx_exact(i,j)= 1/2*sqrt(pi)*sig ...
                * (exp(-x_lim(2)^2/sig^2) - exp(-x_lim(1)^2/sig^2)) ...
                * (erf(y_lim(2)/sig) - erf(y_lim(1)/sig));
        fy_exact(i,j)= 1/2*sqrt(pi)*sig ...
                * (exp(-y_lim(2)^2/sig^2) - exp(-y_lim(1)^2/sig^2)) ...
                * (erf(x_lim(2)/sig) - erf(x_lim(1)/sig));
            
        %% bicubic method
        [f_bicubic(i,j), fx_bicubic(i,j), fy_bicubic(i,j)]=integrate_bicubic(phi, dphi, d2phi, x_lim, y_lim);
        %% on refined grid
        xg = linspace(x_lim(1), x_lim(2), res);
        yg = linspace(y_lim(1), y_lim(2), res);
        [xm,ym] = ndgrid(xg,yg);
        phig= phi(sqrt(xm.^2+ym.^2));
        f_trapz(i,j) = trapz(xg, trapz(yg, phig, 2), 1);
    end
end

f_res_bicubic=f_bicubic-f_exact;
f_res_trapz  =f_trapz-f_exact;
f_res_norm_bicubic=rms(f_res_bicubic(:));
f_res_norm_trapz  =rms(f_res_trapz(:));
fprintf('bicubic residual %f (%f%%)\n', f_res_norm_bicubic, 100*f_res_norm_bicubic/rms(f_exact(:)));
fprintf('trapz   residual %f (%f%%)\n', f_res_norm_trapz  , 100*f_res_norm_trapz  /rms(f_exact(:)));

disp(rms(fx_exact(:)-fx_bicubic(:))/rms(fx_exact(:))*100);
disp(rms(fy_exact(:)-fy_bicubic(:))/rms(fy_exact(:))*100);
%% check derivative
figure(2);
clf;
r_vec=linspace(0,3,1000);
f_r=phi(r_vec);
df_r = dphi(r_vec);
d2f_r = d2phi(r_vec);
df_fd=gradient(f_r,r_vec);
d2f_fd=gradient(df_fd,r_vec);


plot(r_vec, df_r, '-', r_vec,df_fd, '.');
hold on;
plot(r_vec, d2f_r, '-', r_vec, d2f_fd, '.');

%% plot
figure(1);
subplot(2,3,1);
imagesc(x0, y0, f_exact.');
axis equal tight xy;
colorbar;
caxis([0 1]);
title('exact');


subplot(2,3,2);
imagesc(x0, y0, fx_exact.');
axis equal tight xy;
colorbar;
title('exact');

subplot(2,3,3);
imagesc(x0, y0, fy_exact.');
axis equal tight xy;
colorbar;
title('exact');


subplot(2,3,4);
imagesc(x0, y0, f_bicubic.');
axis equal tight xy;
colorbar;
caxis([0 1]);
title('bicubic');

subplot(2,3,5);
imagesc(x0, y0, fx_bicubic.');
axis equal tight xy;
colorbar;
title('bicubic dx')


subplot(2,3,6);
imagesc(x0, y0, fy_bicubic.');
axis equal tight xy;
colorbar;
title('bicubic dy')