%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test interpolation with trlinear coefficients
%
Nx				= 11;
Ny				= 11;
Nz				= 11;
N				= 1000;

%% test with linear function
fprintf('Testing with linear function on random grid ... \n');
A				= [randn(1,3)];
x_grid		= sort([0, rand(1,Nx-2), 1], 'ascend');
y_grid		= sort([0, rand(1,Ny-2), 1], 'ascend');
z_grid		= sort([0, rand(1,Nz-2), 1], 'ascend');

[xm,ym,zm]	= ndgrid(x_grid, y_grid, z_grid);
f_grid		= A(1)*xm + A(2)*ym + A(3)*zm;

xi				= rand(3,N);
W				= trilinear_coeffs(x_grid, y_grid, z_grid, xi);
fi_coeff		= (W * f_grid(:)).';
fi_exact		= A * xi;

fi_diff		= fi_exact - fi_coeff;
fprintf('Norm of error:    %0.3f\n', norm(fi_diff));
fprintf('Norm of function: %0.3f\n', norm(fi_exact));

%% test with quadratic function on regular grid
x_grid		= linspace(0, 1, Nx);
y_grid		= linspace(0, 1, Ny);
z_grid		= linspace(0, 1, Nz);
A				= randn(3,3);
[xm,ym,zm]	= ndgrid(x_grid, y_grid, z_grid);
xyzm			= [xm(:), ym(:), zm(:)].';
f_grid		= sum(xyzm .* (A * xyzm), 1);
f_grid		= reshape(f_grid, [Nx Ny Nz]);


xi				= rand(3, N);
W				= trilinear_coeffs(x_grid, y_grid, z_grid, xi);
fi_coeff		= (W * f_grid(:)).';
%fi_interp3  = interp3(x_grid, y_grid, z_grid, permute(f_grid,[2 1 3]), xi(1,:), xi(2,:), xi(3,:));
fi_interp3  = interpn(x_grid, y_grid, z_grid, f_grid, xi(1,:), xi(2,:), xi(3,:));
fi_exact		= sum(xi .* (A * xi), 1);

fi_diff		= fi_interp3 - fi_coeff;
fprintf('Norm of error:    %0.3f\n', norm(fi_diff));
fprintf('Norm of function: %0.3f\n', norm(fi_exact));

%% plot
figure(1);
clf;
surf(xm(:,:,1), ym(:,:,1), f_grid(:,:,1));
hold on;
plot3(xi(1,:), xi(2,:), fi_coeff, 'ko');
plot3(xi(1,:), xi(2,:), fi_exact, 'k.');